From 8fb69c363f745129896d211701a16d7ef8f3e82e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 17:22:01 +0200 Subject: digital/io-hub/src/apbirthday: pull plates near the borders --- digital/io-hub/src/apbirthday/plate.cc | 30 ++++++++++++++++++++++++++++-- digital/io-hub/src/apbirthday/plate.hh | 4 ++++ digital/io-hub/src/apbirthday/robot.cc | 2 +- digital/io-hub/src/apbirthday/strat.cc | 5 ++++- digital/io-hub/src/apbirthday/strat.hh | 2 ++ digital/io-hub/src/apbirthday/top.cc | 32 +++++++++++++++++++++++++++++--- 6 files changed, 68 insertions(+), 7 deletions(-) (limited to 'digital') 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) -- cgit v1.2.3