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