modules/devices/robot/trajectory_manager/trajectory_manager_utils.h

   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 #define M_2PI (2*M_PI)
  25
  26 #define DEG(x) ((x) * (180.0 / M_PI))
  27 #define RAD(x) ((x) * (M_PI / 180.0))
  28
  29 /* 25 ms */
  30 #define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT)
  31
  32 /** set speed consign in quadramp filter */
  33 void set_quadramp_speed(struct trajectory *traj, double d_speed, double a_speed);
  34
  35 /** set acc consign in quadramp filter */
  36 void set_quadramp_acc(struct trajectory *traj, double d_acc, double a_acc);
  37
  38 /** get angle speed consign in quadramp filter */
  39 double get_quadramp_angle_speed(struct trajectory *traj);
  40
  41 /** get distance speed consign in quadramp filter */
  42 double get_quadramp_distance_speed(struct trajectory *traj);
  43
  44 /** remove event if any */
  45 void delete_event(struct trajectory *traj);
  46
  47 /** schedule the trajectory event */
  48 void schedule_event(struct trajectory *traj);
  49
  50 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
  51 double simple_modulo_2pi(double a);
  52
  53 /** do a modulo 2.pi -> [-Pi,+Pi] */
  54 double modulo_2pi(double a);
  55
  56 /** near the target (dist) ? */
  57 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win);
  58
  59 /** near the target (dist in x,y) ? */
  60 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win);
  61
  62 /** near the angle target in radian ? Only valid if
  63  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
  64  *  command) */
  65 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);
  66
  67 double pos_mm2imp(struct trajectory *traj, double pos);
  68 double pos_imp2mm(struct trajectory *traj, double pos);
  69 double speed_mm2imp(struct trajectory *traj, double speed);
  70 double speed_imp2mm(struct trajectory *traj, double speed);
  71 double acc_mm2imp(struct trajectory *traj, double acc);
  72 double acc_imp2mm(struct trajectory *traj, double acc);
  73 double pos_rd2imp(struct trajectory *traj, double pos);
  74 double pos_imp2rd(struct trajectory *traj, double pos);
  75 double speed_rd2imp(struct trajectory *traj, double speed);
  76 double speed_imp2rd(struct trajectory *traj, double speed);
  77 double acc_rd2imp(struct trajectory *traj, double acc);
  78 double acc_imp2rd(struct trajectory *traj, double acc);