summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-10 17:22:01 +0200
committerNicolas Schodet2013-05-10 17:22:01 +0200
commit8fb69c363f745129896d211701a16d7ef8f3e82e (patch)
tree28e495c3bd7f0660c7c5e8086ee933dd49370387
parenta21bd34bbc86235bc5bf326dff3cbc7034470e89 (diff)
digital/io-hub/src/apbirthday: pull plates near the borders
-rw-r--r--digital/io-hub/src/apbirthday/plate.cc30
-rw-r--r--digital/io-hub/src/apbirthday/plate.hh4
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc2
-rw-r--r--digital/io-hub/src/apbirthday/strat.cc5
-rw-r--r--digital/io-hub/src/apbirthday/strat.hh2
-rw-r--r--digital/io-hub/src/apbirthday/top.cc32
6 files changed, 68 insertions, 7 deletions
diff --git a/digital/io-hub/src/apbirthday/plate.cc b/digital/io-hub/src/apbirthday/plate.cc
index 3111b749..ba452ae3 100644
--- a/digital/io-hub/src/apbirthday/plate.cc
+++ b/digital/io-hub/src/apbirthday/plate.cc
@@ -31,6 +31,13 @@ Plate::Plate ()
is_up = 0;
}
+void
+Plate::take (bool wait_before_up)
+{
+ wait_before_up_ = wait_before_up;
+ ANGFSM_HANDLE (AI, plate_take);
+}
+
inline void Plate::arm_down ()
{
robot->hardware.cherry_plate_down.set (true);
@@ -63,6 +70,7 @@ FSM_STATES (PLATE_OFF,
PLATE_INIT_DOWNING,
PLATE_READY,
PLATE_TAKE_GLUE,
+ PLATE_TAKE_UPING_WAIT,
PLATE_TAKE_UPING,
PLATE_I_HAZ_PLATE,
PLATE_DROP_DOWNING,
@@ -72,7 +80,9 @@ FSM_STATES (PLATE_OFF,
FSM_EVENTS (plate_take,
plate_taken,
plate_drop,
- plate_droped)
+ plate_droped,
+ plate_waiting,
+ plate_up)
FSM_START_WITH (PLATE_OFF)
@@ -116,7 +126,23 @@ FSM_TRANS (PLATE_READY, plate_take, PLATE_TAKE_GLUE)
Plate::clamp_close ();
}
-FSM_TRANS_TIMEOUT (PLATE_TAKE_GLUE, 100, PLATE_TAKE_UPING)
+FSM_TRANS_TIMEOUT (PLATE_TAKE_GLUE, 100,
+ wait, PLATE_TAKE_UPING_WAIT,
+ nowait, PLATE_TAKE_UPING)
+{
+ if (robot->plate.wait_before_up_)
+ {
+ robot->fsm_queue.post (FSM_EVENT (plate_waiting));
+ return ANGFSM_BRANCH (wait);
+ }
+ else
+ {
+ Plate::arm_up ();
+ return ANGFSM_BRANCH (nowait);
+ }
+}
+
+FSM_TRANS (PLATE_TAKE_UPING_WAIT, plate_up, PLATE_TAKE_UPING)
{
Plate::arm_up ();
}
diff --git a/digital/io-hub/src/apbirthday/plate.hh b/digital/io-hub/src/apbirthday/plate.hh
index 8d913170..80113386 100644
--- a/digital/io-hub/src/apbirthday/plate.hh
+++ b/digital/io-hub/src/apbirthday/plate.hh
@@ -35,6 +35,8 @@ class Plate
void ppp ();
// Read only: 1 if plate is up, 0 otherwhise
int is_up;
+ // Take a plate.
+ void take (bool wait_before_up = false);
// GPIO manipulation.
static void arm_down ();
static void arm_up ();
@@ -42,6 +44,8 @@ class Plate
static void clamp_close ();
private:
int nb_plate;
+ public:
+ bool wait_before_up_;
};
inline int Plate::get_plate_nb ()
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index 460cea9f..62285c17 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -447,7 +447,7 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size
// - 00: up
// - 01: down
if (args[0] == 0)
- FSM_HANDLE (AI, plate_take);
+ plate.take ();
else if (args[0] == 1)
FSM_HANDLE (AI, plate_drop);
break;
diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc
index 5eef1776..bb41ffd6 100644
--- a/digital/io-hub/src/apbirthday/strat.cc
+++ b/digital/io-hub/src/apbirthday/strat.cc
@@ -150,7 +150,7 @@ Strat::score_plate (Position &pos)
{
int plate = -1, score = -1;
bool above = false, below = false;
- bool leave = true;
+ bool leave = true, pull = false;
// Important start points.
if (team_color)
{
@@ -166,6 +166,7 @@ Strat::score_plate (Position &pos)
plate = 0;
above = true;
score = 100000;
+ pull = true;
}
}
else
@@ -176,6 +177,7 @@ Strat::score_plate (Position &pos)
above = true;
score = 100000;
leave = false;
+ pull = true;
}
else if (!plate_visited_[2 + 5])
{
@@ -247,6 +249,7 @@ Strat::score_plate (Position &pos)
plate_decision_.approaching_pos = pos;
plate_decision_.plate = plate;
plate_decision_.leave = leave;
+ plate_decision_.pull = pull;
}
return score;
}
diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh
index 3601f227..095c5859 100644
--- a/digital/io-hub/src/apbirthday/strat.hh
+++ b/digital/io-hub/src/apbirthday/strat.hh
@@ -55,6 +55,8 @@ class Strat
int plate;
/// Leave movement after plate is taken.
bool leave;
+ /// Pull the plate after pickup.
+ bool pull;
/// Approach position, where the robot should be before starting
/// approaching.
Position approaching_pos;
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index b36c8daa..59d2b0c8 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -332,6 +332,10 @@ ANGFSM_STATES (
TOP_PLATE_GOTO,
// Plate: go backward until a plate is seen.
TOP_PLATE_APPROACH,
+ // Plate: wait until clamp closed.
+ TOP_PLATE_WAIT_CLOSE,
+ // Plate: pull the plate.
+ TOP_PLATE_PULLING,
// Plate: loading plate.
TOP_PLATE_LOADING,
// Plate: drop plate.
@@ -590,11 +594,33 @@ FSM_TRANS (TOP_PLATE_APPROACH, move_failure, TOP_DECISION)
robot->strat.failure ();
}
-FSM_TRANS (TOP_PLATE_APPROACH, top_plate_present, TOP_PLATE_LOADING)
+FSM_TRANS (TOP_PLATE_APPROACH, top_plate_present,
+ wait, TOP_PLATE_WAIT_CLOSE,
+ nowait, TOP_PLATE_LOADING)
{
robot->strat.success ();
robot->move.stop ();
- ANGFSM_HANDLE (AI, plate_take);
+ robot->plate.take (top.plate.pull);
+ if (top.plate.pull)
+ return FSM_BRANCH (wait);
+ else
+ return FSM_BRANCH (nowait);
+}
+
+FSM_TRANS (TOP_PLATE_WAIT_CLOSE, plate_waiting, TOP_PLATE_PULLING)
+{
+ robot->asserv.move_distance (30);
+}
+
+FSM_TRANS (TOP_PLATE_PULLING, robot_move_success, TOP_PLATE_LOADING)
+{
+ ANGFSM_HANDLE (AI, plate_up);
+}
+
+FSM_TRANS (TOP_PLATE_PULLING, robot_move_failure, TOP_PLATE_LOADING)
+{
+ // Ignore failure, continue.
+ ANGFSM_HANDLE (AI, plate_up);
}
FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_PLATE_DROPING)
@@ -778,7 +804,7 @@ FSM_TRANS (TOP_DEMO_CANDLE_ARM_NEAR, top_demo_candle_arm, TOP_INIT_ACTUATORS)
FSM_TRANS (TOP_INIT_ACTUATORS, top_demo_plate, TOP_DEMO_PLATE_UP)
{
- ANGFSM_HANDLE (AI, plate_take);
+ robot->plate.take ();
}
FSM_TRANS (TOP_DEMO_PLATE_UP, top_demo_plate, TOP_DEMO_PLATE_DOWN)