optim cobboard

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

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index a503f6d..9908f1a 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -1,6 +1,6 @@
 /*
  *  Copyright Droids Corporation (2009)
 /*
  *  Copyright Droids Corporation (2009)

- * 
+ *

  *  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
  *  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


@@ -30,7 +30,7 @@
 #include <uart.h>
 #include <pwm_ng.h>
 #include <i2c.h>
 #include <uart.h>
 #include <pwm_ng.h>
 #include <i2c.h>

-#include <time.h>
+#include <clock_time.h>

 
 #include <pid.h>
 #include <quadramp.h>
 
 #include <pid.h>
 #include <quadramp.h>


@@ -53,6 +53,9 @@
 #include "main.h"
 #include "sensor.h"
 #include "i2c_protocol.h"
 #include "main.h"
 #include "sensor.h"
 #include "i2c_protocol.h"

+#ifdef HOST_VERSION
+#include "robotsim.h"
+#endif

 
 #define I2C_STATE_MAX 4
 
 
 #define I2C_STATE_MAX 4
 


@@ -70,16 +73,19 @@ static volatile uint16_t i2c_errors = 0;
 static volatile uint8_t running_op = OP_READY;
 
 #define I2C_MAX_LOG 3
 static volatile uint8_t running_op = OP_READY;
 
 #define I2C_MAX_LOG 3

+#ifndef HOST_VERSION

 static uint8_t error_log = 0;
 
 static uint8_t error_log = 0;
 

+static int8_t i2c_req_cobboard_status(void);
+static int8_t i2c_req_ballboard_status(void);
+
+#endif
+

 /* used for commands */
 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 volatile int8_t command_dest=-1;
 volatile uint8_t command_size=0;
 
 /* used for commands */
 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 volatile int8_t command_dest=-1;
 volatile uint8_t command_size=0;
 

