From dff9259a55383a5892987f8a3b640a0c0e26cbfc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 20 Apr 2013 16:48:20 +0200 Subject: digital/io-hub/src/apbirthday: plate loading behaviour --- digital/io-hub/src/apbirthday/bot.hh | 2 + digital/io-hub/src/apbirthday/playground_2013.hh | 3 + digital/io-hub/src/apbirthday/strat.cc | 35 +++++++++- digital/io-hub/src/apbirthday/strat.hh | 20 +++++- digital/io-hub/src/apbirthday/top.cc | 82 +++++++++++++++++++++++- 5 files changed, 136 insertions(+), 6 deletions(-) (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/apbirthday/bot.hh b/digital/io-hub/src/apbirthday/bot.hh index 5817a478..405a8c3c 100644 --- a/digital/io-hub/src/apbirthday/bot.hh +++ b/digital/io-hub/src/apbirthday/bot.hh @@ -55,5 +55,7 @@ #endif /// Normal cruise speed. #define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20 +/// Speed used for plate loading. +#define BOT_SPEED_PLATE 0x20, 0x20, 0x20, 0x20 #endif // bot_hh diff --git a/digital/io-hub/src/apbirthday/playground_2013.hh b/digital/io-hub/src/apbirthday/playground_2013.hh index d0911ba4..2e2224d5 100644 --- a/digital/io-hub/src/apbirthday/playground_2013.hh +++ b/digital/io-hub/src/apbirthday/playground_2013.hh @@ -34,4 +34,7 @@ static const vect_t pg_cake_pos = { pg_width / 2, pg_length }; /// Distance from the cake to blow candles. static const int pg_cake_distance = 30; +/// Plate width / 2. +static const int pg_plate_size_border = 170 / 2; + #endif // playground_2013_hh diff --git a/digital/io-hub/src/apbirthday/strat.cc b/digital/io-hub/src/apbirthday/strat.cc index 328c1298..9950cdc6 100644 --- a/digital/io-hub/src/apbirthday/strat.cc +++ b/digital/io-hub/src/apbirthday/strat.cc @@ -25,13 +25,36 @@ #include "robot.hh" #include "top.hh" #include "debug.host.hh" +#include "bot.hh" Strat::Decision -Strat::decision (vect_t &pos) +Strat::decision (Position &pos) { // TODO: this is a stub. - pos = pg_cake_pos; - return CANDLES; + static int step; + static const int plate_app = pg_plate_size_border + BOT_SIZE_RADIUS + 20; + static const int plate_load = pg_plate_size_border + BOT_SIZE_BACK - 40; + switch (step++) + { + 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); + 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); + return PLATE; + default: + pos.v = pg_cake_pos; + pos.a = 0; + return CANDLES; + } } /// Compute score for candles between first and last. @@ -96,6 +119,12 @@ Strat::decision_candles (CandlesDecision &decision, uint16_t robot_angle) } } +void +Strat::decision_plate (PlateDecision &decision) +{ + decision = plate_decision_; +} + void Strat::failure () { diff --git a/digital/io-hub/src/apbirthday/strat.hh b/digital/io-hub/src/apbirthday/strat.hh index fb1a8bf3..15c6a45d 100644 --- a/digital/io-hub/src/apbirthday/strat.hh +++ b/digital/io-hub/src/apbirthday/strat.hh @@ -33,6 +33,7 @@ class Strat enum Decision { CANDLES, + PLATE, }; /// Information on a candle decision. struct CandlesDecision @@ -42,14 +43,31 @@ class Strat /// Angle relative to cake to end the movement. uint16_t end_angle; }; + /// Information on a plate decision. + struct PlateDecision + { + /// Should drop plate before. + bool drop; + /// Approach position, where the robot should be before starting + /// approaching. + Position approaching_pos; + /// Loading position, point where to go backward to load the plate. If + /// the point is reached, there is no plate. + vect_t loading_pos; + }; public: /// Return new decision and associated position. - Decision decision (vect_t &pos); + Decision decision (Position &pos); /// Take a decision related to candles, return false to give up candles. bool decision_candles (CandlesDecision &decision, uint16_t robot_angle); + /// Take a decision related to plate. + void decision_plate (PlateDecision &decision); /// Report a failure to apply the previous decision. void failure (); + private: + /// Last plate decision. + PlateDecision plate_decision_; }; #endif // strat_hh diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index c96b8c12..28d95811 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -35,6 +35,8 @@ struct top_t Strat::CandlesDecision candles; /// Last blown candles. int candles_last_blown[Candles::FLOOR_NB]; + /// Plate decision information. + Strat::PlateDecision plate; }; static top_t top; @@ -221,6 +223,13 @@ top_fsm_gen_event () } } } + // Plate contacts. + if (!robot->hardware.cherry_plate_left_contact.get () + && !robot->hardware.cherry_plate_right_contact.get ()) + { + if (ANGFSM_HANDLE (AI, top_plate_present)) + return true; + } return false; } @@ -257,6 +266,14 @@ ANGFSM_STATES ( TOP_CANDLES_LEAVE_TURN, // Candles: go away so that the robot is free to turn. TOP_CANDLES_LEAVE_GO_AWAY, + // Plate: go to plate, normal move. + TOP_PLATE_GOTO, + // Plate: go backward until a plate is seen. + TOP_PLATE_APPROACH, + // Plate: loading plate. + TOP_PLATE_LOADING, + // Plate: drop plate. + TOP_PLATE_DROPING, // Demo mode: push the wall near the cake. TOP_DEMO_CANDLES_PUSH_WALL, // Demo mode: move away from the wall. @@ -269,6 +286,8 @@ ANGFSM_EVENTS ( top_follow_finished, // Problem with cake following. top_follow_blocked, + // Plate present, can be taken. + top_plate_present, // Start candle demo. top_demo_candles, // Start follow the cake demo. @@ -293,18 +312,24 @@ FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) FSM_TRANS_TIMEOUT (TOP_DECISION, 1, candles, TOP_CANDLES_GOTO_NORMAL, + plate, TOP_PLATE_GOTO, none, TOP_START) { if (robot->demo) return FSM_BRANCH (none); - vect_t d_pos; + robot->asserv.set_speed (BOT_SPEED_NORMAL); + Position d_pos; Strat::Decision d = robot->strat.decision (d_pos); switch (d) { case Strat::CANDLES: - robot->move.start (d_pos, Asserv::BACKWARD, pg_cake_radius + robot->move.start (d_pos.v, Asserv::BACKWARD, pg_cake_radius + pg_cake_distance + BOT_SIZE_SIDE); return FSM_BRANCH (candles); + case Strat::PLATE: + robot->strat.decision_plate (top.plate); + robot->move.start (d_pos); + return FSM_BRANCH (plate); default: ucoo::assert_unreachable (); } @@ -408,6 +433,59 @@ FSM_TRANS (TOP_CANDLES_LEAVE_GO_AWAY, robot_move_failure, TOP_DECISION) { } +/// +/// Plate. +/// + +FSM_TRANS (TOP_PLATE_GOTO, move_success, + load, TOP_PLATE_APPROACH, + drop, TOP_PLATE_DROPING) +{ + if (top.plate.drop) + { + ANGFSM_HANDLE (AI, plate_drop); + return FSM_BRANCH (drop); + } + else + { + robot->asserv.set_speed (BOT_SPEED_PLATE); + robot->move.start (top.plate.loading_pos, Asserv::BACKWARD); + return FSM_BRANCH (load); + } +} + +FSM_TRANS (TOP_PLATE_GOTO, move_failure, TOP_DECISION) +{ + robot->strat.failure (); +} + +FSM_TRANS (TOP_PLATE_DROPING, plate_droped, TOP_PLATE_GOTO) +{ + top.plate.drop = false; + robot->move.start (top.plate.approaching_pos); +} + +FSM_TRANS (TOP_PLATE_APPROACH, move_success, TOP_DECISION) +{ + // TODO: no plate. + robot->strat.failure (); +} + +FSM_TRANS (TOP_PLATE_APPROACH, move_failure, TOP_DECISION) +{ + robot->strat.failure (); +} + +FSM_TRANS (TOP_PLATE_APPROACH, top_plate_present, TOP_PLATE_LOADING) +{ + robot->move.stop (); + ANGFSM_HANDLE (AI, plate_take); +} + +FSM_TRANS (TOP_PLATE_LOADING, plate_taken, TOP_DECISION) +{ +} + /// /// Demo mode. /// -- cgit v1.2.3