robofsm: MoveHelper __target_heading and target2final_heading

Function __target_heading removed. It was just a constructor so in C++ it is useless.

The another function target2final_heading moved to class MoveHelper.

diff --git a/src/robofsm/movehelper.cc b/src/robofsm/movehelper.cc
index d6d634d5fe6fb54daa31f982f2a37bd61ad70780..38a655dccf00099216a9d54b5ae33e298ecc1f3d 100644 (file)
--- a/src/robofsm/movehelper.cc
+++ b/src/robofsm/movehelper.cc
@@ -236,6 +236,18 @@ move_target_heading MoveHelper::trans_heading(move_target_heading h) {
        }
 }
 
+final_heading MoveHelper::target2final_heading(move_target_heading th){
+       switch (th.operation) {
+               case TOP_DONT_TURN: fh.turn_type = FH_DONT_TURN; break;
+               case TOP_TURN_CW: fh.turn_type = FH_CW; break;
+               case TOP_TURN_CCW: fh.turn_type = FH_CCW; break;
+               case TOP_TURN_SHORTEST: fh.turn_type = FH_SHORTEST; break;
+               case TOP_ARRIVE_FROM: fh.turn_type = FH_DONT_TURN; break;
+       }
+       fh.heading = th.angle;
+       return fh;
+}
+
 double trans_angle(double phi) {
        if (robot.team_color == VIOLET) {
                return phi;

diff --git a/src/robofsm/movehelper.h b/src/robofsm/movehelper.h
index 4ee753e6fe6d7f5ae635725658d08668021cad93..99f47bc74324d48bf879a248d7b27e829bdcf0c7 100644 (file)
--- a/src/robofsm/movehelper.h
+++ b/src/robofsm/movehelper.h
@@ -16,36 +16,17 @@ enum move_target_op {
 struct move_target_heading {
        move_target_op operation;
        float angle;
-       float distance;         /* Distance used by TOP_ARRIVE_FROM */
+       float distance; 
+       /* Distance used by TOP_ARRIVE_FROM */
+       move_target_heading(){}
+       move_target_heading(move_target_op op, float ang, float dist) : operation(op), angle(ang), distance(dist) {}
 };
 
-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;
-       return th;
-}
-
-#define TURN(heading) __target_heading(TOP_TURN_SHORTEST, (heading), 0)
-#define TURN_CW(heading) __target_heading(TOP_TURN_CW, (heading), 0)
-#define TURN_CCW(heading) __target_heading(TOP_TURN_CCW, (heading), 0)
-#define NO_TURN() __target_heading(TOP_DONT_TURN, 0, 0)
-#define ARRIVE_FROM(heading, dist) __target_heading(TOP_ARRIVE_FROM, (heading), (dist))
-
-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;
-               case TOP_TURN_CW: fh.turn_type = FH_CW; break;
-               case TOP_TURN_CCW: fh.turn_type = FH_CCW; break;
-               case TOP_TURN_SHORTEST: fh.turn_type = FH_SHORTEST; break;
-               case TOP_ARRIVE_FROM: fh.turn_type = FH_DONT_TURN; break;
-       }
-       fh.heading = th.angle;
-       return fh;
-}
+#define TURN(heading) move_target_heading(TOP_TURN_SHORTEST, (heading), 0)
+#define TURN_CW(heading) move_target_heading(TOP_TURN_CW, (heading), 0)
+#define TURN_CCW(heading) move_target_heading(TOP_TURN_CCW, (heading), 0)
+#define NO_TURN() move_target_heading(TOP_DONT_TURN, 0, 0)
+#define ARRIVE_FROM(heading, dist) move_target_heading(TOP_ARRIVE_FROM, (heading), (dist))
 
 struct move_target {
        double x, y;
@@ -70,35 +51,38 @@ double trans_y(double y);
 class MoveHelper
 {
        struct Trajectory *t;   
+       final_heading fh;
+       move_target_heading mh;
        move_target_heading trans_heading(move_target_heading h);
-    public:
+public:
        /* Low-level trajectory handling */
        MoveHelper& trajectory_new(TrajectoryConstraints *tc, bool backward = false);
        MoveHelper& trajectory_new_backward(TrajectoryConstraints *tc) {
                return trajectory_new(tc, true);
        }
        MoveHelper& add_point_notrans(double x_m, double y_m);
-       void 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));
        }
+       void add_final_point_notrans(double x_m, double y_m, move_target_heading heading);
        void add_final_point_trans(double x, double y, move_target_heading heading) {
                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(double distance, move_target_heading heading, TrajectoryConstraints *tc);
        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);
+       void stop();
+       final_heading target2final_heading(move_target_heading th);
 };
 /* Represents the target position of a move. */