X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=fc51e8e1b9e75196ef48874bc9529abf2d7b54d4;hp=de7351808792e64a1927fe15dab4dc8b202a17fc;hb=0abf8acb258a098ee79cbbdb71f284693489d678;hpb=78150017ab8c5615af414df706a0525fe7c262ae;ds=sidebyside diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index de73518..fc51e8e 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -66,6 +66,7 @@ #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" @@ -301,11 +302,11 @@ static void cmd_start_parsed(void *parsed_result, void *data) 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; } @@ -729,9 +730,9 @@ 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); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; @@ -762,7 +763,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; @@ -772,17 +773,31 @@ 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("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#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); @@ -1116,6 +1131,85 @@ parse_pgm_inst_t cmd_clitoid = { }, }; +/**********************************************************/ +/* Time_Monitor */ + +/* this structure is filled when cmd_time_monitor is parsed successfully */ +struct cmd_time_monitor_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_time_monitor is parsed successfully */ +static void cmd_time_monitor_parsed(void *parsed_result, void *data) +{ +#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 +} + +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) +{ + struct cmd_strat_event_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("on"))) + strat_event_enable(); + else + strat_event_disable(); +} + +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, + }, +}; + /**********************************************************/ /* Test */ @@ -1126,58 +1220,21 @@ struct cmd_test_result { int32_t dist; }; -#if 0 -static void reverse_line(struct line_2pts *l) -{ - 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; -} -#endif - /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - uint8_t err; - -#ifdef HOST_VERSION - strat_reset_pos(298.48, COLOR_Y(309.21), - COLOR_A(70.02)); - mainboard.angle.on = 1; - mainboard.distance.on = 1; - strat_set_speed(250, SPEED_ANGLE_FAST); -#endif - init_corn_table(0, 0); - time_wait_ms(100); - - err = line2line(LINE_UP, 0, LINE_R_DOWN, 2); - err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2); - err = line2line(LINE_R_UP, 2, LINE_UP, 5); - trajectory_hardstop(&mainboard.traj); - - /* ball ejection */ - trajectory_a_abs(&mainboard.traj, COLOR_A(90)); - i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT); - time_wait_ms(2000); - - /* half turn */ - trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847)); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); - trajectory_a_rel(&mainboard.traj, COLOR_A(180)); - err = wait_traj_end(END_INTR|END_TRAJ); - - /* cob ejection */ - trajectory_d_rel(&mainboard.traj, -100); - err = wait_traj_end(END_INTR|END_TRAJ); - i2c_cobboard_mode_eject(); - time_wait_ms(2000); + 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"; @@ -1195,3 +1252,75 @@ 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)