+#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);
+
+
+
+
+
+
+
+
+
+