modules/devices/robot/trajectory_manager/trajectory_manager_core.c

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