From ba68e4ff28fc6e0881d3806960d6ad51a574e086 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 6 May 2013 00:36:26 +0200 Subject: digital/io-hub/src/apbirthday: rework gifts handling --- digital/io-hub/src/apbirthday/bot.hh | 5 ++++ digital/io-hub/src/apbirthday/playground_2013.hh | 2 +- digital/io-hub/src/apbirthday/strat.cc | 6 ++-- digital/io-hub/src/apbirthday/strat.hh | 4 ++- digital/io-hub/src/apbirthday/top.cc | 36 +++++++++++++++++++++--- host/simu/robots/apbirthday/model/gifts_arm.py | 4 ++- 6 files changed, 48 insertions(+), 9 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; } /// diff --git a/host/simu/robots/apbirthday/model/gifts_arm.py b/host/simu/robots/apbirthday/model/gifts_arm.py index 5bbcd817..0f43bc6a 100644 --- a/host/simu/robots/apbirthday/model/gifts_arm.py +++ b/host/simu/robots/apbirthday/model/gifts_arm.py @@ -24,6 +24,7 @@ """APBirthday gifts arm model.""" from utils.observable import Observable from simu.utils.vector import vector +from math import pi class GiftsArm (Observable): @@ -37,7 +38,8 @@ class GiftsArm (Observable): def __arm_notified (self): if self.arm_cyl.pos > .9: push_point = (vector (self.robot_position.pos) - + vector.polar (self.robot_position.angle - 90, 140 + 70)) + + vector.polar (self.robot_position.angle - pi / 2, 140 + 100) + - vector.polar (self.robot_position.angle, 100)) gift = self.table.nearest (push_point, level = 0, max = 150) if gift is not None and hasattr (gift, 'state'): gift.state = True -- cgit v1.2.3