From ade5fda0a16b77de980daff51bba8d92f122cb38 Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Thu, 11 Apr 2013 00:22:09 +0200 Subject: digital/io-hub/src/apbirthday: add drinks FSM --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/drinks.cc | 190 ++++++++++++++++++++++++++++++++ digital/io-hub/src/apbirthday/drinks.hh | 48 ++++++++ digital/io-hub/src/apbirthday/robot.hh | 3 + 4 files changed, 242 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/apbirthday/drinks.cc create mode 100644 digital/io-hub/src/apbirthday/drinks.hh (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 968c563a..6b87fb4c 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 \ + top.cc init.cc move.cc candles.cc drinks.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ $(AVR_SOURCES) diff --git a/digital/io-hub/src/apbirthday/drinks.cc b/digital/io-hub/src/apbirthday/drinks.cc new file mode 100644 index 00000000..c75bde39 --- /dev/null +++ b/digital/io-hub/src/apbirthday/drinks.cc @@ -0,0 +1,190 @@ +// 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 "drinks.hh" + +extern "C" { +#define ANGFSM_NAME AI +#include "angfsm.h" +} + +Drinks::Drinks () +{ + nb_ = 0; +} + +inline int Drinks::nb () +{ + return nb_; +} + +inline void Drinks::add () +{ + robot->drinks.nb_++; +} + +inline void Drinks::clear () +{ + robot->drinks.nb_ = 0; +} + +inline void Drinks::upper_open () +{ + robot->hardware.glass_upper_clamp_close.set (false); + robot->hardware.glass_upper_clamp_open.set (true); +} + +inline void Drinks::upper_close () +{ + robot->hardware.glass_upper_clamp_close.set (true); + robot->hardware.glass_upper_clamp_open.set (false); +} + +inline void Drinks::upper_up () +{ + robot->hardware.glass_upper_clamp_up.set (true); + robot->hardware.glass_upper_clamp_down.set (false); +} + +inline void Drinks::upper_down () +{ + robot->hardware.glass_upper_clamp_up.set (false); + robot->hardware.glass_upper_clamp_down.set (true); +} + +inline void Drinks::upper_unleach () +{ + robot->hardware.glass_upper_clamp_up.set (false); + robot->hardware.glass_upper_clamp_down.set (false); +} + +inline void Drinks::lower_open () +{ + robot->hardware.glass_lower_clamp_close.set (false); + robot->hardware.glass_lower_clamp_open.set (true); +} + +inline void Drinks::lower_close () +{ + robot->hardware.glass_lower_clamp_close.set (true); + robot->hardware.glass_lower_clamp_open.set (false); +} + +// Near analyse FSM. +FSM_STATES (DRINKS_OFF, + DRINKS_INIT_PREPARING, + DRINKS_INIT_UPING, + DRINKS_INIT_DOWNING, + DRINKS_READY, + DRINKS_TAKE_GLUE, + DRINKS_TAKE_DOWNING, + DRINKS_TAKE_FIXING_UP, + DRINKS_SERVE_SERVING, + DRINKS_SERVE_LIBERATING + ) + +FSM_EVENTS (drinks_take, + drinks_taken, + drinks_serve, + drinks_served) + +FSM_START_WITH (DRINKS_OFF) + +FSM_TRANS (DRINKS_OFF, init_actuators, DRINKS_INIT_PREPARING) +{ + Drinks::upper_down (); + Drinks::upper_close (); + Drinks::lower_close (); +} + +FSM_TRANS_TIMEOUT (DRINKS_INIT_PREPARING, 100, DRINKS_INIT_UPING) +{ + Drinks::upper_up (); + Drinks::lower_open (); +} + +FSM_TRANS_TIMEOUT (DRINKS_INIT_UPING, 100, DRINKS_INIT_DOWNING) +{ + Drinks::upper_down (); +} + +FSM_TRANS_TIMEOUT (DRINKS_INIT_DOWNING, 100, DRINKS_READY) +{ + Drinks::upper_open (); +} + +FSM_TRANS (DRINKS_READY, drinks_take, + no_drinks, DRINKS_TAKE_DOWNING, + has_drinks, DRINKS_TAKE_GLUE, + too_much_drinks, DRINKS_READY) +{ + Drinks::lower_close (); + int nb = robot->drinks.nb (); + if (nb == 0) + return FSM_BRANCH (no_drinks); + else if (nb == 4) + return FSM_BRANCH (too_much_drinks); + else + return FSM_BRANCH (has_drinks); +} + +FSM_TRANS_TIMEOUT (DRINKS_TAKE_GLUE, 100, DRINKS_TAKE_DOWNING) +{ + Drinks::upper_unleach (); + Drinks::upper_down (); +} + +FSM_TRANS_TIMEOUT (DRINKS_TAKE_DOWNING, 100, DRINKS_TAKE_FIXING_UP) +{ + Drinks::upper_close (); +} + +FSM_TRANS_TIMEOUT (DRINKS_TAKE_FIXING_UP, 100, DRINKS_READY) +{ + Drinks::upper_up (); + if (robot->drinks.nb () < 4) + Drinks::lower_open (); + robot->drinks.add (); + robot->fsm_queue.post (FSM_EVENT (drinks_taken)); +} + +FSM_TRANS (DRINKS_READY, drinks_serve, DRINKS_SERVE_SERVING) +{ + Drinks::upper_down (); + Drinks::lower_close (); +} + +FSM_TRANS_TIMEOUT (DRINKS_SERVE_SERVING, 100, DRINKS_SERVE_LIBERATING) +{ + Drinks::upper_open (); + Drinks::lower_open (); +} + +FSM_TRANS_TIMEOUT (DRINKS_SERVE_LIBERATING, 100, DRINKS_READY) +{ + robot->drinks.clear (); + robot->fsm_queue.post (FSM_EVENT (drinks_served)); +} diff --git a/digital/io-hub/src/apbirthday/drinks.hh b/digital/io-hub/src/apbirthday/drinks.hh new file mode 100644 index 00000000..ab41e288 --- /dev/null +++ b/digital/io-hub/src/apbirthday/drinks.hh @@ -0,0 +1,48 @@ +#ifndef drinks_hh +#define drinks_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 Drinks +{ + public: + Drinks (); + int nb (); + static void add (); + static void clear (); + // GPIO manipulation. + static void upper_open (); + static void upper_close (); + static void upper_up (); + static void upper_down (); + static void upper_unleach (); + static void lower_open (); + static void lower_close (); + private: + static const int total_drinks = 12; + int nb_; +}; + +#endif // drinks_hh diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index 5ad6418d..95cf16c3 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -38,6 +38,7 @@ #include "path.hh" #include "strat.hh" #include "candles.hh" +#include "drinks.hh" #include "ucoolib/base/proto/proto.hh" #include "ucoolib/dev/usdist/usdist.hh" @@ -101,6 +102,8 @@ class Robot : public ucoo::Proto::Handler Strat strat; /// Candles. Candles candles; + /// Drinks. + Drinks drinks; private: /// FSM debug mode. enum FsmDebugState -- cgit v1.2.3