From: zer0 Date: Sat, 3 Apr 2010 13:02:33 +0000 (+0200) Subject: add ballboard commands on mainboard X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=09e0cfb842943982e9fa3c4792c097bf4be25360;p=aversive.git add ballboard commands on mainboard --- diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c index 24ad07f..1d134bb 100644 --- a/projects/microb2010/mainboard/actuator.c +++ b/projects/microb2010/mainboard/actuator.c @@ -68,12 +68,12 @@ void pwm_set_and_save(void *pwm, int32_t val) void support_balls_deploy(void) { pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510); - pwm_ng_set(SUPPORT_BALLS_L_SERVO, 240); + pwm_ng_set(SUPPORT_BALLS_L_SERVO, 205); } void support_balls_pack(void) { - pwm_ng_set(SUPPORT_BALLS_R_SERVO, 200); + pwm_ng_set(SUPPORT_BALLS_R_SERVO, 250); pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480); } diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 29ecd3d..fcadd07 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -662,6 +662,7 @@ 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("cob_count = %x\r\n"), cobboard.cob_count); 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); } @@ -806,6 +807,171 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = { }, }; +/**********************************************************/ +/* Ballboard_Show */ + +/* this structure is filled when cmd_ballboard_show is parsed successfully */ +struct cmd_ballboard_show_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_ballboard_show is parsed successfully */ +static void cmd_ballboard_show_parsed(void * parsed_result, void * data) +{ + printf_P(PSTR("mode = %x\r\n"), ballboard.mode); + printf_P(PSTR("status = %x\r\n"), ballboard.status); + printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count); +} + +prog_char str_ballboard_show_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0); +prog_char str_ballboard_show_arg1[] = "show"; +parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1); + +prog_char help_ballboard_show[] = "show ballboard status"; +parse_pgm_inst_t cmd_ballboard_show = { + .f = cmd_ballboard_show_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ballboard_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ballboard_show_arg0, + (prog_void *)&cmd_ballboard_show_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Ballboard_Setmode1 */ + +/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */ +struct cmd_ballboard_setmode1_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_ballboard_setmode1 is parsed successfully */ +static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) +{ + struct cmd_ballboard_setmode1_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("init"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT); + else if (!strcmp_P(res->arg1, PSTR("off"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF); + else if (!strcmp_P(res->arg1, PSTR("eject"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("harvest"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + + /* other commands */ +} + +prog_char str_ballboard_setmode1_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0); +prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off"; +parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1); + +prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)"; +parse_pgm_inst_t cmd_ballboard_setmode1 = { + .f = cmd_ballboard_setmode1_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ballboard_setmode1, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ballboard_setmode1_arg0, + (prog_void *)&cmd_ballboard_setmode1_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* Ballboard_Setmode2 */ + +/* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */ +struct cmd_ballboard_setmode2_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_ballboard_setmode2 is parsed successfully */ +static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data) +{ + struct cmd_ballboard_setmode2_result *res = parsed_result; + uint8_t mode = I2C_BALLBOARD_MODE_INIT; + + if (!strcmp_P(res->arg2, PSTR("left"))) { + if (!strcmp_P(res->arg1, PSTR("prepare"))) + mode = I2C_BALLBOARD_MODE_PREP_L_FORK; + else if (!strcmp_P(res->arg1, PSTR("take"))) + mode = I2C_BALLBOARD_MODE_TAKE_L_FORK; + } + else { + if (!strcmp_P(res->arg1, PSTR("prepare"))) + mode = I2C_BALLBOARD_MODE_PREP_R_FORK; + else if (!strcmp_P(res->arg1, PSTR("take"))) + mode = I2C_BALLBOARD_MODE_TAKE_R_FORK; + } + i2c_ballboard_set_mode(mode); +} + +prog_char str_ballboard_setmode2_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0); +prog_char str_ballboard_setmode2_arg1[] = "prepare#take"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1); +prog_char str_ballboard_setmode2_arg2[] = "left#right"; +parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2); + +prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)"; +parse_pgm_inst_t cmd_ballboard_setmode2 = { + .f = cmd_ballboard_setmode2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ballboard_setmode2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ballboard_setmode2_arg0, + (prog_void *)&cmd_ballboard_setmode2_arg1, + (prog_void *)&cmd_ballboard_setmode2_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* Ballboard_Setmode3 */ + +/* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */ +struct cmd_ballboard_setmode3_result { + fixed_string_t arg0; + fixed_string_t arg1; + uint8_t level; +}; + +/* function called when cmd_ballboard_setmode3 is parsed successfully */ +static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data) +{ + struct cmd_ballboard_setmode3_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("xxx"))) + printf("faux\r\n"); +} + +prog_char str_ballboard_setmode3_arg0[] = "ballboard"; +parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0); +prog_char str_ballboard_setmode3_arg1[] = "xxx"; +parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1); +parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8); + +prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)"; +parse_pgm_inst_t cmd_ballboard_setmode3 = { + .f = cmd_ballboard_setmode3_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_ballboard_setmode3, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_ballboard_setmode3_arg0, + (prog_void *)&cmd_ballboard_setmode3_arg1, + (prog_void *)&cmd_ballboard_setmode3_arg2, + NULL, + }, +}; + /**********************************************************/ /* Servo_Balls */ diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index d0726cb..403decd 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -270,14 +270,16 @@ void i2c_recvevent(uint8_t * buf, int8_t size) break; } - + case I2C_ANS_BALLBOARD_STATUS: { struct i2c_ans_ballboard_status * ans = (struct i2c_ans_ballboard_status *)buf; - + if (size != sizeof (*ans)) goto error; + ballboard.mode = ans->mode; ballboard.status = ans->status; + ballboard.ball_count = ans->ball_count; break; } @@ -296,13 +298,11 @@ void i2c_recvevent(uint8_t * buf, int8_t size) i2c_errors = 0; } } - + void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c) { } - - /* ******** ******** ******** ******** */ /* commands */ /* ******** ******** ******** ******** */ @@ -438,3 +438,11 @@ int8_t i2c_cobboard_mode_init(void) return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf)); } +int8_t i2c_ballboard_set_mode(uint8_t mode) +{ + struct i2c_cmd_ballboard_set_mode buf; + buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE; + 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 d788ed7..7ae7283 100644 --- a/projects/microb2010/mainboard/i2c_protocol.h +++ b/projects/microb2010/mainboard/i2c_protocol.h @@ -41,4 +41,6 @@ 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); +int8_t i2c_ballboard_set_mode(uint8_t mode); + #endif diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 2fdd212..c248644 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -115,7 +115,7 @@ struct genboard { struct pwm_ng servo2; struct pwm_ng servo3; struct pwm_ng servo4; - + /* ax12 interface */ AX12 ax12; @@ -165,7 +165,7 @@ struct mainboard { /* state of cobboard, synchronized through i2c */ struct cobboard { - uint8_t mode; + uint8_t mode; uint8_t status; int16_t left_cobroller_speed; int16_t right_cobroller_speed; @@ -174,7 +174,9 @@ struct cobboard { /* state of ballboard, synchronized through i2c */ struct ballboard { + uint8_t mode; uint8_t status; + uint8_t ball_count; }; extern struct genboard gen;