- /* go to nearest waypoint */
- trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ if (TRAJ_SUCCESS(err))
+ break;
+
+ if (cpt == 1)
+ break;
+
+ /* aie... do a S */
+ trajectory_d_a_rel(&mainboard.traj, 100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, 100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ }
+
+ trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));