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=33c2a3040a3838226d70161f10ae719ac69b576a;hp=cc2a58af368ea9df97bf4597c99f1ea53a5c1842;hb=d1cc3debac68c0012f2c5ca33da9bcf0882f4988;hpb=966d2c97e5fda37297568c5e8ed66271e986bcdd diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c index cc2a58a..33c2a30 100644 --- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c +++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c @@ -29,6 +29,8 @@ #include #include #include +#include +#include #include #include @@ -389,8 +391,6 @@ void trajectory_manager_xy_event(struct trajectory *traj) } } - /* XXX circle */ - /* If the robot is correctly oriented to start moving in distance */ /* here limit dist speed depending on v2pol_target.theta */ if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win) @@ -475,16 +475,45 @@ void trajectory_manager_xy_event(struct trajectory *traj) cs_set_consign(traj->csm_distance, d_consign); } +/* + * Compute the fastest distance and angle speeds matching the radius + * from current traj_speed + */ +/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, + double radius_mm, + double *speed_d, + double *speed_a) +{ + /* speed_d = coef * speed_a */ + double coef; + double speed_d2, speed_a2; + + coef = 2. * radius_mm / traj->position->phys.track_mm; + + speed_d2 = traj->a_speed * coef; + if (speed_d2 < traj->d_speed) { + *speed_d = speed_d2; + *speed_a = traj->a_speed; + } + else { + speed_a2 = traj->d_speed / coef; + *speed_d = traj->d_speed; + *speed_a = speed_a2; + } +} + /* trajectory event for circles */ /* XXX static */ void trajectory_manager_circle_event(struct trajectory *traj) { - double coef = 1.0; + double radius; double x = position_get_x_double(traj->position); double y = position_get_y_double(traj->position); double a = position_get_a_rad_double(traj->position); int32_t d_consign = 0, a_consign = 0; double angle_to_center_rad; + double coef_p, coef_d; + double d_speed, a_speed; /* These vectors contain target position of the robot in * its own coordinates */ @@ -500,62 +529,122 @@ void trajectory_manager_circle_event(struct trajectory *traj) vect2_cart2pol(&v2cart_pos, &v2pol_target); v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); - /* wanted direction of center of circle: - * if we are far, go in the center direction, - * if we are at radius, we want to see the center at 90° - * if we are nearer than radius, angle to center is > 90° */ - if (v2pol_target.r > traj->target.circle.radius) { - angle_to_center_rad = traj->target.circle.radius / v2pol_target.r; - angle_to_center_rad *= (M_PI / 2); - } - else { - angle_to_center_rad = 1. - (v2pol_target.r / - (2 * traj->target.circle.radius)); - angle_to_center_rad *= M_PI; - } + /* radius consign */ + radius = traj->target.circle.radius; + + coef_p = v2pol_target.r / radius; + coef_p = 1. * coef_p; + + angle_to_center_rad = v2pol_target.theta - (M_PI / 2.); + angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad); + if (angle_to_center_rad > 0.5) + angle_to_center_rad = 0.5; + if (angle_to_center_rad < -0.5) + angle_to_center_rad = -0.5; + coef_d = exp(5*angle_to_center_rad); + coef_d = coef_d; + + circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d), + &d_speed, &a_speed); + set_quadramp_speed(traj, d_speed, a_speed); + + EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f " + "d_speed=%2.2f a_speed=%2.2f", + angle_to_center_rad, radius, v2pol_target.r, + coef_p, coef_d, d_speed, a_speed); /* XXX check flags */ - v2pol_target.theta -= angle_to_center_rad; + d_consign = 400000 + rs_get_distance(traj->robot); + a_consign = 400000 + rs_get_angle(traj->robot); + + /* angle consign */ +/* a_consign = (int32_t)(v2pol_target.theta * */ +/* (traj->position->phys.distance_imp_per_mm) * */ +/* (traj->position->phys.track_mm) / 2.0); */ +/* a_consign += rs_get_angle(traj->robot); */ + + /* step 2 : update state, or delete event if we reached the + * destination */ + +/* /\* output angle -> delete event *\/ */ +/* if (a_consign >= traj->target.circle.dest_angle) { */ +/* a_consign = traj->target.circle.dest_angle; */ +/* delete_event(traj); */ +/* } */ + + /* step 3 : send the processed commands to cs */ + +/* EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */ +/* ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */ +/* ", a_speed=%" PRIi32 ", radius = %f", */ +/* rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */ +/* rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */ +/* radius); */ + + cs_set_consign(traj->csm_angle, a_consign); + cs_set_consign(traj->csm_distance, d_consign); +} + +/* trajectory event for lines */ +static void trajectory_manager_line_event(struct trajectory *traj) +{ + double x = position_get_x_double(traj->position); + double y = position_get_y_double(traj->position); + double a = position_get_a_rad_double(traj->position); + double advance, dist_to_line; + point_t robot, proj, target_pt; + int32_t d_consign = 0, a_consign = 0; + vect2_cart v2cart_pos; + vect2_pol v2pol_target; + + robot.x = x; + robot.y = y; + + /* target point on the line is further on the line */ + proj_pt_line(&robot, &traj->target.line.line, &proj); + dist_to_line = pt_norm(&robot, &proj); + if (dist_to_line > traj->target.line.advance) + advance = 0; + else + advance = traj->target.line.advance - dist_to_line; + target_pt.x = proj.x + advance * cos(traj->target.line.angle); + target_pt.y = proj.y + advance * sin(traj->target.line.angle); + + /* target vector */ + v2cart_pos.x = target_pt.x - x; + v2cart_pos.y = target_pt.y - y; + vect2_cart2pol(&v2cart_pos, &v2pol_target); + v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a); /* If the robot is correctly oriented to start moving in distance */ /* here limit dist speed depending on v2pol_target.theta */ - if (ABS(v2pol_target.theta) > traj->a_start_rad) + if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win) set_quadramp_speed(traj, 0, traj->a_speed); else { + double coef; coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad; set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed); } - /* XXX check flags */ - d_consign = 40000 + rs_get_distance(traj->robot); + /* position consign is infinite */ + d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm)); + 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.0); + (traj->position->phys.track_mm) / 2.2); a_consign += rs_get_angle(traj->robot); - /* step 2 : update state, or delete event if we reached the - * destination */ - - /* output angle -> delete event */ - if (a_consign >= traj->target.circle.dest_angle) { - a_consign = traj->target.circle.dest_angle; - delete_event(traj); - } - - /* step 3 : send the processed commands to cs */ - - EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 - ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 - ", a_speed=%" PRIi32, - rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), - rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj)); + EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f " + "a_consign=%"PRIi32" d_consign=%"PRIi32, + target_pt.x, target_pt.y, a_consign, d_consign); cs_set_consign(traj->csm_angle, a_consign); cs_set_consign(traj->csm_distance, d_consign); } + /* trajectory event */ void trajectory_manager_event(void * param) { @@ -578,6 +667,10 @@ void trajectory_manager_event(void * param) trajectory_manager_circle_event(traj); break; + case RUNNING_LINE: + trajectory_manager_line_event(traj); + break; + default: break; } @@ -619,33 +712,6 @@ void trajectory_circle_rel(struct trajectory *traj, schedule_event(traj); } -/* - * Compute the fastest distance and angle speeds matching the radius - * from current traj_speed - */ -/* static */void circle_get_da_speed_from_radius(struct trajectory *traj, - double radius_mm, - double *speed_d, - double *speed_a) -{ - /* speed_d = coef * speed_a */ - double coef; - double speed_d2, speed_a2; - - coef = 2. * radius_mm / traj->position->phys.track_mm; - - speed_d2 = traj->a_speed * coef; - if (speed_d2 < traj->d_speed) { - *speed_d = speed_d2; - *speed_a = traj->a_speed; - } - else { - speed_a2 = traj->d_speed / coef; - *speed_d = traj->d_speed; - *speed_a = speed_a2; - } -} - /* return the distance in millimeters that corresponds to an angle in * degree and a radius in mm */ /* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg) @@ -692,3 +758,38 @@ void trajectory_circle_abs_dist_deg(struct trajectory *traj, { } + +/*********** *LINE */ + +/* Follow a line */ +void trajectory_line_abs(struct trajectory *traj, + double x1, double y1, + double x2, double y2, + double advance) +{ + point_t p1, p2; + + delete_event(traj); + + /* find the line EQ */ + p1.x = x1; + p1.y = y1; + p2.x = x2; + p2.y = y2; + pts2line(&p1, &p2, &traj->target.line.line); + + /* find the line angle */ + traj->target.line.angle = atan2(y2-y1, x2-x1); + traj->target.line.advance = advance; + + DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f", + traj->target.line.line.a, + traj->target.line.line.b, + traj->target.line.line.c, + traj->target.line.angle); + + traj->state = RUNNING_LINE; + trajectory_manager_event(traj); + schedule_event(traj); +} +