From e4442bb23654930f4bc50ad48082815ee59ba46c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 18:56:37 +0200 Subject: digital/io-hub/src/apbirthday: set firing duration according to plate load --- digital/io-hub/src/apbirthday/cannon.cc | 34 +++++++++++++++++++++++++++------ digital/io-hub/src/apbirthday/cannon.hh | 3 +++ digital/io-hub/src/apbirthday/robot.cc | 2 +- digital/io-hub/src/apbirthday/top.cc | 4 ++-- 4 files changed, 34 insertions(+), 9 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/apbirthday/cannon.cc b/digital/io-hub/src/apbirthday/cannon.cc index eaa82c72..d14af355 100644 --- a/digital/io-hub/src/apbirthday/cannon.cc +++ b/digital/io-hub/src/apbirthday/cannon.cc @@ -30,6 +30,17 @@ Cannon::Cannon () // Init the cannon and the RGB sensor } +void +Cannon::fire (int duration) +{ + if (duration < 1) + duration = 1; + else if (duration > 3) + duration = 3; + duration_ = duration; + ANGFSM_HANDLE (AI, cannon_fire); +} + inline void Cannon::blower_on (int speed) { // Start the blower @@ -121,13 +132,24 @@ FSM_TRANS (CANNON_READY, cannon_fire, CANNON_FIRING) robot->rgb.start_cannon_color (); } -FSM_TRANS_TIMEOUT (CANNON_FIRING, 2500, CANNON_READY) +FSM_TRANS_TIMEOUT (CANNON_FIRING, 1750, + again, CANNON_FIRING, + end, CANNON_READY) { - // Stop the blower - Cannon::blower_off (); - // Stop the RGB sensor - robot->rgb.stop_cannon_color (); - robot->fsm_queue.post (FSM_EVENT (cannon_fire_ok)); + robot->cannon.duration_--; + if (robot->cannon.duration_) + { + return ANGFSM_BRANCH (again); + } + else + { + // Stop the blower + Cannon::blower_off (); + // Stop the RGB sensor + robot->rgb.stop_cannon_color (); + robot->fsm_queue.post (FSM_EVENT (cannon_fire_ok)); + return ANGFSM_BRANCH (end); + } } diff --git a/digital/io-hub/src/apbirthday/cannon.hh b/digital/io-hub/src/apbirthday/cannon.hh index b11b9478..f5b73776 100644 --- a/digital/io-hub/src/apbirthday/cannon.hh +++ b/digital/io-hub/src/apbirthday/cannon.hh @@ -35,10 +35,13 @@ class Cannon POS1 = 0x1290, POS2 = 0x0a00 }; + void fire (int duration = 1); // GPIO manipulation. static void blower_off (); static void blower_on (int speed); static void set_servo_pos (int pos); + public: + int duration_; }; #endif // cannon_hh diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 62285c17..2ca626aa 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -385,7 +385,7 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size break; case c ('v', 0): // Start the cannon - FSM_HANDLE (AI, cannon_fire); + cannon.fire (); break; case c ('A', 1): // Asserv position stats. diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc index 8c401014..d3a39a99 100644 --- a/digital/io-hub/src/apbirthday/top.cc +++ b/digital/io-hub/src/apbirthday/top.cc @@ -663,7 +663,7 @@ FSM_TRANS (TOP_PLATE_LEAVE, move_failure, TOP_DECISION) FSM_TRANS (TOP_CANNON_GOTO, move_success, TOP_CANNON_FIRING) { - ANGFSM_HANDLE (AI, cannon_fire); + robot->cannon.fire (robot->plate.get_plate_nb ()); } FSM_TRANS (TOP_CANNON_GOTO, move_failure, TOP_DECISION) @@ -822,6 +822,6 @@ FSM_TRANS (TOP_DEMO_PLATE_UP, top_demo_plate, TOP_DEMO_PLATE_DOWN) FSM_TRANS (TOP_DEMO_PLATE_DOWN, top_demo_plate, TOP_INIT_ACTUATORS) { - ANGFSM_HANDLE (AI, cannon_fire); + robot->cannon.fire (); } -- cgit v1.2.3