src/robofsm: Robot leaves playground wall area without path planning

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 7cc0b3f1bf7f0a720d198fcca835f11543fbf305..658e8e87ab8f239100c16ba79c1d46126b03f5dc 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -167,18 +167,27 @@ double get_approach_angle(double xtarget, double ytarget)
 
 FSM_STATE(go_to_A5)
 {
+        static int is_turned = 0;
+
         switch(FSM_EVENT) {
                 case EV_ENTRY:
                         DBG_PRINT_EVENT("go to A5");
 
                         act_jaws(CLOSE);
-                        // A5
-                        robot_goto_notrans(ROBOT_WIDTH_M / 2, 1.5,
+                        // Go to A5 and leave area with flag wall
+                        robot_move_by(1.5 - ROBOT_START_Y_M,
                                 TURN(DEG2RAD(0)), &tcFast);
                         break;
                 case EV_MOTION_DONE:
-                        DBG_PRINT_EVENT("i am on A5");
-                        FSM_TRANSITION(go_to_D5);
+                        if (is_turned) {
+                                DBG_PRINT_EVENT("i am on A5");
+                                FSM_TRANSITION(go_to_D5);
+                        }
+                        else {
+                                robot_move_by(0.3 - ROBOT_START_X_M,
+                                NO_TURN(), &tcFast);
+                                is_turned = 1;
+                        }
                         break;
                 case EV_RETURN:
                         break;