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/drinks.cc | 190 ++++++++++++++++++++++++++++++++ 1 file changed, 190 insertions(+) create mode 100644 digital/io-hub/src/apbirthday/drinks.cc (limited to 'digital/io-hub/src/apbirthday/drinks.cc') 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)); +} -- cgit v1.2.3