modules/devices/robot/trajectory_manager/trajectory_manager_core.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 /**
  25  * update angle and/or distance
  26  * this function is not called directly by the user
  27  *   traj  : pointer to the trajectory structure
  28  *   d_mm  : distance in mm
  29  *   a_rad : angle in radian
  30  *   flags : what to update (UPDATE_A, UPDATE_D)
  31  */
  32 void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
  33                                double a_rad, uint8_t state, uint8_t flags);
  34
  35 /** go straight forward (d is in mm) */
  36 void trajectory_d_rel(struct trajectory *traj, double d_mm);
  37
  38 /** update distance consign without changing angle consign */
  39 void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
  40
  41 /** turn by 'a' degrees */
  42 void trajectory_a_rel(struct trajectory *traj, double a_deg_rel);
  43
  44 /** turn by 'a' degrees */
  45 void trajectory_a_abs(struct trajectory *traj, double a_deg_abs);
  46
  47 /** turn the robot until the point x,y is in front of us */
  48 void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
  49
  50
  51 /** turn the robot until the point x,y is behind us */
  52 void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
  53
  54
  55 /** update angle consign without changing distance consign */
  56 void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
  57
  58 /** update angle consign without changing distance consign */
  59 void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs);
  60
  61
  62 /** turn by 'a' degrees */
  63 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
  64
  65 /** set relative angle and distance consign to 0 */
  66 void trajectory_stop(struct trajectory *traj);
  67
  68 /** set relative angle and distance consign to 0, and break any
  69  * deceleration ramp in quadramp filter */
  70 void trajectory_hardstop(struct trajectory *traj);
  71
  72
  73 /************ GOTO XY, USE EVENTS */
  74
  75 /** goto a x,y point, using a trajectory event */
  76 void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y);
  77
  78 /** go forward to a x,y point, using a trajectory event */
  79 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y);
  80
  81 /** go backward to a x,y point, using a trajectory event */
  82 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y);
  83
  84 /** go forward to a d,a point, using a trajectory event */
  85 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
  86
  87 /** go forward to a x,y relative point, using a trajectory event */
  88 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
  89
  90 /************ FUNCS FOR GETTING TRAJ STATE */
  91
  92 /** return true if the position consign is equal to the filtered
  93  * position consign (after quadramp filter), for angle and
  94  * distance. */
  95 uint8_t trajectory_finished(struct trajectory *traj);
  96
  97 /** return true if traj is nearly finished */
  98 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
  99
 100 /*********** *TRAJECTORY EVENT FUNC */
 101
 102 /** event called for xy trajectories */
 103 void trajectory_manager_xy_event(struct trajectory *traj);
 104
 105 /* trajectory event for circles */
 106 void trajectory_manager_circle_event(struct trajectory *traj);
 107
 108 /* trajectory event */
 109 void trajectory_manager_event(void * param);
 110
 111 /*********** *CIRCLE */
 112
 113 /* XXX TODO */
 114
 115 /*********** CLITOID */
 116
 117 /**
 118  * do a superb curve joining line1 to line2 which is composed of:
 119  *   - a clothoid starting from line1
 120  *   - a circle
 121  *   - another clothoid up to line2
 122  * this curve is called a clitoid (hehe)
 123  *
 124  * the function assumes that the initial linear speed is Vd and
 125  * angular speed is 0.
 126  *
 127  * - x,y,a: starting position
 128  * - advance: parameter for line following
 129  * - alpha: total angle
 130  * - beta: circular part of angle (lower than alpha)
 131  * - R: the radius of the circle (must be != 0)
 132  * - Vd: linear speed to use (in imp per cs period)
 133  * - Amax: maximum angular acceleration
 134  * - d_inter: distance in mm until the intersection of the
 135  *            2 lines
 136  *
 137  * return 0 if trajectory can be loaded, then it is processed in
 138  * background.
 139  */
 140 int8_t trajectory_clitoid(struct trajectory *traj,
 141                           double x, double y, double a, double advance,
 142                           double alpha_deg, double beta_deg, double R_mm,
 143                           double d_inter_mm);