state machine and support balls

diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c
index 6952a7435df625ee9c1a4a49a4a30249c45f1943..feeb32ff8c5d52eab364dd18104d793f49bbf07c 100644 (file)
--- a/projects/microb2010/cobboard/commands.c
+++ b/projects/microb2010/cobboard/commands.c
@@ -76,6 +76,8 @@ extern parse_pgm_inst_t cmd_servo_carry;
 extern parse_pgm_inst_t cmd_spickle;
 extern parse_pgm_inst_t cmd_spickle_params;
 extern parse_pgm_inst_t cmd_spickle_params_show;
+extern parse_pgm_inst_t cmd_spickle_params2;
+extern parse_pgm_inst_t cmd_spickle_params2_show;
 extern parse_pgm_inst_t cmd_shovel;
 extern parse_pgm_inst_t cmd_test;
 
@@ -135,6 +137,8 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_spickle,
        (parse_pgm_inst_t *)&cmd_spickle_params,
        (parse_pgm_inst_t *)&cmd_spickle_params_show,
+       (parse_pgm_inst_t *)&cmd_spickle_params2,
+       (parse_pgm_inst_t *)&cmd_spickle_params2_show,
        (parse_pgm_inst_t *)&cmd_shovel,
        (parse_pgm_inst_t *)&cmd_test,
 

diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
index 53f2ab7d37760c7ecb1043e0d6788876a595fb5e..77846d098bb24b5e5b47cf0eeec67c85f66a8225 100644 (file)
--- a/projects/microb2010/cobboard/i2c_protocol.c
+++ b/projects/microb2010/cobboard/i2c_protocol.c
@@ -93,11 +93,13 @@ static void i2c_send_status(void)
 
        /* status */
        ans.mode = state_get_mode();
-       ans.status = 0x55;
+       ans.status = 0x55; /* TODO */
 
        ans.left_cobroller_speed = cobboard.left_cobroller_speed;
        ans.right_cobroller_speed = cobboard.right_cobroller_speed;
 
+       ans.cob_count = state_get_cob_count();
+;
        i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
                 sizeof(ans), I2C_CTRL_GENERIC);
 }

diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c
index 0889320ad8dafc36d94ca4407bece1bb976b31ad..eec58b174f40ba44a25dc1aa177ac1dfe7a70b3f 100644 (file)
--- a/projects/microb2010/cobboard/sensor.c
+++ b/projects/microb2010/cobboard/sensor.c
@@ -146,7 +146,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = {
        [S_FRONT] =     { 5, 0, 4, 1, 0, 0 },  /* 1 */
        [S_CAP3] =      { 10, 0, 3, 7, 0, 0 }, /* 2 */
        [S_CAP4] =      { 1, 0, 0, 1, 0, 0 }, /* 3 */
-       [S_LCOB] =      { 1, 0, 0, 1, 0, 1 }, /* 4 */
+       [S_LCOB] =      { 1, 0, 0, 1, 0, 0 }, /* 4 */
        [S_LEFT] =      { 5, 0, 4, 1, 0, 0 }, /* 5 */
        [S_RIGHT] =     { 5, 0, 4, 1, 0, 1 }, /* 6 */
        [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index 03565ac63a2f35021dbfacce82eee901eb66f03d..3686eb70173a23c3fdac1f9ae641b701cd760e6c 100644 (file)
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -176,7 +176,7 @@ void spickle_deploy(uint8_t side)
 
 void spickle_pack(uint8_t side)
 {
-       cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
+       cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]);
 }
 
 uint16_t spickle_get_deploy_delay(uint8_t side)

diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index d02e54440a0aca434409f4748e1680fd5c1c6aad..4c9651edc5230bf606ced0f01c4952dd71edf985 100644 (file)
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -68,6 +68,7 @@ static uint8_t cob_count;
 #define R_HARVEST(mode)  (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST))
 #define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode))
 #define EJECT(mode)      (!!((mode) & I2C_COBBOARD_MODE_EJECT))
+#define INIT(mode)       (!!((mode) & I2C_COBBOARD_MODE_INIT))
 
 uint8_t state_debug = 0;
 
@@ -78,6 +79,11 @@ static void state_dump_sensors(void)
 }
 #endif
 
