X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=187596522c3d92df3c58cc759b5a49b32af630dd;hp=8a79c35f3f95b589e4d39ff998e8d3090146523f;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=aed049ab6be4e1916457743d53f6b610a21b4854 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 8a79c35..1875965 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,8 @@ #include "strat.h" #include "strat_utils.h" #include "strat_base.h" +#include "strat_db.h" +#include "strat_corn.h" #include "i2c_protocol.h" #include "actuator.h" @@ -281,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 */ @@ -295,19 +333,16 @@ struct cmd_start_result { /* function called when cmd_start is parsed successfully */ static void cmd_start_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - printf("not implemented\n"); -#else struct cmd_start_result *res = parsed_result; uint8_t old_level = gen.log_level; gen.logs[NB_LOGS] = E_USER_STRAT; if (!strcmp_P(res->debug, PSTR("debug"))) { - strat_infos.dump_enabled = 1; + strat_db.dump_enabled = 1; gen.log_level = 5; } else { - strat_infos.dump_enabled = 0; + strat_db.dump_enabled = 0; gen.log_level = 0; } @@ -322,11 +357,15 @@ 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; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -513,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; } } @@ -732,14 +776,18 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) struct cmd_cobboard_setmode1_result *res = parsed_result; if (!strcmp_P(res->arg1, PSTR("init"))) - i2c_cobboard_mode_init(); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) - i2c_cobboard_mode_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)"; @@ -765,7 +813,7 @@ struct cmd_cobboard_setmode2_result { }; /* function called when cmd_cobboard_setmode2 is parsed successfully */ -static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data) +static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data) { struct cmd_cobboard_setmode2_result *res = parsed_result; uint8_t side = I2C_LEFT_SIDE; @@ -775,17 +823,35 @@ 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("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); + if (!strcmp_P(res->arg1, PSTR("deploy"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_deploy(side); + } + else if (!strcmp_P(res->arg1, PSTR("harvest"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_autoharvest(side); + } + else if (!strcmp_P(res->arg1, PSTR("pack"))) { + 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); + } + else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) { + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_autoharvest_nomove(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[] = "harvest#deploy#pack"; +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); @@ -858,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 } @@ -900,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)"; @@ -934,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); @@ -1120,162 +1176,142 @@ parse_pgm_inst_t cmd_clitoid = { }; /**********************************************************/ -/* Test */ +/* Time_Monitor */ -/* this structure is filled when cmd_test is parsed successfully */ -struct cmd_test_result { +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { fixed_string_t arg0; - int32_t radius; - int32_t dist; -}; - -#define LINE_UP 0 -#define LINE_DOWN 1 -#define LINE_R_UP 2 -#define LINE_L_DOWN 3 -#define LINE_L_UP 4 -#define LINE_R_DOWN 5 - -struct line_2pts { - point_t p1; - point_t p2; + fixed_string_t arg1; }; -static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) { - float n = num; - - switch (dir) { - - case LINE_UP: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(0); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(2100); - break; - case LINE_DOWN: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(2100); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(0); - break; - case LINE_R_UP: - l->p1.x = 150; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 2850; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_L_DOWN: - l->p1.x = 2850; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 150; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - case LINE_L_UP: - l->p1.x = 2850; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 150; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_R_DOWN: - l->p1.x = 150; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 2850; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - default: - break; +#ifndef HOST_VERSION + struct cmd_time_monitor_result *res = parsed_result; + uint16_t seconds; + + if (!strcmp_P(res->arg1, PSTR("reset"))) { + eeprom_write_word(EEPROM_TIME_ADDRESS, 0); } + seconds = eeprom_read_word(EEPROM_TIME_ADDRESS); + printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60); +#endif } -#if 0 -static void reverse_line(struct line_2pts *l) +prog_char str_time_monitor_arg0[] = "time_monitor"; +parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0); +prog_char str_time_monitor_arg1[] = "show#reset"; +parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1); + +prog_char help_time_monitor[] = "Show since how long we are running"; +parse_pgm_inst_t cmd_time_monitor = { + .f = cmd_time_monitor_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_time_monitor, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_time_monitor_arg0, + (prog_void *)&cmd_time_monitor_arg1, + NULL, + }, +}; + + +/**********************************************************/ +/* Strat_Event */ + +/* this structure is filled when cmd_strat_event is parsed successfully */ +struct cmd_strat_event_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_strat_event is parsed successfully */ +static void cmd_strat_event_parsed(void *parsed_result, void *data) { - point_t tmp; - - tmp.x = l->p1.x; - tmp.y = l->p1.y; - l->p1.x = l->p2.x; - l->p1.y = l->p2.y; - l->p2.x = tmp.x; - l->p2.y = tmp.y; + struct cmd_strat_event_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + strat_event_enable(); + else + strat_event_disable(); } -#endif -static void line2line(uint8_t dir1, uint8_t num1, - uint8_t dir2, uint8_t num2) +prog_char str_strat_event_arg0[] = "strat_event"; +parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0); +prog_char str_strat_event_arg1[] = "on#off"; +parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1); + +prog_char help_strat_event[] = "Enable/disable strat_event callback"; +parse_pgm_inst_t cmd_strat_event = { + .f = cmd_strat_event_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_strat_event, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_strat_event_arg0, + (prog_void *)&cmd_strat_event_arg1, + NULL, + }, +}; + +/**********************************************************/ +/* 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) { - double line1_a_rad, line1_a_deg, line2_a_rad; - double diff_a_deg, diff_a_deg_abs, beta_deg; - double radius; - struct line_2pts l1, l2; - line_t ll1, ll2; - point_t p; - - /* convert to 2 points */ - num2line(&l1, dir1, num1); - num2line(&l2, dir2, num2); - - printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); - printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); - - /* convert to line eq and find intersection */ - pts2line(&l1.p1, &l1.p2, &ll1); - pts2line(&l2.p1, &l2.p2, &ll2); - intersect_line(&ll1, &ll2, &p); - - line1_a_rad = atan2(l1.p2.y - l1.p1.y, - l1.p2.x - l1.p1.x); - line1_a_deg = DEG(line1_a_rad); - line2_a_rad = atan2(l2.p2.y - l2.p1.y, - l2.p2.x - l2.p1.x); - diff_a_deg = DEG(line2_a_rad - line1_a_rad); - diff_a_deg_abs = fabs(diff_a_deg); - - if (diff_a_deg_abs < 70.) { - radius = 200; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else if (diff_a_deg_abs < 100.) { - radius = 100; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; - } - else { - radius = 120; - if (diff_a_deg > 0) - beta_deg = 60; - else - beta_deg = -60; - } - trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, - line1_a_deg, 150., diff_a_deg, beta_deg, - radius, xy_norm(l1.p1.x, l1.p1.y, - p.x, p.y)); - wait_traj_end(0xFF); + 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 */ + +/* this structure is filled when cmd_test is parsed successfully */ +struct cmd_test_result { + fixed_string_t arg0; + int32_t radius; + int32_t dist; +}; + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { -#ifdef HOST_VERSION - strat_reset_pos(298.48, 309.21, 70.02); - mainboard.angle.on = 1; - mainboard.distance.on = 1; -#endif - time_wait_ms(100); - - line2line(LINE_UP, 0, LINE_R_DOWN, 2); - line2line(LINE_R_DOWN, 2, LINE_R_UP, 1); - - trajectory_hardstop(&mainboard.traj); + strat_db.dump_enabled = 1; + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK); + strat_db_dump(__FUNCTION__); + + corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); + corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE); + strat_db_dump(__FUNCTION__); } prog_char str_test_arg0[] = "test";