X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=modules%2Fdevices%2Frobot%2Ftrajectory_manager%2Ftrajectory_manager_core.c;h=8ea3bd87a623bc6b29feb7fc925c0dc7cfec9c76;hp=524628559f49f914fe79f422d71d434b4a88e1b5;hb=0abf8acb258a098ee79cbbdb71f284693489d678;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index 5246285..8ea3bd8 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -186,6 +186,7 @@ void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg) /** set relative angle and distance consign to 0 */ void trajectory_stop(struct trajectory *traj) { + DEBUG(E_TRAJECTORY, "stop"); __trajectory_goto_d_a_rel(traj, 0, 0, READY, UPDATE_A | UPDATE_D | RESET_D | RESET_A); } @@ -196,6 +197,8 @@ void trajectory_hardstop(struct trajectory *traj) { struct quadramp_filter *q_d, *q_a; + DEBUG(E_TRAJECTORY, "hardstop"); + q_d = traj->csm_distance->consign_filter_params; q_a = traj->csm_angle->consign_filter_params; __trajectory_goto_d_a_rel(traj, 0, 0, READY, @@ -643,13 +646,11 @@ static void trajectory_manager_line_event(struct trajectory *traj) } /* position consign is infinite */ - d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + d_consign = pos_mm2imp(traj, v2pol_target.r); d_consign += rs_get_distance(traj->robot); - /* angle consign */ - a_consign = (int32_t)(v2pol_target.theta * - (traj->position->phys.distance_imp_per_mm) * - (traj->position->phys.track_mm) / 2.2); + /* angle consign (1.1 to avoid oscillations) */ + a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1; a_consign += rs_get_angle(traj->robot); EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " @@ -849,7 +850,7 @@ void trajectory_line_abs(struct trajectory *traj, * - Va_out: the angular speed to configure in quadramp * - remain_d_mm_out: remaining distance before start to turn */ -static uint8_t calc_clitoid(struct trajectory *traj, +static int8_t calc_clitoid(struct trajectory *traj, double x, double y, double a_rad, double alpha_deg, double beta_deg, double R_mm, double Vd, double Amax, double d_inter_mm, @@ -857,12 +858,14 @@ static uint8_t calc_clitoid(struct trajectory *traj, { double Vd_mm_s; double Va, Va_rd_s; - double t, d_mm, alpha_rad, beta_rad; + double t, tau, d_mm, alpha_rad, beta_rad; double remain_d_mm; double Aa, Aa_rd_s2; line_t line1, line2; - point_t robot, intersect, pt2, center, proj; + line_t line1_int, line2_int; + point_t robot, intersect, pt2, center, proj, M; vect_t v; + double xm, ym, L, A; /* param check */ if (fabs(alpha_deg) <= fabs(beta_deg)) { @@ -882,8 +885,8 @@ static uint8_t calc_clitoid(struct trajectory *traj, alpha_rad = RAD(alpha_deg); beta_rad = RAD(beta_deg); t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s); - DEBUG(E_TRAJECTORY, "R_mm=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", - R_mm, alpha_rad, beta_rad, t); + DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f", + R_mm, a_rad, alpha_rad, beta_rad, t); /* process the angular acceleration */ Aa_rd_s2 = Va_rd_s / t; @@ -896,11 +899,6 @@ static uint8_t calc_clitoid(struct trajectory *traj, return -1; } - /* 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; @@ -913,46 +911,80 @@ static uint8_t calc_clitoid(struct trajectory *traj, DEBUG(E_TRAJECTORY, "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.; + /* L and A are the parameters of the clothoid, xm and ym are + * the relative coords (starting from the beginning of + * clothoid) of the crossing point between the clothoid and + * the circle. */ + L = Vd_mm_s * t; + A = R_mm * sqrt(fabs(alpha_rad - beta_rad)); + xm = + L + - (pow(L, 5) / (40. * pow(A, 4))) + + (pow(L, 9) / (3456. * pow(A, 8))) + - (pow(L, 13) / (599040. * pow(A, 12))); + ym = + (pow(L, 3) / (6. * pow(A, 2))) + - (pow(L, 7) / (336. * pow(A, 6))) + + (pow(L, 11) / (42240. * pow(A, 10))) + - (pow(L, 15) / (9676800. * pow(A, 14))); + DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)", + xm, ym); + + /* the center of the circle is at d_mm when we have to start + * the clothoid */ + tau = (alpha_rad - beta_rad) / 2.; + d_mm = ym + (R_mm * cos(tau)); DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm); /* translate line1 */ + memcpy(&line1_int, &line1, sizeof(line1_int)); + memcpy(&line2_int, &line2, sizeof(line2_int)); v.x = intersect.x - robot.x; v.y = intersect.y - robot.y; - if (a_rad > 0) + if (alpha_rad > 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line1, &v); + line_translate(&line1_int, &v); + DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y); - /* translate line2 */ + /* translate line2_int */ v.x = intersect.x - pt2.x; v.y = intersect.y - pt2.y; - if (a_rad > 0) + if (alpha_rad < 0) vect_rot_trigo(&v); else vect_rot_retro(&v); vect_resize(&v, d_mm); - line_translate(&line2, &v); + line_translate(&line2_int, &v); + DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y); /* find the center of the circle, at the intersection of the * new translated lines */ - if (intersect_line(&line1, &line2, ¢er) != 1) { + if (intersect_line(&line1_int, &line2_int, ¢er) != 1) { DEBUG(E_TRAJECTORY, "cannot find circle center"); return -1; } DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y); - /* project center of circle on line1 */ - proj_pt_line(¢er, &line1, &proj); - DEBUG(E_TRAJECTORY, "proj=(%2.2f,%2.2f)", proj.x, proj.y); + /* M is the same point than xm, ym but in absolute coords */ + if (alpha_rad < 0) { + M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm; + } + else { + M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm; + M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm; + } + DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y); + + /* project M on line 1 */ + proj_pt_line(&M, &line1, &proj); + DEBUG(E_TRAJECTORY, "proj M = (%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); + remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm); DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm); if (remain_d_mm < 0) { DEBUG(E_TRAJECTORY, "too late, cannot turn"); @@ -966,6 +998,7 @@ static uint8_t calc_clitoid(struct trajectory *traj, return 0; } +/* after the line, start the clothoid */ static void start_clitoid(struct trajectory *traj) { double Aa = traj->target.line.Aa; @@ -974,13 +1007,15 @@ static void start_clitoid(struct trajectory *traj) double R_mm = traj->target.line.R; double d; + DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f", + __FUNCTION__, Va, Aa); delete_event(traj); - traj->state = RUNNING_CLITOID_CURVE; - set_quadramp_acc(traj, Aa, Aa); - set_quadramp_speed(traj, Va, Va); - d = R_mm * a_rad; - d *= 2.; /* margin to avoid deceleration */ + d = fabs(R_mm * a_rad); + d *= 3.; /* margin to avoid deceleration */ trajectory_d_a_rel(traj, d, DEG(a_rad)); + set_quadramp_acc(traj, traj->d_acc, Aa); + set_quadramp_speed(traj, traj->d_speed, Va); + traj->state = RUNNING_CLITOID_CURVE; } @@ -994,7 +1029,7 @@ static void start_clitoid(struct trajectory *traj) * the function assumes that the initial linear speed is Vd and * angular speed is 0. * - * - x,y,a: starting position + * - x,y,a_deg: starting position * - advance: parameter for line following * - alpha: total angle * - beta: circular part of angle (lower than alpha) @@ -1008,27 +1043,34 @@ static void start_clitoid(struct trajectory *traj) * background. */ int8_t trajectory_clitoid(struct trajectory *traj, - double x, double y, double a, double advance, + double x, double y, double a_deg, double advance, double alpha_deg, double beta_deg, double R_mm, - double Vd, double d_inter_mm) + double d_inter_mm) { - double remain = 0, Aa = 0, Va = 0; + double remain = 0, Aa = 0, Va = 0, Vd; double turnx, turny; + double a_rad = RAD(a_deg); - if (calc_clitoid(traj, x, y, a, alpha_deg, beta_deg, R_mm, + Vd = traj->d_speed; + if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm, Vd, traj->a_acc, d_inter_mm, - &Aa, &Va, &remain) < 0) + &Aa, &Va, &remain) < 0) { + DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error"); return -1; + } delete_event(traj); - turnx = x + cos(a) * remain; - turny = y + sin(a) * remain; + turnx = x + cos(a_rad) * remain; + turny = y + sin(a_rad) * remain; traj->target.line.Aa = Aa; traj->target.line.Va = Va; traj->target.line.alpha = RAD(alpha_deg); traj->target.line.R = R_mm; traj->target.line.turn_pt.x = turnx; traj->target.line.turn_pt.y = turny; + DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f", + __FUNCTION__, turnx, turny); + __trajectory_line_abs(traj, x, y, turnx, turny, advance); traj->state = RUNNING_CLITOID_LINE;