+uint8_t state_get_cob_count(void)
+{
+       return cob_count;
+}
+
 static void state_debug_wait_key_pressed(void)
 {
        if (!state_debug)
@@ -187,10 +193,20 @@ void state_machine(void)
 {
        while (state_want_exit() == 0) {
 
+               /* init */
+               if (INIT(state_mode)) {
+                       state_mode &= (~I2C_COBBOARD_MODE_INIT);
+                       state_init();
+               }
+
                /* pack spickles */
-               if (!L_DEPLOY(state_mode))
+               if (L_DEPLOY(state_mode))
+                       spickle_deploy(I2C_LEFT_SIDE);
+               else
                        spickle_pack(I2C_LEFT_SIDE);
-               if (!R_DEPLOY(state_mode))
+               if (R_DEPLOY(state_mode))
+                       spickle_deploy(I2C_RIGHT_SIDE);
+               else
                        spickle_pack(I2C_RIGHT_SIDE);
 
                /* harvest */
@@ -200,8 +216,10 @@ void state_machine(void)
                        state_do_harvest(I2C_RIGHT_SIDE);
 
                /* eject */
-               if (EJECT(state_mode))
+               if (EJECT(state_mode)) {
+                       state_mode &= (~I2C_COBBOARD_MODE_EJECT);
                        state_do_eject();
+               }
        }
 }
 

diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h
index f3135ac036b7ffd345738a007fa26d0df55f8d02..118926a194f8da1ffee7b022251d560c8cca6d76 100644 (file)
--- a/projects/microb2010/cobboard/state.h
+++ b/projects/microb2010/cobboard/state.h
@@ -28,6 +28,8 @@ int8_t state_set_mode(uint8_t mode);
 /* get current state */
 uint8_t state_get_mode(void);
 
+uint8_t state_get_cob_count(void);
+
 /* launch state machine */
 void state_machine(void);
 

diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index 43eed34fc70c0e23e93f746a913d0d784f82a6e9..49cdb8e87e202dfe8cdaa46050e695f435e3a40b 100644 (file)
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -76,6 +76,7 @@ struct i2c_cmd_cobboard_set_mode {
 #define I2C_COBBOARD_MODE_R_DEPLOY     0x04 /* deploy the spickle */
 #define I2C_COBBOARD_MODE_R_HARVEST    0x08 /* auto harvest withe cobs */
 #define I2C_COBBOARD_MODE_EJECT        0x10 /* eject cobs */
+#define I2C_COBBOARD_MODE_INIT         0x20 /* init state machine */
        uint8_t mode;
 };
 

diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c
index 2088874567fb2c1ea38acbeee00aeff7d96655af..24ad07f2f3e8926edfe5072f3bedec79370f4777 100644 (file)
--- a/projects/microb2010/mainboard/actuator.c
+++ b/projects/microb2010/mainboard/actuator.c
@@ -65,3 +65,19 @@ void pwm_set_and_save(void *pwm, int32_t val)
        pwm_ng_set(pwm, val);
 }
 
+void support_balls_deploy(void)
+{
+       pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510);
+       pwm_ng_set(SUPPORT_BALLS_L_SERVO, 240);
+}
+
+void support_balls_pack(void)
+{
+       pwm_ng_set(SUPPORT_BALLS_R_SERVO, 200);
+       pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480);
+}
+
+void actuator_init(void)
+{
+
+}

diff --git a/projects/microb2010/mainboard/actuator.h b/projects/microb2010/mainboard/actuator.h
index 22d10b71fefd0d7160acd72bbcbee4626082b264..52f23c775e465a029bf04ad0bdaacf09f3dfef8a 100644 (file)
--- a/projects/microb2010/mainboard/actuator.h
+++ b/projects/microb2010/mainboard/actuator.h
@@ -21,3 +21,6 @@
 
 void pwm_set_and_save(void *pwm, int32_t val);
 
+void support_balls_deploy(void);
+void support_balls_pack(void);
+void actuator_init(void);

diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 5d49e785077659a0f2afe80564366305e7e989f0..b1f09d0a1b99516dba742176a2453d201c63a276 100644 (file)
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -75,6 +75,7 @@ extern parse_pgm_inst_t cmd_cobboard_setmode1;
 extern parse_pgm_inst_t cmd_cobboard_setmode2;
 extern parse_pgm_inst_t cmd_cobboard_setmode3;
 extern parse_pgm_inst_t cmd_beacon_start;
+extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -157,6 +158,7 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_cobboard_setmode1,
        (parse_pgm_inst_t *)&cmd_cobboard_setmode2,
        (parse_pgm_inst_t *)&cmd_cobboard_setmode3,
