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