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
  86
  87         ROBOT_LOCK(est_pos);
  88         init_ekf(x, y, phi);
  89         robot.est_pos.x = x;
  90         robot.est_pos.y = y;
  91         robot.est_pos.phi = phi;
  92         ROBOT_UNLOCK(est_pos);
  93 }
  94
  95 /**
  96  * Adds point in absolute coordinates to previously initialized trajectory object.
  97  *
  98  * @param x_m X coordinate in meters.
  99  * @param y_m Y coordinate in meters.
 100  */
 101 void robot_trajectory_add_point_notrans(double x_m, double y_m)
 102 {
 103         Trajectory *t;
 104
 105         t = (Trajectory*)robot.new_trajectory;
 106         if (t) {
 107                 t->addPoint(x_m, y_m);
 108         }
 109 }
 110
 111 bool get_arrive_from_point(double target_x_m, double target_y_m, struct move_target_heading heading,
 112                            double *point_x_m, double *point_y_m)
 113 {
 114         double x, y;
 115         if (heading.operation == TOP_ARRIVE_FROM) {
 116                 x = target_x_m +
 117                         heading.distance*cos(heading.angle-M_PI);
 118                 y = target_y_m +
 119                         heading.distance*sin(heading.angle-M_PI);
 120                 *point_x_m = x;
 121                 *point_y_m = y;
 122                 return true;
 123         } else {
 124                 return false;
 125         }
 126 }
 127
 128 /**
 129  * Adds final point to trajectory objects and starts robot movement.
 130  *
 131  * @param x_m X coordinate in meters.
 132  * @param y_m Y coordinate in meters.
 133  * @param heading Desired heading (in degrees) of the robot in this
 134  *                point. NAN means don't care. Positive number or zero
 135  *                means turn counter-clockwise, negative number means
 136  *                turn clockwise. Example: DEG2RAD(90) means turn up
 137  *                counter-clockwise and DEG2RAD(-270) means turn up
 138  *                clockwise.
 139  */
 140 void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading)
 141 {
 142         struct move_target *target;
 143         Trajectory *t;
 144
 145         t = (Trajectory*)robot.new_trajectory;
 146         if (t) {
 147                 if (heading.operation == TOP_ARRIVE_FROM) {
 148                         double ax, ay;
 149                         get_arrive_from_point(x_m, y_m, heading, &ax, &ay);
 150                         robot_trajectory_add_point_notrans(ax, ay);
 151                 }
 152                 robot_trajectory_add_point_notrans(x_m, y_m);
 153
 154                 t->finalHeading = target2final_heading(heading);
 155                 target = (struct move_target*)malloc(sizeof(*target));
 156                 memset(target, 0, sizeof(*target));
 157                 target->x = x_m;
 158                 target->y = y_m;
 159                 target->heading = heading;
 160                 target->trajectory = t;
 161                 target->use_planning = false;
 162                 FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 163                 robot.new_trajectory = NULL;
 164         } else {
 165                 printf("%s: Error - No trajectory\n", __FUNCTION__);
 166         }
 167 }
 168
 169
 170 /**
 171  * Stops actual movement.
 172  *
 173  */
 174 void robot_stop()
 175 {
 176         FSM_SIGNAL(MOTION, EV_MOVE_STOP, NULL);
 177 }
 178
 179
 180 void robot_send_speed(double left, double right) {
 181         double mul;
 182         unsigned l, r;
 183
 184         mul = 1000.0 / ROBOT_WHEEL_RADIUS_MM; // to angular speed
 185         mul *= 28.0/(2.0*M_PI)/*gear*/*(1<<8);  // to pxmc speed
 186
 187         // I hope it is not neccessary to lock here
 188         l = (int)(left * mul);
 189         r = (int)(right * mul);
 190         robot.orte.motion_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
 191         robot.orte.motion_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
 192 //      DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)",
 193 //          left, l, // robot.orte.motion_speed.left,
 194 //          right, r //robot.orte.motion_speed.right
 195 //          );
 196         ORTEPublicationSend(robot.orte.publication_motion_speed);
 197 }
 198
 199 /**
 200  * Makes move the robot to a target position. Use path panner to find
 201  * the trajectory. This function is intended to be called from main
 202  * FSM.
 203  *
 204  * @param x X coordinate in meters.
 205  * @param y Y coordinate in meters.
 206  * @param heading Desired heading of the robot at goal point.
 207  */
 208 void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc)
 209 {
 210         struct move_target *target;
 211         target = (struct move_target*)malloc(sizeof(*target));
 212         memset(target, 0, sizeof(*target));
 213         target->x = x;
 214         target->y = y;
 215         target->heading = heading;
 216         target->use_planning = true;
 217         if (tc) target->tc = *tc;
 218         else target->tc = trajectoryConstraintsDefault;
 219
 220         FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 221 }
 222
 223 void robot_moveto_notrans(double x, double y,
 224                           struct move_target_heading heading,
 225                           struct TrajectoryConstraints *tc)
 226 {
 227         struct move_target *target;
 228         target = (struct move_target*)malloc(sizeof(*target));
 229         memset(target, 0, sizeof(*target));
 230         target->x = x;
 231         target->y = y;
 232         target->heading = heading;
 233         if (tc) target->tc = *tc;
 234         else target->tc = trajectoryConstraintsDefault;
 235         target->use_planning = false;
 236
 237         FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 238 }
 239
 240 /**
 241  * Move robot forward or backward
 242  *
 243  * @param distance Distance in meters ( >0 forward, <0 backward)
 244  * @param heading Final heading
 245  * @param tc Trajectory constrains
 246  */
 247 void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc)
 248 {
 249         double x, y, phi;
 250
 251         ROBOT_LOCK(est_pos);
 252         x = robot.est_pos.x;
 253         y = robot.est_pos.y ;
 254         phi = robot.est_pos.phi;
 255         ROBOT_UNLOCK(est_pos);
 256
 257         x += distance*cos(phi);
 258         y += distance*sin(phi);
 259
 260         Trajectory *t;
 261         if (!tc) tc = &trajectoryConstraintsDefault;
 262         bool backward = distance < 0;
 263         t = new Trajectory(*tc, backward);
 264         t->addPoint(x, y);
 265         t->finalHeading = target2final_heading(heading);
 266
 267         struct move_target *target;
 268         target = (struct move_target*)malloc(sizeof(*target));
 269         memset(target, 0, sizeof(*target));
 270         target->x = x;
 271         target->y = y;
 272         target->heading = heading;
 273         target->trajectory = t;
 274         FSM_SIGNAL(MOTION, EV_NEW_TARGET, target);
 275 }
 276
 277