modules/devices/robot/trajectory_manager/trajectory_manager.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.h,v 1.4.4.10 2009-05-02 10:03:04 zer0 Exp $
  19  *
  20  */
  21
  22 #ifndef TRAJECTORY_MANAGER
  23 #define TRAJECTORY_MANAGER
  24
  25 #include <aversive.h>
  26 #include <vect2.h>
  27 #include <robot_system.h>
  28 #include <vect_base.h>
  29 #include <lines.h>
  30
  31 enum trajectory_state {
  32         READY,
  33
  34         /* simple trajectories */
  35         RUNNING_A,
  36         RUNNING_D,
  37         RUNNING_AD,
  38
  39         /* trajectories using events */
  40         RUNNING_XY_START,
  41         RUNNING_XY_ANGLE,
  42         RUNNING_XY_ANGLE_OK,
  43         RUNNING_XY_F_START,
  44         RUNNING_XY_F_ANGLE,
  45         RUNNING_XY_F_ANGLE_OK,
  46         RUNNING_XY_B_START,
  47         RUNNING_XY_B_ANGLE,
  48         RUNNING_XY_B_ANGLE_OK,
  49
  50         /* circle */
  51         RUNNING_CIRCLE,
  52
  53         /* line */
  54         RUNNING_LINE,
  55
  56         /* clitoid */
  57         RUNNING_CLITOID_LINE,
  58         RUNNING_CLITOID_CURVE,
  59 };
  60
  61 struct circle_target {
  62         vect2_cart center;   /**< center of the circle */
  63         double radius;       /**< radius of the circle */
  64         int32_t dest_angle;  /**< dst angle in inc */
  65
  66 #define TRIGO   1 /* rotation is counterclockwise */
  67 #define FORWARD 2 /* go forward or backward */
  68         uint8_t flags;   /**< flags for this trajectory */
  69 };
  70
  71 /* for line and clitoid */
  72 struct line_target {
  73         line_t line;
  74         double angle;
  75         double advance;
  76
  77         /* only for clitoid */
  78         point_t turn_pt;
  79         double Aa;
  80         double Va;
  81         double alpha;
  82         double R;
  83 };
  84
  85 struct trajectory {
  86         enum trajectory_state state; /*<< describe the type of target, and if we reached the target */
  87
  88         union {
  89                 vect2_cart cart;     /**<< target, if it is a x,y vector */
  90                 struct rs_polar pol; /**<< target, if it is a d,a vector */
  91                 struct circle_target circle; /**<< target, if it is a circle */
  92                 struct line_target line; /**<< target, if it is a line */
  93         } target;
  94
  95         double d_win;      /**<< distance window (for END_NEAR) */
  96         double a_win_rad;  /**<< angle window (for END_NEAR) */
  97         double a_start_rad;/**<< in xy consigns, start to move in distance
  98                             *    when a_target < a_start */
  99         double circle_coef;/**<< corrective circle coef */
 100
 101         double d_speed;  /**<< distance speed consign */
 102         double a_speed;  /**<< angle speed consign */
 103
 104         double d_acc;    /**<< distance acceleration consign */
 105         double a_acc;    /**<< angle acceleration consign */
 106
 107         struct robot_position *position; /**<< associated robot_position */
 108         struct robot_system *robot;      /**<< associated robot_system */
 109         struct cs *csm_angle;     /**<< associated control system (angle) */
 110         struct cs *csm_distance;  /**<< associated control system (distance) */
 111
 112         double cs_hz;
 113
 114         int8_t scheduler_task;    /**<< id of current task (-1 if no running task) */
 115 };
 116
 117 /** structure initialization */
 118 void trajectory_init(struct trajectory *traj, double cs_hz);
 119
 120 /** structure initialization */
 121 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
 122                        struct cs * cs_a);
 123
 124 /** structure initialization */
 125 void trajectory_set_robot_params(struct trajectory *traj,
 126                                  struct robot_system *rs,
 127                                  struct robot_position *pos) ;
 128
 129 /** set speed consign */
 130 void trajectory_set_speed(struct trajectory *traj, double d_speed, double a_speed);
 131
 132 /** set speed consign */
 133 void trajectory_set_acc(struct trajectory *traj, double d_acc, double a_acc);
 134
 135 /**
 136  * set windows for trajectory.
 137  * params: distance window, angle window: we the robot enters this
 138  * position window, we deletes the event and the last consign is
 139  * used.
 140  * a_start_deg used in xy consigns (start to move in distance when
 141  * a_target < a_start)
 142  */
 143 void trajectory_set_windows(struct trajectory *traj, double d_win,
 144                             double a_win_deg, double a_start_deg);
 145
 146 /**
 147  * Set coef for circle trajectory. The objective of this value is to
 148  * fix the radius of the circle which is not correctly what we asked.
 149  */
 150 void trajectory_set_circle_coef(struct trajectory *traj, double coef);
 151
 152 /** return true if the position consign is equal to the filtered
 153  * position consign (after quadramp filter), for angle and
 154  * distance. */
 155 uint8_t trajectory_finished(struct trajectory *traj);
 156 uint8_t trajectory_angle_finished(struct trajectory *traj);
 157 uint8_t trajectory_distance_finished(struct trajectory *traj);
 158
 159 /** return true if traj is nearly finished depending on specified
 160  *  parameters */
 161 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
 162
 163 /* simple commands */
 164
 165 /** set relative angle and distance consign to 0 */
 166 void trajectory_stop(struct trajectory *traj);
 167
 168 /** set relative angle and distance consign to 0, and break any
 169  * deceleration ramp in quadramp filter */
 170 void trajectory_hardstop(struct trajectory *traj);
 171
 172 /** go straight forward (d is in mm) */
 173 void trajectory_d_rel(struct trajectory *traj, double d_mm);
 174
 175 /** update distance consign without changing angle consign */
 176 void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
 177
 178 /** turn by 'a' degrees */
 179 void trajectory_a_rel(struct trajectory *traj, double a_deg);
 180
 181 /** go to angle 'a' in degrees */
 182 void trajectory_a_abs(struct trajectory *traj, double a_deg);
 183
 184 /** turn the robot until the point x,y is in front of us */
 185 void trajectory_turnto_xy(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
 186
 187 /** turn the robot until the point x,y is behind us */
 188 void trajectory_turnto_xy_behind(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
 189
 190 /** update angle consign without changing distance consign */
 191 void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
 192
 193 /** update angle consign without changing distance consign */
 194 void trajectory_only_a_abs(struct trajectory *traj, double a_deg);
 195
 196 /** turn by 'a' degrees and go by 'd' mm */
 197 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
 198
 199 /* commands using events */
 200
 201 /** goto a x,y point, using a trajectory event */
 202 void trajectory_goto_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 203
 204 /** go forward to a x,y point, using a trajectory event */
 205 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 206
 207 /** go backward to a x,y point, using a trajectory event */
 208 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 209
 210 /** go forward to a d,a point, using a trajectory event */
 211 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
 212
 213 /** go forward to a x,y relative point, using a trajectory event */
 214 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
 215
 216 /** make the robot orbiting around (x,y) on a circle whose radius is
 217  * radius_mm, and exit when relative destination angle is reached. The
 218  * flags set if we go forward or backwards, and CW/CCW. */
 219 void trajectory_circle_rel(struct trajectory *traj, double x, double y,
 220                            double radius_mm, double rel_a_deg, uint8_t flags);
 221
 222 /*
 223  * Compute the fastest distance and angle speeds matching the radius
 224  * from current traj_speed
 225  */
 226 void circle_get_da_speed_from_radius(struct trajectory *traj,
 227                                      double radius_mm,
 228                                      double *speed_d,
 229                                      double *speed_a);
 230
 231 /* do a line */
 232 void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
 233                          double x2, double y2, double advance);
 234
 235 #endif //TRAJECTORY_MANAGER