src/robofsm/movehelper.h

   1 #ifndef MOVEHELPER_EB2008_H
   2 #define MOVEHELPER_EB2008_H
   3
   4 #include <robodata.h>
   5 #include <robot.h>
   6 #include <trgenconstr.h>
   7 #include <math.h>
   8 #include <stdbool.h>
   9
  10 enum move_target_op {
  11         TOP_DONT_TURN,
  12         TOP_TURN_CW,            /* Clockwise */
  13         TOP_TURN_CCW,           /* Counter-clockwise */
  14         TOP_TURN_SHORTEST,      /* Shortest turn */
  15         TOP_ARRIVE_FROM /* Arrive to the final point from the given direction */
  16 };
  17
  18 struct move_target_heading {
  19         enum move_target_op operation;
  20         float angle;
  21         float distance;         /* Distance used by TOP_ARRIVE_FROM */
  22 };
  23
  24 static inline struct move_target_heading __target_heading(enum move_target_op op, float angle, float distance)
  25 {
  26         struct move_target_heading th;
  27         th.operation = op;
  28         th.angle = angle;
  29         th.distance = distance;
  30         return th;
  31 }
  32
  33 #define TURN(heading) __target_heading(TOP_TURN_SHORTEST, (heading), 0)
  34 #define TURN_CW(heading) __target_heading(TOP_TURN_CW, (heading), 0)
  35 #define TURN_CCW(heading) __target_heading(TOP_TURN_CCW, (heading), 0)
  36 #define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0)
  37 #define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist))
  38
  39 static inline struct final_heading target2final_heading(struct move_target_heading th)
  40 {
  41         struct final_heading fh;
  42
  43         switch (th.operation) {
  44                 case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break;
  45                 case TOP_TURN_CW: fh.turn_type = FH_CW; break;
  46                 case TOP_TURN_CCW: fh.turn_type = FH_CCW; break;
  47                 case TOP_TURN_SHORTEST: fh.turn_type = FH_SHORTEST; break;
  48                 case TOP_ARRIVE_FROM: fh.turn_type = FH_DONT_TURN; break;
  49         }
  50         fh.heading = th.angle;
  51         return fh;
  52 }
  53
  54 /* Represents the target position of a move. */
  55 struct move_target {
  56         double x, y;
  57         struct move_target_heading heading;
  58         struct TrajectoryConstraints tc; /* if (trajectory) taken from it */
  59         bool use_planning;
  60
  61         /** If trajectory is NULL, it is generated by motion FSM
  62          * according to the above fields. Otherwise, the trajectory
  63          * specified here is used and the above fields are used only
  64          * for replanning in case of colision. */
  65         void *trajectory;
  66 };
  67
  68 extern struct TrajectoryConstraints trajectoryConstraintsDefault;
  69
  70 #ifdef __cplusplus
  71 extern "C" {
  72 #endif
  73
  74 /* extern double target_x, target_y; */
  75
  76 #if defined(WE_ARE_RED) && defined(WE_ARE_GREEN)
  77 #error Both colors defined
  78 #endif
  79
  80 #if !defined(WE_ARE_RED) && !defined(WE_ARE_GREEN)
  81 #error No colors defined
  82 #endif
  83
  84 #ifdef WE_ARE_RED
  85 static inline double __trans_ang(double phi)
  86 {
  87         double a;
  88         a = M_PI/2.0 - phi + M_PI/2.0;
  89         a = fmod(a, 2.0*M_PI);
  90         if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
  91         if (phi < 0 && a < 0) a += 2.0*M_PI;
  92         return a;
  93 }
  94 static inline struct move_target_heading __trans_heading(struct move_target_heading h)
  95 {
  96         if (h.operation != TOP_DONT_TURN) {
  97                 h.angle = __trans_ang(h.angle);
  98                 switch (h.operation) {
  99                         case FH_CW:
 100                                 h.operation = TOP_TURN_CCW;
 101                                 break;
 102                         case FH_CCW:
 103                                 h.operation = TOP_TURN_CW;
 104                                 break;
 105                         default:
 106                                 break;
 107                 }
 108         }
 109         return h;
 110 }
 111
 112 #define TRANS_X(x) (PLAYGROUND_WIDTH_M - (x))
 113 #define TRANS_Y(y) (y)
 114 #define TRANS_ANG(a) __trans_ang(a)
 115 #define TRANS_HEADING(h) __trans_heading(h)
 116 #endif
 117
 118 #ifdef WE_ARE_GREEN
 119 #define TRANS_X(x) (x)
 120 #define TRANS_Y(y) (y)
 121 #define TRANS_ANG(a) (a)
 122 #define TRANS_HEADING(h) (h)
 123 #endif
 124
 125 bool get_arrive_from_point(double target_x_m, double target_y_m,
 126                            struct move_target_heading heading,
 127                            double *point_x_m, double *point_y_m);
 128
 129
 130 void robot_set_est_pos_notrans(double x, double y, double phi);
 131 #define robot_set_est_pos_trans(x, y, phi) robot_set_est_pos_notrans(TRANS_X(x), TRANS_Y(y), TRANS_ANG(phi))
 132
 133 void robot_send_speed(double left, double right);
 134
 135 /* Low-level trajectory handling */
 136 void robot_trajectory_new(struct TrajectoryConstraints *tc);
 137 void robot_trajectory_new_backward(struct TrajectoryConstraints *tc);
 138
 139 void robot_trajectory_add_point_notrans(double x_m, double y_m);
 140 #define robot_trajectory_add_point_trans(x, y) robot_trajectory_add_point_notrans(TRANS_X(x), TRANS_Y(y))
 141
 142 void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading);
 143 #define robot_trajectory_add_final_point_trans(x, y, heading) robot_trajectory_add_final_point_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading))
 144
 145 /** Stop robot immediately */
 146 void robot_stop();
 147
 148 /** Go to a point using path planning to avoid apriori known obstacles */
 149 void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc);
 150 #define robot_goto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc)
 151
 152 /** Move to a point using straight line trajectory. If ARIVE_FROM is
 153  * set, then the trajectory will be composed two lines.... TODO */
 154 void robot_moveto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc);
 155 #define robot_moveto_trans(x, y, heading, tc) robot_goto_notrans(TRANS_X(x), TRANS_Y(y), TRANS_HEADING(heading), tc)
 156
 157 /* Relative movement */
 158 void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc);
 159 #define robot_move_by_trans(distance, heading, tc) robot_move_by(distance, TRANS_HEADING(heading), tc)
 160
 161 /* FIXME: not used anywhere */
 162 static inline void robot_translate_coordinates(double *x, double *y, double *phi)
 163 {
 164         if (x) {
 165                 *x = TRANS_X(*x);
 166         }
 167         if (phi) {
 168                 *phi = TRANS_ANG(*phi);
 169         }
 170 }
 171
 172 void init_ekf(float x, float y, float ang);
 173
 174
 175 #ifdef __cplusplus
 176 }
 177 #endif
 178
 179
 180 #endif /* MOVEHELPER_EB2008_H */