+       (parse_pgm_inst_t *)&cmd_servo_balls,
        (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 6349ad70ea1b2b5e09cf4374946b1a0ad8f47340..f4e37a8fbd886d8dd6214746f7a826b3c45869f9 100644 (file)
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -662,6 +662,8 @@ static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
 {
        printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
        printf_P(PSTR("status = %x\r\n"), cobboard.status);
+       printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
+       printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
 }
 
 prog_char str_cobboard_show_arg0[] = "cobboard";
@@ -697,15 +699,13 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 
        if (!strcmp_P(res->arg1, PSTR("init")))
                i2c_cobboard_mode_init();
-       else if (!strcmp_P(res->arg1, PSTR("manual")))
-               i2c_cobboard_mode_manual();
-       else if (!strcmp_P(res->arg1, PSTR("harvest")))
-               i2c_cobboard_mode_harvest();
+       else if (!strcmp_P(res->arg1, PSTR("eject")))
+               i2c_cobboard_mode_eject();
 }
 
 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0);
-prog_char str_cobboard_setmode1_arg1[] = "init#manual#harvest";
+prog_char str_cobboard_setmode1_arg1[] = "init#eject";
 parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1);
 
 prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)";
@@ -741,15 +741,17 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
        else if (!strcmp_P(res->arg2, PSTR("right")))
                side = I2C_RIGHT_SIDE;
 
-       if (!strcmp_P(res->arg1, PSTR("yyy")))
-               printf("faux\r\n");
-       else if (!strcmp_P(res->arg1, PSTR("xxx")))
-               printf("faux\r\n");
+       if (!strcmp_P(res->arg1, PSTR("deploy")))
+               i2c_cobboard_mode_deploy(side);
+       else if (!strcmp_P(res->arg1, PSTR("harvest")))
+               i2c_cobboard_mode_harvest(side);
+       else if (!strcmp_P(res->arg1, PSTR("pack")))
+               i2c_cobboard_mode_pack(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[] = "xxx";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
 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);
@@ -804,6 +806,46 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = {
        },
 };
 
+/**********************************************************/
+/* Servo_Balls */
+
+/* this structure is filled when cmd_servo_balls is parsed successfully */
+struct cmd_servo_balls_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_servo_balls is parsed successfully */
+static void cmd_servo_balls_parsed(void *parsed_result,
+                                  __attribute__((unused)) void *data)
+{
+       struct cmd_servo_balls_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("deploy")))
+               support_balls_deploy();
+       else if (!strcmp_P(res->arg1, PSTR("pack")))
+               support_balls_pack();
+}
+
+prog_char str_servo_balls_arg0[] = "support_balls";
+parse_pgm_token_string_t cmd_servo_balls_arg0 =
+       TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0);
+prog_char str_servo_balls_arg1[] = "deploy#pack";
+parse_pgm_token_string_t cmd_servo_balls_arg1 =
+       TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1);
+
+prog_char help_servo_balls[] = "control support balls";
+parse_pgm_inst_t cmd_servo_balls = {
+       .f = cmd_servo_balls_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_servo_balls,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_servo_balls_arg0, 
+               (prog_void *)&cmd_servo_balls_arg1, 
+               NULL,
+       },
+};
+
 /**********************************************************/
 /* Test */
 

diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c
index 8fe9718270439c3a87c0e0a45ef0aea50e0b9b65..39e39efc211e6098fc236ac93d84d8e73a9810d7 100644 (file)
--- a/projects/microb2010/mainboard/cs.c
+++ b/projects/microb2010/mainboard/cs.c
@@ -180,9 +180,9 @@ void microb_cs_init(void)
        rs_set_right_pwm(&mainboard.rs,  pwm_set_and_save, RIGHT_PWM);
        /* increase gain to decrease dist, increase left and it will turn more left */
        rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
-                               LEFT_ENCODER, IMP_COEF * 1.0015);
+                               LEFT_ENCODER, IMP_COEF * -1.00);
        rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, 
-                                RIGHT_ENCODER, IMP_COEF * -1.006);
+                                RIGHT_ENCODER, IMP_COEF * 1.00);
        /* rs will use external encoders */
        rs_set_flags(&mainboard.rs, RS_USE_EXT);
 
@@ -231,7 +231,7 @@ void microb_cs_init(void)
        /* ---- CS distance */
        /* PID */
        pid_init(&mainboard.distance.pid);
-       pid_set_gains(&mainboard.distance.pid, 500, 10, 7000);
+       pid_set_gains(&mainboard.distance.pid, 500, 100, 7000);
        pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095);
        pid_set_out_shift(&mainboard.distance.pid, 10);
        pid_set_derivate_filter(&mainboard.distance.pid, 6);
@@ -257,19 +257,13 @@ void microb_cs_init(void)
        /* ---- CS left_cobroller */
        /* PID */
        pid_init(&mainboard.left_cobroller.pid);
