AVR: aversive_10-03-12/modules/devices/robot/trajectory_manager/trajectory

00001 /*  
00002  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
00003  * 
00004  *  This program is free software; you can redistribute it and/or modify
00005  *  it under the terms of the GNU General Public License as published by
00006  *  the Free Software Foundation; either version 2 of the License, or
00007  *  (at your option) any later version.
00008  *
00009  *  This program is distributed in the hope that it will be useful,
00010  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *  GNU General Public License for more details.
00013  *
00014  *  You should have received a copy of the GNU General Public License
00015  *  along with this program; if not, write to the Free Software
00016  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017  *
00018  *  Revision : $Id: trajectory_manager.c,v 1.10 2009-03-15 21:51:18 zer0 Exp $
00019  *
00020  */
00021 
00022 /* Trajectory Manager v2 - zer0 - for Eurobot 2008 */
00023 
00024 #include <string.h>
00025 #include <stdlib.h>
00026 #include <math.h>
00027 
00028 #include <aversive.h>
00029 #include <aversive/error.h>
00030 #include <scheduler.h>
00031 #include <vect2.h>
00032 
00033 #include <position_manager.h>
00034 #include <robot_system.h>
00035 #include <control_system_manager.h>
00036 #include <quadramp.h>
00037 
00038 #include <trajectory_manager.h>
00039 
00040 #define M_2PI (2*M_PI)
00041 
00042 #define DEG(x) ((x) * (180.0 / M_PI))
00043 #define RAD(x) ((x) * (M_PI / 180.0))
00044 
00045 static void trajectory_manager_event(void *param);
00046 
00047 /************ INIT FUNCS */
00048 
00050 void trajectory_init(struct trajectory *traj)
00051 {
00052         uint8_t flags;
00053 
00054         IRQ_LOCK(flags);
00055         memset(traj, 0, sizeof(struct trajectory));
00056         traj->state = READY;
00057         traj->scheduler_task = -1;
00058         IRQ_UNLOCK(flags);
00059 }
00060 
00062 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d, 
00063                        struct cs *cs_a)
00064 {
00065         uint8_t flags;
00066 
00067         IRQ_LOCK(flags);
00068         traj->csm_distance = cs_d;
00069         traj->csm_angle = cs_a;
00070         IRQ_UNLOCK(flags);
00071 }
00072 
00074 void trajectory_set_robot_params(struct trajectory *traj, 
00075                                  struct robot_system *rs, 
00076                                  struct robot_position *pos) 
00077 {
00078         uint8_t flags;
00079         IRQ_LOCK(flags);
00080         traj->robot = rs;
00081         traj->position = pos;
00082         IRQ_UNLOCK(flags);
00083 }
00084 
00086 void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
00087 {
00088         uint8_t flags;
00089         IRQ_LOCK(flags);
00090         traj->d_speed = d_speed;
00091         traj->a_speed = a_speed;
00092         IRQ_UNLOCK(flags);
00093 }
00094 
00096 void trajectory_set_windows(struct trajectory *traj, double d_win,
00097                             double a_win_deg, double a_start_deg)
00098 {
00099         uint8_t flags;
00100         IRQ_LOCK(flags);
00101         traj->d_win = d_win ;
00102         traj->a_win_rad = RAD(a_win_deg);
00103         traj->a_start_rad = RAD(a_start_deg);
00104         IRQ_UNLOCK(flags);
00105 }
00106 
00107 /************ STATIC [ AND USEFUL ] FUNCS */
00108 
00110 static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
00111 {
00112         struct quadramp_filter * q_d, * q_a;
00113         q_d = traj->csm_distance->consign_filter_params;
00114         q_a = traj->csm_angle->consign_filter_params;
00115         quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
00116         quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
00117 }
00118 
00120 static uint32_t get_quadramp_angle_speed(struct trajectory *traj)
00121 {
00122         struct quadramp_filter *q_a;
00123         q_a = traj->csm_angle->consign_filter_params;
00124         return q_a->var_1st_ord_pos;
00125 }
00126 
00128 static uint32_t get_quadramp_distance_speed(struct trajectory *traj)
00129 {
00130         struct quadramp_filter *q_d;
00131         q_d = traj->csm_distance->consign_filter_params;
00132         return q_d->var_1st_ord_pos;
00133 }
00134 
00136 static void delete_event(struct trajectory *traj)
00137 {
00138         set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
00139         if ( traj->scheduler_task != -1) {
00140                 DEBUG(E_TRAJECTORY, "Delete event");
00141                 scheduler_del_event(traj->scheduler_task);
00142                 traj->scheduler_task = -1;
00143         }
00144 }
00145 
00147 static void schedule_event(struct trajectory *traj)
00148 {
00149         if ( traj->scheduler_task != -1) {
00150                 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
00151         }
00152         else {
00153                 traj->scheduler_task = 
00154                         scheduler_add_periodical_event_priority(&trajectory_manager_event,
00155                                                                 (void*)traj,
00156                                                                 100000L/SCHEDULER_UNIT, 30);
00157         }
00158 }
00159 
00161 static double simple_modulo_2pi(double a)
00162 {
00163         if(a < -M_PI) {
00164                 a += M_2PI;
00165         }
00166         else if(a > M_PI) {
00167                 a -= M_2PI;
00168         }
00169         return a;
00170 }
00171 
00173 static double modulo_2pi(double a)
00174 {
00175         double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
00176         return simple_modulo_2pi(res);
00177 }
00178 
00180 static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
00181 {
00182         double d = ABS(traj->target.pol.distance-rs_get_distance(traj->robot));
00183         d = d / traj->position->phys.distance_imp_per_cm;
00184         return (d < d_win);
00185 }
00186 
00188 static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
00189 {
00190         double x1 = traj->target.cart.x;
00191         double y1 = traj->target.cart.y;
00192         double x2 = position_get_x_double(traj->position);
00193         double y2 = position_get_y_double(traj->position);
00194         return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
00195 }
00196 
00198 static inline uint8_t 
00199 __is_robot_in_angle_window(struct trajectory *traj, double target_angle, double a_win_rad)
00200 {
00201         return ( ABS(target_angle*2) < a_win_rad );
00202 }
00203 
00207 static uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
00208 {
00209         return __is_robot_in_angle_window(traj, traj->target.pol.angle -
00210                                           position_get_a_rad_double(traj->position),
00211                                           a_win_rad);
00212 }
00213 
00214 
00215 /************ SIMPLE TRAJS, NO EVENT */
00216 
00217 #define UPDATE_D 1
00218 #define UPDATE_A 2
00219 #define RESET_D  4
00220 #define RESET_A  8
00221 
00230 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_cm, double a_rad, uint8_t flags)
00231 {
00232         int32_t a_consign, d_consign;
00233         /* XXX only used for debug */
00234         double posx = position_get_x_double(traj->position); 
00235         double posy = position_get_y_double(traj->position);
00236         double posa = position_get_a_rad_double(traj->position);
00237 
00238         DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_cm, a_rad);
00239         DEBUG(E_TRAJECTORY, "current pos = x=%f, y=%f, a_rad=%f", posx, posy, posa);
00240         delete_event(traj);
00241         if (flags & UPDATE_A) {
00242                 if (flags & RESET_A) {
00243                         a_consign = 0;
00244                 }
00245                 else {
00246                         a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_cm) *
00247                                               (traj->position->phys.track_cm) / 2); 
00248                         traj->target.pol.angle = a_consign;
00249                 }
00250                 cs_set_consign(traj->csm_angle, a_consign + rs_get_angle(traj->robot));
00251         }
00252         if (flags & UPDATE_D) {
00253                 if (flags & RESET_D) {
00254                         d_consign = 0;
00255                 }
00256                 else {
00257                         d_consign = (int32_t)((d_cm) * (traj->position->phys.distance_imp_per_cm));
00258                         traj->target.pol.distance = d_consign;
00259                 }
00260                 cs_set_consign(traj->csm_distance, d_consign + rs_get_distance(traj->robot));
00261         }
00262 }
00263 
00265 void trajectory_d_rel(struct trajectory *traj, double d_cm)
00266 {
00267         traj->state = RUNNING_D;
00268         __trajectory_goto_d_a_rel(traj, d_cm, 0, UPDATE_D | UPDATE_A | RESET_A);
00269 }
00270 
00272 void trajectory_only_d_rel(struct trajectory *traj, double d_cm)
00273 {
00274         traj->state = RUNNING_D;
00275         __trajectory_goto_d_a_rel(traj, d_cm, 0, UPDATE_D);
00276 }
00277 
00279 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
00280 {
00281         traj->state = RUNNING_A;
00282         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), UPDATE_A | UPDATE_D | RESET_D);
00283 }
00284 
00286 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
00287 {
00288         double posa = position_get_a_rad_double(traj->position);
00289         double a;
00290 
00291         traj->state = RUNNING_A;
00292         a = RAD(a_deg_abs) - posa;
00293         a = modulo_2pi(a);
00294         __trajectory_goto_d_a_rel(traj, 0, a, UPDATE_A | UPDATE_D | RESET_D);
00295 }
00296 
00298 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_cm, double y_abs_cm)
00299 {
00300         double posx = position_get_x_double(traj->position); 
00301         double posy = position_get_y_double(traj->position);
00302         double posa = position_get_a_rad_double(traj->position);
00303 
00304         traj->state = RUNNING_A;
00305         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_cm, y_abs_cm);
00306         __trajectory_goto_d_a_rel(traj, 0,
00307                         simple_modulo_2pi(atan2(y_abs_cm - posy, x_abs_cm - posx) - posa),
00308                                   UPDATE_A | UPDATE_D | RESET_D);
00309 }
00310 
00312 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_cm, double y_abs_cm)
00313 {
00314         double posx = position_get_x_double(traj->position); 
00315         double posy = position_get_y_double(traj->position);
00316         double posa = position_get_a_rad_double(traj->position);
00317 
00318         traj->state = RUNNING_A;
00319         DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_cm, y_abs_cm);
00320         __trajectory_goto_d_a_rel(traj, 0, 
00321                         modulo_2pi(atan2(y_abs_cm - posy, x_abs_cm - posx) - posa + M_PI),
00322                                   UPDATE_A | UPDATE_D | RESET_D);
00323 }
00324 
00326 void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
00327 {
00328         traj->state = RUNNING_A;
00329         __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), UPDATE_A);
00330 }
00331 
00333 void trajectory_d_a_rel(struct trajectory *traj, double d_cm, double a_deg)
00334 {
00335         traj->state = RUNNING_AD;
00336         __trajectory_goto_d_a_rel(traj, d_cm, RAD(a_deg), UPDATE_A | UPDATE_D);
00337 }
00338 
00340 void trajectory_stop(struct trajectory *traj)
00341 {
00342         traj->state = READY;
00343         __trajectory_goto_d_a_rel(traj, 0, 0, UPDATE_A | UPDATE_D | RESET_D | RESET_A);
00344 }
00345 
00348 void trajectory_hardstop(struct trajectory *traj)
00349 {
00350         struct quadramp_filter *q_d, *q_a;
00351 
00352         traj->state = READY;
00353         q_d = traj->csm_distance->consign_filter_params;
00354         q_a = traj->csm_angle->consign_filter_params;
00355         __trajectory_goto_d_a_rel(traj, 0, 0, UPDATE_A | UPDATE_D | RESET_D | RESET_A);
00356 
00357         q_d->previous_var = 0;
00358         q_d->previous_out = rs_get_distance(traj->robot);
00359         q_a->previous_var = 0;
00360         q_a->previous_out = rs_get_angle(traj->robot);
00361 }
00362 
00363 
00364 /************ GOTO XY, USE EVENTS */
00365 
00367 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
00368 {
00369         DEBUG(E_TRAJECTORY, "Goto XY");
00370         delete_event(traj);
00371         traj->target.cart.x = x;
00372         traj->target.cart.y = y;
00373         traj->state = RUNNING_XY_START;
00374         trajectory_manager_event(traj);
00375         schedule_event(traj);
00376 }
00377 
00379 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
00380 {
00381         DEBUG(E_TRAJECTORY, "Goto XY_F");
00382         delete_event(traj);
00383         traj->target.cart.x = x;
00384         traj->target.cart.y = y;
00385         traj->state = RUNNING_XY_F_START;
00386         trajectory_manager_event(traj);
00387         schedule_event(traj);
00388 }
00389 
00391 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
00392 {
00393         vect2_pol p;
00394         double x = position_get_x_double(traj->position); 
00395         double y = position_get_y_double(traj->position);
00396         
00397         DEBUG(E_TRAJECTORY, "Goto DA rel");
00398 
00399         delete_event(traj);
00400         p.r = d;
00401         p.theta = RAD(a) + position_get_a_rad_double(traj->position);
00402         vect2_pol2cart(&p, &traj->target.cart);
00403         traj->target.cart.x += x;
00404         traj->target.cart.y += y;
00405         
00406         traj->state = RUNNING_XY_START;
00407         trajectory_manager_event(traj);
00408         schedule_event(traj);
00409 }
00410 
00412 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_cm, double y_rel_cm)
00413 {
00414         vect2_cart c;
00415         vect2_pol p;
00416         double x = position_get_x_double(traj->position); 
00417         double y = position_get_y_double(traj->position);
00418 
00419         DEBUG(E_TRAJECTORY, "Goto XY rel");
00420 
00421         delete_event(traj);
00422         c.x = x_rel_cm;
00423         c.y = y_rel_cm;
00424 
00425         vect2_cart2pol(&c, &p);
00426         p.theta += position_get_a_rad_double(traj->position);;
00427         vect2_pol2cart(&p, &traj->target.cart);
00428 
00429         traj->target.cart.x += x;
00430         traj->target.cart.y += y;
00431         
00432         traj->state = RUNNING_XY_START;
00433         trajectory_manager_event(traj);
00434         schedule_event(traj);
00435 }
00436 
00437 /************ FUNCS FOR GETTING TRAJ STATE */
00438 
00442 uint8_t trajectory_finished(struct trajectory *traj)
00443 {
00444         return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
00445                 cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
00446 }
00447 
00449 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
00450 {
00451         switch(traj->state) {
00452 
00453         case RUNNING_XY_ANGLE_OK: 
00454         case RUNNING_XY_F_ANGLE_OK: 
00455                 /* if robot coordinates are near the x,y target */
00456                 return is_robot_in_xy_window(traj, d_win);
00457 
00458         case RUNNING_A: 
00459                 return is_robot_in_angle_window(traj, a_win_rad);
00460 
00461         case RUNNING_D:
00462                 return is_robot_in_dist_window(traj, d_win);
00463 
00464         case RUNNING_AD:
00465                 return is_robot_in_dist_window(traj, d_win) && 
00466                         is_robot_in_angle_window(traj, a_win_rad);
00467 
00468         case RUNNING_XY_START: 
00469         case RUNNING_XY_F_START:
00470         case RUNNING_XY_ANGLE: 
00471         case RUNNING_XY_F_ANGLE:
00472         default:
00473                 return 0;
00474         }
00475 }
00476 
00477 /*********** *TRAJECTORY EVENT FUNC */
00478 
00480 static void trajectory_manager_event(void * param)
00481 {
00482         struct trajectory *traj = (struct trajectory *)param;
00483         double coef=1.0;
00484         double x = position_get_x_double(traj->position); 
00485         double y = position_get_y_double(traj->position);
00486         double a = position_get_a_rad_double(traj->position);
00487         int32_t d_consign=0, a_consign=0;
00488         
00489         /* These vectors contain target position of the robot in
00490          * its own coordinates */
00491         vect2_cart v2cart_pos;
00492         vect2_pol v2pol_target;
00493 
00494         /* step 1 : process new commands to quadramps */
00495 
00496         switch (traj->state) {
00497         case RUNNING_XY_START:
00498         case RUNNING_XY_ANGLE:
00499         case RUNNING_XY_ANGLE_OK:
00500         case RUNNING_XY_F_START:
00501         case RUNNING_XY_F_ANGLE:
00502         case RUNNING_XY_F_ANGLE_OK:
00503 
00504                 /* process the command vector from absolute target and
00505                  * current position */
00506                 v2cart_pos.x = traj->target.cart.x - x;
00507                 v2cart_pos.y = traj->target.cart.y - y;
00508                 vect2_cart2pol(&v2cart_pos, &v2pol_target);
00509                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
00510 
00511                 /* if we don't need to go forward */
00512                 if( traj->state >= RUNNING_XY_START &&
00513                     traj->state <= RUNNING_XY_ANGLE_OK ) {
00514                         /* If the target is behind the robot, we need to go
00515                          * backwards. 0.52 instead of 0.5 because we prefer to
00516                          * go forward */
00517                         if( (v2pol_target.theta > 0.52*M_PI) ||
00518                             (v2pol_target.theta < -0.52*M_PI ) ) {
00519                                 v2pol_target.r = -v2pol_target.r;
00520                                 v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
00521                         }
00522                 }
00523                 
00524                 /* If the robot is correctly oriented to start moving in distance */
00525                 /* here limit dist speed depending on v2pol_target.theta */
00526                 if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
00527                         set_quadramp_speed(traj, 0, traj->a_speed);
00528                 else {
00529                         coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
00530                         set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
00531                 }
00532                 
00533                 d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_cm));
00534                 d_consign += rs_get_distance(traj->robot);
00535                 
00536                 /* angle consign */
00537                 /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
00538                 a_consign = (int32_t)(v2pol_target.theta *
00539                                       (traj->position->phys.distance_imp_per_cm) *
00540                                       (traj->position->phys.track_cm) / 2.2); 
00541                 a_consign += rs_get_angle(traj->robot);
00542                 
00543                 break;
00544 
00545         default:
00546                 /* hmmm quite odd, delete the event */
00547                 DEBUG(E_TRAJECTORY, "GNI ???");
00548                 delete_event(traj);
00549                 traj->state = READY;
00550         }
00551 
00552 
00553         /* step 2 : update state, or delete event if we reached the
00554          * destination */
00555 
00556         /* XXX if target is our pos !! */
00557 
00558         switch (traj->state) {
00559         case RUNNING_XY_START:
00560         case RUNNING_XY_F_START:
00561                 /* START -> ANGLE */
00562                 DEBUG(E_TRAJECTORY, "-> ANGLE");
00563                 traj->state ++;
00564                 break;
00565 
00566         case RUNNING_XY_ANGLE:
00567         case RUNNING_XY_F_ANGLE: {
00568                 struct quadramp_filter *q_a;
00569                 q_a = traj->csm_angle->consign_filter_params;
00570                 /* if d_speed is not 0, we are in start_angle_win */
00571                 if (get_quadramp_distance_speed(traj)) {
00572                         if(is_robot_in_xy_window(traj, traj->d_win)) {
00573                                 delete_event(traj);
00574                         }
00575                         /* ANGLE -> ANGLE_OK */
00576                         traj->state ++;
00577                         DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
00578                 }
00579                 break;
00580         }
00581 
00582         case RUNNING_XY_ANGLE_OK:
00583         case RUNNING_XY_F_ANGLE_OK:
00584                 /* If we reached the destination */
00585                 if(is_robot_in_xy_window(traj, traj->d_win)) {
00586                         delete_event(traj);
00587                 }
00588         break;
00589 
00590         default:
00591                 break;
00592         }
00593 
00594         /* step 3 : send the processed commands to cs */
00595 
00596         DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
00597               x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
00598         
00599         DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
00600               "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
00601               rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
00602               rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
00603                 
00604         cs_set_consign(traj->csm_angle, a_consign);
00605         cs_set_consign(traj->csm_distance, d_consign);
00606 }
aversive_10-03-12/modules/devices/robot/trajectory_manager/trajectory_manager.c
