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, int16_t d_speed, int16_t a_speed);
  34
  35 /** get angle speed consign in quadramp filter */
  36 uint32_t get_quadramp_angle_speed(struct trajectory *traj);
  37
  38 /** get distance speed consign in quadramp filter */
  39 uint32_t get_quadramp_distance_speed(struct trajectory *traj);
  40
  41 /** remove event if any */
  42 void delete_event(struct trajectory *traj);
  43
  44 /** schedule the trajectory event */
  45 void schedule_event(struct trajectory *traj);
  46
  47 /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
  48 double simple_modulo_2pi(double a);
  49
  50 /** do a modulo 2.pi -> [-Pi,+Pi] */
  51 double modulo_2pi(double a);
  52
  53 /** near the target (dist) ? */
  54 uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win);
  55
  56 /** near the target (dist in x,y) ? */
  57 uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win);
  58
  59 /** near the angle target in radian ? Only valid if
  60  *  traj->target.pol.angle is set (i.e. an angle command, not an xy
  61  *  command) */
  62 uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);