};
struct move_target_heading {
- enum move_target_op operation;
+ move_target_op operation;
float angle;
float distance; /* Distance used by TOP_ARRIVE_FROM */
};
-static inline struct move_target_heading __target_heading(enum move_target_op op, float angle, float distance)
-{
- struct move_target_heading th;
+static inline move_target_heading __target_heading(move_target_op op, float angle, float distance) {
+ move_target_heading th;
th.operation = op;
th.angle = angle;
th.distance = distance;
#define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0)
#define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist))
-static inline struct final_heading target2final_heading(struct move_target_heading th)
-{
- struct final_heading fh;
+static inline final_heading target2final_heading(move_target_heading th) {
+ final_heading fh;
switch (th.operation) {
case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break;
/* Represents the target position of a move. */
struct move_target {
double x, y;
- struct move_target_heading heading;
- struct TrajectoryConstraints tc; /* if (trajectory) taken from it */
+ move_target_heading heading;
+ TrajectoryConstraints tc; /* if (trajectory) taken from it */
bool use_planning; /* FIXME (FJ): this variable is not used anywhere? */
/** If trajectory is NULL, it is generated by motion FSM
* according to the above fields. Otherwise, the trajectory
* specified here is used and the above fields are used only
* for replanning in case of colision. */
- void *trajectory;
+ struct Trajectory *trajectory;
};
extern struct TrajectoryConstraints trajectoryConstraintsDefault;
-#ifdef __cplusplus
-extern "C" {
-#endif
-
/* extern double target_x, target_y; */
-static inline double __trans_ang(double phi)
-{
- if (robot.team_color == BLUE) {
- return phi;
- } else {
- double a;
- a = M_PI/2.0 - phi + M_PI/2.0;
- a = fmod(a, 2.0*M_PI);
- if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
- if (phi < 0 && a < 0) a += 2.0*M_PI;
- return a;
- }
-}
-
-static inline struct move_target_heading __trans_heading(struct move_target_heading h)
-{
- if (robot.team_color == BLUE) {
- return h;
- } else {
- if (h.operation != TOP_DONT_TURN) {
- h.angle = __trans_ang(h.angle);
- switch (h.operation) {
- case FH_CW:
- h.operation = TOP_TURN_CCW;
- break;
- case FH_CCW:
- h.operation = TOP_TURN_CW;
- break;
- default:
- break;
- }
- }
- return h;
- }
-}
-
-static inline double __trans_x(double x)
-{
- if (robot.team_color == BLUE)
- return x;
- else
- return PLAYGROUND_WIDTH_M - x;
-}
+double trans_angle(double phi);
-static inline double __trans_y(double y)
-{
- return y;
-}
-
-bool get_arrive_from_point(double target_x_m, double target_y_m,
- struct move_target_heading heading,
- double *point_x_m, double *point_y_m);
+double trans_x(double x); //BLUE
+double trans_y(double y);
extern bool init_ekf_flag;
-void robot_set_est_pos_notrans(double x, double y, double phi);
-#define robot_set_est_pos_trans(x, y, phi) robot_set_est_pos_notrans(__trans_x(x), __trans_y(y), __trans_ang(phi))
-
-void robot_send_speed(double left, double right);
-
-/* Low-level trajectory handling */
-void robot_trajectory_new(struct TrajectoryConstraints *tc);
-void robot_trajectory_new_backward(struct TrajectoryConstraints *tc);
-
-void robot_trajectory_add_point_notrans(double x_m, double y_m);
-#define robot_trajectory_add_point_trans(x, y) robot_trajectory_add_point_notrans(__trans_x(x), __trans_y(y))
-
-void robot_trajectory_add_final_point_notrans(double x_m, double y_m, struct move_target_heading heading);
-#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))
-
-/** Stop robot immediately */
-void robot_stop();
-
-/** Go to a point using path planning to avoid apriori known obstacles */
-void robot_goto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc);
-#define robot_goto_trans(x, y, heading, tc) robot_goto_notrans(__trans_x(x), __trans_y(y), __trans_heading(heading), tc)
-
-/** Move to a point using straight line trajectory. If ARIVE_FROM is
- * set, then the trajectory will be composed two lines.... TODO */
-void robot_moveto_notrans(double x, double y, struct move_target_heading heading, struct TrajectoryConstraints *tc);
-#define robot_moveto_trans(x, y, heading, tc) robot_goto_notrans(__trans_x(x), __trans_y(y), __trans_heading(heading), tc)
-
-/* Relative movement */
-void robot_move_by(double distance, struct move_target_heading heading, struct TrajectoryConstraints *tc);
-#define robot_move_by_trans(distance, heading, tc) robot_move_by(distance, __trans_heading(heading), tc)
-
-/* FIXME: not used anywhere */
-static inline void robot_translate_coordinates(double *x, double *y, double *phi)
+class MoveHelper
{
- if (x) {
- *x = __trans_x(*x);
+ struct Trajectory *t;
+ move_target_heading trans_heading(move_target_heading h);
+ public:
+ /* Low-level trajectory handling */
+ MoveHelper& trajectory_new(TrajectoryConstraints *tc, bool backward = false);
+ MoveHelper& trajectory_new_backward(TrajectoryConstraints *tc) {
+ return trajectory_new(tc, true);
}
- if (phi) {
- *phi = __trans_ang(*phi);
+ MoveHelper& add_point_notrans(double x_m, double y_m);
+ MoveHelper& add_final_point_notrans(double x_m, double y_m, move_target_heading heading);
+ MoveHelper& add_point_trans(double x, double y) {
+ return add_point_notrans(trans_x(x), trans_y(y));
}
-}
-
-
-
-#ifdef __cplusplus
-}
-#endif
+ MoveHelper& add_final_point_trans(double x, double y, move_target_heading heading) {
+ return add_final_point_notrans(trans_x(x), trans_y(y), trans_heading(heading));
+ }
+ void send_speed(double left, double right);
+ void goto_notrans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc, bool planning);
+ void move_by(double distance, move_target_heading heading, TrajectoryConstraints *tc);
+ void stop();
+ void goto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
+ goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, true);
+ }
+ void moveto_trans(double x, double y, move_target_heading heading, TrajectoryConstraints *tc) {
+ goto_notrans(trans_x(x), trans_y(y), trans_heading(heading), tc, false);
+ }
+ void move_by_trans(double distance, move_target_heading heading, TrajectoryConstraints *tc) {
+ move_by(distance, trans_heading(heading), tc);
+ }
+ bool get_arrive_from_point(double target_x_m, double target_y_m, move_target_heading heading, double &point_x_m, double &point_y_m);
+};
#endif /* MOVEHELPER_EB2008_H */
projects / eurobot / public.git / blobdiff