+ int32_t d_consign = 0, a_consign = 0;
+ double angle_to_center_rad;
+
+ /* These vectors contain target position of the robot in
+ * its own coordinates */
+ vect2_cart v2cart_pos;
+ vect2_pol v2pol_target;
+
+ /* step 1 : process new commands to quadramps */
+
+ /* process the command vector from current position to the
+ * center of the circle. */
+ v2cart_pos.x = traj->target.circle.center.x - x;
+ v2cart_pos.y = traj->target.circle.center.y - y;
+ 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;
+ }
+
+ /* XXX check flags */
+ v2pol_target.theta -= angle_to_center_rad;
+
+ /* 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)
+ 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);
+ }
+
+ /* XXX check flags */
+ d_consign = 40000 + 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);
+ 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));
+
+ cs_set_consign(traj->csm_angle, a_consign);
+ cs_set_consign(traj->csm_distance, d_consign);