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
  29 enum trajectory_state {
  30         READY,
  31
  32         /* simple trajectories */
  33         RUNNING_A,
  34         RUNNING_D,
  35         RUNNING_AD,
  36
  37         /* trajectories using events */
  38         RUNNING_XY_START,
  39         RUNNING_XY_ANGLE,
  40         RUNNING_XY_ANGLE_OK,
  41         RUNNING_XY_F_START,
  42         RUNNING_XY_F_ANGLE,
  43         RUNNING_XY_F_ANGLE_OK,
  44         RUNNING_XY_B_START,
  45         RUNNING_XY_B_ANGLE,
  46         RUNNING_XY_B_ANGLE_OK,
  47         RUNNING_CIRCLE,
  48 };
  49
  50
  51 struct trajectory {
  52         enum trajectory_state state; /*<< describe the type of target, and if we reached the target */
  53
  54         union {
  55                 vect2_cart cart;     /**<< target, if it is a x,y vector */
  56                 struct rs_polar pol; /**<< target, if it is a d,a vector */
  57         } target;
  58
  59         double d_win;      /**<< distance window (for END_NEAR) */
  60         double a_win_rad;  /**<< angle window (for END_NEAR) */
  61         double a_start_rad;/**<< in xy consigns, start to move in distance
  62                             *    when a_target < a_start */
  63
  64         uint16_t d_speed;  /**<< distance speed consign */
  65         uint16_t a_speed;  /**<< angle speed consign */
  66
  67         struct robot_position *position; /**<< associated robot_position */
  68         struct robot_system *robot;      /**<< associated robot_system */
  69         struct cs *csm_angle;     /**<< associated control system (angle) */
  70         struct cs *csm_distance;  /**<< associated control system (distance) */
  71
  72         int8_t scheduler_task;    /**<< id of current task (-1 if no running task) */
  73 };
  74
  75 /** structure initialization */
  76 void trajectory_init(struct trajectory *traj);
  77
  78 /** structure initialization */
  79 void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
  80                        struct cs * cs_a);
  81
  82 /** structure initialization */
  83 void trajectory_set_robot_params(struct trajectory *traj,
  84                                  struct robot_system *rs,
  85                                  struct robot_position *pos) ;
  86
  87 /** set speed consign */
  88 void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
  89
  90 /**
  91  * set windows for trajectory.
  92  * params: distance window, angle window: we the robot enters this
  93  * position window, we deletes the event and the last consign is
  94  * used.
  95  * a_start_deg used in xy consigns (start to move in distance when
  96  * a_target < a_start)
  97  */
  98 void trajectory_set_windows(struct trajectory *traj, double d_win,
  99                             double a_win_deg, double a_start_deg);
 100
 101 /** return true if the position consign is equal to the filtered
 102  * position consign (after quadramp filter), for angle and
 103  * distance. */
 104 uint8_t trajectory_finished(struct trajectory *traj);
 105
 106 /** return true if traj is nearly finished depending on specified
 107  *  parameters */
 108 uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
 109
 110 /* simple commands */
 111
 112 /** set relative angle and distance consign to 0 */
 113 void trajectory_stop(struct trajectory *traj);
 114
 115 /** set relative angle and distance consign to 0, and break any
 116  * deceleration ramp in quadramp filter */
 117 void trajectory_hardstop(struct trajectory *traj);
 118
 119 /** go straight forward (d is in mm) */
 120 void trajectory_d_rel(struct trajectory *traj, double d_mm);
 121
 122 /** update distance consign without changing angle consign */
 123 void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
 124
 125 /** turn by 'a' degrees */
 126 void trajectory_a_rel(struct trajectory *traj, double a_deg);
 127
 128 /** go to angle 'a' in degrees */
 129 void trajectory_a_abs(struct trajectory *traj, double a_deg);
 130
 131 /** turn the robot until the point x,y is in front of us */
 132 void trajectory_turnto_xy(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
 133
 134 /** turn the robot until the point x,y is behind us */
 135 void trajectory_turnto_xy_behind(struct trajectory*traj, double x_abs_mm, double y_abs_mm);
 136
 137 /** update angle consign without changing distance consign */
 138 void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
 139
 140 /** update angle consign without changing distance consign */
 141 void trajectory_only_a_abs(struct trajectory *traj, double a_deg);
 142
 143 /** turn by 'a' degrees and go by 'd' mm */
 144 void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
 145
 146 /* commands using events */
 147
 148 /** goto a x,y point, using a trajectory event */
 149 void trajectory_goto_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 150
 151 /** go forward to a x,y point, using a trajectory event */
 152 void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 153
 154 /** go backward to a x,y point, using a trajectory event */
 155 void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
 156
 157 /** go forward to a d,a point, using a trajectory event */
 158 void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
 159
 160 /** go forward to a x,y relative point, using a trajectory event */
 161 void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
 162
 163 #endif //TRAJECTORY_MANAGER