summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/Makefile2
-rw-r--r--digital/io-hub/src/apbirthday/drinks.cc190
-rw-r--r--digital/io-hub/src/apbirthday/drinks.hh48
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh3
4 files changed, 242 insertions, 1 deletions
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