-static int8_t i2c_req_mechboard_status(void);
-static int8_t i2c_req_sensorboard_status(void);
-

 #define I2C_ERROR(args...) do {                                                \
                if (error_log < I2C_MAX_LOG) {                          \
                        ERROR(E_USER_I2C_PROTO, args);                  \
 #define I2C_ERROR(args...) do {                                                \
                if (error_log < I2C_MAX_LOG) {                          \
                        ERROR(E_USER_I2C_PROTO, args);                  \


@@ -99,6 +105,9 @@ void i2c_protocol_init(void)
 
 void i2c_protocol_debug(void)
 {
 
 void i2c_protocol_debug(void)
 {

+#ifdef HOST_VERSION
+       return;
+#else

        printf_P(PSTR("I2C protocol debug infos:\r\n"));
        printf_P(PSTR("  i2c_state=%d\r\n"), i2c_state);
        printf_P(PSTR("  i2c_errors=%d\r\n"), i2c_errors);
        printf_P(PSTR("I2C protocol debug infos:\r\n"));
        printf_P(PSTR("  i2c_state=%d\r\n"), i2c_state);
        printf_P(PSTR("  i2c_errors=%d\r\n"), i2c_errors);


@@ -106,8 +115,10 @@ void i2c_protocol_debug(void)
        printf_P(PSTR("  command_size=%d\r\n"), command_size);
        printf_P(PSTR("  command_dest=%d\r\n"), command_dest);
        printf_P(PSTR("  i2c_status=%x\r\n"), i2c_status());
        printf_P(PSTR("  command_size=%d\r\n"), command_size);
        printf_P(PSTR("  command_dest=%d\r\n"), command_dest);
        printf_P(PSTR("  i2c_status=%x\r\n"), i2c_status());

+#endif

 }
 
 }
 

+#ifndef HOST_VERSION

 static void i2cproto_next_state(uint8_t inc)
 {
        i2c_state += inc;
 static void i2cproto_next_state(uint8_t inc)
 {
        i2c_state += inc;


@@ -131,11 +142,11 @@ void i2c_poll_slaves(void *dummy)
        uint8_t flags;
        int8_t err;
        static uint8_t a = 0;
        uint8_t flags;
        int8_t err;
        static uint8_t a = 0;

-       
+

        a++;
        if (a & 0x4)
                LED2_TOGGLE();
        a++;
        if (a & 0x4)
                LED2_TOGGLE();

-       
+

        /* already running */
        IRQ_LOCK(flags);
        if (running_op != OP_READY) {
        /* already running */
        IRQ_LOCK(flags);
        if (running_op != OP_READY) {


@@ -159,32 +170,32 @@ void i2c_poll_slaves(void *dummy)
 
        switch(i2c_state) {
 
 
        switch(i2c_state) {
 

-       /* poll status of mechboard */
-#define I2C_REQ_MECHBOARD 0
-       case I2C_REQ_MECHBOARD:
-               if ((err = i2c_req_mechboard_status()))
+       /* poll status of cobboard */
+#define I2C_REQ_COBBOARD 0
+       case I2C_REQ_COBBOARD:
+               if ((err = i2c_req_cobboard_status()))

                        goto error;
                break;
 
                        goto error;
                break;
 

-#define I2C_ANS_MECHBOARD 1
-       case I2C_ANS_MECHBOARD:
-               if ((err = i2c_recv(I2C_MECHBOARD_ADDR, 
-                                   sizeof(struct i2c_ans_mechboard_status), 
+#define I2C_ANS_COBBOARD 1
+       case I2C_ANS_COBBOARD:
+               if ((err = i2c_recv(I2C_COBBOARD_ADDR,
+                                   sizeof(struct i2c_ans_cobboard_status),

                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;
 
                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;
 

-       /* poll status of sensorboard */
-#define I2C_REQ_SENSORBOARD 2
-       case I2C_REQ_SENSORBOARD:
-               if ((err = i2c_req_sensorboard_status()))
+       /* poll status of ballboard */
+#define I2C_REQ_BALLBOARD 2
+       case I2C_REQ_BALLBOARD:
+               if ((err = i2c_req_ballboard_status()))

                        goto error;
                break;
 
                        goto error;
                break;
 

-#define I2C_ANS_SENSORBOARD 3
-       case I2C_ANS_SENSORBOARD:
-               if ((err = i2c_recv(I2C_SENSORBOARD_ADDR, 
-                                   sizeof(struct i2c_ans_sensorboard_status), 
+#define I2C_ANS_BALLBOARD 3
+       case I2C_ANS_BALLBOARD:
+               if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
+                                   sizeof(struct i2c_ans_ballboard_status),

                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;
                                    I2C_CTRL_GENERIC)))
                        goto error;
                break;


@@ -203,7 +214,7 @@ void i2c_poll_slaves(void *dummy)
        IRQ_UNLOCK(flags);
        i2c_errors++;
        if (i2c_errors > I2C_MAX_ERRORS) {
        IRQ_UNLOCK(flags);
        i2c_errors++;
        if (i2c_errors > I2C_MAX_ERRORS) {

-               I2C_ERROR("I2C send is_cmd=%d proto_state=%d " 
+               I2C_ERROR("I2C send is_cmd=%d proto_state=%d "

                      "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
                i2c_reset();
                i2c_errors = 0;
                      "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
                i2c_reset();
                i2c_errors = 0;


@@ -229,7 +240,7 @@ void i2c_sendevent(int8_t size)
                        i2c_reset();
                        i2c_errors = 0;
                }
                        i2c_reset();
                        i2c_errors = 0;
                }

-               
+

                if (running_op == OP_POLL) {
                        /* skip associated answer */
                        i2cproto_next_state(2);
                if (running_op == OP_POLL) {
                        /* skip associated answer */
                        i2cproto_next_state(2);


@@ -246,60 +257,48 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 
        /* recv is only trigged after a poll */
        running_op = OP_READY;
 
        /* recv is only trigged after a poll */
        running_op = OP_READY;

-       
+

        if (size < 0) {
                goto error;
        }
 
        switch (buf[0]) {
 
        if (size < 0) {
                goto error;
        }
 
        switch (buf[0]) {
 

-       case I2C_ANS_MECHBOARD_STATUS: {
-               struct i2c_ans_mechboard_status * ans = 
-                       (struct i2c_ans_mechboard_status *)buf;
-               
+       case I2C_ANS_COBBOARD_STATUS: {
+               struct i2c_ans_cobboard_status * ans =
+                       (struct i2c_ans_cobboard_status *)buf;
+

                if (size != sizeof (*ans))
                        goto error;
 
                /* status */
                if (size != sizeof (*ans))
                        goto error;
 
                /* status */

-               mechboard.mode = ans->mode;
-               mechboard.status = ans->status;
-               mechboard.lintel_count = ans->lintel_count;
-               mechboard.column_flags = ans->column_flags;
-               /* pumps pwm */
-               mechboard.pump_left1 = ans->pump_left1;
-               mechboard.pump_left2 = ans->pump_left2;
-               mechboard.pump_right1 = ans->pump_right1;
-               mechboard.pump_right2 = ans->pump_right2;
-               pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1);
-               pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2);
-               /* pumps current */
-               mechboard.pump_right1_current = ans->pump_right1_current;
-               mechboard.pump_right2_current = ans->pump_right2_current;
-               /* servos */
-               mechboard.servo_lintel_left = ans->servo_lintel_left;
-               mechboard.servo_lintel_right = ans->servo_lintel_right;
-               pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right);
-               pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left);
+               cobboard.mode = ans->mode;
+               cobboard.status = ans->status;
+               cobboard.cob_count = ans->cob_count;
+               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;
        }
 
                break;
        }

-               
-       case I2C_ANS_SENSORBOARD_STATUS: {
-               struct i2c_ans_sensorboard_status * ans = 
-                       (struct i2c_ans_sensorboard_status *)buf;
-               
+
+       case I2C_ANS_BALLBOARD_STATUS: {
+               uint8_t tmp;
+               struct i2c_ans_ballboard_status * ans =
+                       (struct i2c_ans_ballboard_status *)buf;
+

                if (size != sizeof (*ans))
                        goto error;
                if (size != sizeof (*ans))
                        goto error;

-               sensorboard.status = ans->status;
-               sensorboard.opponent_x = ans->opponent_x;
-               sensorboard.opponent_y = ans->opponent_y;
-               sensorboard.opponent_a = ans->opponent_a;
-               sensorboard.opponent_d = ans->opponent_d;
-
-               sensorboard.scan_status = ans->scan_status;
-               sensorboard.dropzone_h = ans->dropzone_h;
-               sensorboard.dropzone_x = ans->dropzone_x;
-               sensorboard.dropzone_y = ans->dropzone_y;
+               ballboard.mode = ans->mode;
+               ballboard.status = ans->status;
+               ballboard.ball_count = ans->ball_count;
+               tmp = ans->lcob;
+               if (tmp != I2C_COB_NONE)
+                       ballboard.lcob = tmp;
+               tmp = ans->rcob;
+               if (tmp != I2C_COB_NONE)
+                       ballboard.rcob = tmp;

                break;
        }
 
                break;
        }
 


@@ -310,7 +309,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
        return;
  error:
        i2c_errors++;
        return;
  error:
        i2c_errors++;

-       NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d", 
+       NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",

               i2c_state, running_op);
        if (i2c_errors > I2C_MAX_ERRORS) {
                I2C_ERROR("I2C error, slave not ready");
               i2c_state, running_op);
        if (i2c_errors > I2C_MAX_ERRORS) {
                I2C_ERROR("I2C error, slave not ready");


@@ -318,12 +317,11 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                i2c_errors = 0;
        }
 }
                i2c_errors = 0;
        }
 }

-       
+

 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
 {
 }
 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
 {
 }

-
-
+#endif /* !HOST_VERSION */

 
 /* ******** ******** ******** ******** */
 /* commands */
 
 /* ******** ******** ******** ******** */
 /* commands */


@@ -331,8 +329,11 @@ void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
 
 
 static int8_t
 
 
 static int8_t

-i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) 
+i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)

 {
 {

+#ifdef HOST_VERSION
+       return robotsim_i2c(addr, buf, size);
+#else

        uint8_t flags;
         microseconds us = time_get_us2();
 
        uint8_t flags;
         microseconds us = time_get_us2();
 


@@ -351,43 +352,39 @@ i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
         * interrupt context, but it's forbidden */
        I2C_ERROR("I2C command send failed");
        return -EBUSY;
         * interrupt context, but it's forbidden */
        I2C_ERROR("I2C command send failed");
        return -EBUSY;

+#endif

 }
 
 }
 

-static int8_t i2c_req_mechboard_status(void)
+#ifndef HOST_VERSION
+static int8_t i2c_req_cobboard_status(void)

 {
 {

-       struct i2c_req_mechboard_status buf;
+       struct i2c_req_cobboard_status buf;

        int8_t err;
 
        int8_t err;
 

-       buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS;
-       buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3);
-       buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4);
-       err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,
+       buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
+       buf.lspickle = cobboard.lspickle;
+       buf.rspickle = cobboard.rspickle;
+       err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,

                        sizeof(buf), I2C_CTRL_GENERIC);
 
        return err;
 }
 
                        sizeof(buf), I2C_CTRL_GENERIC);
 
        return err;
 }
 

-static int8_t i2c_req_sensorboard_status(void)
+static int8_t i2c_req_ballboard_status(void)

 {
 {

-       struct i2c_req_sensorboard_status buf;
-       
-       buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS;
-       /* robot position */
-       buf.x = position_get_x_s16(&mainboard.pos);
-       buf.y = position_get_y_s16(&mainboard.pos);
-       buf.a = position_get_a_deg_s16(&mainboard.pos);
-       /* pickup wheels */
-       buf.enable_pickup_wheels = mainboard.enable_pickup_wheels;
-       
-       return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf,
+       struct i2c_req_ballboard_status buf;
+
+       buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
+       return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,

                        sizeof(buf), I2C_CTRL_GENERIC);
 }
                        sizeof(buf), I2C_CTRL_GENERIC);
 }

+#endif /* !HOST_VERSION */

 
 int8_t i2c_set_color(uint8_t addr, uint8_t color)
 {
        struct i2c_cmd_generic_color buf;
 
 
 int8_t i2c_set_color(uint8_t addr, uint8_t color)
 {
        struct i2c_cmd_generic_color buf;
 

-       if (addr == I2C_SENSORBOARD_ADDR)
+       if (addr == I2C_BALLBOARD_ADDR)

                return 0; /* XXX disabled for now */
        buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
        buf.color = color;
                return 0; /* XXX disabled for now */
        buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
        buf.color = color;


@@ -403,285 +400,49 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
        return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 }
 
        return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 }
 

-int8_t i2c_mechboard_mode_manual(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_MANUAL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_harvest(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_HARVEST;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_lazy_harvest(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-       buf.prep_pickup.next_mode = next_mode;
-       buf.prep_pickup.side = side;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_pickup(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PICKUP;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_eject(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_EJECT;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_manivelle(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_MANIVELLE;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_push_temple(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE;
-       buf.push_temple.level = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
-       buf.prep_build.level_l = level;
-       buf.prep_build.level_r = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
-       buf.prep_build.level_l = level_l;
-       buf.prep_build.level_r = level_r;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
-       buf.prep_inside.level_l = level;
-       buf.prep_inside.level_r = level;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
-       buf.prep_inside.level_l = level_l;
-       buf.prep_inside.level_r = level_r;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
-       buf.autobuild.level_left = level;
-       buf.autobuild.level_right = level;
-       buf.autobuild.count_left = 2;
-       buf.autobuild.count_right = 2;
-       buf.autobuild.do_lintel = 1;
-       buf.autobuild.distance_left = 210;
-       buf.autobuild.distance_right = 210;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l,
-                                   uint8_t dist_l,
-                                   uint8_t level_r, uint8_t count_r,
-                                   uint8_t dist_r,
-                                   uint8_t do_lintel)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
-       buf.autobuild.level_left = level_l;
-       buf.autobuild.level_right = level_r;
-       buf.autobuild.count_left = count_l;
-       buf.autobuild.count_right = count_r;
-       buf.autobuild.distance_left = dist_l;
-       buf.autobuild.distance_right = dist_r;
-       buf.autobuild.do_lintel = do_lintel;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_init(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_INIT;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_prepare_get_lintel(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_get_lintel(void)
+int8_t i2c_cobboard_set_mode(uint8_t mode)

 {
 {

-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_put_lintel(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_mechboard_mode_clear(void)
-{
-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_CLEAR;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+#ifdef HOST_VERSION
+       return robotsim_i2c_cobboard_set_mode(mode);
+#else
+       struct i2c_cmd_cobboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
+       buf.mode = mode;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+#endif

 }
 
 }
 

-int8_t i2c_mechboard_mode_loaded(void)
+static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags)

 {
 {

-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LOADED;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       if (side == I2C_LEFT_SIDE)
+               cobboard.lspickle = flags;
+       else
+               cobboard.rspickle = flags;
+       return 0;

 }
 
 }
 

-int8_t i2c_mechboard_mode_store(void)
+int8_t i2c_cobboard_pack(uint8_t side)

 {
 {

-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_STORE;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       return i2c_cobboard_set_spickle(side, 0);

 }
 
 }
 

-int8_t i2c_mechboard_mode_lazy_pickup(void)
+int8_t i2c_cobboard_autoharvest(uint8_t side)

 {
 {

-       struct i2c_cmd_mechboard_set_mode buf;
-       buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
-       buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
-       return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       return i2c_cobboard_set_spickle(side,
+                                       I2C_COBBOARD_SPK_DEPLOY |
+                                       I2C_COBBOARD_SPK_AUTOHARVEST);

 }
 
 }
 

-int8_t i2c_sensorboard_set_beacon(uint8_t enable)
+int8_t i2c_cobboard_deploy(uint8_t side)

 {
 {

-       struct i2c_cmd_sensorboard_start_beacon buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON;
-       buf.enable = enable;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);

 }
 
 }
 

-int8_t i2c_sensorboard_scanner_set(uint8_t mode)
+int8_t i2c_ballboard_set_mode(uint8_t mode)

 {
 {

-       struct i2c_cmd_sensorboard_scanner buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER;
+       struct i2c_cmd_ballboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;

        buf.mode = mode;
        buf.mode = mode;

-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+       return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));

 }
 
 }
 

-int8_t i2c_sensorboard_scanner_calib(void)
-{
-       struct i2c_cmd_sensorboard_calib_scanner buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
-                                          int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE;
-       buf.drop_zone.working_zone = zone;
-       buf.drop_zone.center_x = x;
-       buf.drop_zone.center_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
-                                          int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE;
-       buf.drop_zone.working_zone = zone;
-       buf.drop_zone.center_x = x;
-       buf.drop_zone.center_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}
-
-int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
-                                         int16_t x, int16_t y)
-{
-       struct i2c_cmd_sensorboard_scanner_algo buf;
-       buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
-       buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE;
-       buf.check_temple.level = level;
-       buf.check_temple.temple_x = x;
-       buf.check_temple.temple_y = y;
-       return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
-}