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
  59 /**
  60  * update angle and/or distance
  61  * this function is not called directly by the user
  62  *   traj  : pointer to the trajectory structure
  63  *   d_mm  : distance in mm
  64  *   a_rad : angle in radian
  65  *   flags : what to update (UPDATE_A, UPDATE_D)
  66  */
  67 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
  68                                double a_rad, uint8_t state, uint8_t flags)
  69 {
  70         int32_t a_consign, d_consign;
  71
  72         DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
  73         delete_event(traj);
  74         traj->state = state;
  75         if (flags & UPDATE_A) {
  76                 if (flags & RESET_A) {
  77                         a_consign = 0;
  78                 }
  79                 else {
  80                         a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
  81                                               (traj->position->phys.track_mm) / 2);
  82                 }
  83                 a_consign +=  rs_get_angle(traj->robot);
  84                 traj->target.pol.angle = a_consign;
  85                 cs_set_consign(traj->csm_angle, a_consign);
  86         }
  87         if (flags & UPDATE_D) {
  88                 if (flags & RESET_D) {
  89                         d_consign = 0;
  90                 }
  91                 else {
  92                         d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
  93                 }
  94                 d_consign += rs_get_distance(traj->robot);
  95                 traj->target.pol.distance = d_consign;
  96                 cs_set_consign(traj->csm_distance, d_consign);
  97         }
  98 }
  99
 100 /** go straight forward (d is in mm) */
 101 void trajectory_d_rel(struct trajectory *traj, double d_mm)
 102 {
 103         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
 104                                   UPDATE_D | UPDATE_A | RESET_A);
 105 }
 106
 107 /** update distance consign without changing angle consign */
 108 void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
 109 {
 110         __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
 111 }
 112
 113 /** turn by 'a' degrees */
 114 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
 115 {
 116         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
 117                                   UPDATE_A | UPDATE_D | RESET_D);
 118 }
 119
 120 /** turn by 'a' degrees */
 121 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
 122 {
 123         double posa = position_get_a_rad_double(traj->position);
 124         double a;
 125
 126         a = RAD(a_deg_abs) - posa;
 127         a = modulo_2pi(a);
 128         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
 129                                   UPDATE_A | UPDATE_D | RESET_D);
 130 }
 131
 132 /** turn the robot until the point x,y is in front of us */
 133 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
 134 {
 135         double posx = position_get_x_double(traj->position);
 136         double posy = position_get_y_double(traj->position);
 137         double posa = position_get_a_rad_double(traj->position);
 138
 139         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
 140         __trajectory_goto_d_a_rel(traj, 0,
 141                         simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa),
 142                                   RUNNING_A,
 143                                   UPDATE_A | UPDATE_D | RESET_D);
 144 }
 145
 146 /** turn the robot until the point x,y is behind us */
 147 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
 148 {
 149         double posx = position_get_x_double(traj->position);
 150         double posy = position_get_y_double(traj->position);
 151         double posa = position_get_a_rad_double(traj->position);
 152
 153         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
 154         __trajectory_goto_d_a_rel(traj, 0,
 155                         modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) - posa + M_PI),
 156                                   RUNNING_A,
 157                                   UPDATE_A | UPDATE_D | RESET_D);
 158 }
 159
 160 /** update angle consign without changing distance consign */
 161 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
 162 {
 163         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
 164                                   UPDATE_A);
 165 }
 166
 167 /** update angle consign without changing distance consign */
 168 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
 169 {
 170         double posa = position_get_a_rad_double(traj->position);
 171         double a;
 172
 173         a = RAD(a_deg_abs) - posa;
 174         a = modulo_2pi(a);
 175         __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
 176 }
 177
 178 /** turn by 'a' degrees */
 179 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
 180 {
 181         __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
 182                                   RUNNING_AD, UPDATE_A | UPDATE_D);
 183 }
 184
 185 /** set relative angle and distance consign to 0 */
 186 void trajectory_stop(struct trajectory *traj)
 187 {
 188         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
 189                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 190 }
 191
 192 /** set relative angle and distance consign to 0, and break any
 193  * deceleration ramp in quadramp filter */
 194 void trajectory_hardstop(struct trajectory *traj)
 195 {
 196         struct quadramp_filter *q_d, *q_a;
 197
 198         q_d = traj->csm_distance->consign_filter_params;
 199         q_a = traj->csm_angle->consign_filter_params;
 200         __trajectory_goto_d_a_rel(traj, 0, 0, READY,
 201                                   UPDATE_A | UPDATE_D | RESET_D | RESET_A);
 202
 203         q_d->previous_var = 0;
 204         q_d->previous_out = rs_get_distance(traj->robot);
 205         q_a->previous_var = 0;
 206         q_a->previous_out = rs_get_angle(traj->robot);
 207 }
 208
 209
 210 /************ GOTO XY, USE EVENTS */
 211
 212 /** goto a x,y point, using a trajectory event */
 213 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
 214 {
 215         DEBUG(E_TRAJECTORY, "Goto XY");
 216         delete_event(traj);
 217         traj->target.cart.x = x;
 218         traj->target.cart.y = y;
 219         traj->state = RUNNING_XY_START;
 220         trajectory_manager_event(traj);
 221         schedule_event(traj);
 222 }
 223
 224 /** go forward to a x,y point, using a trajectory event */
 225 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
 226 {
 227         DEBUG(E_TRAJECTORY, "Goto XY_F");
 228         delete_event(traj);
 229         traj->target.cart.x = x;
 230         traj->target.cart.y = y;
 231         traj->state = RUNNING_XY_F_START;
 232         trajectory_manager_event(traj);
 233         schedule_event(traj);
 234 }
 235
 236 /** go backward to a x,y point, using a trajectory event */
 237 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
 238 {
 239         DEBUG(E_TRAJECTORY, "Goto XY_B");
 240         delete_event(traj);
 241         traj->target.cart.x = x;
 242         traj->target.cart.y = y;
 243         traj->state = RUNNING_XY_B_START;
 244         trajectory_manager_event(traj);
 245         schedule_event(traj);
 246 }
 247
 248 /** go forward to a d,a point, using a trajectory event */
 249 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
 250 {
 251         vect2_pol p;
 252         double x = position_get_x_double(traj->position);
 253         double y = position_get_y_double(traj->position);
 254
 255         DEBUG(E_TRAJECTORY, "Goto DA rel");
 256
 257         delete_event(traj);
 258         p.r = d;
 259         p.theta = RAD(a) + position_get_a_rad_double(traj->position);
 260         vect2_pol2cart(&p, &traj->target.cart);
 261         traj->target.cart.x += x;
 262         traj->target.cart.y += y;
 263
 264         traj->state = RUNNING_XY_START;
 265         trajectory_manager_event(traj);
 266         schedule_event(traj);
 267 }
 268
 269 /** go forward to a x,y relative point, using a trajectory event */
 270 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
 271 {
 272         vect2_cart c;
 273         vect2_pol p;
 274         double x = position_get_x_double(traj->position);
 275         double y = position_get_y_double(traj->position);
 276
 277         DEBUG(E_TRAJECTORY, "Goto XY rel");
 278
 279         delete_event(traj);
 280         c.x = x_rel_mm;
 281         c.y = y_rel_mm;
 282
 283         vect2_cart2pol(&c, &p);
 284         p.theta += position_get_a_rad_double(traj->position);;
 285         vect2_pol2cart(&p, &traj->target.cart);
 286
 287         traj->target.cart.x += x;
 288         traj->target.cart.y += y;
 289
 290         traj->state = RUNNING_XY_START;
 291         trajectory_manager_event(traj);
 292         schedule_event(traj);
 293 }
 294
 295 /************ FUNCS FOR GETTING TRAJ STATE */
 296
 297 /** return true if the position consign is equal to the filtered
 298  * position consign (after quadramp filter), for angle and
 299  * distance. */
 300 uint8_t trajectory_finished(struct trajectory *traj)
 301 {
 302         return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
 303                 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
 304 }
 305
 306 /** return true if traj is nearly finished */
 307 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
 308 {
 309         switch(traj->state) {
 310
 311         case RUNNING_XY_ANGLE_OK:
 312         case RUNNING_XY_F_ANGLE_OK:
 313         case RUNNING_XY_B_ANGLE_OK:
 314                 /* if robot coordinates are near the x,y target */
 315                 return is_robot_in_xy_window(traj, d_win);
 316
 317         case RUNNING_A:
 318                 return is_robot_in_angle_window(traj, a_win_rad);
 319
 320         case RUNNING_D:
 321                 return is_robot_in_dist_window(traj, d_win);
 322
 323         case RUNNING_AD:
 324                 return is_robot_in_dist_window(traj, d_win) &&
 325                         is_robot_in_angle_window(traj, a_win_rad);
 326
 327         case RUNNING_XY_START:
 328         case RUNNING_XY_F_START:
 329         case RUNNING_XY_B_START:
 330         case RUNNING_XY_ANGLE:
 331         case RUNNING_XY_F_ANGLE:
 332         case RUNNING_XY_B_ANGLE:
 333         default:
 334                 return 0;
 335         }
 336 }
 337
 338 /*********** *TRAJECTORY EVENT FUNC */
 339
 340 /** event called for xy trajectories */
 341 void trajectory_manager_xy_event(struct trajectory *traj)
 342 {
 343         double coef = 1.0;
 344         double x = position_get_x_double(traj->position);
 345         double y = position_get_y_double(traj->position);
 346         double a = position_get_a_rad_double(traj->position);
 347         int32_t d_consign=0, a_consign=0;
 348
 349         /* These vectors contain target position of the robot in
 350          * its own coordinates */
 351         vect2_cart v2cart_pos;
 352         vect2_pol v2pol_target;
 353
 354         /* step 1 : process new commands to quadramps */
 355
 356         switch (traj->state) {
 357         case RUNNING_XY_START:
 358         case RUNNING_XY_ANGLE:
 359         case RUNNING_XY_ANGLE_OK:
 360         case RUNNING_XY_F_START:
 361         case RUNNING_XY_F_ANGLE:
 362         case RUNNING_XY_F_ANGLE_OK:
 363         case RUNNING_XY_B_START:
 364         case RUNNING_XY_B_ANGLE:
 365         case RUNNING_XY_B_ANGLE_OK:
 366
 367                 /* process the command vector from current position to
 368                  * absolute target. */
 369                 v2cart_pos.x = traj->target.cart.x - x;
 370                 v2cart_pos.y = traj->target.cart.y - y;
 371                 vect2_cart2pol(&v2cart_pos, &v2pol_target);
 372                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 373
 374                 /* asked to go backwards */
 375                 if (traj->state >= RUNNING_XY_B_START &&
 376                     traj->state <= RUNNING_XY_B_ANGLE_OK ) {
 377                         v2pol_target.r = -v2pol_target.r;
 378                         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
 379                 }
 380
 381                 /* if we don't need to go forward */
 382                 if (traj->state >= RUNNING_XY_START &&
 383                     traj->state <= RUNNING_XY_ANGLE_OK ) {
 384                         /* If the target is behind the robot, we need to go
 385                          * backwards. 0.52 instead of 0.5 because we prefer to
 386                          * go forward */
 387                         if ((v2pol_target.theta > 0.52*M_PI) ||
 388                             (v2pol_target.theta < -0.52*M_PI ) ) {
 389                                 v2pol_target.r = -v2pol_target.r;
 390                                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
 391                         }
 392                 }
 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 /*
 479  * Compute the fastest distance and angle speeds matching the radius
 480  * from current traj_speed
 481  */
 482 /* static  */void circle_get_da_speed_from_radius(struct trajectory *traj,
 483                                                   double radius_mm,
 484                                                   double *speed_d,
 485                                                   double *speed_a)
 486 {
 487         /* speed_d = coef * speed_a */
 488         double coef;
 489         double speed_d2, speed_a2;
 490
 491         coef = 2. * radius_mm / traj->position->phys.track_mm;
 492
 493         speed_d2 = traj->a_speed * coef;
 494         if (speed_d2 < traj->d_speed) {
 495                 *speed_d = speed_d2;
 496                 *speed_a = traj->a_speed;
 497         }
 498         else {
 499                 speed_a2 = traj->d_speed / coef;
 500                 *speed_d = traj->d_speed;
 501                 *speed_a = speed_a2;
 502         }
 503 }
 504
 505 /* trajectory event for circles */
 506 /* XXX static */
 507 void trajectory_manager_circle_event(struct trajectory *traj)
 508 {
 509         double radius;
 510         double x = position_get_x_double(traj->position);
 511         double y = position_get_y_double(traj->position);
 512         double a = position_get_a_rad_double(traj->position);
 513         int32_t d_consign = 0, a_consign = 0;
 514         double angle_to_center_rad;
 515         double coef_p, coef_d;
 516         double d_speed, a_speed;
 517
 518         /* These vectors contain target position of the robot in
 519          * its own coordinates */
 520         vect2_cart v2cart_pos;
 521         vect2_pol v2pol_target;
 522
 523         /* step 1 : process new commands to quadramps */
 524
 525         /* process the command vector from current position to the
 526          * center of the circle. */
 527         v2cart_pos.x = traj->target.circle.center.x - x;
 528         v2cart_pos.y = traj->target.circle.center.y - y;
 529         vect2_cart2pol(&v2cart_pos, &v2pol_target);
 530         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 531
 532         /* radius consign */
 533         radius = traj->target.circle.radius;
 534
 535         coef_p = v2pol_target.r / radius;
 536         coef_p = 1. * coef_p;
 537
 538         angle_to_center_rad = v2pol_target.theta - (M_PI / 2.);
 539         angle_to_center_rad = simple_modulo_2pi(angle_to_center_rad);
 540         if (angle_to_center_rad > 0.5)
 541                 angle_to_center_rad = 0.5;
 542         if (angle_to_center_rad < -0.5)
 543                 angle_to_center_rad = -0.5;
 544         coef_d = exp(5*angle_to_center_rad);
 545         coef_d = coef_d;
 546
 547         circle_get_da_speed_from_radius(traj, radius / (coef_p * coef_d),
 548                                         &d_speed, &a_speed);
 549         set_quadramp_speed(traj, d_speed, a_speed);
 550
 551         EVT_DEBUG(E_TRAJECTORY, "angle=%2.2f radius=%2.2f r=%2.2f coef_p=%2.2f coef_d=%2.2f "
 552               "d_speed=%2.2f a_speed=%2.2f",
 553                   angle_to_center_rad, radius, v2pol_target.r,
 554               coef_p, coef_d, d_speed, a_speed);
 555
 556         /* XXX check flags */
 557         d_consign = 400000 + rs_get_distance(traj->robot);
 558         a_consign = 400000 + rs_get_angle(traj->robot);
 559
 560         /* angle consign */
 561 /*      a_consign = (int32_t)(v2pol_target.theta * */
 562 /*                            (traj->position->phys.distance_imp_per_mm) * */
 563 /*                            (traj->position->phys.track_mm) / 2.0); */
 564 /*      a_consign += rs_get_angle(traj->robot); */
 565
 566         /* step 2 : update state, or delete event if we reached the
 567          * destination */
 568
 569 /*      /\* output angle -> delete event *\/ */
 570 /*      if (a_consign >= traj->target.circle.dest_angle) { */
 571 /*              a_consign = traj->target.circle.dest_angle; */
 572 /*              delete_event(traj); */
 573 /*      } */
 574
 575         /* step 3 : send the processed commands to cs */
 576
 577 /*      EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32 */
 578 /*                ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32 */
 579 /*                ", a_speed=%" PRIi32 ", radius = %f", */
 580 /*                rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj), */
 581 /*                rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj), */
 582 /*                radius); */
 583
 584         cs_set_consign(traj->csm_angle, a_consign);
 585         cs_set_consign(traj->csm_distance, d_consign);
 586 }
 587
 588 /* trajectory event for lines */
 589 static void trajectory_manager_line_event(struct trajectory *traj)
 590 {
 591         double x = position_get_x_double(traj->position);
 592         double y = position_get_y_double(traj->position);
 593         double a = position_get_a_rad_double(traj->position);
 594         double advance, dist_to_line;
 595         point_t robot, proj, target_pt;
 596         int32_t d_consign = 0, a_consign = 0;
 597         vect2_cart v2cart_pos;
 598         vect2_pol v2pol_target;
 599
 600         robot.x = x;
 601         robot.y = y;
 602
 603         /* target point on the line is further on the line */
 604         proj_pt_line(&robot, &traj->target.line.line, &proj);
 605         dist_to_line = pt_norm(&robot, &proj);
 606         if (dist_to_line > traj->target.line.advance)
 607                 advance = 0;
 608         else
 609                 advance = traj->target.line.advance - dist_to_line;
 610         target_pt.x = proj.x + advance * cos(traj->target.line.angle);
 611         target_pt.y = proj.y + advance * sin(traj->target.line.angle);
 612
 613         /* target vector */
 614         v2cart_pos.x = target_pt.x - x;
 615         v2cart_pos.y = target_pt.y - y;
 616         vect2_cart2pol(&v2cart_pos, &v2pol_target);
 617         v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
 618
 619         /* If the robot is correctly oriented to start moving in distance */
 620         /* here limit dist speed depending on v2pol_target.theta */
 621         if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
 622                 set_quadramp_speed(traj, 0, traj->a_speed);
 623         else {
 624                 double coef;
 625                 coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
 626                 set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
 627         }
 628
 629         /* position consign is infinite */
 630         d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
 631         d_consign += rs_get_distance(traj->robot);
 632
 633         /* angle consign */
 634         a_consign = (int32_t)(v2pol_target.theta *
 635                               (traj->position->phys.distance_imp_per_mm) *
 636                               (traj->position->phys.track_mm) / 2.2);
 637         a_consign += rs_get_angle(traj->robot);
 638
 639         EVT_DEBUG(E_TRAJECTORY, "target.x=%2.2f target.y=%2.2f "
 640                   "a_consign=%"PRIi32" d_consign=%"PRIi32,
 641                   target_pt.x, target_pt.y, a_consign, d_consign);
 642
 643         cs_set_consign(traj->csm_angle, a_consign);
 644         cs_set_consign(traj->csm_distance, d_consign);
 645 }
 646
 647
 648 /* trajectory event */
 649 void trajectory_manager_event(void * param)
 650 {
 651         struct trajectory *traj = (struct trajectory *)param;
 652
 653         switch (traj->state) {
 654         case RUNNING_XY_START:
 655         case RUNNING_XY_ANGLE:
 656         case RUNNING_XY_ANGLE_OK:
 657         case RUNNING_XY_F_START:
 658         case RUNNING_XY_F_ANGLE:
 659         case RUNNING_XY_F_ANGLE_OK:
 660         case RUNNING_XY_B_START:
 661         case RUNNING_XY_B_ANGLE:
 662         case RUNNING_XY_B_ANGLE_OK:
 663                 trajectory_manager_xy_event(traj);
 664                 break;
 665
 666         case RUNNING_CIRCLE:
 667                 trajectory_manager_circle_event(traj);
 668                 break;
 669
 670         case RUNNING_LINE:
 671                 trajectory_manager_line_event(traj);
 672                 break;
 673
 674         default:
 675                 break;
 676         }
 677 }
 678
 679 /*********** *CIRCLE */
 680
 681 /* make the robot orbiting around (x,y) on a circle whose radius is
 682  * radius_mm, and exit when relative destination angle is reached. The
 683  * flags set if we go forward or backwards, and CW/CCW. */
 684 void trajectory_circle_rel(struct trajectory *traj,
 685                            double x, double y,
 686                            double radius_mm,
 687                            double rel_a_deg,
 688                            uint8_t flags)
 689 {
 690         double dst_angle;
 691
 692         delete_event(traj);
 693
 694         traj->target.circle.center.x = x;
 695         traj->target.circle.center.y = y;
 696         traj->target.circle.radius = radius_mm;
 697         traj->target.circle.flags = flags;
 698
 699         /* convert in steps  */
 700         dst_angle = RAD(rel_a_deg) *
 701                 (traj->position->phys.distance_imp_per_mm) *
 702                 (traj->position->phys.track_mm) / 2.0;
 703
 704         traj->target.circle.dest_angle = rs_get_angle(traj->robot);
 705         traj->target.circle.dest_angle += dst_angle;
 706
 707         DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
 708               x, y, radius_mm, flags, traj->target.circle.dest_angle);
 709
 710         traj->state = RUNNING_CIRCLE;
 711         trajectory_manager_event(traj);
 712         schedule_event(traj);
 713 }
 714
 715 /* return the distance in millimeters that corresponds to an angle in
 716  * degree and a radius in mm */
 717 /* static  */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
 718 {
 719         double a_rad = RAD(a_deg);
 720         return a_rad * radius_mm;
 721 }
 722
 723 /*
 724  * Start a circle of specified radius around the specified center
 725  * (relative with d,a). The distance is specified in mm.
 726  */
 727 void trajectory_circle(struct trajectory *traj,
 728                        double center_d_mm, double center_a_rad,
 729                        double radius_mm, double dist_mm)
 730 {
 731 /*      double */
 732
 733 /*      DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
 734 /*            center_a_rad); */
 735 /*      delete_event(traj); */
 736 /*      traj->state = RUNNING_CIRCLE; */
 737
 738
 739 }
 740
 741 /*
 742  * Start a circle of specified radius around the specified center
 743  * (absolute). The distance is specified in mm.
 744  */
 745 void trajectory_circle_abs_dist_mm(struct trajectory *traj,
 746                                    double x_rel_mm, double y_rel_mm,
 747                                    double radius_mm, double dist_mm)
 748 {
 749 }
 750
 751 /*
 752  * Start a circle of specified radius around the specified center
 753  * (absolute). The distance is specified in degrees.
 754  */
 755 void trajectory_circle_abs_dist_deg(struct trajectory *traj,
 756                                     double x_rel_mm, double y_rel_mm,
 757                                     double radius_mm, double dist_degrees)
 758 {
 759
 760 }
 761
 762 /*********** *LINE */
 763
 764 /* Follow a line */
 765 void trajectory_line_abs(struct trajectory *traj,
 766                          double x1, double y1,
 767                          double x2, double y2,
 768                          double advance)
 769 {
 770         point_t p1, p2;
 771
 772         delete_event(traj);
 773
 774         /* find the line EQ */
 775         p1.x = x1;
 776         p1.y = y1;
 777         p2.x = x2;
 778         p2.y = y2;
 779         pts2line(&p1, &p2, &traj->target.line.line);
 780
 781         /* find the line angle */
 782         traj->target.line.angle = atan2(y2-y1, x2-x1);
 783         traj->target.line.advance = advance;
 784
 785         DEBUG(E_TRAJECTORY, "Line rel (a,b,c)=%2.2f,%2.2f,%2.2f",
 786               traj->target.line.line.a,
 787               traj->target.line.line.b,
 788               traj->target.line.line.c,
 789               traj->target.line.angle);
 790
 791         traj->state = RUNNING_LINE;
 792         trajectory_manager_event(traj);
 793         schedule_event(traj);
 794 }
 795