bad trajectory workaround
[aversive.git] / modules / devices / robot / trajectory_manager / trajectory_manager_core.c
1 /*
2  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
3  *
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
19  *
20  */
21
22 /* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
23
24 #include <string.h>
25 #include <stdlib.h>
26 #include <math.h>
27
28 #include <aversive.h>
29 #include <aversive/error.h>
30 #include <scheduler.h>
31 #include <vect2.h>
32 #include <vect_base.h>
33 #include <lines.h>
34
35 #include <position_manager.h>
36 #include <robot_system.h>
37 #include <control_system_manager.h>
38 #include <quadramp.h>
39
40 #include <trajectory_manager.h>
41 #include "trajectory_manager_utils.h"
42 #include "trajectory_manager_core.h"
43
44 /************ SIMPLE TRAJS, NO EVENT */
45
46 #define UPDATE_D 1
47 #define UPDATE_A 2
48 #define RESET_D  4
49 #define RESET_A  8
50
51 static uint8_t evt_debug_cpt = 0;
52 #define EVT_DEBUG(args...) do {                         \
53                 if (((evt_debug_cpt ++) & 0x07) == 0) { \
54                         DEBUG(args);                    \
55                 }                                       \
56         } while (0)
57
58 static void start_clitoid(struct trajectory *traj);
59
60 /**
61  * update angle and/or distance
62  * this function is not called directly by the user
63  *   traj  : pointer to the trajectory structure
64  *   d_mm  : distance in mm
65  *   a_rad : angle in radian
66  *   flags : what to update (UPDATE_A, UPDATE_D)
67  */
68 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
69                                double a_rad, uint8_t state, uint8_t flags)
70 {
71         int32_t a_consign, d_consign;
72
73         DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
74         delete_event(traj);
75         traj->state = state;
76         if (flags & UPDATE_A) {
77                 if (flags & RESET_A) {
78                         a_consign = 0;
79                 }
80                 else {
81                         a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
82                                               (traj->position->phys.track_mm) / 2);
83                 }
84                 a_consign +=  rs_get_angle(traj->robot);
85                 traj->target.pol.angle = a_consign;
86                 cs_set_consign(traj->csm_angle, a_consign);
87         }
88         if (flags & UPDATE_D) {
89                 if (flags & RESET_D) {
90                         d_consign = 0;
91                 }
92                 else {
93                         d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
94                 }
95                 d_consign += rs_get_distance(traj->robot);
96                 traj->target.pol.distance = d_consign;
97                 cs_set_consign(traj->csm_distance, d_consign);
98         }
99 }
100
101 /** go straight forward (d is in mm) */
102 void trajectory_d_rel(struct trajectory *traj, double d_mm)
103 {
104         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
105                                   UPDATE_D | UPDATE_A | RESET_A);
106 }
107
108 /** update distance consign without changing angle consign */
109 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
110 {
111         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
112 }
113
114 /** turn by 'a' degrees */
115 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
116 {
117         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
118                                   UPDATE_A | UPDATE_D | RESET_D);
119 }
120
121 /** turn by 'a' degrees */
122 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
123 {
124         double posa = position_get_a_rad_double(traj->position);
125         double a;
126
127         a = RAD(a_deg_abs) - posa;
128         a = modulo_2pi(a);
129         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
130                                   UPDATE_A | UPDATE_D | RESET_D);
131 }
132
133 /** turn the robot until the point x,y is in front of us */
134 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
135 {
136         double posx = position_get_x_double(traj->position);
137         double posy = position_get_y_double(traj->position);
138         double posa = position_get_a_rad_double(traj->position);
139
140         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
141         __trajectory_goto_d_a_rel(traj, 0,
142                         simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
143                                   RUNNING_A,
144                                   UPDATE_A | UPDATE_D | RESET_D);
145 }
146
147 /** turn the robot until the point x,y is behind us */
148 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
149 {
150         double posx = position_get_x_double(traj->position);
151         double posy = position_get_y_double(traj->position);
152         double posa = position_get_a_rad_double(traj->position);
153
154         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
155         __trajectory_goto_d_a_rel(traj, 0,
156                         modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
157                                   RUNNING_A,
158                                   UPDATE_A | UPDATE_D | RESET_D);
159 }
160
161 /** update angle consign without changing distance consign */
162 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
163 {
164         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
165                                   UPDATE_A);
166 }
167
168 /** update angle consign without changing distance consign */
169 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
170 {
171         double posa = position_get_a_rad_double(traj->position);
172         double a;
173
174         a = RAD(a_deg_abs) - posa;
175         a = modulo_2pi(a);
176         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
177 }
178
179 /** turn by 'a' degrees */
180 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
181 {
182         __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
183                                   RUNNING_AD, UPDATE_A | UPDATE_D);
184 }
185
186 /** set relative angle and distance consign to 0 */
187 void trajectory_stop(struct trajectory *traj)
188 {
189         DEBUG(E_TRAJECTORY, "stop");
190         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
191                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
192 }
193
194 /** set relative angle and distance consign to 0, and break any
195  * deceleration ramp in quadramp filter */
196 void trajectory_hardstop(struct trajectory *traj)
197 {
198         struct quadramp_filter *q_d, *q_a;
199
200         DEBUG(E_TRAJECTORY, "hardstop");
201
202         q_d = traj->csm_distance->consign_filter_params;
203         q_a = traj->csm_angle->consign_filter_params;
204         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
205                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
206
207         q_d->previous_var = 0;
208         q_d->previous_out = rs_get_distance(traj->robot);
209         q_a->previous_var = 0;
210         q_a->previous_out = rs_get_angle(traj->robot);
211 }
212
213
214 /************ GOTO XY, USE EVENTS */
215
216 /** goto a x,y point, using a trajectory event */
217 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
218 {
219         DEBUG(E_TRAJECTORY, "Goto XY");
220         delete_event(traj);
221         traj->target.cart.x = x;
222         traj->target.cart.y = y;
223         traj->state = RUNNING_XY_START;
224         trajectory_manager_event(traj);
225         schedule_event(traj);
226 }
227
228 /** go forward to a x,y point, using a trajectory event */
229 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
230 {
231         DEBUG(E_TRAJECTORY, "Goto XY_F");
232         delete_event(traj);
233         traj->target.cart.x = x;
234         traj->target.cart.y = y;
235         traj->state = RUNNING_XY_F_START;
236         trajectory_manager_event(traj);
237         schedule_event(traj);
238 }
239
240 /** go backward to a x,y point, using a trajectory event */
241 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
242 {
243         DEBUG(E_TRAJECTORY, "Goto XY_B");
244         delete_event(traj);
245         traj->target.cart.x = x;
246         traj->target.cart.y = y;
247         traj->state = RUNNING_XY_B_START;
248         trajectory_manager_event(traj);
249         schedule_event(traj);
250 }
251
252 /** go forward to a d,a point, using a trajectory event */
253 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
254 {
255         vect2_pol p;
256         double x = position_get_x_double(traj->position);
257         double y = position_get_y_double(traj->position);
258
259         DEBUG(E_TRAJECTORY, "Goto DA rel");
260
261         delete_event(traj);
262         p.r = d;
263         p.theta = RAD(a) + position_get_a_rad_double(traj->position);
264         vect2_pol2cart(&p, &traj->target.cart);
265         traj->target.cart.x += x;
266         traj->target.cart.y += y;
267
268         traj->state = RUNNING_XY_START;
269         trajectory_manager_event(traj);
270         schedule_event(traj);
271 }
272
273 /** go forward to a x,y relative point, using a trajectory event */
274 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
275 {
276         vect2_cart c;
277         vect2_pol p;
278         double x = position_get_x_double(traj->position);
279         double y = position_get_y_double(traj->position);
280
281         DEBUG(E_TRAJECTORY, "Goto XY rel");
282
283         delete_event(traj);
284         c.x = x_rel_mm;
285         c.y = y_rel_mm;
286
287         vect2_cart2pol(&c, &p);
288         p.theta += position_get_a_rad_double(traj->position);;
289         vect2_pol2cart(&p, &traj->target.cart);
290
291         traj->target.cart.x += x;
292         traj->target.cart.y += y;
293
294         traj->state = RUNNING_XY_START;
295         trajectory_manager_event(traj);
296         schedule_event(traj);
297 }
298
299 /************ FUNCS FOR GETTING TRAJ STATE */
300
301 uint8_t trajectory_angle_finished(struct trajectory *traj)
302 {
303         return cs_get_consign(traj->csm_angle) ==
304                 cs_get_filtered_consign(traj->csm_angle);
305 }
306
307 uint8_t trajectory_distance_finished(struct trajectory *traj)
308 {
309         if (traj->state == RUNNING_CLITOID_CURVE)
310                 return 1;
311
312         return cs_get_consign(traj->csm_distance) ==
313                 cs_get_filtered_consign(traj->csm_distance) ;
314 }
315
316 /** return true if the position consign is equal to the filtered
317  * position consign (after quadramp filter), for angle and
318  * distance. */
319 uint8_t trajectory_finished(struct trajectory *traj)
320 {
321         uint8_t flags, ret;
322         //      uint8_t ret2;
323
324         IRQ_LOCK(flags);
325         ret = trajectory_distance_finished(traj) &&
326                 trajectory_angle_finished(traj);
327         IRQ_UNLOCK(flags);
328
329 #if 0
330         /* XXX THIS IS A VERY BAD WORKAROUND (fix a race) */
331         IRQ_LOCK(flags);
332         ret2 = trajectory_distance_finished(traj) &&
333                 trajectory_angle_finished(traj);
334         IRQ_UNLOCK(flags);
335
336         return ret && ret2;
337 #else
338         return ret;
339 #endif
340 }
341
342 /** return true if traj is nearly finished */
343 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
344 {
345         switch(traj->state) {
346
347         case RUNNING_XY_ANGLE_OK:
348         case RUNNING_XY_F_ANGLE_OK:
349         case RUNNING_XY_B_ANGLE_OK:
350                 /* if robot coordinates are near the x,y target */
351                 return is_robot_in_xy_window(traj, d_win);
352
353         case RUNNING_A:
354                 return is_robot_in_angle_window(traj, a_win_rad);
355
356         case RUNNING_D:
357                 return is_robot_in_dist_window(traj, d_win);
358
359         case RUNNING_AD:
360                 return is_robot_in_dist_window(traj, d_win) &&
361                         is_robot_in_angle_window(traj, a_win_rad);
362
363         case RUNNING_XY_START:
364         case RUNNING_XY_F_START:
365         case RUNNING_XY_B_START:
366         case RUNNING_XY_ANGLE:
367         case RUNNING_XY_F_ANGLE:
368         case RUNNING_XY_B_ANGLE:
369         default:
370                 return 0;
371         }
372 }
373
374 /*********** *TRAJECTORY EVENT FUNC */
375
376 /** event called for xy trajectories */
377 void trajectory_manager_xy_event(struct trajectory *traj)
378 {
379         double coef = 1.0;
380         double x = position_get_x_double(traj->position);
381         double y = position_get_y_double(traj->position);
382         double a = position_get_a_rad_double(traj->position);
383         int32_t d_consign=0, a_consign=0;
384
385         /* These vectors contain target position of the robot in
386          * its own coordinates */
387         vect2_cart v2cart_pos;
388         vect2_pol v2pol_target;
389
390         /* step 1 : process new commands to quadramps */
391
392         switch (traj->state) {
393         case RUNNING_XY_START:
394         case RUNNING_XY_ANGLE:
395         case RUNNING_XY_ANGLE_OK:
396         case RUNNING_XY_F_START:
397         case RUNNING_XY_F_ANGLE:
398         case RUNNING_XY_F_ANGLE_OK:
399         case RUNNING_XY_B_START:
400         case RUNNING_XY_B_ANGLE:
401         case RUNNING_XY_B_ANGLE_OK:
402
403                 /* process the command vector from current position to
404                  * absolute target. */
405                 v2cart_pos.x = traj->target.cart.x - x;
406                 v2cart_pos.y = traj->target.cart.y - y;
407                 vect2_cart2pol(&v2cart_pos, &v2pol_target);
408                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
409
410                 /* asked to go backwards */
411                 if (traj->state >= RUNNING_XY_B_START &&
412                     traj->state <= RUNNING_XY_B_ANGLE_OK ) {
413                         v2pol_target.r = -v2pol_target.r;
414                         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
415                 }
416
417                 /* if we don't need to go forward */
418                 if (traj->state >= RUNNING_XY_START &&
419                     traj->state <= RUNNING_XY_ANGLE_OK ) {
420                         /* If the target is behind the robot, we need to go
421                          * backwards. 0.52 instead of 0.5 because we prefer to
422                          * go forward */
423                         if ((v2pol_target.theta > 0.52*M_PI) ||
424                             (v2pol_target.theta < -0.52*M_PI ) ) {
425                                 v2pol_target.r = -v2pol_target.r;
426                                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
427                         }
428                 }
429
430                 /* If the robot is correctly oriented to start moving in distance */
431                 /* here limit dist speed depending on v2pol_target.theta */
432                 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
433                         set_quadramp_speed(traj, 0, traj->a_speed);
434                 else {
435                         coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
436                         set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
437                 }
438
439                 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
440                 d_consign += rs_get_distance(traj->robot);
441
442                 /* angle consign */
443                 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
444                 a_consign = (int32_t)(v2pol_target.theta *
445                                       (traj->position->phys.distance_imp_per_mm) *
446                                       (traj->position->phys.track_mm) / 2.2);
447                 a_consign += rs_get_angle(traj->robot);
448
449                 break;
450
451         default:
452                 /* hmmm quite odd, delete the event */
453                 DEBUG(E_TRAJECTORY, "GNI ???");
454                 delete_event(traj);
455                 traj->state = READY;
456         }
457
458
459         /* step 2 : update state, or delete event if we reached the
460          * destination */
461
462         /* XXX if target is our pos !! */
463
464         switch (traj->state) {
465         case RUNNING_XY_START:
466         case RUNNING_XY_F_START:
467         case RUNNING_XY_B_START:
468                 /* START -> ANGLE */
469                 DEBUG(E_TRAJECTORY, "-> ANGLE");
470                 traj->state ++;
471                 break;
472
473         case RUNNING_XY_ANGLE:
474         case RUNNING_XY_F_ANGLE:
475         case RUNNING_XY_B_ANGLE: {
476                 struct quadramp_filter *q_a;
477                 q_a = traj->csm_angle->consign_filter_params;
478                 /* if d_speed is not 0, we are in start_angle_win */
479                 if (get_quadramp_distance_speed(traj)) {
480                         if (is_robot_in_xy_window(traj, traj->d_win)) {
481                                 delete_event(traj);
482                         }
483                         /* ANGLE -> ANGLE_OK */
484                         traj->state ++;
485                         DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
486                 }
487                 break;
488         }
489
490         case RUNNING_XY_ANGLE_OK:
491         case RUNNING_XY_F_ANGLE_OK:
492         case RUNNING_XY_B_ANGLE_OK:
493                 /* If we reached the destination */
494                 if (is_robot_in_xy_window(traj, traj->d_win)) {
495                         delete_event(traj);
496                 }
497         break;
498
499         default:
500                 break;
501         }
502
503         /* step 3 : send the processed commands to cs */
504
505         EVT_DEBUG(E_TRAJECTORY,"EVENT XY d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
506               "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
507               rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
508               rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
509
510         cs_set_consign(traj->csm_angle, a_consign);
511         cs_set_consign(traj->csm_distance, d_consign);
512 }
513
514 /*
515  * Compute the fastest distance and angle speeds matching the radius
516  * from current traj_speed
517  */
518 void circle_get_da_speed_from_radius(struct trajectory *traj,
519                                      double radius_mm,
520                                      double *speed_d,
521                                      double *speed_a)
522 {
523         /* speed_d = coef * speed_a */
524         double coef;
525         double speed_d2, speed_a2;
526
527         coef = 2. * radius_mm / traj->position->phys.track_mm;
528
529         speed_d2 = traj->a_speed * coef;
530         if (speed_d2 < traj->d_speed) {
531                 *speed_d = speed_d2;
532                 *speed_a = traj->a_speed;
533         }
534         else {
535                 speed_a2 = traj->d_speed / coef;
536                 *speed_d = traj->d_speed;
537                 *speed_a = speed_a2;
538         }
539 }
540
541 /* trajectory event for circles */
542 /* XXX static */
543 void trajectory_manager_circle_event(struct trajectory *traj)
544 {
545         double radius;
546         double x = position_get_x_double(traj->position);
547         double y = position_get_y_double(traj->position);
548         double a = position_get_a_rad_double(traj->position);
549         int32_t d_consign = 0, a_consign = 0;
550         double angle_to_center_rad;
551         double coef_p, coef_d;
552         double d_speed, a_speed;
553
554         /* These vectors contain target position of the robot in
555          * its own coordinates */
556         vect2_cart v2cart_pos;
557         vect2_pol v2pol_target;
558
559         /* step 1 : process new commands to quadramps */
560
561         /* process the command vector from current position to the
562          * center of the circle. */
563         v2cart_pos.x = traj->target.circle.center.x - x;
564         v2cart_pos.y = traj->target.circle.center.y - y;
565         vect2_cart2pol(&v2cart_pos, &v2pol_target);
566         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
567
568         /* radius consign */
569         radius = traj->target.circle.radius;
570
571         coef_p = v2pol_target.r / radius;
572         coef_p = 1. * coef_p;
573
574         angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
575         angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad);
576         if (angle_to_center_rad > 0.5)
577                 angle_to_center_rad = 0.5;
578         if (angle_to_center_rad < -0.5)
579                 angle_to_center_rad = -0.5;
580         coef_d = exp(5*angle_to_center_rad);
581         coef_d = coef_d;
582
583         circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d),
584                                         &d_speed, &a_speed);
585         set_quadramp_speed(traj, d_speed, a_speed);
586
587         EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f "
588               "d_speed=%2.2f a_speed=%2.2f",
589                   angle_to_center_rad, radius, v2pol_target.r,
590               coef_p, coef_d, d_speed, a_speed);
591
592         /* XXX check flags */
593         d_consign = 400000 + rs_get_distance(traj->robot);
594         a_consign = 400000 + rs_get_angle(traj->robot);
595
596         /* angle consign */
597 /*      a_consign = (int32_t)(v2pol_target.theta * */
598 /*                            (traj->position->phys.distance_imp_per_mm) * */
599 /*                            (traj->position->phys.track_mm) / 2.0); */
600 /*      a_consign += rs_get_angle(traj->robot); */
601
602         /* step 2 : update state, or delete event if we reached the
603          * destination */
604
605 /*      /\* output angle -> delete event *\/ */
606 /*      if (a_consign >= traj->target.circle.dest_angle) { */
607 /*              a_consign = traj->target.circle.dest_angle; */
608 /*              delete_event(traj); */
609 /*      } */
610
611         /* step 3 : send the processed commands to cs */
612
613 /*      EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */
614 /*                ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */
615 /*                ", a_speed=%" PRIi32 ", radius = %f", */
616 /*                rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */
617 /*                rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */
618 /*                radius); */
619
620         cs_set_consign(traj->csm_angle, a_consign);
621         cs_set_consign(traj->csm_distance, d_consign);
622 }
623
624 /* trajectory event for lines */
625 static void trajectory_manager_line_event(struct trajectory *traj)
626 {
627         double x = position_get_x_double(traj->position);
628         double y = position_get_y_double(traj->position);
629         double a = position_get_a_rad_double(traj->position);
630         double advance, dist_to_line;
631         point_t robot, proj, target_pt;
632         int32_t d_consign = 0, a_consign = 0;
633         vect2_cart v2cart_pos;
634         vect2_pol v2pol_target;
635
636         robot.x = x;
637         robot.y = y;
638
639         /* target point on the line is further on the line */
640         proj_pt_line(&robot, &traj->target.line.line, &proj);
641         dist_to_line = pt_norm(&robot, &proj);
642         if (dist_to_line > traj->target.line.advance)
643                 advance = 0;
644         else
645                 advance = traj->target.line.advance - dist_to_line;
646         target_pt.x = proj.x + advance * cos(traj->target.line.angle);
647         target_pt.y = proj.y + advance * sin(traj->target.line.angle);
648
649         /* target vector */
650         v2cart_pos.x = target_pt.x - x;
651         v2cart_pos.y = target_pt.y - y;
652         vect2_cart2pol(&v2cart_pos, &v2pol_target);
653         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
654
655         /* If the robot is correctly oriented to start moving in distance */
656         /* here limit dist speed depending on v2pol_target.theta */
657         if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
658                 set_quadramp_speed(traj, 0, traj->a_speed);
659         else {
660                 double coef;
661                 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
662                 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
663         }
664
665         /* position consign is infinite */
666         d_consign = pos_mm2imp(traj, v2pol_target.r);
667         d_consign += rs_get_distance(traj->robot);
668
669         /* angle consign (1.1 to avoid oscillations) */
670         a_consign = pos_rd2imp(traj, v2pol_target.theta) / 1.1;
671         a_consign += rs_get_angle(traj->robot);
672
673         EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
674                   "a_consign=%"PRIi32" d_consign=%"PRIi32,
675                   target_pt.x, target_pt.y, a_consign, d_consign);
676
677         cs_set_consign(traj->csm_angle, a_consign);
678         cs_set_consign(traj->csm_distance, d_consign);
679
680         /* we reached dest, start clitoid */
681         if (traj->state == RUNNING_CLITOID_LINE &&
682             xy_norm(proj.x,
683                     proj.y,
684                     traj->target.line.turn_pt.x,
685                     traj->target.line.turn_pt.y) <
686             xy_norm(proj.x + cos(traj->target.line.angle),
687                     proj.y + sin(traj->target.line.angle),
688                     traj->target.line.turn_pt.x,
689                     traj->target.line.turn_pt.y)) {
690                 start_clitoid(traj);
691         }
692 }
693
694
695 /* trajectory event */
696 void trajectory_manager_event(void * param)
697 {
698         struct trajectory *traj = (struct trajectory *)param;
699
700         switch (traj->state) {
701         case RUNNING_XY_START:
702         case RUNNING_XY_ANGLE:
703         case RUNNING_XY_ANGLE_OK:
704         case RUNNING_XY_F_START:
705         case RUNNING_XY_F_ANGLE:
706         case RUNNING_XY_F_ANGLE_OK:
707         case RUNNING_XY_B_START:
708         case RUNNING_XY_B_ANGLE:
709         case RUNNING_XY_B_ANGLE_OK:
710                 trajectory_manager_xy_event(traj);
711                 break;
712
713         case RUNNING_CIRCLE:
714                 trajectory_manager_circle_event(traj);
715                 break;
716
717         case RUNNING_LINE:
718         case RUNNING_CLITOID_LINE:
719                 trajectory_manager_line_event(traj);
720                 break;
721
722         default:
723                 break;
724         }
725 }
726
727 /*********** *CIRCLE */
728
729 /* make the robot orbiting around (x,y) on a circle whose radius is
730  * radius_mm, and exit when relative destination angle is reached. The
731  * flags set if we go forward or backwards, and CW/CCW. */
732 void trajectory_circle_rel(struct trajectory *traj,
733                            double x, double y,
734                            double radius_mm,
735                            double rel_a_deg,
736                            uint8_t flags)
737 {
738         double dst_angle;
739
740         delete_event(traj);
741
742         traj->target.circle.center.x = x;
743         traj->target.circle.center.y = y;
744         traj->target.circle.radius = radius_mm;
745         traj->target.circle.flags = flags;
746
747         /* convert in steps  */
748         dst_angle = RAD(rel_a_deg) *
749                 (traj->position->phys.distance_imp_per_mm) *
750                 (traj->position->phys.track_mm) / 2.0;
751
752         traj->target.circle.dest_angle = rs_get_angle(traj->robot);
753         traj->target.circle.dest_angle += dst_angle;
754
755         DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
756               x, y, radius_mm, flags, traj->target.circle.dest_angle);
757
758         traj->state = RUNNING_CIRCLE;
759         trajectory_manager_event(traj);
760         schedule_event(traj);
761 }
762
763 /* return the distance in millimeters that corresponds to an angle in
764  * degree and a radius in mm */
765 /* static  */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
766 {
767         double a_rad = RAD(a_deg);
768         return a_rad * radius_mm;
769 }
770
771 /*
772  * Start a circle of specified radius around the specified center
773  * (relative with d,a). The distance is specified in mm.
774  */
775 void trajectory_circle(struct trajectory *traj,
776                        double center_d_mm, double center_a_rad,
777                        double radius_mm, double dist_mm)
778 {
779 /*      double */
780
781 /*      DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
782 /*            center_a_rad); */
783 /*      delete_event(traj); */
784 /*      traj->state = RUNNING_CIRCLE; */
785
786
787 }
788
789 /*
790  * Start a circle of specified radius around the specified center
791  * (absolute). The distance is specified in mm.
792  */
793 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
794                                    double x_rel_mm, double y_rel_mm,
795                                    double radius_mm, double dist_mm)
796 {
797 }
798
799 /*
800  * Start a circle of specified radius around the specified center
801  * (absolute). The distance is specified in degrees.
802  */
803 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
804                                     double x_rel_mm, double y_rel_mm,
805                                     double radius_mm, double dist_degrees)
806 {
807
808 }
809
810 /*********** *LINE */
811
812 /* Follow a line */
813 static void __trajectory_line_abs(struct trajectory *traj,
814                                   double x1, double y1,
815                                   double x2, double y2,
816                                   double advance)
817 {
818         point_t p1, p2;
819
820         /* find the line EQ */
821         p1.x = x1;
822         p1.y = y1;
823         p2.x = x2;
824         p2.y = y2;
825         pts2line(&p1, &p2, &traj->target.line.line);
826
827         /* find the line angle */
828         traj->target.line.angle = atan2(y2-y1, x2-x1);
829         traj->target.line.advance = advance;
830
831         DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f",
832               traj->target.line.line.a,
833               traj->target.line.line.b,
834               traj->target.line.line.c,
835               traj->target.line.angle);
836
837 }
838
839 /* Follow a line */
840 void trajectory_line_abs(struct trajectory *traj,
841                          double x1, double y1,
842                          double x2, double y2,
843                          double advance)
844 {
845         delete_event(traj);
846         __trajectory_line_abs(traj, x1, y1, x2, y2, advance);
847         traj->state = RUNNING_LINE;
848         trajectory_manager_event(traj);
849         schedule_event(traj);
850 }
851
852 /*** CLOTHOID */
853
854 /**
855  * process clitoid parameters
856  *
857  * - alpha: total angle
858  * - beta: circular part of angle (lower than alpha)
859  * - R: the radius of the circle (must be != 0)
860  * - Vd: linear speed to use (in imp per cs period)
861  * - Amax: maximum angular acceleration
862  * - d_inter: distance in mm until the intersection of the
863  *            2 lines
864  *
865  * return 0 on success: in this case these parameters are filled:
866  * - Aa_out: the angular acceleration to configure in quadramp
867  * - Va_out: the angular speed to configure in quadramp
868  * - remain_d_mm_out: remaining distance before start to turn
869  */
870 static int8_t calc_clitoid(struct trajectory *traj,
871                             double x, double y, double a_rad,
872                             double alpha_deg, double beta_deg, double R_mm,
873                             double Vd, double Amax, double d_inter_mm,
874                             double *Aa_out, double *Va_out, double *remain_d_mm_out)
875 {
876         double Vd_mm_s;
877         double Va, Va_rd_s;
878         double t, tau, d_mm, alpha_rad, beta_rad;
879         double remain_d_mm;
880         double Aa, Aa_rd_s2;
881         line_t line1, line2;
882         line_t line1_int, line2_int;
883         point_t robot, intersect, pt2, center, proj, M;
884         vect_t v;
885         double xm, ym, L, A;
886
887         /* param check */
888         if (fabs(alpha_deg) <= fabs(beta_deg)) {
889                 DEBUG(E_TRAJECTORY, "alpha is smaller than beta");
890                 return -1;
891         }
892
893         /* get angular speed Va */
894         Vd_mm_s = speed_imp2mm(traj, Vd);
895         DEBUG(E_TRAJECTORY, "Vd_mm_s=%2.2f", Vd_mm_s);
896         Va_rd_s = Vd_mm_s / R_mm;
897         Va = speed_rd2imp(traj, Va_rd_s);
898         DEBUG(E_TRAJECTORY, "Va_rd_s=%2.2f Va=%2.2f", Va_rd_s, Va);
899
900         /* process 't', the time in seconds that we will take to do
901          * the first clothoid */
902         alpha_rad = RAD(alpha_deg);
903         beta_rad = RAD(beta_deg);
904         t = fabs(((alpha_rad - beta_rad) * R_mm) / Vd_mm_s);
905         DEBUG(E_TRAJECTORY, "R_mm=%2.2f a_rad=%2.2f alpha_rad=%2.2f beta_rad=%2.2f t=%2.2f",
906               R_mm, a_rad, alpha_rad, beta_rad, t);
907
908         /* process the angular acceleration */
909         Aa_rd_s2 = Va_rd_s / t;
910         Aa = acc_rd2imp(traj, Aa_rd_s2);
911         DEBUG(E_TRAJECTORY, "Aa_rd_s2=%2.2f Aa=%2.2f", Aa_rd_s2, Aa);
912
913         /* exit if the robot cannot physically do it */
914         if (Aa > Amax) {
915                 DEBUG(E_TRAJECTORY, "greater than max acceleration");
916                 return -1;
917         }
918
919         /* define line1 and line2 */
920         robot.x = x;
921         robot.y = y;
922         intersect.x = x + cos(a_rad) * d_inter_mm;
923         intersect.y = y + sin(a_rad) * d_inter_mm;
924         pts2line(&robot, &intersect, &line1);
925         pt2.x = intersect.x + cos(a_rad + alpha_rad);
926         pt2.y = intersect.y + sin(a_rad + alpha_rad);
927         pts2line(&intersect, &pt2, &line2);
928         DEBUG(E_TRAJECTORY, "intersect=(%2.2f, %2.2f)",
929               intersect.x, intersect.y);
930
931         /* L and A are the parameters of the clothoid, xm and ym are
932          * the relative coords (starting from the beginning of
933          * clothoid) of the crossing point between the clothoid and
934          * the circle. */
935         L = Vd_mm_s * t;
936         A = R_mm * sqrt(fabs(alpha_rad - beta_rad));
937         xm =
938                 L
939                 - (pow(L, 5) / (40. * pow(A, 4)))
940                 + (pow(L, 9) / (3456. * pow(A, 8)))
941                 - (pow(L, 13) / (599040. * pow(A, 12)));
942         ym =
943                 (pow(L, 3) / (6. * pow(A, 2)))
944                 - (pow(L, 7) / (336. * pow(A, 6)))
945                 + (pow(L, 11) / (42240. * pow(A, 10)))
946                 - (pow(L, 15) / (9676800. * pow(A, 14)));
947         DEBUG(E_TRAJECTORY, "relative xm,ym = (%2.2f, %2.2f)",
948               xm, ym);
949
950         /* the center of the circle is at d_mm when we have to start
951          * the clothoid */
952         tau = (alpha_rad - beta_rad) / 2.;
953         d_mm = ym + (R_mm * cos(tau));
954         DEBUG(E_TRAJECTORY, "d_mm=%2.2f", d_mm);
955
956         /* translate line1 */
957         memcpy(&line1_int, &line1, sizeof(line1_int));
958         memcpy(&line2_int, &line2, sizeof(line2_int));
959         v.x = intersect.x - robot.x;
960         v.y = intersect.y - robot.y;
961         if (alpha_rad > 0)
962                 vect_rot_trigo(&v);
963         else
964                 vect_rot_retro(&v);
965         vect_resize(&v, d_mm);
966         line_translate(&line1_int, &v);
967         DEBUG(E_TRAJECTORY, "translate line1 by %2.2f,%2.2f", v.x, v.y);
968
969         /* translate line2_int */
970         v.x = intersect.x - pt2.x;
971         v.y = intersect.y - pt2.y;
972         if (alpha_rad < 0)
973                 vect_rot_trigo(&v);
974         else
975                 vect_rot_retro(&v);
976         vect_resize(&v, d_mm);
977         line_translate(&line2_int, &v);
978         DEBUG(E_TRAJECTORY, "translate line2 by %2.2f,%2.2f", v.x, v.y);
979
980         /* find the center of the circle, at the intersection of the
981          * new translated lines */
982         if (intersect_line(&line1_int, &line2_int, &center) != 1) {
983                 DEBUG(E_TRAJECTORY, "cannot find circle center");
984                 return -1;
985         }
986         DEBUG(E_TRAJECTORY, "center=(%2.2f,%2.2f)", center.x, center.y);
987
988         /* M is the same point than xm, ym but in absolute coords */
989         if (alpha_rad < 0) {
990                 M.x = center.x + cos(a_rad + M_PI/2 + tau) * R_mm;
991                 M.y = center.y + sin(a_rad + M_PI/2 + tau) * R_mm;
992         }
993         else {
994                 M.x = center.x + cos(a_rad - M_PI/2 + tau) * R_mm;
995                 M.y = center.y + sin(a_rad - M_PI/2 + tau) * R_mm;
996         }
997         DEBUG(E_TRAJECTORY, "absolute M = (%2.2f, %2.2f)", M.x, M.y);
998
999         /* project M on line 1 */
1000         proj_pt_line(&M, &line1, &proj);
1001         DEBUG(E_TRAJECTORY, "proj M = (%2.2f, %2.2f)", proj.x, proj.y);
1002
1003         /* process remaining distance before start turning */
1004         remain_d_mm = d_inter_mm - (pt_norm(&proj, &intersect) + xm);
1005         DEBUG(E_TRAJECTORY, "remain_d=%2.2f", remain_d_mm);
1006         if (remain_d_mm < 0) {
1007                 DEBUG(E_TRAJECTORY, "too late, cannot turn");
1008                 return -1;
1009         }
1010
1011         /* return result */
1012         *Aa_out = Aa;
1013         *Va_out = Va;
1014         *remain_d_mm_out = remain_d_mm;
1015         return 0;
1016 }
1017
1018 /* after the line, start the clothoid */
1019 static void start_clitoid(struct trajectory *traj)
1020 {
1021         double Aa = traj->target.line.Aa;
1022         double Va = traj->target.line.Va;
1023         double a_rad = traj->target.line.alpha;
1024         double R_mm = traj->target.line.R;
1025         double d;
1026
1027         DEBUG(E_TRAJECTORY, "%s() Va=%2.2f Aa=%2.2f",
1028               __FUNCTION__, Va, Aa);
1029         delete_event(traj);
1030         d = fabs(R_mm * a_rad);
1031         d *= 3.; /* margin to avoid deceleration */
1032         trajectory_d_a_rel(traj, d, DEG(a_rad));
1033         set_quadramp_acc(traj, traj->d_acc, Aa);
1034         set_quadramp_speed(traj, traj->d_speed, Va);
1035         traj->state = RUNNING_CLITOID_CURVE;
1036 }
1037
1038
1039 /**
1040  * do a superb curve joining line1 to line2 which is composed of:
1041  *   - a clothoid starting from line1
1042  *   - a circle
1043  *   - another clothoid up to line2
1044  * this curve is called a clitoid (hehe)
1045  *
1046  * the function assumes that the initial linear speed is Vd and
1047  * angular speed is 0.
1048  *
1049  * - x,y,a_deg: starting position
1050  * - advance: parameter for line following
1051  * - alpha: total angle
1052  * - beta: circular part of angle (lower than alpha)
1053  * - R: the radius of the circle (must be != 0)
1054  * - Vd: linear speed to use (in imp per cs period)
1055  * - Amax: maximum angular acceleration
1056  * - d_inter: distance in mm until the intersection of the
1057  *            2 lines
1058  *
1059  * return 0 if trajectory can be loaded, then it is processed in
1060  * background.
1061  */
1062 int8_t trajectory_clitoid(struct trajectory *traj,
1063                           double x, double y, double a_deg, double advance,
1064                           double alpha_deg, double beta_deg, double R_mm,
1065                           double d_inter_mm)
1066 {
1067         double remain = 0, Aa = 0, Va = 0, Vd;
1068         double turnx, turny;
1069         double a_rad = RAD(a_deg);
1070
1071         Vd = traj->d_speed;
1072         if (calc_clitoid(traj, x, y, a_rad, alpha_deg, beta_deg, R_mm,
1073                          Vd, traj->a_acc, d_inter_mm,
1074                          &Aa, &Va, &remain) < 0) {
1075                 DEBUG(E_TRAJECTORY, "%s() calc_clitoid returned an error",
1076                       __FUNCTION__);
1077                 return -1;
1078         }
1079
1080         delete_event(traj);
1081         turnx = x + cos(a_rad) * remain;
1082         turny = y + sin(a_rad) * remain;
1083         traj->target.line.Aa = Aa;
1084         traj->target.line.Va = Va;
1085         traj->target.line.alpha = RAD(alpha_deg);
1086         traj->target.line.R = R_mm;
1087         traj->target.line.turn_pt.x = turnx;
1088         traj->target.line.turn_pt.y = turny;
1089         DEBUG(E_TRAJECTORY, "%s() turn_pt=%2.2f,%2.2f",
1090               __FUNCTION__, turnx, turny);
1091
1092         __trajectory_line_abs(traj, x, y, turnx, turny,
1093                               advance);
1094         traj->state = RUNNING_CLITOID_LINE;
1095         trajectory_manager_event(traj);
1096         schedule_event(traj);
1097         return 0;
1098 }