From 23cfdec0c1178a5020594165ce90526ca179ee3b Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Wed, 24 Apr 2013 00:22:33 +0200 Subject: digital/io-hub/src/apbirthday: First crude cannon fsm No action currently implemented. --- digital/io-hub/src/apbirthday/cannon.cc | 175 ++++++++++++++++++++++++++++++++ 1 file changed, 175 insertions(+) create mode 100644 digital/io-hub/src/apbirthday/cannon.cc (limited to 'digital/io-hub/src/apbirthday/cannon.cc') diff --git a/digital/io-hub/src/apbirthday/cannon.cc b/digital/io-hub/src/apbirthday/cannon.cc new file mode 100644 index 00000000..5af0c850 --- /dev/null +++ b/digital/io-hub/src/apbirthday/cannon.cc @@ -0,0 +1,175 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Maxime Hadjinlian +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "robot.hh" +#include "defs.hh" +#include "cannon.hh" + +Cannon::Cannon () +{ + // Init the cannon and the RGB sensor +} + +inline void Cannon::blower_on () +{ + // Start the blower +} + +inline void Cannon::blower_off () +{ + // Shutdown the blower +} + +inline void Cannon::set_servo_pos (int pos) +{ + // Switch the servo to BLOCK, POS1 or POS2 +} + +inline void Cannon::set_router_state (int state) +{ + // Set router to OPEN or CLOSE +} + +// Trap FSM +FSM_STATES (CANNON_TRAP_OFF, + CANNON_TRAP_BLOCK, + CANNON_TRAP_MOVE_1, + CANNON_TRAP_MOVE_2) + +FSM_EVENTS (cannon_fire, + cannon_fire_ok) + +FSM_START_WITH (CANNON_TRAP_OFF) + +FSM_TRANS (CANNON_TRAP_OFF, init_actuators, CANNON_TRAP_BLOCK) +{ + Cannon::set_servo_pos (Cannon::BLOCK); +} + +FSM_TRANS (CANNON_TRAP_BLOCK, cannon_fire, CANNON_TRAP_MOVE_1) +{ + Cannon::set_servo_pos (Cannon::POS1); +} + +FSM_TRANS_TIMEOUT (CANNON_TRAP_MOVE_1, 75, CANNON_TRAP_MOVE_2) +{ + Cannon::set_servo_pos (Cannon::POS2); +} + +FSM_TRANS_TIMEOUT (CANNON_TRAP_MOVE_2, 75, CANNON_TRAP_MOVE_1) +{ + Cannon::set_servo_pos (Cannon::POS1); +} + +FSM_TRANS (CANNON_TRAP_MOVE_1, cannon_fire_ok, CANNON_TRAP_BLOCK) +{ + Cannon::set_servo_pos (Cannon::BLOCK); +} + +FSM_TRANS (CANNON_TRAP_MOVE_2, cannon_fire_ok, CANNON_TRAP_BLOCK) +{ + Cannon::set_servo_pos (Cannon::BLOCK); +} + + +// Router FSM + +FSM_STATES (CANNON_ROUTER_OFF, + CANNON_ROUTER_SLEEPING, + CANNON_ROUTER_TRACKING, + CANNON_ROUTER_PUSHING) + +FSM_START_WITH (CANNON_ROUTER_OFF) + +FSM_TRANS (CANNON_ROUTER_OFF, init_actuators, CANNON_ROUTER_SLEEPING) +{ + robot->cannon.set_router_state (Cannon::OPEN); +} + +FSM_TRANS (CANNON_ROUTER_SLEEPING, cannon_fire, CANNON_ROUTER_TRACKING) +{ + // We do nothing here +} + +FSM_TRANS (CANNON_ROUTER_TRACKING, cannon_fire_ok, CANNON_ROUTER_SLEEPING) +{ + // We do nothing here. +} + +FSM_TRANS_TIMEOUT (CANNON_ROUTER_TRACKING, 250, + nothing_to_push, CANNON_ROUTER_TRACKING, + destroy, CANNON_ROUTER_PUSHING) +{ + // Check if we have something to push or not + // return FSM_BRANCH(nothing_to_push); + return FSM_BRANCH(destroy); + // If we have something to push, push next event +} + +FSM_TRANS_TIMEOUT (CANNON_ROUTER_PUSHING, 250, CANNON_ROUTER_TRACKING) +{ + // Open the router after the timeout + robot->cannon.set_router_state (Cannon::OPEN); +} + +FSM_TRANS (CANNON_ROUTER_PUSHING, cannon_fire_ok, CANNON_ROUTER_SLEEPING) +{ + // Open router + robot->cannon.set_router_state (Cannon::OPEN); +} + +// Cannon main FSM + +FSM_STATES (CANNON_OFF, + CANNON_PURGING, + CANNON_READY, + CANNON_FIRING) + +FSM_START_WITH (CANNON_OFF) + +FSM_TRANS (CANNON_OFF, init_actuators, CANNON_PURGING) +{ + // Start the blower to purge the canon +} + +FSM_TRANS_TIMEOUT (CANNON_PURGING, 500, CANNON_READY) +{ + // Stop the blower +} + +FSM_TRANS (CANNON_READY, cannon_fire, CANNON_FIRING) +{ + // Start the blower + Cannon::blower_on (); + // Start RGB sensor +} + +FSM_TRANS_TIMEOUT (CANNON_FIRING, 1250, CANNON_READY) +{ + // Stop the blower + Cannon::blower_off (); + // Stop the RGB sensor + robot->fsm_queue.post (FSM_EVENT (cannon_fire_ok)); +} + + -- cgit v1.2.3