cobboard no move mode

diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 515f3d5..669c4c8 100644 (file)
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -253,11 +253,22 @@ static void cmd_state2_parsed(void *parsed_result,
                state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
                                  I2C_COBBOARD_SPK_AUTOHARVEST);
        }
+       else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+               state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+                                 I2C_COBBOARD_SPK_NO_MOVE);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+               state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+                                 I2C_COBBOARD_SPK_AUTOHARVEST |
+                                 I2C_COBBOARD_SPK_NO_MOVE);
+       }
 }
 
 prog_char str_state2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "harvest#deploy#pack";
+prog_char str_state2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 prog_char str_state2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index f8e1613..7296d8e 100644 (file)
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -137,7 +137,6 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2)
        spickle.k2 = k2;
 }
 
-
 void spickle_set_pos(uint8_t side, int32_t pos_packed,
                     int32_t pos_mid, int32_t pos_deployed)
 {
@@ -159,6 +158,32 @@ void spickle_dump_params(void)
                 spickle.pos_deployed[I2C_RIGHT_SIDE]);
 }
 
+static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
+{
+       int32_t diff;
+       int32_t enc;
+       if (side == I2C_LEFT_SIDE)
+               enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
+       else
+               enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
+       diff = pos - enc;
+       if (diff < 0)
+               diff = -diff;
+       if (diff < 500)
+               return 1;
+       return 0;
+}
+
+uint8_t spickle_is_packed(uint8_t side)
+{
+       return spickle_is_at_pos(side, spickle.pos_packed[side]);
+}
+
+uint8_t spickle_is_deployed(uint8_t side)
+{
+       return spickle_is_at_pos(side, spickle.pos_deployed[side]);
+}
+
 void spickle_deploy(uint8_t side)
 {
        cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);

diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h
index 81c74d8..f34894a 100644 (file)
--- a/projects/microb2010/cobboard/spickle.h
+++ b/projects/microb2010/cobboard/spickle.h
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation (2010)
- * 
+ *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
@@ -29,6 +29,9 @@ void spickle_set_pos(uint8_t side, int32_t pos_pack,
 
 void spickle_dump_params(void);
 
+uint8_t spickle_is_packed(void);
+uint8_t spickle_is_deployed(void);
+
 void spickle_deploy(uint8_t side);
 void spickle_mid(uint8_t side);
 void spickle_pack(uint8_t side);

diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index 86afce9..e315da0 100644 (file)
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -96,7 +96,7 @@ static uint8_t state_no_cob_inside(void)
                !sensor_get(S_COB_INSIDE_R);
 }
 
-static uint8_t is_deployed(uint8_t side)
+static uint8_t state_spicklemode_deployed(uint8_t side)
 {
        if (side == I2C_LEFT_SIDE)
                return lspickle & I2C_COBBOARD_SPK_DEPLOY;
@@ -104,7 +104,7 @@ static uint8_t is_deployed(uint8_t side)
                return rspickle & I2C_COBBOARD_SPK_DEPLOY;
 }
 
-static uint8_t is_autoharvest(uint8_t side)
+static uint8_t state_spicklemode_autoharvest(uint8_t side)
 {
        if (side == I2C_LEFT_SIDE)
                return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
@@ -112,14 +112,22 @@ static uint8_t is_autoharvest(uint8_t side)
                return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
 }
 
