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/apbirthday/init_defs.hh | 10 +++----- digital/io-hub/src/apbirthday/strat.cc | 10 ++++---- digital/io-hub/src/common-cc/init.cc | 40 +++++++++++++++++++++++++++--- 3 files changed, 45 insertions(+), 15 deletions(-) diff --git a/digital/io-hub/src/apbirthday/init_defs.hh b/digital/io-hub/src/apbirthday/init_defs.hh index ae2099ca..a2468f71 100644 --- a/digital/io-hub/src/apbirthday/init_defs.hh +++ b/digital/io-hub/src/apbirthday/init_defs.hh @@ -29,19 +29,15 @@ Asserv::FORWARD, pg_x (650), pg_y (pg_length - BOT_FRONT_CONTACT_DIST), \ pg_a_deg (90 + BOT_BACK_CONTACT_ANGLE_ERROR_DEG) /// Parameters to go away from the first wall. -#define INIT_FIRST_WALL_AWAY (-(400 - BOT_FRONT_CONTACT_DIST)) +#define INIT_FIRST_WALL_AWAY (-(1000 - BOT_FRONT_CONTACT_DIST)) /// Parameter to face the second wall. #define INIT_SECOND_WALL_ANGLE pg_a_deg (180) /// Parameters to push the second wall. #define INIT_SECOND_WALL_PUSH \ Asserv::FORWARD, pg_x (BOT_FRONT_CONTACT_DIST), -1, -1 +/// Start match next to the wall. +#define INIT_START_SECOND_WALL 1 /// Parameters to go away from the second wall. #define INIT_SECOND_WALL_AWAY (-(200 - BOT_FRONT_CONTACT_DIST)) -/// Parameter to face the start position. -#define INIT_START_POSITION_ANGLE \ - pg_a_deg (-90) -/// Start position. -#define INIT_START_POSITION \ - pg_position_deg (200, pg_length - 600, 0), Asserv::REVERT_OK #endif // init_defs_hh diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index 4eae3825..4fcfb79b 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -39,16 +39,16 @@ Strat::decision (Position &pos) case 0: plate_decision_.drop = false; plate_decision_.approaching_pos = - pg_position_deg (200, 250 + plate_app, 90); - plate_decision_.loading_pos = pg_vect (200, 250 + plate_load); + pg_position_deg (200, 600 + plate_app, 90); + plate_decision_.loading_pos = pg_vect (200, 600 + plate_load); pos = plate_decision_.approaching_pos; return PLATE; case 1: plate_decision_.drop = true; plate_decision_.approaching_pos = - pg_position_deg (200, 1000 - plate_app, -90); - plate_decision_.loading_pos = pg_vect (200, 1000 - plate_load); - pos = pg_position_deg (200, 250 + plate_load, 90); + pg_position_deg (200, 1400 - plate_app, -90); + plate_decision_.loading_pos = pg_vect (200, 1400 - plate_load); + pos = pg_position_deg (200, 600 + plate_load, 90); return PLATE; default: pos.v = pg_cake_pos; 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