From: Olivier Matz Date: Sun, 28 Mar 2010 00:45:48 +0000 (+0100) Subject: state machine and support balls X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=4fb57a4dab8bd564445e824696a2dab470af8628 state machine and support balls --- diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c index 6952a74..feeb32f 100644 --- a/projects/microb2010/cobboard/commands.c +++ b/projects/microb2010/cobboard/commands.c @@ -76,6 +76,8 @@ extern parse_pgm_inst_t cmd_servo_carry; extern parse_pgm_inst_t cmd_spickle; extern parse_pgm_inst_t cmd_spickle_params; extern parse_pgm_inst_t cmd_spickle_params_show; +extern parse_pgm_inst_t cmd_spickle_params2; +extern parse_pgm_inst_t cmd_spickle_params2_show; extern parse_pgm_inst_t cmd_shovel; extern parse_pgm_inst_t cmd_test; @@ -135,6 +137,8 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_spickle, (parse_pgm_inst_t *)&cmd_spickle_params, (parse_pgm_inst_t *)&cmd_spickle_params_show, + (parse_pgm_inst_t *)&cmd_spickle_params2, + (parse_pgm_inst_t *)&cmd_spickle_params2_show, (parse_pgm_inst_t *)&cmd_shovel, (parse_pgm_inst_t *)&cmd_test, diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index 53f2ab7..77846d0 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -93,11 +93,13 @@ static void i2c_send_status(void) /* status */ ans.mode = state_get_mode(); - ans.status = 0x55; + ans.status = 0x55; /* TODO */ 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); } diff --git a/projects/microb2010/cobboard/sensor.c b/projects/microb2010/cobboard/sensor.c index 0889320..eec58b1 100644 --- a/projects/microb2010/cobboard/sensor.c +++ b/projects/microb2010/cobboard/sensor.c @@ -146,7 +146,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_FRONT] = { 5, 0, 4, 1, 0, 0 }, /* 1 */ [S_CAP3] = { 10, 0, 3, 7, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */ + [S_LCOB] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ [S_COL_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 7 */ diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index 03565ac..3686eb7 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -176,7 +176,7 @@ void spickle_deploy(uint8_t side) void spickle_pack(uint8_t side) { - cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_packed[side]); } uint16_t spickle_get_deploy_delay(uint8_t side) diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index d02e544..4c9651e 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -68,6 +68,7 @@ static uint8_t cob_count; #define R_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST)) #define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode)) #define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT)) +#define INIT(mode) (!!((mode) & I2C_COBBOARD_MODE_INIT)) uint8_t state_debug = 0; @@ -78,6 +79,11 @@ static void state_dump_sensors(void) } #endif +uint8_t state_get_cob_count(void) +{ + return cob_count; +} + static void state_debug_wait_key_pressed(void) { if (!state_debug) @@ -187,10 +193,20 @@ void state_machine(void) { while (state_want_exit() == 0) { + /* init */ + if (INIT(state_mode)) { + state_mode &= (~I2C_COBBOARD_MODE_INIT); + state_init(); + } + /* pack spickles */ - if (!L_DEPLOY(state_mode)) + if (L_DEPLOY(state_mode)) + spickle_deploy(I2C_LEFT_SIDE); + else spickle_pack(I2C_LEFT_SIDE); - if (!R_DEPLOY(state_mode)) + if (R_DEPLOY(state_mode)) + spickle_deploy(I2C_RIGHT_SIDE); + else spickle_pack(I2C_RIGHT_SIDE); /* harvest */ @@ -200,8 +216,10 @@ void state_machine(void) state_do_harvest(I2C_RIGHT_SIDE); /* eject */ - if (EJECT(state_mode)) + if (EJECT(state_mode)) { + state_mode &= (~I2C_COBBOARD_MODE_EJECT); state_do_eject(); + } } } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index f3135ac..118926a 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -28,6 +28,8 @@ int8_t state_set_mode(uint8_t mode); /* get current state */ uint8_t state_get_mode(void); +uint8_t state_get_cob_count(void); + /* launch state machine */ void state_machine(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 43eed34..49cdb8e 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -76,6 +76,7 @@ struct i2c_cmd_cobboard_set_mode { #define I2C_COBBOARD_MODE_R_DEPLOY 0x04 /* deploy the spickle */ #define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ #define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ +#define I2C_COBBOARD_MODE_INIT 0x20 /* init state machine */ uint8_t mode; }; diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c index 2088874..24ad07f 100644 --- a/projects/microb2010/mainboard/actuator.c +++ b/projects/microb2010/mainboard/actuator.c @@ -65,3 +65,19 @@ void pwm_set_and_save(void *pwm, int32_t val) pwm_ng_set(pwm, val); } +void support_balls_deploy(void) +{ + pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510); + pwm_ng_set(SUPPORT_BALLS_L_SERVO, 240); +} + +void support_balls_pack(void) +{ + pwm_ng_set(SUPPORT_BALLS_R_SERVO, 200); + pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480); +} + +void actuator_init(void) +{ + +} diff --git a/projects/microb2010/mainboard/actuator.h b/projects/microb2010/mainboard/actuator.h index 22d10b7..52f23c7 100644 --- a/projects/microb2010/mainboard/actuator.h +++ b/projects/microb2010/mainboard/actuator.h @@ -21,3 +21,6 @@ void pwm_set_and_save(void *pwm, int32_t val); +void support_balls_deploy(void); +void support_balls_pack(void); +void actuator_init(void); diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c index 5d49e78..b1f09d0 100644 --- a/projects/microb2010/mainboard/commands.c +++ b/projects/microb2010/mainboard/commands.c @@ -75,6 +75,7 @@ extern parse_pgm_inst_t cmd_cobboard_setmode1; extern parse_pgm_inst_t cmd_cobboard_setmode2; extern parse_pgm_inst_t cmd_cobboard_setmode3; extern parse_pgm_inst_t cmd_beacon_start; +extern parse_pgm_inst_t cmd_servo_balls; extern parse_pgm_inst_t cmd_test; /* commands_traj.c */ @@ -157,6 +158,7 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_cobboard_setmode1, (parse_pgm_inst_t *)&cmd_cobboard_setmode2, (parse_pgm_inst_t *)&cmd_cobboard_setmode3, + (parse_pgm_inst_t *)&cmd_servo_balls, (parse_pgm_inst_t *)&cmd_test, /* commands_traj.c */ diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 6349ad7..f4e37a8 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -662,6 +662,8 @@ static void cmd_cobboard_show_parsed(void * parsed_result, void * data) { printf_P(PSTR("mode = %x\r\n"), cobboard.mode); printf_P(PSTR("status = %x\r\n"), cobboard.status); + printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed); + printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed); } prog_char str_cobboard_show_arg0[] = "cobboard"; @@ -697,15 +699,13 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) if (!strcmp_P(res->arg1, PSTR("init"))) i2c_cobboard_mode_init(); - else if (!strcmp_P(res->arg1, PSTR("manual"))) - i2c_cobboard_mode_manual(); - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - i2c_cobboard_mode_harvest(); + else if (!strcmp_P(res->arg1, PSTR("eject"))) + i2c_cobboard_mode_eject(); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0); -prog_char str_cobboard_setmode1_arg1[] = "init#manual#harvest"; +prog_char str_cobboard_setmode1_arg1[] = "init#eject"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1); prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)"; @@ -741,15 +741,17 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) else if (!strcmp_P(res->arg2, PSTR("right"))) side = I2C_RIGHT_SIDE; - if (!strcmp_P(res->arg1, PSTR("yyy"))) - printf("faux\r\n"); - else if (!strcmp_P(res->arg1, PSTR("xxx"))) - printf("faux\r\n"); + if (!strcmp_P(res->arg1, PSTR("deploy"))) + i2c_cobboard_mode_deploy(side); + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + i2c_cobboard_mode_harvest(side); + else if (!strcmp_P(res->arg1, PSTR("pack"))) + i2c_cobboard_mode_pack(side); } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0); -prog_char str_cobboard_setmode2_arg1[] = "xxx"; +prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack"; parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1); prog_char str_cobboard_setmode2_arg2[] = "left#right"; parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2); @@ -804,6 +806,46 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { }, }; +/**********************************************************/ +/* Servo_Balls */ + +/* this structure is filled when cmd_servo_balls is parsed successfully */ +struct cmd_servo_balls_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_servo_balls is parsed successfully */ +static void cmd_servo_balls_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_servo_balls_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("deploy"))) + support_balls_deploy(); + else if (!strcmp_P(res->arg1, PSTR("pack"))) + support_balls_pack(); +} + +prog_char str_servo_balls_arg0[] = "support_balls"; +parse_pgm_token_string_t cmd_servo_balls_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg0, str_servo_balls_arg0); +prog_char str_servo_balls_arg1[] = "deploy#pack"; +parse_pgm_token_string_t cmd_servo_balls_arg1 = + TOKEN_STRING_INITIALIZER(struct cmd_servo_balls_result, arg1, str_servo_balls_arg1); + +prog_char help_servo_balls[] = "control support balls"; +parse_pgm_inst_t cmd_servo_balls = { + .f = cmd_servo_balls_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_servo_balls, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_servo_balls_arg0, + (prog_void *)&cmd_servo_balls_arg1, + NULL, + }, +}; + /**********************************************************/ /* Test */ diff --git a/projects/microb2010/mainboard/cs.c b/projects/microb2010/mainboard/cs.c index 8fe9718..39e39ef 100644 --- a/projects/microb2010/mainboard/cs.c +++ b/projects/microb2010/mainboard/cs.c @@ -180,9 +180,9 @@ void microb_cs_init(void) rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM); /* increase gain to decrease dist, increase left and it will turn more left */ rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value, - LEFT_ENCODER, IMP_COEF * 1.0015); + LEFT_ENCODER, IMP_COEF * -1.00); rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value, - RIGHT_ENCODER, IMP_COEF * -1.006); + RIGHT_ENCODER, IMP_COEF * 1.00); /* rs will use external encoders */ rs_set_flags(&mainboard.rs, RS_USE_EXT); @@ -231,7 +231,7 @@ void microb_cs_init(void) /* ---- CS distance */ /* PID */ pid_init(&mainboard.distance.pid); - pid_set_gains(&mainboard.distance.pid, 500, 10, 7000); + pid_set_gains(&mainboard.distance.pid, 500, 100, 7000); pid_set_maximums(&mainboard.distance.pid, 0, 2000, 4095); pid_set_out_shift(&mainboard.distance.pid, 10); pid_set_derivate_filter(&mainboard.distance.pid, 6); @@ -257,19 +257,13 @@ void microb_cs_init(void) /* ---- CS left_cobroller */ /* PID */ pid_init(&mainboard.left_cobroller.pid); - pid_set_gains(&mainboard.left_cobroller.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.left_cobroller.pid, 0, 2000, 4095); - pid_set_out_shift(&mainboard.left_cobroller.pid, 10); + pid_set_gains(&mainboard.left_cobroller.pid, 80, 10, 10); + pid_set_maximums(&mainboard.left_cobroller.pid, 0, 30000, 4095); + pid_set_out_shift(&mainboard.left_cobroller.pid, 5); pid_set_derivate_filter(&mainboard.left_cobroller.pid, 6); - /* QUADRAMP */ - quadramp_init(&mainboard.left_cobroller.qr); - quadramp_set_1st_order_vars(&mainboard.left_cobroller.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.left_cobroller.qr, 17, 17); /* set accel */ - /* CS */ cs_init(&mainboard.left_cobroller.cs); - cs_set_consign_filter(&mainboard.left_cobroller.cs, quadramp_do_filter, &mainboard.left_cobroller.qr); cs_set_correct_filter(&mainboard.left_cobroller.cs, pid_do_filter, &mainboard.left_cobroller.pid); cs_set_process_in(&mainboard.left_cobroller.cs, pwm_ng_set, LEFT_COBROLLER_PWM); cs_set_process_out(&mainboard.left_cobroller.cs, encoders_left_cobroller_speed, LEFT_COBROLLER_ENCODER); @@ -283,19 +277,13 @@ void microb_cs_init(void) /* ---- CS right_cobroller */ /* PID */ pid_init(&mainboard.right_cobroller.pid); - pid_set_gains(&mainboard.right_cobroller.pid, 500, 10, 7000); - pid_set_maximums(&mainboard.right_cobroller.pid, 0, 2000, 4095); - pid_set_out_shift(&mainboard.right_cobroller.pid, 10); + pid_set_gains(&mainboard.right_cobroller.pid, 80, 10, 10); + pid_set_maximums(&mainboard.right_cobroller.pid, 0, 30000, 4095); + pid_set_out_shift(&mainboard.right_cobroller.pid, 5); pid_set_derivate_filter(&mainboard.right_cobroller.pid, 6); - /* QUADRAMP */ - quadramp_init(&mainboard.right_cobroller.qr); - quadramp_set_1st_order_vars(&mainboard.right_cobroller.qr, 2000, 2000); /* set speed */ - quadramp_set_2nd_order_vars(&mainboard.right_cobroller.qr, 17, 17); /* set accel */ - /* CS */ cs_init(&mainboard.right_cobroller.cs); - cs_set_consign_filter(&mainboard.right_cobroller.cs, quadramp_do_filter, &mainboard.right_cobroller.qr); cs_set_correct_filter(&mainboard.right_cobroller.cs, pid_do_filter, &mainboard.right_cobroller.pid); cs_set_process_in(&mainboard.right_cobroller.cs, pwm_ng_set, RIGHT_COBROLLER_PWM); cs_set_process_out(&mainboard.right_cobroller.cs, encoders_left_cobroller_speed, RIGHT_COBROLLER_ENCODER); @@ -307,10 +295,10 @@ void microb_cs_init(void) bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50); /* set them on !! */ - mainboard.angle.on = 1; - mainboard.distance.on = 1; + mainboard.angle.on = 0; + mainboard.distance.on = 0; mainboard.left_cobroller.on = 1; - mainboard.right_cobroller.on = 1; + mainboard.right_cobroller.on = 0; scheduler_add_periodical_event_priority(do_cs, NULL, diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 2ceb7e8..d0726cb 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -263,6 +263,10 @@ void i2c_recvevent(uint8_t * buf, int8_t size) /* status */ cobboard.mode = ans->mode; cobboard.status = ans->status; + 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; } @@ -368,19 +372,61 @@ 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_cobboard_mode_manual(void) +int8_t i2c_cobboard_mode_eject(void) { struct i2c_cmd_cobboard_set_mode buf; buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = I2C_COBBOARD_MODE_MANUAL; + buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } -int8_t i2c_cobboard_mode_harvest(void) +int8_t i2c_cobboard_mode_harvest(uint8_t side) { struct i2c_cmd_cobboard_set_mode buf; + uint8_t mode = cobboard.mode; + + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + if (side == I2C_LEFT_SIDE) { + mode |= I2C_COBBOARD_MODE_L_DEPLOY; + mode |= I2C_COBBOARD_MODE_L_HARVEST; + } + else { + mode |= I2C_COBBOARD_MODE_R_DEPLOY; + mode |= I2C_COBBOARD_MODE_R_HARVEST; + } + buf.mode = mode; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_cobboard_mode_deploy(uint8_t side) +{ + struct i2c_cmd_cobboard_set_mode buf; + uint8_t mode = cobboard.mode; + + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; + if (side == I2C_LEFT_SIDE) { + mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); + mode |= I2C_COBBOARD_MODE_L_DEPLOY; + } + else { + mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); + mode |= I2C_COBBOARD_MODE_R_DEPLOY; + } + buf.mode = mode; + return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); +} + +int8_t i2c_cobboard_mode_pack(uint8_t side) +{ + struct i2c_cmd_cobboard_set_mode buf; + uint8_t mode = cobboard.mode; + buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE; - buf.mode = I2C_COBBOARD_MODE_HARVEST; + if (side == I2C_LEFT_SIDE) + mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); + else + mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); + buf.mode = mode; return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h index 1e22a4f..d788ed7 100644 --- a/projects/microb2010/mainboard/i2c_protocol.h +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -35,8 +35,10 @@ void i2c_sendevent(int8_t size); int8_t i2c_set_color(uint8_t addr, uint8_t color); int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state); -int8_t i2c_cobboard_mode_manual(void); -int8_t i2c_cobboard_mode_harvest(void); int8_t i2c_cobboard_mode_init(void); +int8_t i2c_cobboard_mode_eject(void); +int8_t i2c_cobboard_mode_harvest(uint8_t side); +int8_t i2c_cobboard_mode_deploy(uint8_t side); +int8_t i2c_cobboard_mode_pack(uint8_t side); #endif diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index d0c9d14..6a36e80 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -181,7 +181,7 @@ int main(void) # error not supported #endif - //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); + eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_MAINBOARD) { @@ -216,8 +216,8 @@ int main(void) PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10, TIMER4_PRESCALER_DIV_1); - PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED | - PWM_NG_MODE_SIGN_INVERTED, &PORTD, 4); + PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED, + &PORTD, 4); PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED | PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5); PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED, @@ -239,6 +239,7 @@ int main(void) NULL, 0); PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL, NULL, 0); + support_balls_deploy(); /* init pwm for servos */ /* SCHEDULER */ scheduler_init(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 7f21058..2fdd212 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -66,16 +66,20 @@ #define IMP_COEF 10. #define DIST_IMP_MM (((IMP_ENCODERS*4) / WHEEL_PERIM_MM) * IMP_COEF) -#define LEFT_ENCODER ((void *)1) #define RIGHT_ENCODER ((void *)0) +#define LEFT_ENCODER ((void *)1) #define LEFT_COBROLLER_ENCODER ((void *)2) #define RIGHT_COBROLLER_ENCODER ((void *)3) -#define LEFT_PWM ((void *)&gen.pwm1_4A) -#define RIGHT_PWM ((void *)&gen.pwm2_4B) +#define RIGHT_PWM ((void *)&gen.pwm1_4A) +#define LEFT_PWM ((void *)&gen.pwm2_4B) #define LEFT_COBROLLER_PWM ((void *)&gen.pwm3_1A) #define RIGHT_COBROLLER_PWM ((void *)&gen.pwm4_1B) +#define SUPPORT_BALLS_R_SERVO ((void *)&gen.servo2) +#define SUPPORT_BALLS_L_SERVO ((void *)&gen.servo3) + + /** ERROR NUMS */ #define E_USER_STRAT 194 #define E_USER_I2C_PROTO 195 @@ -163,6 +167,9 @@ struct mainboard { struct cobboard { uint8_t mode; uint8_t status; + int16_t left_cobroller_speed; + int16_t right_cobroller_speed; + uint8_t cob_count; }; /* state of ballboard, synchronized through i2c */