-#endif
-
- d_speed/=100;
- if (d_speed > v2pol_target.r)
- d_speed = v2pol_target.r/2;
- if (d_speed == 0)
- d_speed = v2pol_target.r/2;
- new_angle = (d_speed * d_speed);
- new_angle += (v2pol_target.r * v2pol_target.r);
- new_angle -= (traj->target.circle.radius * traj->target.circle.radius);
- new_angle /= 2 * d_speed * v2pol_target.r;
-
- printf("robot %f %f dspeed %d polr %f pola %f cnewa %f\n",
- x, y,
- d_speed,
- v2pol_target.r, v2pol_target.theta * 180 / M_PI,
- new_angle);
-
- if (new_angle < -1)
- new_angle = -1;
-
- if (new_angle > 1)
- new_angle = 1;
-
-
- new_angle = acos(new_angle);
- printf("new_a: %f\n", new_angle*180/M_PI);
-
- new_angle = v2pol_target.theta - new_angle;
- a_consign = (int32_t)(new_angle *
- (traj->position->phys.distance_imp_per_mm) *
- (traj->position->phys.track_mm) / 2.0);
- a_consign += rs_get_angle(traj->robot);
-
-
- /* XXX check flags */
- d_consign = 40000 + rs_get_distance(traj->robot);
-
-
-
-
-
-
-
-
-
-