X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=187596522c3d92df3c58cc759b5a49b32af630dd;hp=fc51e8e1b9e75196ef48874bc9529abf2d7b54d4;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=0abf8acb258a098ee79cbbdb71f284693489d678 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fc51e8e..1875965 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -283,6 +283,42 @@ parse_pgm_inst_t cmd_opponent_set = { }, }; +/**********************************************************/ +/* Beacon_Start */ + +/* this structure is filled when cmd_beacon_start is parsed successfully */ +struct cmd_beacon_start_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_beacon_start is parsed successfully */ +static void cmd_beacon_start_parsed(void *parsed_result, void *data) +{ + struct cmd_beacon_start_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("start"))) + i2c_ballboard_set_beacon(1); + else + i2c_ballboard_set_beacon(0); +} + +prog_char str_beacon_start_arg0[] = "beacon"; +parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0); +prog_char str_beacon_start_arg1[] = "start#stop"; +parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1); + +prog_char help_beacon_start[] = "Beacon enabled/disable"; +parse_pgm_inst_t cmd_beacon_start = { + .f = cmd_beacon_start_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_beacon_start, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_beacon_start_arg0, + (prog_void *)&cmd_beacon_start_arg1, + NULL, + }, +}; /**********************************************************/ /* Start */ @@ -321,6 +357,11 @@ static void cmd_start_parsed(void *parsed_result, void *data) i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } + printf_P(PSTR("Press a key when beacon ready\r\n")); + i2c_ballboard_set_beacon(0); + while(!cmdline_keypressed()); + i2c_ballboard_set_beacon(1); + strat_start(); gen.logs[NB_LOGS] = 0; @@ -511,22 +552,27 @@ static void cmd_interact_parsed(void * parsed_result, void * data) } } else { +#ifdef HOST_VERSION +#define PWM_INTERACT 1200 +#else +#define PWM_INTERACT 1200 +#endif switch(cmd) { case KEY_UP_ARR: - pwm_set_and_save(LEFT_PWM, 1200); - pwm_set_and_save(RIGHT_PWM, 1200); + pwm_set_and_save(LEFT_PWM, PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, PWM_INTERACT); break; case KEY_LEFT_ARR: - pwm_set_and_save(LEFT_PWM, -1200); - pwm_set_and_save(RIGHT_PWM, 1200); + pwm_set_and_save(LEFT_PWM, -PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, PWM_INTERACT); break; case KEY_DOWN_ARR: - pwm_set_and_save(LEFT_PWM, -1200); - pwm_set_and_save(RIGHT_PWM, -1200); + pwm_set_and_save(LEFT_PWM, -PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT); break; case KEY_RIGHT_ARR: - pwm_set_and_save(LEFT_PWM, 1200); - pwm_set_and_save(RIGHT_PWM, -1200); + pwm_set_and_save(LEFT_PWM, PWM_INTERACT); + pwm_set_and_save(RIGHT_PWM, -PWM_INTERACT); break; } } @@ -733,11 +779,15 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("kickstand_up"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP); + else if (!strcmp_P(res->arg1, PSTR("kickstand_down"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN); } 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#eject"; +prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand_up#kickstand_down"; 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)"; @@ -785,6 +835,10 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); i2c_cobboard_pack(side); } + else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_pack_weak(side); + } else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) { i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); i2c_cobboard_deploy_nomove(side); @@ -797,7 +851,7 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) 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[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove"; +prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove"; 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); @@ -870,6 +924,8 @@ 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); + printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob); + printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob); #endif } @@ -912,13 +968,17 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); else if (!strcmp_P(res->arg1, PSTR("harvest"))) i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); + else if (!strcmp_P(res->arg1, PSTR("prepare"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_PREP_FORK); + else if (!strcmp_P(res->arg1, PSTR("take"))) + i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK); /* 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"; +prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off#take#prepare"; 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)"; @@ -946,27 +1006,11 @@ struct cmd_ballboard_setmode2_result { /* 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"; +prog_char str_ballboard_setmode2_arg1[] = "xxx"; 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); @@ -1210,6 +1254,39 @@ parse_pgm_inst_t cmd_strat_event = { }, }; +/**********************************************************/ +/* Sleep */ + +/* this structure is filled when cmd_sleep is parsed successfully */ +struct cmd_sleep_result { + fixed_string_t arg0; + uint32_t ms; +}; + +/* function called when cmd_sleep is parsed successfully */ +static void cmd_sleep_parsed(void *parsed_result, void *data) +{ + struct cmd_sleep_result *res = parsed_result; + time_wait_ms(res->ms); +} + +prog_char str_sleep_arg0[] = "sleep"; +parse_pgm_token_string_t cmd_sleep_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sleep_result, arg0, str_sleep_arg0); +parse_pgm_token_num_t cmd_sleep_ms = TOKEN_NUM_INITIALIZER(struct cmd_sleep_result, ms, UINT32); + +prog_char help_sleep[] = "Sleep during some miliseconds"; +parse_pgm_inst_t cmd_sleep = { + .f = cmd_sleep_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_sleep, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_sleep_arg0, + (prog_void *)&cmd_sleep_ms, + NULL, + }, +}; + + /**********************************************************/ /* Test */ @@ -1252,75 +1329,3 @@ parse_pgm_inst_t cmd_test = { NULL, }, }; - - - - -/**********************************************************/ -/* Climb */ - -/* this structure is filled when cmd_climb is parsed successfully */ -struct cmd_climb_result { - fixed_string_t arg0; - int32_t radius; - int32_t dist; -}; - - -//#define DIM_BALLS_DOWN 620 -//#define DIM_BALLS_UP 700 - -#define DIM_BALLS_DOWN 530 -#define DIM_BALLS_UP 700 - -/* function called when cmd_climb is parsed successfully */ -static void cmd_climb_parsed(void *parsed_result, void *data) -{ - uint8_t err; - - printf_P(PSTR("starting Clifenger\r\n")); - strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW ); - - trajectory_goto_xy_abs(&mainboard.traj, 250, 250); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - trajectory_a_abs(&mainboard.traj, 0); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - - - trajectory_goto_xy_abs(&mainboard.traj, 1250, 250); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF); - printf_P(PSTR("ball_pack\r\n")); - support_balls_pack(); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF); - printf_P(PSTR("ball_deploy\r\n")); - support_balls_deploy(); - err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST); - - /* reach top, go down */ - trajectory_goto_xy_abs(&mainboard.traj, 250, 250); - err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)