X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fi2c_protocol.c;h=b7f13ee1ad8ed090f8436c110f7f0a5d9bd213af;hp=8d86b490ef22de4248290029026ae9e1d2242e60;hb=b28db6087ed5fdab0bda8f23417ef7098d2a9ae5;hpb=6914527de2ecfef9d790740c71739e7418246b96

diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
index 8d86b49..b7f13ee 100644
--- a/projects/microb2010/cobboard/i2c_protocol.c
+++ b/projects/microb2010/cobboard/i2c_protocol.c
@@ -99,17 +99,11 @@ static void i2c_send_status(void)
 	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,7 @@ 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);
+			state_set_mode(cmd->mode);
 			break;
 		}
 
@@ -155,7 +149,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 +158,20 @@ 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_set_spickle(I2C_LEFT_SIDE, cmd->lspickle);
+				state_set_spickle(I2C_RIGHT_SIDE, cmd->rspickle);
+			}
 			i2c_send_status();
 			break;
 		}