better cobboard commands in strat_event

[aversive.git] / projects / microb2010 / cobboard / i2c_protocol.c

diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
index 9de2d4c..21a9bd8 100644 (file)
--- a/projects/microb2010/cobboard/i2c_protocol.c
+++ b/projects/microb2010/cobboard/i2c_protocol.c
@@ -93,23 +93,17 @@ static void i2c_send_status(void)
 
        /* status */
        ans.mode = state_get_mode();
-       ans.status = 0x55; /* TODO */
+       ans.status = state_get_status();
 
        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);
 }
 
-static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd)
-{
-       state_set_mode(cmd->mode);
-       return 0;
-}
-
 void i2c_recvevent(uint8_t * buf, int8_t size)
 {
        void *void_cmd = buf;
@@ -141,7 +135,8 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                        struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
                        if (size != sizeof(struct i2c_cmd_cobboard_set_mode))
                                goto error;
-                       i2c_set_mode(cmd);
+                       if (!state_get_i2c_ignore())
+                               state_set_mode(cmd->mode);
                        break;
                }
 
@@ -155,7 +150,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                }
 
 #ifdef notyet
-       case I2C_CMD_EXTENSION_TEST: 
+       case I2C_CMD_EXTENSION_TEST:
                {
                        struct i2c_cmd_extension_test *cmd = void_cmd;
                        if (size != sizeof (*cmd))
@@ -164,16 +159,21 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                        break;
                }
 #endif
-               
-       /* Add other commands here ...*/
 
+       /* Add other commands here ...*/
 
        case I2C_REQ_COBBOARD_STATUS:
                {
-                       //struct i2c_req_cobboard_status *cmd = void_cmd;
+                       struct i2c_req_cobboard_status *cmd = void_cmd;
                        if (size != sizeof (struct i2c_req_cobboard_status))
                                goto error;
-                       
+
+                       /* mode is in req */
+                       if (state_get_status() != I2C_COBBOARD_STATUS_OFF &&
+                           !state_get_i2c_ignore()) {
+                               state_set_spickle(I2C_LEFT_SIDE, cmd->lspickle);
+                               state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle);
+                       }
                        i2c_send_status();
                        break;
                }