X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fi2c_protocol.c;fp=projects%2Fmicrob2010%2Fmainboard%2Fi2c_protocol.c;h=2ceb7e8818146a03cd37001fd480f755deee813b;hp=a503f6df8fec67c00bddda701f3b98349071eec7;hb=1714f4ee916fca95ce24120ea6e698237913f947;hpb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305 diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index a503f6d..2ceb7e8 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -30,7 +30,7 @@ #include #include #include -#include +#include #include #include @@ -77,8 +77,8 @@ 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); +static int8_t i2c_req_cobboard_status(void); +static int8_t i2c_req_ballboard_status(void); #define I2C_ERROR(args...) do { \ if (error_log < I2C_MAX_LOG) { \ @@ -159,32 +159,32 @@ void i2c_poll_slaves(void *dummy) 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; -#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; - /* 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; -#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; @@ -253,53 +253,27 @@ void i2c_recvevent(uint8_t * buf, int8_t size) 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 */ - 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; break; } - case I2C_ANS_SENSORBOARD_STATUS: { - struct i2c_ans_sensorboard_status * ans = - (struct i2c_ans_sensorboard_status *)buf; + case I2C_ANS_BALLBOARD_STATUS: { + struct i2c_ans_ballboard_status * ans = + (struct i2c_ans_ballboard_status *)buf; 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.status = ans->status; break; } @@ -353,33 +327,24 @@ i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size) return -EBUSY; } -static int8_t i2c_req_mechboard_status(void) +static int8_t i2c_req_cobboard_status(void) { - struct i2c_req_mechboard_status buf; + struct i2c_req_cobboard_status buf; 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; + err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf, 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; + struct i2c_req_ballboard_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, + buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS; + return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf), I2C_CTRL_GENERIC); } @@ -387,7 +352,7 @@ 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; @@ -403,285 +368,27 @@ 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_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) +int8_t i2c_cobboard_mode_manual(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)); + struct i2c_cmd_cobboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + buf.mode = I2C_COBBOARD_MODE_MANUAL; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_mechboard_mode_lazy_harvest(void) +int8_t i2c_cobboard_mode_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)); + struct i2c_cmd_cobboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + buf.mode = I2C_COBBOARD_MODE_HARVEST; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side) +int8_t i2c_cobboard_mode_init(void) { - 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)); + struct i2c_cmd_cobboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + buf.mode = I2C_COBBOARD_MODE_INIT; + return i2c_send_command(I2C_COBBOARD_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) -{ - 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)); -} - -int8_t i2c_mechboard_mode_loaded(void) -{ - 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)); -} - -int8_t i2c_mechboard_mode_store(void) -{ - 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)); -} - -int8_t i2c_mechboard_mode_lazy_pickup(void) -{ - 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)); -} - -int8_t i2c_sensorboard_set_beacon(uint8_t enable) -{ - 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)); -} - -int8_t i2c_sensorboard_scanner_set(uint8_t mode) -{ - struct i2c_cmd_sensorboard_scanner buf; - buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER; - buf.mode = mode; - return i2c_send_command(I2C_SENSORBOARD_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)); -}