+static uint8_t state_spicklemode_nomove(uint8_t side)
+{
+       if (side == I2C_LEFT_SIDE)
+               return lspickle & I2C_COBBOARD_SPK_NO_MOVE;
+       else
+               return rspickle & I2C_COBBOARD_SPK_NO_MOVE;
+}
+
 /* pack/deploy spickles depending on mode */
 static void spickle_prepare(uint8_t side)
 {
-       if (cob_count >= 5)
-               spickle_pack(side);
-       else if (is_deployed(side) && !is_autoharvest(side))
-               spickle_deploy(side); /*spickle_mid(side);*/
-       else if (is_deployed(side) && is_autoharvest(side))
+       /* we do nothing in mode no out */
+       if (state_spicklemode_nomove(side))
+               return;
+
+       if (state_spicklemode_deployed(side))
                spickle_deploy(side);
        else
                spickle_pack(side);
@@ -247,12 +255,15 @@ static void state_do_harvest(uint8_t side)
        cobroller_off(side);
        cob_count ++;
 
+       state_debug_wait_key_pressed();
+
        /* last cob, nothing to do */
        if (cob_count == 5)
                return;
 
-       /* redeploy the spickle */
-       spickle_deploy(side);
+       /* redeploy the spickle if there is a black cob */
+       if (!state_spicklemode_nomove(side))
+               spickle_deploy(side);
        state_debug_wait_key_pressed();
 
        /* let the loaded cobs go */
@@ -326,11 +337,13 @@ void state_machine(void)
 
                /* harvest */
                if (cob_count < 5) {
-                       if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) &&
-                           (lspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+                       if (state_spicklemode_deployed(I2C_LEFT_SIDE) &&
+                           state_spicklemode_autoharvest(I2C_LEFT_SIDE) &&
+                           !state_spicklemode_nomove(I2C_LEFT_SIDE))
                                state_do_harvest(I2C_LEFT_SIDE);
-                       if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) &&
-                           (rspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+                       if (state_spicklemode_deployed(I2C_RIGHT_SIDE) &&
+                           state_spicklemode_autoharvest(I2C_RIGHT_SIDE) &&
+                           !state_spicklemode_nomove(I2C_RIGHT_SIDE))
                                state_do_harvest(I2C_RIGHT_SIDE);
                }
 

diff --git a/projects/microb2010/common/avr6.x b/projects/microb2010/common/avr6.x
index a758e96..3ad392f 100644 (file)
--- a/projects/microb2010/common/avr6.x
+++ b/projects/microb2010/common/avr6.x
@@ -123,6 +123,18 @@ SECTIONS
     KEEP (*(.init9))
     *(.text.*) /* trucs de gcc ? */
     . = ALIGN(2048);
+    /* some libc stuff */
+    strc*(.text)
+    mem*(.text)
+    printf*(.text)
+    vfprintf*(.text)
+    sprintf*(.text)
+    snprintf*(.text)
+    malloc*(.text)
+    free*(.text)
+    fdevopen*(.text)
+    fputc*(.text)
+    . = ALIGN(2048);
     uart*(.text)
     pwm*(.text)
     parse*(.text)
@@ -151,18 +163,6 @@ SECTIONS
     spi*(.text)
     ax12*(.text)
     . = ALIGN(2048);
-    /* some libc stuff */
-    str*(.text)
-    mem*(.text)
-    printf*(.text)
-    vfprintf*(.text)
-    sprintf*(.text)
-    snprintf*(.text)
-    malloc*(.text)
-    free*(.text)
-    fdevopen*(.text)
-    fputc*(.text)
-    . = ALIGN(2048);
     *(.text)
     . = ALIGN(2);
     *(.fini9)  /* _exit() starts here.  */

diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index fa17fca..d64ed8b 100644 (file)
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -108,6 +108,7 @@ struct i2c_req_cobboard_status {
 
 #define I2C_COBBOARD_SPK_DEPLOY  0x01 /* deploy the spickle */
 #define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */
+#define I2C_COBBOARD_SPK_NO_MOVE 0x04 /* if enabled, don't change state */
        uint8_t lspickle;
        uint8_t rspickle;
 };

diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 255f26b..e79562b 100644 (file)
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -82,6 +82,7 @@ extern parse_pgm_inst_t cmd_beacon_start;
 extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_clitoid;
 extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_strat_event;
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -173,6 +174,7 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_servo_balls,
        (parse_pgm_inst_t *)&cmd_clitoid,
        (parse_pgm_inst_t *)&cmd_time_monitor,
+       (parse_pgm_inst_t *)&cmd_strat_event,
        (parse_pgm_inst_t *)&cmd_test,
 
        /* commands_traj.c */

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index ee0ffa4..13a327a 100644 (file)
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -785,11 +785,19 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
                i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
                i2c_cobboard_pack(side);
        }
+       else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_deploy_nomove(side);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_autoharvest_nomove(side);
+       }
 }
 
 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
