X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=2335ce38662fc347a09540081d0c547470bf8724;hp=a69208bb2028efc7418b7dad4b738974779c74b7;hb=90945acf48061ebc3960195cabcb11175475e426;hpb=6914527de2ecfef9d790740c71739e7418246b96 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index a69208b..2335ce3 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" @@ -295,19 +297,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; } @@ -326,7 +325,6 @@ static void cmd_start_parsed(void *parsed_result, void *data) gen.logs[NB_LOGS] = 0; gen.log_level = old_level; -#endif } prog_char str_start_arg0[] = "start"; @@ -513,22 +511,27 @@ static void cmd_interact_parsed(void * parsed_result, void * data) } } else { +#ifdef HOST_VERSION +#define PWM_INTERACT 300 +#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,9 +735,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"; @@ -765,7 +768,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 +778,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("pack_weak"))) { + 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#pack_weak#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 +879,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 } @@ -1063,139 +1086,6 @@ struct cmd_clitoid_result { float d_inter_mm; }; -/** - * do a superb curve joining line1 to line2 which is composed of: - * - a clothoid starting from line1 - * - a circle - * - another clothoid up to line2 - * - * the function assumes that the initial linear speed is Vd and - * angular speed is 0. - * - * - alpha: total angle - * - beta: circular part of angle (lower than alpha) - * - R: the radius of the circle (must be != 0) - * - Vd: linear speed to use (in imp per cs period) - * - Amax: maximum angular acceleration - * - d_inter: distance in mm until the intersection of the - * 2 lines - * - * return 0 on success: in this case these parameters are filled: - * - Aa_out: the angular acceleration to configure in quadramp - * - remain_d_mm_out: remaining distance before start to turn - */ -uint8_t clitoid(double alpha_deg, double beta_deg, double R_mm, - double Vd, double Amax, double d_inter_mm) -{ - double Vd_mm_s; - double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; - double remain_d_mm; - double Aa, Aa_rd_s2; - line_t line1, line2; - double x, y, a_rad; - point_t robot, intersect, pt2, center, proj; - vect_t v; - - /* param check */ - if (fabs(alpha_deg) <= fabs(beta_deg)) { - DEBUG(E_USER_STRAT, "alpha is smaller than beta"); - return END_ERROR; - } - - /* get angular speed Va */ - Vd_mm_s = Vd * (CS_HZ/DIST_IMP_MM); - DEBUG(E_USER_STRAT, "Vd_mm_s=%2.2f", Vd_mm_s); - Va_rd_s = Vd_mm_s / R_mm; - Va = Va_rd_s * (DIST_IMP_MM * EXT_TRACK_MM / (2 * CS_HZ)); - DEBUG(E_USER_STRAT, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va); - - /* process 't', the time in seconds that we will take to do - * the first clothoid */ - alpha_rad = RAD(alpha_deg); - beta_rad = RAD(beta_deg); - t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); - DEBUG(E_USER_STRAT, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", - R_mm, alpha_rad, beta_rad, t); - - /* process the angular acceleration */ - Aa_rd_s2 = Va_rd_s / t; - Aa = Aa_rd_s2 * (DIST_IMP_MM * EXT_TRACK_MM / - (2 * CS_HZ * CS_HZ)); - DEBUG(E_USER_STRAT, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa); - - /* exit if the robot cannot physically do it */ - if (Aa > Amax) { - DEBUG(E_USER_STRAT, "greater than max acceleration"); - return END_ERROR; - } - - /* the robot position */ - x = position_get_x_double(&mainboard.pos); - y = position_get_y_double(&mainboard.pos); - a_rad = position_get_a_rad_double(&mainboard.pos); - - /* define line1 and line2 */ - robot.x = x; - robot.y = y; - intersect.x = x + cos(a_rad) * d_inter_mm; - intersect.y = y + sin(a_rad) * d_inter_mm; - pts2line(&robot, &intersect, &line1); - pt2.x = intersect.x + cos(a_rad + alpha_rad); - pt2.y = intersect.y + sin(a_rad + alpha_rad); - pts2line(&intersect, &pt2, &line2); - DEBUG(E_USER_STRAT, "intersect=(%2.2f, %2.2f)", - intersect.x, intersect.y); - - /* the center of the circle is at (d_mm, d_mm) when we have to - * start the clothoid */ - d_mm = R_mm * sqrt(fabs(alpha_rad - beta_rad)) * - sqrt(M_PI) / 2.; - DEBUG(E_USER_STRAT, "d_mm=%2.2f", d_mm); - - /* translate line1 */ - v.x = intersect.x - robot.x; - v.y = intersect.y - robot.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line1, &v); - - /* translate line2 */ - v.x = intersect.x - pt2.x; - v.y = intersect.y - pt2.y; - if (a_rad > 0) - vect_rot_trigo(&v); - else - vect_rot_retro(&v); - vect_resize(&v, d_mm); - line_translate(&line2, &v); - - /* find the center of the circle, at the intersection of the - * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { - DEBUG(E_USER_STRAT, "cannot find circle center"); - return END_ERROR; - } - DEBUG(E_USER_STRAT, "center=(%2.2f,%2.2f)", center.x, center.y); - - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_USER_STRAT, "proj=(%2.2f,%2.2f)", proj.x, proj.y); - - /* process remaining distance before start turning */ - remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + d_mm); - DEBUG(E_USER_STRAT, "remain_d=%2.2f", remain_d_mm); - if (remain_d_mm < 0) { - DEBUG(E_USER_STRAT, "too late, cannot turn"); - return END_ERROR; - } - - return END_TRAJ; -} - /* function called when cmd_test is parsed successfully */ static void cmd_clitoid_parsed(void *parsed_result, void *data) { @@ -1252,6 +1142,118 @@ 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, + }, +}; + +/**********************************************************/ +/* 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 */ @@ -1265,7 +1267,18 @@ struct cmd_test_result { /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { - 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"; @@ -1283,3 +1296,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)