-/************ STATIC [ AND USEFUL ] FUNCS */
-
-/** set speed consign in quadramp filter */
-static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
-{
- struct quadramp_filter * q_d, * q_a;
- q_d = traj->csm_distance->consign_filter_params;
- q_a = traj->csm_angle->consign_filter_params;
- quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
- quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
-}
-
-/** get angle speed consign in quadramp filter */
-static uint32_t get_quadramp_angle_speed(struct trajectory *traj)
-{
- struct quadramp_filter *q_a;
- q_a = traj->csm_angle->consign_filter_params;
- return q_a->var_1st_ord_pos;
-}
-
-/** get distance speed consign in quadramp filter */
-static uint32_t get_quadramp_distance_speed(struct trajectory *traj)
-{
- struct quadramp_filter *q_d;
- q_d = traj->csm_distance->consign_filter_params;
- return q_d->var_1st_ord_pos;
-}
-
-/** remove event if any */
-static void delete_event(struct trajectory *traj)
-{
- set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
- if ( traj->scheduler_task != -1) {
- DEBUG(E_TRAJECTORY, "Delete event");
- scheduler_del_event(traj->scheduler_task);
- traj->scheduler_task = -1;
- }
-}
-
-/** schedule the trajectory event */
-static void schedule_event(struct trajectory *traj)
-{
- if ( traj->scheduler_task != -1) {
- DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
- }
- else {
- traj->scheduler_task =
- scheduler_add_periodical_event_priority(&trajectory_manager_event,
- (void*)traj,
- 100000L/SCHEDULER_UNIT, 30);
- }
-}
-
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
-static double simple_modulo_2pi(double a)
-{
- if (a < -M_PI) {
- a += M_2PI;
- }
- else if (a > M_PI) {
- a -= M_2PI;
- }
- return a;
-}
-
-/** do a modulo 2.pi -> [-Pi,+Pi] */
-static double modulo_2pi(double a)
-{
- double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
- return simple_modulo_2pi(res);
-}
-
-/** near the target (dist) ? */
-static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
-{
- double d = traj->target.pol.distance - rs_get_distance(traj->robot);
- d = ABS(d);
- d = d / traj->position->phys.distance_imp_per_mm;
- return (d < d_win);
-}
-
-/** near the target (dist in x,y) ? */
-static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
-{
- double x1 = traj->target.cart.x;
- double y1 = traj->target.cart.y;
- double x2 = position_get_x_double(traj->position);
- double y2 = position_get_y_double(traj->position);
- return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
-}
-
-/** near the angle target in radian ? Only valid if
- * traj->target.pol.angle is set (i.e. an angle command, not an xy
- * command) */
-static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
-{
- double a;
-
- /* convert relative angle from imp to rad */
- a = traj->target.pol.angle - rs_get_angle(traj->robot);
- a /= traj->position->phys.distance_imp_per_mm;
- a /= traj->position->phys.track_mm;
- a *= 2.;
- return ABS(a) < a_win_rad;
-}
-
-
-/************ SIMPLE TRAJS, NO EVENT */
-
-#define UPDATE_D 1
-#define UPDATE_A 2
-#define RESET_D 4
-#define RESET_A 8
-
-/**
- * update angle and/or distance
- * this function is not called directly by the user
- * traj : pointer to the trajectory structure
- * d_mm : distance in mm
- * a_rad : angle in radian
- * flags : what to update (UPDATE_A, UPDATE_D)
- */
-void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
- double a_rad, uint8_t state, uint8_t flags)
-{
- int32_t a_consign, d_consign;
-
- DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
- delete_event(traj);
- traj->state = state;
- if (flags & UPDATE_A) {
- if (flags & RESET_A) {
- a_consign = 0;
- }
- else {
- a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
- (traj->position->phys.track_mm) / 2);
- }
- a_consign += rs_get_angle(traj->robot);
- traj->target.pol.angle = a_consign;
- cs_set_consign(traj->csm_angle, a_consign);
- }
- if (flags & UPDATE_D) {
- if (flags & RESET_D) {
- d_consign = 0;
- }
- else {
- d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
- }
- d_consign += rs_get_distance(traj->robot);
- traj->target.pol.distance = d_consign;
- cs_set_consign(traj->csm_distance, d_consign);
- }
-}
-
-/** go straight forward (d is in mm) */
-void trajectory_d_rel(struct trajectory *traj, double d_mm)
-{
- __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
- UPDATE_D | UPDATE_A | RESET_A);
-}
-
-/** update distance consign without changing angle consign */
-void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
-{
- __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
-}
-
-/** turn by 'a' degrees */
-void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
-{
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
- UPDATE_A | UPDATE_D | RESET_D);
-}
-
-/** turn by 'a' degrees */
-void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
-{
- double posa = position_get_a_rad_double(traj->position);
- double a;
-
- a = RAD(a_deg_abs) - posa;
- a = modulo_2pi(a);
- __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
- UPDATE_A | UPDATE_D | RESET_D);
-}
-
-/** turn the robot until the point x,y is in front of us */
-void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
-{
- double posx = position_get_x_double(traj->position);
- double posy = position_get_y_double(traj->position);
- double posa = position_get_a_rad_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
- __trajectory_goto_d_a_rel(traj, 0,
- simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
- RUNNING_A,
- UPDATE_A | UPDATE_D | RESET_D);
-}
-
-/** turn the robot until the point x,y is behind us */
-void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
-{
- double posx = position_get_x_double(traj->position);
- double posy = position_get_y_double(traj->position);
- double posa = position_get_a_rad_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
- __trajectory_goto_d_a_rel(traj, 0,
- modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
- RUNNING_A,
- UPDATE_A | UPDATE_D | RESET_D);
-}
-
-/** update angle consign without changing distance consign */
-void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
-{
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
- UPDATE_A);
-}
-
-/** update angle consign without changing distance consign */
-void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
-{
- double posa = position_get_a_rad_double(traj->position);
- double a;
-
- a = RAD(a_deg_abs) - posa;
- a = modulo_2pi(a);
- __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
-}
-
-/** turn by 'a' degrees */
-void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
-{
- __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
- RUNNING_AD, UPDATE_A | UPDATE_D);
-}
-
-/** set relative angle and distance consign to 0 */
-void trajectory_stop(struct trajectory *traj)
-{
- __trajectory_goto_d_a_rel(traj, 0, 0, READY,
- UPDATE_A | UPDATE_D | RESET_D | RESET_A);
-}
-
-/** set relative angle and distance consign to 0, and break any
- * deceleration ramp in quadramp filter */
-void trajectory_hardstop(struct trajectory *traj)
-{
- struct quadramp_filter *q_d, *q_a;
-
- q_d = traj->csm_distance->consign_filter_params;
- q_a = traj->csm_angle->consign_filter_params;
- __trajectory_goto_d_a_rel(traj, 0, 0, READY,
- UPDATE_A | UPDATE_D | RESET_D | RESET_A);
-
- q_d->previous_var = 0;
- q_d->previous_out = rs_get_distance(traj->robot);
- q_a->previous_var = 0;
- q_a->previous_out = rs_get_angle(traj->robot);
-}
-
-
-/************ GOTO XY, USE EVENTS */
-
-/** goto a x,y point, using a trajectory event */
-void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a x,y point, using a trajectory event */
-void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)