@@ -1164,6 +1172,44 @@ parse_pgm_inst_t cmd_time_monitor = {
        },
 };
 
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_event_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("on")))
+               strat_event_enable();
+       else
+               strat_event_disable();
+}
+
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+       .f = cmd_strat_event_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_event,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_event_arg0,
+               (prog_void *)&cmd_strat_event_arg1,
+               NULL,
+       },
+};
+
 /**********************************************************/
 /* Test */
 

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 9908f1a..7a2c378 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -438,6 +438,20 @@ int8_t i2c_cobboard_deploy(uint8_t side)
        return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);
 }
 
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side)
+{
+       return i2c_cobboard_set_spickle(side,
+                                       I2C_COBBOARD_SPK_DEPLOY |
+                                       I2C_COBBOARD_SPK_AUTOHARVEST |
+                                       I2C_COBBOARD_SPK_NO_MOVE);
+}
+
+int8_t i2c_cobboard_deploy_nomove(uint8_t side)
+{
+       return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+                                       I2C_COBBOARD_SPK_NO_MOVE);
+}
+
 int8_t i2c_ballboard_set_mode(uint8_t mode)
 {
        struct i2c_cmd_ballboard_set_mode buf;

diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index b107c44..06cd47e 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -39,6 +39,9 @@ int8_t i2c_cobboard_set_mode(uint8_t mode);
 int8_t i2c_cobboard_pack(uint8_t side);
 int8_t i2c_cobboard_autoharvest(uint8_t side);
 int8_t i2c_cobboard_deploy(uint8_t side);
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side);
+int8_t i2c_cobboard_deploy_nomove(uint8_t side);
+
 int8_t i2c_ballboard_set_mode(uint8_t mode);
 
 #endif

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 423b0f7..94b9b6f 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -67,7 +67,7 @@
 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
 #define COL_SCAN_PRE_MARGIN 250
 
-static uint8_t strat_running = 0;
+static volatile uint8_t strat_running = 0;
 struct strat_conf strat_conf;
 
 /*************************************************************/
@@ -98,6 +98,16 @@ void strat_conf_dump(const char *caller)
 
 }
 
+void strat_event_enable(void)
+{
+       strat_running = 1;
+}
+
+void strat_event_disable(void)
+{
+       strat_running = 0;
+}
+
 /* call it just before launching the strat */
 void strat_init(void)
 {
@@ -173,11 +183,9 @@ void strat_event(void *dummy)
 
        /* detect cob on left side */
        if (corn_is_near(&lidx, I2C_LEFT_SIDE)) {
-               if (lcob != I2C_COB_NONE) {
+               if (lcob != I2C_COB_NONE)
                        corn_set_color(strat_db.corn_table[lidx], lcob);
-                       DEBUG(E_USER_STRAT, "lcob %s %d",
-                             lcob == I2C_COB_WHITE ? "white" : "black", lidx);
-               }
+
                if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
                        i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
                else
@@ -189,11 +197,9 @@ void strat_event(void *dummy)
 
        /* detect cob on right side */
        if (corn_is_near(&ridx, I2C_RIGHT_SIDE)) {
-               if (rcob != I2C_COB_NONE) {
+               if (rcob != I2C_COB_NONE)
                        corn_set_color(strat_db.corn_table[ridx], rcob);
-                       DEBUG(E_USER_STRAT, "rcob %s %d",
-                             rcob == I2C_COB_WHITE ? "white" : "black", ridx);
-               }
+
                if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
                        i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
                else
@@ -216,12 +222,18 @@ static uint8_t strat_harvest(void)
 
 static uint8_t strat_eject(void)
 {
+       uint8_t err;
+
+       //XXX return vals
+       strat_set_speed(600, SPEED_ANGLE_SLOW);
+
        trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
-       err = wait_traj_end(END_INTR|END_TRAJ);
+       err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+       trajectory_a_abs(&mainboard.traj, COLOR_A(70));
+       err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 
        DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
        strat_hardstop();
-       strat_set_speed(600, SPEED_ANGLE_FAST);
 
        /* ball ejection */
        trajectory_a_abs(&mainboard.traj, COLOR_A(90));
@@ -263,6 +275,7 @@ static uint8_t strat_beginning(void)
                                    TRAJ_FLAGS_STD);
 
        strat_set_acc(ACC_DIST, ACC_ANGLE);
+       strat_set_speed(250, SPEED_ANGLE_SLOW);
 
 #if 1
  l1:

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 9fa720a..daf607c 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -162,5 +162,7 @@ void strat_dump_flags(void);
 void strat_goto_near(int16_t x, int16_t y, uint16_t dist);
 uint8_t strat_main(void);
 void strat_event(void *dummy);
+void strat_event_enable(void);
+void strat_event_disable(void);
 
 #endif

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index f8a9666..8d3f839 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -218,6 +218,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        strat_get_speed(&d_speed, &a_speed);
 
        /* XXX 600 -> cste */
+       /* XXX does not work, do better */
 /*     if (err == 0 && d_speed < 600 && */
 /*         mainboard.traj.state == RUNNING_CLITOID_LINE) */
 /*             strat_set_speed(600, SPEED_ANGLE_FAST); */