X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fi2c_protocol.c;h=9b2a9e767c6995f27b9a49c3f50458558b28ca80;hp=b790128b86ded7389bc857bd05d7c5efa4a0fb11;hb=b022f257a5ee568737e1a684a83d0154397fffcb;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305

diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c
index b790128..9b2a9e7 100644
--- a/projects/microb2010/cobboard/i2c_protocol.c
+++ b/projects/microb2010/cobboard/i2c_protocol.c
@@ -93,36 +93,35 @@ static void i2c_send_status(void)
 
 	/* status */
 	ans.mode = state_get_mode();
-	ans.status = 0x55;
+	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);
-	return 0;
-}
-
 void i2c_recvevent(uint8_t * buf, int8_t size)
 {
 	void *void_cmd = buf;
-	
+
 	static uint8_t a = 0;
-	
+
 	a++;
 	if (a & 0x10)
 		LED2_TOGGLE();
-	
+
 	if (size <= 0) {
 		goto error;
 	}
-	
+
 	switch (buf[0]) {
 
 	/* Commands (no answer needed) */
-	case I2C_CMD_GENERIC_LED_CONTROL: 
+	case I2C_CMD_GENERIC_LED_CONTROL:
 		{
 			struct i2c_cmd_led_control *cmd = void_cmd;
 			if (size != sizeof (*cmd))
@@ -130,7 +129,8 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			i2c_led_control(cmd->led_num, cmd->state);
 			break;
 		}
-		
+
+#if 0
 	case I2C_CMD_COBBOARD_SET_MODE:
 		{
 			struct i2c_cmd_cobboard_set_mode *cmd = void_cmd;
@@ -139,6 +139,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			i2c_set_mode(cmd);
 			break;
 		}
+#endif
 
 	case I2C_CMD_GENERIC_SET_COLOR:
 		{
@@ -159,16 +160,18 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 			break;
 		}
 #endif
-		
+
 	/* 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 */
+			state_set_mode(cmd->mode);
 			i2c_send_status();
 			break;
 		}