summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/apbirthday
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-06 00:36:26 +0200
committerNicolas Schodet2013-05-06 00:36:26 +0200
commitba68e4ff28fc6e0881d3806960d6ad51a574e086 (patch)
treed4e01c2bcd774c06938a934b54930eed0d441e82 /digital/io-hub/src/apbirthday
parent891d15dc3f5c28d9b7bbbae3abbd2fcb35e46dd9 (diff)
digital/io-hub/src/apbirthday: rework gifts handling
Diffstat (limited to 'digital/io-hub/src/apbirthday')
-rw-r--r--digital/io-hub/src/apbirthday/bot.hh5
-rw-r--r--digital/io-hub/src/apbirthday/playground_2013.hh2
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc6
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh4
-rw-r--r--digital/io-hub/src/apbirthday/top.cc36
5 files changed, 45 insertions, 8 deletions
diff --git a/digital/io-hub/src/apbirthday/bot.hh b/digital/io-hub/src/apbirthday/bot.hh
index 594b40b7..fed8f20f 100644
--- a/digital/io-hub/src/apbirthday/bot.hh
+++ b/digital/io-hub/src/apbirthday/bot.hh
@@ -57,8 +57,13 @@
#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20
/// Speed used for plate loading.
#define BOT_SPEED_PLATE 0x20, 0x20, 0x20, 0x20
+/// Speed used for gifts.
+#define BOT_SPEED_GIFTS 0x28, 0x30, 0x20, 0x20
/// Normal pneumatic pressure.
#define BOT_NORMAL_PRESSURE 0x0c00
+/// X position of the gift arm when extended.
+static const int bot_gift_arm_x = -100;
+
#endif // bot_hh
diff --git a/digital/io-hub/src/apbirthday/playground_2013.hh b/digital/io-hub/src/apbirthday/playground_2013.hh
index b85585c7..010f5a50 100644
--- a/digital/io-hub/src/apbirthday/playground_2013.hh
+++ b/digital/io-hub/src/apbirthday/playground_2013.hh
@@ -38,7 +38,7 @@ static const int pg_cake_distance = 40;
static const int pg_plate_size_border = 170 / 2;
/// Distance from the border to open gifts.
-static const int pg_gifts_distance = 70;
+static const int pg_gifts_distance = 80;
/// Width of a gift.
static const int pg_gift_width = 176;
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index dd284b5b..1b6430cc 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -50,15 +50,17 @@ Strat::decision (Position &pos)
plate_decision_.loading_pos = pg_vect (200, 1400 - plate_load);
pos = pg_position_deg (200, 600 + plate_load, 90);
return PLATE;
- case 2:
+ case 3:
+ gifts_decision_.go_first = true;
gifts_decision_.begin_pos = (vect_t) { 600, pg_gifts_distance
+ BOT_SIZE_SIDE };
gifts_decision_.end_pos = (vect_t) { 2400, pg_gifts_distance
+ BOT_SIZE_SIDE };
gifts_decision_.dir = Asserv::FORWARD;
- pos.v = gifts_decision_.begin_pos;
+ pos.v = (vect_t) { 900, pg_gifts_distance + BOT_SIZE_SIDE };
pos.a = 0;
return GIFTS;
+ case 2:
default:
pos.v = pg_cake_pos;
pos.a = 0;
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index 4e962824..5dfa0768 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -60,7 +60,9 @@ class Strat
/// Information on a gift decision.
struct GiftsDecision
{
- /// Begin of movement position, same as reported by decision method.
+ /// Need to go to begin_pos first.
+ bool go_first;
+ /// Begin of movement position, only used if go_first.
vect_t begin_pos;
/// End of movement position.
vect_t end_pos;
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index b3a91825..0f4d23a5 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -249,10 +249,11 @@ top_fsm_gen_event ()
if (ANGFSM_CAN_HANDLE (AI, top_gifts_open))
{
Position pos = robot->asserv.get_position ();
+ int arm_x = pos.v.x + bot_gift_arm_x;
for (int i = 0; i < Gifts::nb; i++)
{
if (!robot->gifts.open[i]
- && std::abs (pos.v.x - robot->gifts.x[i]) < 25)
+ && std::abs (arm_x - robot->gifts.x[i]) < pg_gift_width / 2)
{
if (ANGFSM_HANDLE (AI, top_gifts_open))
{
@@ -308,6 +309,8 @@ ANGFSM_STATES (
TOP_PLATE_DROPING,
// Gifts: go to gifts.
TOP_GIFTS_GOTO,
+ // Gifts: go to first gift.
+ TOP_GIFTS_GOTO_FIRST,
// Gifts: going along the wall, opening gifts.
TOP_GIFTS_OPEN,
// Demo mode: push the wall near the cake.
@@ -535,15 +538,40 @@ FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_DECISION)
/// Gifts mode.
///
-FSM_TRANS (TOP_GIFTS_GOTO, move_success, TOP_GIFTS_OPEN)
+FSM_TRANS (TOP_GIFTS_GOTO, move_success,
+ go_first, TOP_GIFTS_GOTO_FIRST,
+ open, TOP_GIFTS_OPEN)
{
- robot->move.start (top.gifts.end_pos, top.gifts.dir);
+ if (top.gifts.go_first)
+ {
+ robot->move.start (top.gifts.begin_pos, Asserv::REVERT_OK);
+ return FSM_BRANCH (go_first);
+ }
+ else
+ {
+ robot->asserv.set_speed (BOT_SPEED_GIFTS);
+ robot->move.start (top.gifts.end_pos, top.gifts.dir);
+ return FSM_BRANCH (open);
+ }
}
FSM_TRANS (TOP_GIFTS_GOTO, move_failure, TOP_DECISION)
{
}
+FSM_TRANS (TOP_GIFTS_GOTO_FIRST, move_success, TOP_GIFTS_OPEN)
+{
+ robot->asserv.set_speed (BOT_SPEED_GIFTS);
+ robot->move.start (top.gifts.end_pos, top.gifts.dir);
+}
+
+FSM_TRANS (TOP_GIFTS_GOTO_FIRST, move_failure, TOP_GIFTS_OPEN)
+{
+ // Continue anyway.
+ robot->asserv.set_speed (BOT_SPEED_GIFTS);
+ robot->move.start (top.gifts.end_pos, top.gifts.dir);
+}
+
FSM_TRANS (TOP_GIFTS_OPEN, move_success, TOP_DECISION)
{
}
@@ -557,7 +585,7 @@ FSM_TRANS (TOP_GIFTS_OPEN, top_gifts_open, TOP_GIFTS_OPEN)
robot->hardware.gift_out.set ();
robot->hardware.gift_in.reset ();
// This is the delay to keep the arm out.
- top.gifts_opening = 75;
+ top.gifts_opening = 250 / 4;
}
///