src/robofsm/movehelper.h

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