modules/devices/robot/trajectory_manager/trajectory_manager_utils.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
  25 #include <string.h>
  26 #include <stdlib.h>
  27 #include <math.h>
  28
  29 #include <aversive.h>
  30 #include <aversive/error.h>
  31 #include <scheduler.h>
  32 #include <vect2.h>
  33
  34 #include <position_manager.h>
  35 #include <robot_system.h>
  36 #include <control_system_manager.h>
  37 #include <quadramp.h>
  38
  39 #include <trajectory_manager.h>
  40 #include "trajectory_manager_utils.h"
  41 #include "trajectory_manager_core.h"
  42
  43 /** set speed consign in quadramp filter */
  44 void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed)
  45 {
  46         struct quadramp_filter * q_d, * q_a;
  47         q_d = traj->csm_distance->consign_filter_params;
  48         q_a = traj->csm_angle->consign_filter_params;
  49         quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
  50         quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
  51 }
  52
  53 /** get angle speed consign in quadramp filter */
  54 double get_quadramp_angle_speed(struct trajectory *traj)
  55 {
  56         struct quadramp_filter *q_a;
  57         q_a = traj->csm_angle->consign_filter_params;
  58         return q_a->var_1st_ord_pos;
  59 }
  60
  61 /** get distance speed consign in quadramp filter */
  62 double get_quadramp_distance_speed(struct trajectory *traj)
  63 {
  64         struct quadramp_filter *q_d;
  65         q_d = traj->csm_distance->consign_filter_params;
  66         return q_d->var_1st_ord_pos;
  67 }
  68
  69 /** set speed consign in quadramp filter */
  70 void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc)
  71 {
  72         struct quadramp_filter * q_d, * q_a;
  73         q_d = traj->csm_distance->consign_filter_params;
  74         q_a = traj->csm_angle->consign_filter_params;
  75         quadramp_set_2nd_order_vars(q_d, ABS(d_acc), ABS(d_acc));
  76         quadramp_set_2nd_order_vars(q_a, ABS(a_acc), ABS(a_acc));
  77 }
  78
  79 /** remove event if any */
  80 void delete_event(struct trajectory *traj)
  81 {
  82         set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
  83         set_quadramp_acc(traj, traj->d_acc, traj->a_acc);
  84         if ( traj->scheduler_task != -1) {
  85                 DEBUG(E_TRAJECTORY, "Delete event");
  86                 scheduler_del_event(traj->scheduler_task);
  87                 traj->scheduler_task = -1;
  88         }
  89 }
  90
  91 /** schedule the trajectory event */
  92 void schedule_event(struct trajectory *traj)
  93 {
  94         if ( traj->scheduler_task != -1) {
  95                 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
  96         }
  97         else {
  98                 traj->scheduler_task =
  99                         scheduler_add_periodical_event_priority(&trajectory_manager_event,
 100                                                                 (void*)traj,
 101                                                                 TRAJ_EVT_PERIOD, 30);
 102         }
 103 }
 104
 105 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
 106 double simple_modulo_2pi(double a)
 107 {
 108         if (a < -M_PI) {
 109                 a += M_2PI;
 110         }
 111         else if (a > M_PI) {
 112                 a -= M_2PI;
 113         }
 114         return a;
 115 }
 116
 117 /** do a modulo 2.pi -> [-Pi,+Pi] */
 118 double modulo_2pi(double a)
 119 {
 120         double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
 121         return simple_modulo_2pi(res);
 122 }
 123
 124 /** near the target (dist) ? */
 125 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
 126 {
 127         double d = traj->target.pol.distance - rs_get_distance(traj->robot);
 128         d = ABS(d);
 129         d = d / traj->position->phys.distance_imp_per_mm;
 130         return (d < d_win);
 131 }
 132
 133 /** near the target (dist in x,y) ? */
 134 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
 135 {
 136         double x1 = traj->target.cart.x;
 137         double y1 = traj->target.cart.y;
 138         double x2 = position_get_x_double(traj->position);
 139         double y2 = position_get_y_double(traj->position);
 140         return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
 141 }
 142
 143 /** near the angle target in radian ? Only valid if
 144  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
 145  *  command) */
 146 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
 147 {
 148         double a;
 149
 150         /* convert relative angle from imp to rad */
 151         a = traj->target.pol.angle - rs_get_angle(traj->robot);
 152         a /= traj->position->phys.distance_imp_per_mm;
 153         a /= traj->position->phys.track_mm;
 154         a *= 2.;
 155         return ABS(a) < a_win_rad;
 156 }
 157
 158 /* distance unit conversions */
 159
 160 double pos_mm2imp(struct trajectory *traj, double pos)
 161 {
 162         return pos * traj->position->phys.distance_imp_per_mm;
 163 }
 164
 165 double pos_imp2mm(struct trajectory *traj, double pos)
 166 {
 167         return pos / traj->position->phys.distance_imp_per_mm;
 168 }
 169
 170 double speed_mm2imp(struct trajectory *traj, double speed)
 171 {
 172         return speed *
 173                 traj->position->phys.distance_imp_per_mm /
 174                 traj->cs_hz;
 175 }
 176
 177 double speed_imp2mm(struct trajectory *traj, double speed)
 178 {
 179         return speed * traj->cs_hz /
 180                 traj->position->phys.distance_imp_per_mm;
 181 }
 182
 183 double acc_mm2imp(struct trajectory *traj, double acc)
 184 {
 185         return acc * traj->position->phys.distance_imp_per_mm /
 186                 (traj->cs_hz * traj->cs_hz);
 187 }
 188
 189 double acc_imp2mm(struct trajectory *traj, double acc)
 190 {
 191         return acc * traj->cs_hz * traj->cs_hz /
 192                 traj->position->phys.distance_imp_per_mm;
 193 }
 194
 195 /* angle unit conversions */
 196
 197 double pos_rd2imp(struct trajectory *traj, double pos)
 198 {
 199         return pos *
 200                 (traj->position->phys.distance_imp_per_mm *
 201                  traj->position->phys.track_mm / 2.);
 202 }
 203
 204 double pos_imp2rd(struct trajectory *traj, double pos)
 205 {
 206         return pos /
 207                 (traj->position->phys.distance_imp_per_mm *
 208                  traj->position->phys.track_mm / 2.);
 209 }
 210
 211 double speed_rd2imp(struct trajectory *traj, double speed)
 212 {
 213         return speed *
 214                 (traj->position->phys.distance_imp_per_mm *
 215                  traj->position->phys.track_mm /
 216                  (2. * traj->cs_hz));
 217 }
 218
 219 double speed_imp2rd(struct trajectory *traj, double speed)
 220 {
 221         return speed /
 222                 (traj->position->phys.distance_imp_per_mm *
 223                  traj->position->phys.track_mm /
 224                  (2. * traj->cs_hz));
 225 }
 226
 227 double acc_rd2imp(struct trajectory *traj, double acc)
 228 {
 229         return acc *
 230                 (traj->position->phys.distance_imp_per_mm *
 231                  traj->position->phys.track_mm /
 232                  (2. * traj->cs_hz * traj->cs_hz));
 233 }
 234
 235 double acc_imp2rd(struct trajectory *traj, double acc)
 236 {
 237         return acc /
 238                 (traj->position->phys.distance_imp_per_mm *
 239                  traj->position->phys.track_mm /
 240                  (2. * traj->cs_hz * traj->cs_hz));
 241 }
 242