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