1 #ifndef MOVEHELPER_EB2008_H
2 #define MOVEHELPER_EB2008_H
5 #include <trgenconstr.h>
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 */
17 struct move_target_heading {
18 enum move_target_op operation;
20 float distance; /* Distance used by TOP_ARRIVE_FROM */
23 static inline struct move_target_heading __target_heading(enum move_target_op op, float angle, float distance)
25 struct move_target_heading th;
28 th.distance = distance;
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))
38 static inline struct final_heading target2final_heading(struct move_target_heading th)
40 struct final_heading fh;
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;
49 fh.heading = th.angle;
53 /* Represents the target position of a move. */
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? */
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. */
67 extern struct TrajectoryConstraints trajectoryConstraintsDefault;
73 /* extern double target_x, target_y; */
75 static inline double __trans_ang(double phi)
77 if (robot.team_color == RED) {
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;
89 static inline struct move_target_heading __trans_heading(struct move_target_heading h)
91 if (robot.team_color == RED) {
94 if (h.operation != TOP_DONT_TURN) {
95 h.angle = __trans_ang(h.angle);
96 switch (h.operation) {
98 h.operation = TOP_TURN_CCW;
101 h.operation = TOP_TURN_CW;
111 static inline double __trans_x(double x)
113 if (robot.team_color == RED)
116 return PLAYGROUND_WIDTH_M - x;
119 static inline double __trans_y(double y)
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);
129 extern bool init_ekf_flag;
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))
134 void robot_send_speed(double left, double right);
136 /* Low-level trajectory handling */
137 void robot_trajectory_new(struct TrajectoryConstraints *tc);
138 void robot_trajectory_new_backward(struct TrajectoryConstraints *tc);
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))
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))
146 /** Stop robot immediately */
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)
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)
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)
162 /* FIXME: not used anywhere */
163 static inline void robot_translate_coordinates(double *x, double *y, double *phi)
169 *phi = __trans_ang(*phi);
180 #endif /* MOVEHELPER_EB2008_H */