From 566f78ca8597c231838fad52e33761cc718723e4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 26 Apr 2013 23:36:33 +0200 Subject: digital/io-hub/src/apbirthday: change start sequence --- digital/io-hub/src/common-cc/init.cc | 40 +++++++++++++++++++++++++++++++++--- 1 file changed, 37 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/common-cc/init.cc') diff --git a/digital/io-hub/src/common-cc/init.cc b/digital/io-hub/src/common-cc/init.cc index 761e4485..9b3ce7ab 100644 --- a/digital/io-hub/src/common-cc/init.cc +++ b/digital/io-hub/src/common-cc/init.cc @@ -120,13 +120,15 @@ FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 250 / 2, robot->asserv.push_wall (INIT_SECOND_WALL_PUSH); } +#if !INIT_START_SECOND_WALL + FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success, INIT_GOING_AWAY_SECOND_WALL) { robot->asserv.move_distance (INIT_SECOND_WALL_AWAY); } -#ifdef INIT_START_POSITION_ANGLE +# ifdef INIT_START_POSITION_ANGLE FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, INIT_FACING_START_POSITION) { @@ -139,7 +141,7 @@ FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success, robot->asserv.goto_xya (INIT_START_POSITION); } -#else +# else FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, INIT_GOING_TO_START_POSITION) @@ -147,7 +149,7 @@ FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, robot->asserv.goto_xya (INIT_START_POSITION); } -#endif +# endif FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success, INIT_WAITING_SECOND_JACK_OUT) @@ -162,3 +164,35 @@ FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED) robot->fsm_queue.post (FSM_EVENT (init_start_round)); } +#else // INIT_START_SECOND_WALL + +FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success, + INIT_WAITING_SECOND_JACK_OUT) +{ + robot->asserv.stop (); + robot->fsm_queue.post (FSM_EVENT (init_done)); +} + +FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, + INIT_GOING_AWAY_SECOND_WALL) +{ + robot->chrono.start (); + robot->asserv.move_distance (INIT_SECOND_WALL_AWAY); +} + +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_FINISHED) +{ + robot->asserv.set_speed (BOT_SPEED_NORMAL); + robot->fsm_queue.post (FSM_EVENT (init_start_round)); +} + +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_failure, + INIT_FINISHED) +{ + // Well... try to continue. + robot->asserv.set_speed (BOT_SPEED_NORMAL); + robot->fsm_queue.post (FSM_EVENT (init_start_round)); +} + +#endif // INIT_START_SECOND_WALL -- cgit v1.2.3