X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=e4eee5750221ae638cdf7cd3dbb58d9d2825c0b7;hp=a69208bb2028efc7418b7dad4b738974779c74b7;hb=4e7801883ed4076cb14b63a0571467747894c0f8;hpb=6914527de2ecfef9d790740c71739e7418246b96 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index a69208b..e4eee57 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"; @@ -1063,139 +1061,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) { @@ -1265,7 +1130,6 @@ 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); } prog_char str_test_arg0[] = "test";