From 052b8cb32effc9de99276f967c2fad42b1f75301 Mon Sep 17 00:00:00 2001 From: Matous Pokorny Date: Fri, 7 Jun 2013 12:17:57 +0200 Subject: [PATCH] src/robofsm: Robot leaves playground wall area without path planning --- src/robofsm/common-states.cc | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index 7cc0b3f1..658e8e87 100644 --- 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; -- 2.39.2