-       pid_set_gains(&mainboard.left_cobroller.pid, 500, 10, 7000);
-       pid_set_maximums(&mainboard.left_cobroller.pid, 0, 2000, 4095);
-       pid_set_out_shift(&mainboard.left_cobroller.pid, 10);
+       pid_set_gains(&mainboard.left_cobroller.pid, 80, 10, 10);
+       pid_set_maximums(&mainboard.left_cobroller.pid, 0, 30000, 4095);
+       pid_set_out_shift(&mainboard.left_cobroller.pid, 5);
        pid_set_derivate_filter(&mainboard.left_cobroller.pid, 6);
 
-       /* QUADRAMP */
-       quadramp_init(&mainboard.left_cobroller.qr);
-       quadramp_set_1st_order_vars(&mainboard.left_cobroller.qr, 2000, 2000); /* set speed */
-       quadramp_set_2nd_order_vars(&mainboard.left_cobroller.qr, 17, 17); /* set accel */
-
        /* CS */
        cs_init(&mainboard.left_cobroller.cs);
-       cs_set_consign_filter(&mainboard.left_cobroller.cs, quadramp_do_filter, &mainboard.left_cobroller.qr);
        cs_set_correct_filter(&mainboard.left_cobroller.cs, pid_do_filter, &mainboard.left_cobroller.pid);
        cs_set_process_in(&mainboard.left_cobroller.cs, pwm_ng_set, LEFT_COBROLLER_PWM);
        cs_set_process_out(&mainboard.left_cobroller.cs, encoders_left_cobroller_speed, LEFT_COBROLLER_ENCODER);
@@ -283,19 +277,13 @@ void microb_cs_init(void)
        /* ---- CS right_cobroller */
        /* PID */
        pid_init(&mainboard.right_cobroller.pid);
-       pid_set_gains(&mainboard.right_cobroller.pid, 500, 10, 7000);
-       pid_set_maximums(&mainboard.right_cobroller.pid, 0, 2000, 4095);
-       pid_set_out_shift(&mainboard.right_cobroller.pid, 10);
+       pid_set_gains(&mainboard.right_cobroller.pid, 80, 10, 10);
+       pid_set_maximums(&mainboard.right_cobroller.pid, 0, 30000, 4095);
+       pid_set_out_shift(&mainboard.right_cobroller.pid, 5);
        pid_set_derivate_filter(&mainboard.right_cobroller.pid, 6);
 
-       /* QUADRAMP */
-       quadramp_init(&mainboard.right_cobroller.qr);
-       quadramp_set_1st_order_vars(&mainboard.right_cobroller.qr, 2000, 2000); /* set speed */
-       quadramp_set_2nd_order_vars(&mainboard.right_cobroller.qr, 17, 17); /* set accel */
-
        /* CS */
        cs_init(&mainboard.right_cobroller.cs);
-       cs_set_consign_filter(&mainboard.right_cobroller.cs, quadramp_do_filter, &mainboard.right_cobroller.qr);
        cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid);
        cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM);
        cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER);
@@ -307,10 +295,10 @@ void microb_cs_init(void)
        bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50);
 
        /* set them on !! */
-       mainboard.angle.on = 1;
-       mainboard.distance.on = 1;
+       mainboard.angle.on = 0;
+       mainboard.distance.on = 0;
        mainboard.left_cobroller.on = 1;
-       mainboard.right_cobroller.on = 1;
+       mainboard.right_cobroller.on = 0;
 
 
        scheduler_add_periodical_event_priority(do_cs, NULL,

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 2ceb7e8818146a03cd37001fd480f755deee813b..d0726cb8ee9a8ea4bb5ebc6886f8de80dc6a3e12 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -263,6 +263,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                /* status */
                cobboard.mode = ans->mode;
                cobboard.status = ans->status;
+               cobboard.left_cobroller_speed = ans->left_cobroller_speed;
+               cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
+               cobboard.right_cobroller_speed = ans->right_cobroller_speed;
+               cs_set_consign(&mainboard.right_cobroller.cs, cobboard.right_cobroller_speed);
 
                break;
        }
@@ -368,19 +372,61 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
        return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_cobboard_mode_manual(void)
