/* 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);
* 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;
+ radius = traj->target.circle.radius;
+ if (v2pol_target.r > radius) {
+ angle_to_center_rad = 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));
+ (2 * radius));
angle_to_center_rad *= M_PI;
}
if (ABS(v2pol_target.theta) > traj->a_start_rad)
set_quadramp_speed(traj, 0, traj->a_speed);
else {
- coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
- set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
+ set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
}
/* XXX check flags */
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));
+ ", 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);