summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-26 23:36:33 +0200
committerNicolas Schodet2013-04-26 23:36:33 +0200
commit566f78ca8597c231838fad52e33761cc718723e4 (patch)
treecb7074f85eb330992521293c238b3d08ff07960f /digital
parent78d2f3d295c56e976c13b187b7723670411e4101 (diff)
digital/io-hub/src/apbirthday: change start sequence
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/init_defs.hh10
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc10
-rw-r--r--digital/io-hub/src/common-cc/init.cc40
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