modules/devices/robot/trajectory_manager/trajectory_manager.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 v2 - zer0 - for Eurobot 2008 */
  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
  33 #include <position_manager.h>
  34 #include <robot_system.h>
  35 #include <control_system_manager.h>
  36 #include <quadramp.h>
  37
  38 #include <trajectory_manager.h>
  39
  40 #define M_2PI (2*M_PI)
  41
  42 #define DEG(x) ((x) * (180.0 / M_PI))
  43 #define RAD(x) ((x) * (M_PI / 180.0))
  44
  45 static void trajectory_manager_event(void *param);
  46
  47 /************ INIT FUNCS */
  48
  49 /** structure initialization */
  50 void trajectory_init(struct trajectory *traj)
  51 {
  52         uint8_t flags;
  53
  54         IRQ_LOCK(flags);
  55         memset(traj, 0, sizeof(struct trajectory));
  56         traj->state = READY;
  57         traj->scheduler_task = -1;
  58         IRQ_UNLOCK(flags);
  59 }
  60
  61 /** structure initialization */
  62 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
  63                        struct cs *cs_a)
  64 {
  65         uint8_t flags;
  66
  67         IRQ_LOCK(flags);
  68         traj->csm_distance = cs_d;
  69         traj->csm_angle = cs_a;
  70         IRQ_UNLOCK(flags);
  71 }
  72
  73 /** structure initialization */
  74 void trajectory_set_robot_params(struct trajectory *traj,
  75                                  struct robot_system *rs,
  76                                  struct robot_position *pos)
  77 {
  78         uint8_t flags;
  79         IRQ_LOCK(flags);
  80         traj->robot = rs;
  81         traj->position = pos;
  82         IRQ_UNLOCK(flags);
  83 }
  84
  85 /** set speed consign */
  86 void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
  87 {
  88         uint8_t flags;
  89         IRQ_LOCK(flags);
  90         traj->d_speed = d_speed;
  91         traj->a_speed = a_speed;
  92         IRQ_UNLOCK(flags);
  93 }
  94
  95 /** set windows for trajectory */
  96 void trajectory_set_windows(struct trajectory *traj, double d_win,
  97                             double a_win_deg, double a_start_deg)
  98 {
  99         uint8_t flags;
 100         IRQ_LOCK(flags);
 101         traj->d_win = d_win ;
 102         traj->a_win_rad = RAD(a_win_deg);
 103         traj->a_start_rad = RAD(a_start_deg);
 104         IRQ_UNLOCK(flags);
 105 }
 106
 107 /************ STATIC [ AND USEFUL ] FUNCS */
 108
 109 /** set speed consign in quadramp filter */
 110 static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
 111 {
 112         struct quadramp_filter * q_d, * q_a;
 113         q_d = traj->csm_distance->consign_filter_params;
 114         q_a = traj->csm_angle->consign_filter_params;
 115         quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
 116         quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
 117 }
 118
 119 /** get angle speed consign in quadramp filter */
 120 static uint32_t get_quadramp_angle_speed(struct trajectory *traj)
 121 {
 122         struct quadramp_filter *q_a;
 123         q_a = traj->csm_angle->consign_filter_params;
 124         return q_a->var_1st_ord_pos;
 125 }
 126
 127 /** get distance speed consign in quadramp filter */
 128 static uint32_t get_quadramp_distance_speed(struct trajectory *traj)
 129 {
 130         struct quadramp_filter *q_d;
 131         q_d = traj->csm_distance->consign_filter_params;
 132         return q_d->var_1st_ord_pos;
 133 }
 134
 135 /** remove event if any */
 136 static void delete_event(struct trajectory *traj)
 137 {
 138         set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
 139         if ( traj->scheduler_task != -1) {
 140                 DEBUG(E_TRAJECTORY, "Delete event");
 141                 scheduler_del_event(traj->scheduler_task);
 142                 traj->scheduler_task = -1;
 143         }
 144 }
 145
 146 /** schedule the trajectory event */
 147 static void schedule_event(struct trajectory *traj)
 148 {
 149         if ( traj->scheduler_task != -1) {
 150                 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
 151         }
 152         else {
 153                 traj->scheduler_task =
 154                         scheduler_add_periodical_event_priority(&trajectory_manager_event,
 155                                                                 (void*)traj,
 156                                                                 100000L/SCHEDULER_UNIT, 30);
 157         }
 158 }
 159
 160 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
 161 static double simple_modulo_2pi(double a)
 162 {
 163         if (a < -M_PI) {
 164                 a += M_2PI;
 165         }
 166         else if (a > M_PI) {
 167                 a -= M_2PI;
 168         }
 169         return a;
 170 }
 171
 172 /** do a modulo 2.pi -> [-Pi,+Pi] */
 173 static double modulo_2pi(double a)
 174 {
 175         double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
 176         return simple_modulo_2pi(res);
 177 }
 178
 179 #include <aversive/wait.h>
 180 /** near the target (dist) ? */
 181 static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
 182 {
 183         double d = traj->target.pol.distance - rs_get_distance(traj->robot);
 184         d = ABS(d);
 185         d = d / traj->position->phys.distance_imp_per_mm;
 186         return (d < d_win);
 187 }
 188
 189 /** near the target (dist in x,y) ? */
 190 static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
 191 {
 192         double x1 = traj->target.cart.x;
 193         double y1 = traj->target.cart.y;
 194         double x2 = position_get_x_double(traj->position);
 195         double y2 = position_get_y_double(traj->position);
 196         return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
 197 }
 198
 199 /** near the angle target in radian ? Only valid if
 200  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
 201  *  command) */
 202 static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
 203 {
 204         double a;
 205
 206         /* convert relative angle from imp to rad */
 207         a = traj->target.pol.angle - rs_get_angle(traj->robot);
 208         a /= traj->position->phys.distance_imp_per_mm;
 209         a /= traj->position->phys.track_mm;
 210         a *= 2.;
 211         return ABS(a) < a_win_rad;
 212 }
 213
 214
 215 /************ SIMPLE TRAJS, NO EVENT */
 216
 217 #define UPDATE_D 1
 218 #define UPDATE_A 2
 219 #define RESET_D  4
 220 #define RESET_A  8
 221
 222 /**
 223  * update angle and/or distance
 224  * this function is not called directly by the user
 225  *   traj  : pointer to the trajectory structure
 226  *   d_mm  : distance in mm
 227  *   a_rad : angle in radian
 228  *   flags : what to update (UPDATE_A, UPDATE_D)
 229  */
 230 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
 231                                double a_rad, uint8_t state, uint8_t flags)
 232 {
 233         int32_t a_consign, d_consign;
 234
 235         DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
 236         delete_event(traj);
 237         traj->state = state;
 238         if (flags & UPDATE_A) {
 239                 if (flags & RESET_A) {
 240                         a_consign = 0;
 241                 }
 242                 else {
 243                         a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
 244                                               (traj->position->phys.track_mm) / 2);
 245                 }
 246                 a_consign +=  rs_get_angle(traj->robot);
 247                 traj->target.pol.angle = a_consign;
 248                 cs_set_consign(traj->csm_angle, a_consign);
 249         }
 250         if (flags & UPDATE_D) {
 251                 if (flags & RESET_D) {
 252                         d_consign = 0;
 253                 }
 254                 else {
 255                         d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
 256                 }
 257                 d_consign += rs_get_distance(traj->robot);
 258                 traj->target.pol.distance = d_consign;
 259                 cs_set_consign(traj->csm_distance, d_consign);
 260         }
 261 }
 262
 263 /** go straight forward (d is in mm) */
 264 void trajectory_d_rel(struct trajectory *traj, double d_mm)
 265 {
 266         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
 267                                   UPDATE_D | UPDATE_A | RESET_A);
 268 }
 269
 270 /** update distance consign without changing angle consign */
 271 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
 272 {
 273         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
 274 }
 275
 276 /** turn by 'a' degrees */
 277 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
 278 {
 279         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
 280                                   UPDATE_A | UPDATE_D | RESET_D);
 281 }
 282
 283 /** turn by 'a' degrees */
 284 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
 285 {
 286         double posa = position_get_a_rad_double(traj->position);
 287         double a;
 288
 289         a = RAD(a_deg_abs) - posa;
 290         a = modulo_2pi(a);
 291         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
 292                                   UPDATE_A | UPDATE_D | RESET_D);
 293 }
 294
 295 /** turn the robot until the point x,y is in front of us */
 296 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
 297 {
 298         double posx = position_get_x_double(traj->position);
 299         double posy = position_get_y_double(traj->position);
 300         double posa = position_get_a_rad_double(traj->position);
 301
 302         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
 303         __trajectory_goto_d_a_rel(traj, 0,
 304                         simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
 305                                   RUNNING_A,
 306                                   UPDATE_A | UPDATE_D | RESET_D);
 307 }
 308
 309 /** turn the robot until the point x,y is behind us */
 310 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
 311 {
 312         double posx = position_get_x_double(traj->position);
 313         double posy = position_get_y_double(traj->position);
 314         double posa = position_get_a_rad_double(traj->position);
 315
 316         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
 317         __trajectory_goto_d_a_rel(traj, 0,
 318                         modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
 319                                   RUNNING_A,
 320                                   UPDATE_A | UPDATE_D | RESET_D);
 321 }
 322
 323 /** update angle consign without changing distance consign */
 324 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
 325 {
 326         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
 327                                   UPDATE_A);
 328 }
 329
 330 /** update angle consign without changing distance consign */
 331 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
 332 {
 333         double posa = position_get_a_rad_double(traj->position);
 334         double a;
 335
 336         a = RAD(a_deg_abs) - posa;
 337         a = modulo_2pi(a);
 338         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
 339 }
 340
 341 /** turn by 'a' degrees */
 342 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
 343 {
 344         __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
 345                                   RUNNING_AD, UPDATE_A | UPDATE_D);
 346 }
 347
 348 /** set relative angle and distance consign to 0 */
 349 void trajectory_stop(struct trajectory *traj)
 350 {
 351         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
 352                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 353 }
 354
 355 /** set relative angle and distance consign to 0, and break any
 356  * deceleration ramp in quadramp filter */
 357 void trajectory_hardstop(struct trajectory *traj)
 358 {
 359         struct quadramp_filter *q_d, *q_a;
 360
 361         q_d = traj->csm_distance->consign_filter_params;
 362         q_a = traj->csm_angle->consign_filter_params;
 363         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
 364                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 365
 366         q_d->previous_var = 0;
 367         q_d->previous_out = rs_get_distance(traj->robot);
 368         q_a->previous_var = 0;
 369         q_a->previous_out = rs_get_angle(traj->robot);
 370 }
 371
 372
 373 /************ GOTO XY, USE EVENTS */
 374
 375 /** goto a x,y point, using a trajectory event */
 376 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
 377 {
 378         DEBUG(E_TRAJECTORY, "Goto XY");
 379         delete_event(traj);
 380         traj->target.cart.x = x;
 381         traj->target.cart.y = y;
 382         traj->state = RUNNING_XY_START;
 383         trajectory_manager_event(traj);
 384         schedule_event(traj);
 385 }
 386
 387 /** go forward to a x,y point, using a trajectory event */
 388 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
 389 {
 390         DEBUG(E_TRAJECTORY, "Goto XY_F");
 391         delete_event(traj);
 392         traj->target.cart.x = x;
 393         traj->target.cart.y = y;
 394         traj->state = RUNNING_XY_F_START;
 395         trajectory_manager_event(traj);
 396         schedule_event(traj);
 397 }
 398
 399 /** go backward to a x,y point, using a trajectory event */
 400 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
 401 {
 402         DEBUG(E_TRAJECTORY, "Goto XY_B");
 403         delete_event(traj);
 404         traj->target.cart.x = x;
 405         traj->target.cart.y = y;
 406         traj->state = RUNNING_XY_B_START;
 407         trajectory_manager_event(traj);
 408         schedule_event(traj);
 409 }
 410
 411 /** go forward to a d,a point, using a trajectory event */
 412 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
 413 {
 414         vect2_pol p;
 415         double x = position_get_x_double(traj->position);
 416         double y = position_get_y_double(traj->position);
 417
 418         DEBUG(E_TRAJECTORY, "Goto DA rel");
 419
 420         delete_event(traj);
 421         p.r = d;
 422         p.theta = RAD(a) + position_get_a_rad_double(traj->position);
 423         vect2_pol2cart(&p, &traj->target.cart);
 424         traj->target.cart.x += x;
 425         traj->target.cart.y += y;
 426
 427         traj->state = RUNNING_XY_START;
 428         trajectory_manager_event(traj);
 429         schedule_event(traj);
 430 }
 431
 432 /** go forward to a x,y relative point, using a trajectory event */
 433 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
 434 {
 435         vect2_cart c;
 436         vect2_pol p;
 437         double x = position_get_x_double(traj->position);
 438         double y = position_get_y_double(traj->position);
 439
 440         DEBUG(E_TRAJECTORY, "Goto XY rel");
 441
 442         delete_event(traj);
 443         c.x = x_rel_mm;
 444         c.y = y_rel_mm;
 445
 446         vect2_cart2pol(&c, &p);
 447         p.theta += position_get_a_rad_double(traj->position);;
 448         vect2_pol2cart(&p, &traj->target.cart);
 449
 450         traj->target.cart.x += x;
 451         traj->target.cart.y += y;
 452
 453         traj->state = RUNNING_XY_START;
 454         trajectory_manager_event(traj);
 455         schedule_event(traj);
 456 }
 457
 458 /************ FUNCS FOR GETTING TRAJ STATE */
 459
 460 /** return true if the position consign is equal to the filtered
 461  * position consign (after quadramp filter), for angle and
 462  * distance. */
 463 uint8_t trajectory_finished(struct trajectory *traj)
 464 {
 465         return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
 466                 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
 467 }
 468
 469 /** return true if traj is nearly finished */
 470 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
 471 {
 472         switch(traj->state) {
 473
 474         case RUNNING_XY_ANGLE_OK:
 475         case RUNNING_XY_F_ANGLE_OK:
 476         case RUNNING_XY_B_ANGLE_OK:
 477                 /* if robot coordinates are near the x,y target */
 478                 return is_robot_in_xy_window(traj, d_win);
 479
 480         case RUNNING_A:
 481                 return is_robot_in_angle_window(traj, a_win_rad);
 482
 483         case RUNNING_D:
 484                 return is_robot_in_dist_window(traj, d_win);
 485
 486         case RUNNING_AD:
 487                 return is_robot_in_dist_window(traj, d_win) &&
 488                         is_robot_in_angle_window(traj, a_win_rad);
 489
 490         case RUNNING_XY_START:
 491         case RUNNING_XY_F_START:
 492         case RUNNING_XY_B_START:
 493         case RUNNING_XY_ANGLE:
 494         case RUNNING_XY_F_ANGLE:
 495         case RUNNING_XY_B_ANGLE:
 496         default:
 497                 return 0;
 498         }
 499 }
 500
 501 /*********** *TRAJECTORY EVENT FUNC */
 502
 503 /** event called for xy trajectories */
 504 static void trajectory_manager_event(void * param)
 505 {
 506         struct trajectory *traj = (struct trajectory *)param;
 507         double coef=1.0;
 508         double x = position_get_x_double(traj->position);
 509         double y = position_get_y_double(traj->position);
 510         double a = position_get_a_rad_double(traj->position);
 511         int32_t d_consign=0, a_consign=0;
 512
 513         /* These vectors contain target position of the robot in
 514          * its own coordinates */
 515         vect2_cart v2cart_pos;
 516         vect2_pol v2pol_target;
 517
 518         /* step 1 : process new commands to quadramps */
 519
 520         switch (traj->state) {
 521         case RUNNING_XY_START:
 522         case RUNNING_XY_ANGLE:
 523         case RUNNING_XY_ANGLE_OK:
 524         case RUNNING_XY_F_START:
 525         case RUNNING_XY_F_ANGLE:
 526         case RUNNING_XY_F_ANGLE_OK:
 527         case RUNNING_XY_B_START:
 528         case RUNNING_XY_B_ANGLE:
 529         case RUNNING_XY_B_ANGLE_OK:
 530
 531                 /* process the command vector from current position to
 532                  * absolute target, or to the center of the circle. */
 533                 v2cart_pos.x = traj->target.cart.x - x;
 534                 v2cart_pos.y = traj->target.cart.y - y;
 535                 vect2_cart2pol(&v2cart_pos, &v2pol_target);
 536                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 537
 538                 /* asked to go backwards */
 539                 if (traj->state >= RUNNING_XY_B_START &&
 540                     traj->state <= RUNNING_XY_B_ANGLE_OK ) {
 541                         v2pol_target.r = -v2pol_target.r;
 542                         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
 543                 }
 544
 545                 /* if we don't need to go forward */
 546                 if (traj->state >= RUNNING_XY_START &&
 547                     traj->state <= RUNNING_XY_ANGLE_OK ) {
 548                         /* If the target is behind the robot, we need to go
 549                          * backwards. 0.52 instead of 0.5 because we prefer to
 550                          * go forward */
 551                         if ((v2pol_target.theta > 0.52*M_PI) ||
 552                             (v2pol_target.theta < -0.52*M_PI ) ) {
 553                                 v2pol_target.r = -v2pol_target.r;
 554                                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
 555                         }
 556                 }
 557
 558                 /* XXX circle */
 559
 560                 /* If the robot is correctly oriented to start moving in distance */
 561                 /* here limit dist speed depending on v2pol_target.theta */
 562                 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
 563                         set_quadramp_speed(traj, 0, traj->a_speed);
 564                 else {
 565                         coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
 566                         set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
 567                 }
 568
 569                 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
 570                 d_consign += rs_get_distance(traj->robot);
 571
 572                 /* angle consign */
 573                 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
 574                 a_consign = (int32_t)(v2pol_target.theta *
 575                                       (traj->position->phys.distance_imp_per_mm) *
 576                                       (traj->position->phys.track_mm) / 2.2);
 577                 a_consign += rs_get_angle(traj->robot);
 578
 579                 break;
 580
 581         default:
 582                 /* hmmm quite odd, delete the event */
 583                 DEBUG(E_TRAJECTORY, "GNI ???");
 584                 delete_event(traj);
 585                 traj->state = READY;
 586         }
 587
 588
 589         /* step 2 : update state, or delete event if we reached the
 590          * destination */
 591
 592         /* XXX if target is our pos !! */
 593
 594         switch (traj->state) {
 595         case RUNNING_XY_START:
 596         case RUNNING_XY_F_START:
 597         case RUNNING_XY_B_START:
 598                 /* START -> ANGLE */
 599                 DEBUG(E_TRAJECTORY, "-> ANGLE");
 600                 traj->state ++;
 601                 break;
 602
 603         case RUNNING_XY_ANGLE:
 604         case RUNNING_XY_F_ANGLE:
 605         case RUNNING_XY_B_ANGLE: {
 606                 struct quadramp_filter *q_a;
 607                 q_a = traj->csm_angle->consign_filter_params;
 608                 /* if d_speed is not 0, we are in start_angle_win */
 609                 if (get_quadramp_distance_speed(traj)) {
 610                         if(is_robot_in_xy_window(traj, traj->d_win)) {
 611                                 delete_event(traj);
 612                         }
 613                         /* ANGLE -> ANGLE_OK */
 614                         traj->state ++;
 615                         DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
 616                 }
 617                 break;
 618         }
 619
 620         case RUNNING_XY_ANGLE_OK:
 621         case RUNNING_XY_F_ANGLE_OK:
 622         case RUNNING_XY_B_ANGLE_OK:
 623                 /* If we reached the destination */
 624                 if(is_robot_in_xy_window(traj, traj->d_win)) {
 625                         delete_event(traj);
 626                 }
 627         break;
 628
 629         default:
 630                 break;
 631         }
 632
 633         /* step 3 : send the processed commands to cs */
 634
 635         DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
 636               x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
 637
 638         DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
 639               "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
 640               rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
 641               rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
 642
 643         cs_set_consign(traj->csm_angle, a_consign);
 644         cs_set_consign(traj->csm_distance, d_consign);
 645 }
 646
 647 /*********** *CIRCLE */
 648
 649 /*
 650  * Compute the fastest distance and angle speeds matching the radius
 651  * from current traj_speed
 652  */
 653 /* static  */void circle_get_da_speed_from_radius(struct trajectory *traj,
 654                                                   double radius_mm,
 655                                                   double *speed_d,
 656                                                   double *speed_a)
 657 {
 658         /* speed_d = coef * speed_a */
 659         double coef;
 660         double speed_d2, speed_a2;
 661
 662         coef = 2. * radius_mm / traj->position->phys.track_mm;
 663
 664         speed_d2 = traj->a_speed * coef;
 665         if (speed_d2 < traj->d_speed) {
 666                 *speed_d = speed_d2;
 667                 *speed_a = traj->a_speed;
 668         }
 669         else {
 670                 speed_a2 = traj->d_speed / coef;
 671                 *speed_d = traj->d_speed;
 672                 *speed_a = speed_a2;
 673         }
 674 }
 675
 676 /* return the distance in millimeters that corresponds to an angle in
 677  * degree and a radius in mm */
 678 /* static  */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
 679 {
 680         double a_rad = RAD(a_deg);
 681         return a_rad * radius_mm;
 682 }
 683
 684 /*
 685  * Start a circle of specified radius around the specified center
 686  * (relative with d,a). The distance is specified in mm.
 687  */
 688 void trajectory_circle(struct trajectory *traj,
 689                        double center_d_mm, double center_a_rad,
 690                        double radius_mm, double dist_mm)
 691 {
 692 /*      double */
 693
 694 /*      DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
 695 /*            center_a_rad); */
 696 /*      delete_event(traj); */
 697 /*      traj->state = RUNNING_CIRCLE; */
 698
 699
 700 }
 701
 702 /*
 703  * Start a circle of specified radius around the specified center
 704  * (absolute). The distance is specified in mm.
 705  */
 706 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
 707                                    double x_rel_mm, double y_rel_mm,
 708                                    double radius_mm, double dist_mm)
 709 {
 710 }
 711
 712 /*
 713  * Start a circle of specified radius around the specified center
 714  * (absolute). The distance is specified in degrees.
 715  */
 716 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
 717                                     double x_rel_mm, double y_rel_mm,
 718                                     double radius_mm, double dist_degrees)
 719 {
 720
 721 }