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, int16_t d_speed, int16_t 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 uint32_t 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 uint32_t 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 /** remove event if any */
  70 void delete_event(struct trajectory *traj)
  71 {
  72         set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
  73         if ( traj->scheduler_task != -1) {
  74                 DEBUG(E_TRAJECTORY, "Delete event");
  75                 scheduler_del_event(traj->scheduler_task);
  76                 traj->scheduler_task = -1;
  77         }
  78 }
  79
  80 /** schedule the trajectory event */
  81 void schedule_event(struct trajectory *traj)
  82 {
  83         if ( traj->scheduler_task != -1) {
  84                 DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
  85         }
  86         else {
  87                 traj->scheduler_task =
  88                         scheduler_add_periodical_event_priority(&trajectory_manager_event,
  89                                                                 (void*)traj,
  90                                                                 TRAJ_EVT_PERIOD, 30);
  91         }
  92 }
  93
  94 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
  95 double simple_modulo_2pi(double a)
  96 {
  97         if (a < -M_PI) {
  98                 a += M_2PI;
  99         }
 100         else if (a > M_PI) {
 101                 a -= M_2PI;
 102         }
 103         return a;
 104 }
 105
 106 /** do a modulo 2.pi -> [-Pi,+Pi] */
 107 double modulo_2pi(double a)
 108 {
 109         double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
 110         return simple_modulo_2pi(res);
 111 }
 112
 113 /** near the target (dist) ? */
 114 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
 115 {
 116         double d = traj->target.pol.distance - rs_get_distance(traj->robot);
 117         d = ABS(d);
 118         d = d / traj->position->phys.distance_imp_per_mm;
 119         return (d < d_win);
 120 }
 121
 122 /** near the target (dist in x,y) ? */
 123 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
 124 {
 125         double x1 = traj->target.cart.x;
 126         double y1 = traj->target.cart.y;
 127         double x2 = position_get_x_double(traj->position);
 128         double y2 = position_get_y_double(traj->position);
 129         return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
 130 }
 131
 132 /** near the angle target in radian ? Only valid if
 133  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
 134  *  command) */
 135 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad)
 136 {
 137         double a;
 138
 139         /* convert relative angle from imp to rad */
 140         a = traj->target.pol.angle - rs_get_angle(traj->robot);
 141         a /= traj->position->phys.distance_imp_per_mm;
 142         a /= traj->position->phys.track_mm;
 143         a *= 2.;
 144         return ABS(a) < a_win_rad;
 145 }