X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=ee0ffa4524f71377431343dd4e71e0036bf0c76c;hp=a69208bb2028efc7418b7dad4b738974779c74b7;hb=d776dc0dab6dd3ddf92041f1ad42e521847ea9cf;hpb=6914527de2ecfef9d790740c71739e7418246b96 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index a69208b..ee0ffa4 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"; @@ -732,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"; @@ -765,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; @@ -775,12 +773,18 @@ 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); + } } prog_char str_cobboard_setmode2_arg0[] = "cobboard"; @@ -1063,139 +1067,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 +1123,47 @@ 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, + }, +}; + /**********************************************************/ /* Test */ @@ -1265,7 +1177,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";