From 3959c59fc8daa26757e31e01b415966027642673 Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Thu, 11 Apr 2013 12:28:32 +0200 Subject: digital/io-hub/src/apbirthday: add plate FSM --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/plate.cc | 140 +++++++++++++++++++++++++++++++++ digital/io-hub/src/apbirthday/plate.hh | 38 +++++++++ digital/io-hub/src/apbirthday/robot.hh | 3 + 4 files changed, 182 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/apbirthday/plate.cc create mode 100644 digital/io-hub/src/apbirthday/plate.hh (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 6b87fb4c..19296e0f 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -9,7 +9,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ radar.cc radar_2013.cc obstacles.cc path.cc strat.cc \ outputs.cc \ - top.cc init.cc move.cc candles.cc drinks.cc \ + top.cc init.cc move.cc candles.cc drinks.cc plate.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ $(AVR_SOURCES) diff --git a/digital/io-hub/src/apbirthday/plate.cc b/digital/io-hub/src/apbirthday/plate.cc new file mode 100644 index 00000000..46918753 --- /dev/null +++ b/digital/io-hub/src/apbirthday/plate.cc @@ -0,0 +1,140 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Jerome Jutteau +// +// 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 "plate.hh" + +extern "C" { +#define ANGFSM_NAME AI +#include "angfsm.h" +} + +inline void Plate::arm_down () +{ + robot->hardware.cherry_plate_down.set (true); + robot->hardware.cherry_plate_up.set (false); +} + +inline void Plate::arm_up () +{ + robot->hardware.cherry_plate_down.set (false); + robot->hardware.cherry_plate_up.set (true); +} + +inline void Plate::clamp_open () +{ + robot->hardware.cherry_plate_clamp.set (true); +} + +inline void Plate::clamp_close () +{ + robot->hardware.cherry_plate_clamp.set (false); +} + +FSM_STATES (PLATE_OFF, + PLATE_INIT_PREPARE, + PLATE_INIT_TAKING, + PLATE_INIT_UPING, + PLATE_INIT_DOWNING, + PLATE_READY, + PLATE_TAKE_GLUE, + PLATE_TAKE_UPING, + PLATE_I_HAZ_PLATE, + PLATE_DROP_DOWNING, + PLATE_DROP_OPENING + ) + +FSM_EVENTS (plate_take, + plate_taken, + plate_drop, + plate_droped) + +FSM_START_WITH (PLATE_OFF) + +FSM_TRANS (PLATE_OFF, init_actuators, PLATE_INIT_PREPARE) +{ + Plate::arm_down (); + Plate::clamp_open (); +} + +FSM_TRANS_TIMEOUT (PLATE_INIT_PREPARE, 100, PLATE_INIT_TAKING) +{ + Plate::clamp_close (); +} + +FSM_TRANS_TIMEOUT (PLATE_INIT_TAKING, 100, PLATE_INIT_UPING) +{ + Plate::arm_up (); +} + +FSM_TRANS_TIMEOUT (PLATE_INIT_UPING, 100, PLATE_INIT_DOWNING) +{ + Plate::arm_down (); +} + +FSM_TRANS_TIMEOUT (PLATE_INIT_DOWNING, 100, PLATE_READY) +{ + Plate::clamp_open (); +} + +FSM_TRANS (PLATE_READY, plate_drop, PLATE_READY) +{ + robot->fsm_queue.post (FSM_EVENT (plate_droped)); +} + +FSM_TRANS (PLATE_READY, plate_take, PLATE_TAKE_GLUE) +{ + Plate::clamp_close (); +} + +FSM_TRANS_TIMEOUT (PLATE_TAKE_GLUE, 100, PLATE_TAKE_UPING) +{ + Plate::arm_up (); +} + +FSM_TRANS_TIMEOUT (PLATE_TAKE_UPING, 100, PLATE_I_HAZ_PLATE) +{ + robot->fsm_queue.post (FSM_EVENT (plate_taken)); +} + +FSM_TRANS (PLATE_I_HAZ_PLATE, plate_take, PLATE_I_HAZ_PLATE) +{ + robot->fsm_queue.post (FSM_EVENT (plate_taken)); +} + +FSM_TRANS (PLATE_I_HAZ_PLATE, plate_drop, PLATE_DROP_DOWNING) +{ + Plate::arm_down (); +} + +FSM_TRANS_TIMEOUT (PLATE_DROP_DOWNING, 100, PLATE_DROP_OPENING) +{ + Plate::clamp_open (); +} + +FSM_TRANS_TIMEOUT (PLATE_DROP_OPENING, 100, PLATE_READY) +{ + robot->fsm_queue.post (FSM_EVENT (plate_droped)); +} diff --git a/digital/io-hub/src/apbirthday/plate.hh b/digital/io-hub/src/apbirthday/plate.hh new file mode 100644 index 00000000..2740b001 --- /dev/null +++ b/digital/io-hub/src/apbirthday/plate.hh @@ -0,0 +1,38 @@ +#ifndef plate_hh +#define plate_hh + +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Jerome Jutteau +// +// 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. +// +// }}} + +class Plate +{ + public: + // GPIO manipulation. + static void arm_down (); + static void arm_up (); + static void clamp_open (); + static void clamp_close (); +}; + +#endif // plate_hh diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 95cf16c3..30dabc90 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -39,6 +39,7 @@ #include "strat.hh" #include "candles.hh" #include "drinks.hh" +#include "plate.hh" #include "ucoolib/base/proto/proto.hh" #include "ucoolib/dev/usdist/usdist.hh" @@ -104,6 +105,8 @@ class Robot : public ucoo::Proto::Handler Candles candles; /// Drinks. Drinks drinks; + /// Plate. + Plate plate; private: /// FSM debug mode. enum FsmDebugState -- cgit v1.2.3