src/robofsm/movehelper.cc

   1 /**
   2  * @file   movehelper.cc
   3  * @author Michal Sojka
   4  * @date   Wed Jun 13 13:27:58 2007
   5  *
   6  * @brief  Convenience functions for motion control.
   7  *
   8  *
   9  */
  10
  11 #include <trgen.h>
  12 #include <robodata.h>
  13 #include <robot.h>
  14 #include <movehelper.h>
  15 #include <path_simplifier.h>
  16 #include <stdio.h>
  17 #include <stdlib.h>
  18 #include <sys/stat.h>
  19 #include <math.h>
  20 #include <map.h>
  21 #include <robomath.h>
  22 #include <mcl_robot.h>
  23 #include <string.h>
  24
  25 #if 0
  26 struct TrajectoryConstraints trajectoryConstraintsDefault = {
  27         maxv: 0.3,              // m/s
  28         maxomega: 1.5,          // rad/s
  29         maxangacc: 2,           // rad/s^2
  30         maxacc: 0.3,            // m/s^2
  31         maxcenacc: 1,           // m/s^2
  32         maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
  33 };
  34 #else
  35 struct TrajectoryConstraints trajectoryConstraintsDefault = {
  36         maxv: 1,                // m/s
  37         maxomega: 2,            // rad/s
  38         maxangacc: 3,           // rad/s^2
  39         maxacc: 0.7,            // m/s^2
  40         maxcenacc: 3*1,         // m/s^2
  41         maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
  42 };
  43 #endif
  44
  45 /**
  46  *
  47  *
  48  *
  49  */
  50 static void robot_trajectory_new_ex(struct TrajectoryConstraints *tc, bool backward)
  51 {
  52         Trajectory *t;
  53
  54         if (!tc) tc = &trajectoryConstraintsDefault;
  55
  56         t = (Trajectory*)robot.new_trajectory;
  57         if (t) delete(t);
  58         t = new Trajectory(*tc, backward);
  59         robot.new_trajectory = t;
  60 }
  61
  62
  63 /**
  64  * Initializes new trajectory object for adding points. Path planner
  65  * will not be used.
  66  *
  67  * @param tc Constrains for the trajectory.
  68  */
  69 void robot_trajectory_new(struct TrajectoryConstraints *tc) {
  70         robot_trajectory_new_ex(tc, false);
  71 }
  72 void robot_trajectory_new_backward(struct TrajectoryConstraints *tc) {
  73         robot_trajectory_new_ex(tc, true);
  74 }
  75
  76 /** Sets actual position of the robot and with respoect to color of
  77  * the team. Should be used for setting initial position of the
  78  * robot. */
  79 void robot_set_est_pos_notrans(double x, double y, double phi)
  80 {
  81         if (x<0) x=0;
  82         if (x>PLAYGROUND_WIDTH_M) x=PLAYGROUND_WIDTH_M;
  83         if (y<0) y=0;
  84         if (y>PLAYGROUND_HEIGHT_M) y=PLAYGROUND_HEIGHT_M;
  85         struct mcl_robot_pos *newpos = (struct mcl_robot_pos*)malloc(sizeof(struct mcl_robot_pos));
  86         memset(newpos, 0, sizeof(*newpos));
  87         newpos->x = x;
  88         newpos->y = y;
  89         newpos->angle = fmod(phi, 2.0*M_PI);
  90         // FIXME: This should be synchronous signal - we need to
  91         // define FSM_SIGNAL_SYNC
  92         //FSM_SIGNAL(LOC, EV_SET_ESTIMATED, newpos);
  93         ROBOT_LOCK(est_pos);
  94         robot.est_pos.x = x;
  95         robot.est_pos.y = y;
  96         robot.est_pos.phi = phi;
  97         ROBOT_UNLOCK(est_pos);
  98 }
  99
 100 /**
 101  * Adds point in absolute coordinates to previously initialized trajectory object.
 102  *
 103  * @param x_m X coordinate in meters.
 104  * @param y_m Y coordinate in meters.
 105  */
 106 void robot_trajectory_add_point_notrans(double x_m, double y_m)
 107 {
 108         Trajectory *t;
 109
 110         t = (Trajectory*)robot.new_trajectory;
 111         if (t) {
 112                 t->addPoint(x_m, y_m);
 113         }
 114 }
 115
 116 /**
 117  * Adds final point to trajectory objects and starts robot movement.
 118  *
 119  * @param x_m X coordinate in meters.
 120  * @param y_m Y coordinate in meters.
 121  * @param heading Desired heading (in degrees) of the robot in this
 122  *                point. NAN means don't care. Positive number or zero
 123  *                means turn counter-clockwise, negative number means
 124  *                turn clockwise. Example: DEG2RAD(90) means turn up
 125  *                counter-clockwise and DEG2RAD(-270) means turn up
 126  *                clockwise.
 127  */
 128 void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct final_heading *heading)
 129 {
 130         struct move_target *target;
 131         robot_trajectory_add_point_notrans(x_m, y_m);
 132         Trajectory *t;
 133
 134         t = (Trajectory*)robot.new_trajectory;
 135         if (t) {
 136                 t->finalHeading = *heading;
 137                 target = (struct move_target*)malloc(sizeof(*target));
 138                 memset(target, 0, sizeof(*target));
 139                 target->x = x_m;
 140                 target->y = y_m;
 141                 target->h = *heading;
 142                 target->trajectory = t;
 143                 FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 144                 robot.new_trajectory = NULL;
 145         } else {
 146                 printf("%s: Error - No trajectory\n", __FUNCTION__);
 147         }
 148 }
 149
 150
 151 /**
 152  * Stops actual movement.
 153  *
 154  */
 155 void robot_stop()
 156 {
 157         FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
 158 }
 159
 160
 161 void robot_send_speed(double left, double right) {
 162         double mul;
 163         unsigned l, r;
 164
 165         mul = 1000.0 / ROBOT_WHEEL_RADIUS_MM; // to angular speed
 166         mul *= 28.0/(2.0*M_PI)/*gear*/*(1<<8);  // to pxmc speed
 167
 168         // I hope it is not neccessary to lock here
 169         l = (int)(left * mul);
 170         r = (int)(right * mul);
 171         robot.orte.motion_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
 172         robot.orte.motion_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
 173 //      DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)",
 174 //          left, l, // robot.orte.motion_speed.left,
 175 //          right, r //robot.orte.motion_speed.right
 176 //          );
 177         ORTEPublicationSend(robot.orte.publication_motion_speed);
 178 }
 179
 180 /**
 181  * Makes move the robot to a target position. Use path panner to find
 182  * the trajectory. This function is intended to be called from main
 183  * FSM.
 184  *
 185  * @param x X coordinate in meters.
 186  * @param y Y coordinate in meters.
 187  * @param heading Desired heading of the robot at goal point.
 188  */
 189 void robot_goto_notrans(double x, double y, struct final_heading *heading, struct TrajectoryConstraints *tc)
 190 {
 191         struct move_target *target;
 192         target = (struct move_target*)malloc(sizeof(*target));
 193         memset(target, 0, sizeof(*target));
 194         target->x = x;
 195         target->y = y;
 196         target->h = *heading;
 197         if (tc) target->tc = *tc;
 198         else target->tc = trajectoryConstraintsDefault;
 199
 200         FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 201 }
 202
 203
 204 /**
 205  * Move robot forward or backward
 206  *
 207  * @param distance Distance in meters ( >0 forward, <0 backward)
 208  * @param heading Final heading
 209  * @param tc Trajectory constrains
 210  */
 211 void robot_move_by(double distance, struct final_heading *heading, struct TrajectoryConstraints *tc)
 212 {
 213         double x, y, phi;
 214
 215         ROBOT_LOCK(est_pos);
 216         x = robot.est_pos.x;
 217         y = robot.est_pos.y ;
 218         phi = robot.est_pos.phi;
 219         ROBOT_UNLOCK(est_pos);
 220
 221         x += distance*cos(phi);
 222         y += distance*sin(phi);
 223
 224         Trajectory *t;
 225         if (!tc) tc = &trajectoryConstraintsDefault;
 226         bool backward = distance < 0;
 227         t = new Trajectory(*tc, backward);
 228         t->addPoint(x, y);
 229         t->finalHeading = *heading;
 230
 231         struct move_target *target;
 232         target = (struct move_target*)malloc(sizeof(*target));
 233         memset(target, 0, sizeof(*target));
 234         target->x = x;
 235         target->y = y;
 236         target->h = *heading;
 237         target->trajectory = t;
 238         FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 239 }