+int8_t i2c_cobboard_mode_eject(void)
 {
        struct i2c_cmd_cobboard_set_mode buf;
        buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
-       buf.mode = I2C_COBBOARD_MODE_MANUAL;
+       buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT;
        return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
-int8_t i2c_cobboard_mode_harvest(void)
+int8_t i2c_cobboard_mode_harvest(uint8_t side)
 {
        struct i2c_cmd_cobboard_set_mode buf;
+       uint8_t mode = cobboard.mode;
+
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       if (side == I2C_LEFT_SIDE) {
+               mode |= I2C_COBBOARD_MODE_L_DEPLOY;
+               mode |= I2C_COBBOARD_MODE_L_HARVEST;
+       }
+       else {
+               mode |= I2C_COBBOARD_MODE_R_DEPLOY;
+               mode |= I2C_COBBOARD_MODE_R_HARVEST;
+       }
+       buf.mode = mode;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_cobboard_mode_deploy(uint8_t side)
+{
+       struct i2c_cmd_cobboard_set_mode buf;
+       uint8_t mode = cobboard.mode;
+
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       if (side == I2C_LEFT_SIDE) {
+               mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
+               mode |= I2C_COBBOARD_MODE_L_DEPLOY;
+       }
+       else {
+               mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
+               mode |= I2C_COBBOARD_MODE_R_DEPLOY;
+       }
+       buf.mode = mode;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
+int8_t i2c_cobboard_mode_pack(uint8_t side)
+{
+       struct i2c_cmd_cobboard_set_mode buf;
+       uint8_t mode = cobboard.mode;
+
        buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
-       buf.mode = I2C_COBBOARD_MODE_HARVEST;
+       if (side == I2C_LEFT_SIDE)
+               mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
+       else
+               mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
+       buf.mode = mode;
        return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 

diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index 1e22a4f94c53763e67336e74b9e534f22b50666c..d788ed737e70e591709746daaed3512806f6517d 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -35,8 +35,10 @@ void i2c_sendevent(int8_t size);
 int8_t i2c_set_color(uint8_t addr, uint8_t color);
 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state);
 
-int8_t i2c_cobboard_mode_manual(void);
-int8_t i2c_cobboard_mode_harvest(void);
 int8_t i2c_cobboard_mode_init(void);
+int8_t i2c_cobboard_mode_eject(void);
+int8_t i2c_cobboard_mode_harvest(uint8_t side);
+int8_t i2c_cobboard_mode_deploy(uint8_t side);
+int8_t i2c_cobboard_mode_pack(uint8_t side);
 
 #endif

diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index d0c9d14fd34eaf2b2d8d4dc5dd48e200d5c4dfd8..6a36e805134fbdee451a6c1c499feb679fd16fa3 100755 (executable)
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -181,7 +181,7 @@ int main(void)
 #  error not supported
 #endif
 
-       //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
+       eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
        /* check eeprom to avoid to run the bad program */
        if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
            EEPROM_MAGIC_MAINBOARD) {
@@ -216,8 +216,8 @@ int main(void)
        PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, 
                                 TIMER4_PRESCALER_DIV_1);
        
-       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | 
-                     PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4);
+       PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
+                     &PORTD, 4);
        PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | 
                      PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5);
        PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
@@ -239,6 +239,7 @@ int main(void)
                      NULL, 0);
        PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
                      NULL, 0);
+       support_balls_deploy(); /* init pwm for servos */
 
        /* SCHEDULER */
        scheduler_init();

diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 7f210588b0418ebb4db2891ce689d688fdc09101..2fdd212c2551b74e165f534c51d8503518be411d 100755 (executable)
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -66,16 +66,20 @@
 #define IMP_COEF 10.
 #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF)
 
-#define LEFT_ENCODER            ((void *)1)
 #define RIGHT_ENCODER           ((void *)0)
+#define LEFT_ENCODER            ((void *)1)
 #define LEFT_COBROLLER_ENCODER  ((void *)2)
 #define RIGHT_COBROLLER_ENCODER ((void *)3)
 
-#define LEFT_PWM            ((void *)&gen.pwm1_4A)
-#define RIGHT_PWM           ((void *)&gen.pwm2_4B)
+#define RIGHT_PWM           ((void *)&gen.pwm1_4A)
+#define LEFT_PWM            ((void *)&gen.pwm2_4B)
 #define LEFT_COBROLLER_PWM  ((void *)&gen.pwm3_1A)
 #define RIGHT_COBROLLER_PWM ((void *)&gen.pwm4_1B)
 
+#define SUPPORT_BALLS_R_SERVO ((void *)&gen.servo2)
+#define SUPPORT_BALLS_L_SERVO ((void *)&gen.servo3)
+
+
 /** ERROR NUMS */
 #define E_USER_STRAT           194
 #define E_USER_I2C_PROTO       195
@@ -163,6 +167,9 @@ struct mainboard {
 struct cobboard {
        uint8_t mode;   
        uint8_t status;
+       int16_t left_cobroller_speed;
+       int16_t right_cobroller_speed;
+       uint8_t cob_count;
 };
 
 /* state of ballboard, synchronized through i2c */