summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/apbirthday/Makefile2
-rw-r--r--digital/io-hub/src/apbirthday/candles.cc246
-rw-r--r--digital/io-hub/src/apbirthday/candles.hh82
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc3
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh3
5 files changed, 334 insertions, 2 deletions
diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile
index 02cd0d4e..f9aa26ed 100644
--- a/digital/io-hub/src/apbirthday/Makefile
+++ b/digital/io-hub/src/apbirthday/Makefile
@@ -6,7 +6,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \
zb_avrisp.stm32.cc \
i2c_queue.cc asserv.cc chrono.host.cc chrono.stm32.cc \
outputs.cc \
- top.cc init.cc move.cc \
+ top.cc init.cc move.cc candles.cc \
angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \
$(AVR_SOURCES)
diff --git a/digital/io-hub/src/apbirthday/candles.cc b/digital/io-hub/src/apbirthday/candles.cc
new file mode 100644
index 00000000..793053aa
--- /dev/null
+++ b/digital/io-hub/src/apbirthday/candles.cc
@@ -0,0 +1,246 @@
+// 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 "candles.hh"
+
+extern "C" {
+#define ANGFSM_NAME AI
+#include "angfsm.h"
+}
+
+Candles::Candles (int calif_mode)
+{
+ int i;
+ /* Init candles color. */
+ for (i = 0; i < CANDLES_NB; i++)
+ {
+ color[i] = CANDLE_UNKNOWN;
+ state[i] = CANDLE_UNPUNCHED;
+ }
+ color[0] = CANDLE_BLUE;
+ color[8] = CANDLE_BLUE;
+ color[7] = CANDLE_RED;
+ color[19] = CANDLE_RED;
+ if (calif_mode)
+ {
+ color[12] = CANDLE_WHITE;
+ color[13] = CANDLE_WHITE;
+ color[14] = CANDLE_WHITE;
+ color[15] = CANDLE_WHITE;
+ }
+ actual_pos[FLOOR_NEAR] = CANDLES_NB;
+ actual_pos[FLOOR_FAR] = CANDLES_NB;
+}
+
+void Candles::blow (int candle)
+{
+ if (CANDLE_IS_FAR(candle))
+ actual_pos[FLOOR_FAR] = candle;
+ else
+ actual_pos[FLOOR_NEAR] = candle;
+ deduce ();
+ robot->fsm_queue.post (FSM_EVENT (ai_candle_blow));
+}
+
+void Candles::deduce ()
+{
+ int i;
+ /* Far. */
+ for (i = 1; i < 4; i++)
+ if (color[i] != color[i + 3])
+ {
+ if (color[i] == CANDLE_RED)
+ color[i + 3] = CANDLE_BLUE;
+ else if (color[i] == CANDLE_BLUE)
+ color[i + 3] = CANDLE_RED;
+ else if (color[i + 3] == CANDLE_RED)
+ color[i] = CANDLE_BLUE;
+ else if (color[i + 3] == CANDLE_BLUE)
+ color[i] = CANDLE_RED;
+ }
+ /* Near. */
+ for (i = 9; i < 14; i++)
+ if (color[i] != color[i + 5])
+ {
+ if (color[i] == CANDLE_RED)
+ color[i + 5] = CANDLE_BLUE;
+ else if (color[i] == CANDLE_BLUE)
+ color[i + 5] = CANDLE_RED;
+ else if (color[i + 5] == CANDLE_RED)
+ color[i] = CANDLE_BLUE;
+ else if (color[i + 5] == CANDLE_BLUE)
+ color[i] = CANDLE_RED;
+ }
+}
+
+/* Global candle FSM */
+FSM_STATES (AI_CANDLE_SLEEPING,
+ AI_CANDLE_READY,
+ AI_CANDLE_UNDEPLOYING)
+
+FSM_EVENTS (ai_candle_deploy,
+ ai_candle_undeploy,
+ ai_candle_blow)
+
+FSM_START_WITH (AI_CANDLE_SLEEPING)
+
+FSM_TRANS (AI_CANDLE_SLEEPING, ai_candle_deploy, AI_CANDLE_READY)
+{
+ /* TODO: piston arm IN */
+ /* TODO: piston far OUT */
+}
+
+FSM_TRANS (AI_CANDLE_READY, ai_candle_blow, AI_CANDLE_READY)
+{
+ int i;
+ for (i = 0; i < CANDLE_FLOOR_NB; i++)
+ {
+ if (robot->candles.actual_pos[i] != CANDLE_FLOOR_NB)
+ {
+ /* We can already punch if we know the color. */
+ if (robot->candles.state[robot->candles.actual_pos[i]] == CANDLE_UNPUNCHED
+ && (robot->candles.color[robot->candles.actual_pos[i]] == (candle_color_t) team_color
+ || robot->candles.color[robot->candles.actual_pos[i]] == CANDLE_WHITE))
+ {
+ if (CANDLE_IS_FAR (robot->candles.actual_pos[i]))
+ FSM_HANDLE (AI, ai_candle_far_punch);
+ else
+ FSM_HANDLE (AI, ai_candle_near_punch);
+ robot->candles.state[robot->candles.actual_pos[i]] = CANDLE_PUNCHED;
+ robot->candles.actual_pos[i] = CANDLE_FLOOR_NB;
+ }
+ /* We need to analyse color. */
+ else if (robot->candles.color[robot->candles.actual_pos[i]] == CANDLE_UNKNOWN)
+ {
+ if (CANDLE_IS_FAR (robot->candles.actual_pos[i]))
+ FSM_HANDLE (AI, ai_candle_far_analyse);
+ else
+ FSM_HANDLE (AI, ai_candle_near_analyse);
+ }
+ }
+ }
+}
+
+FSM_TRANS (AI_CANDLE_READY, ai_candle_undeploy, AI_CANDLE_UNDEPLOYING)
+{
+ /* TODO: piston far OUT */
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_UNDEPLOYING, 10, AI_CANDLE_SLEEPING) //TODO timeout value
+{
+ /* TODO: piston far unleach */
+ /* TODO: piston arm IN */
+}
+
+/* Far puncher FSM */
+FSM_STATES (AI_CANDLE_FAR_SLEEPING,
+ AI_CANDLE_FAR_PUNCHING)
+
+FSM_EVENTS (ai_candle_far_punch)
+
+FSM_START_WITH (AI_CANDLE_FAR_SLEEPING)
+
+FSM_TRANS (AI_CANDLE_FAR_SLEEPING, ai_candle_far_punch, AI_CANDLE_FAR_PUNCHING)
+{
+ /* TODO: piston far OUT */
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_PUNCHING, 12, AI_CANDLE_FAR_SLEEPING) //TODO timeout value
+{
+ /* TODO: piston far OUT */
+}
+
+/* Near puncher FSM */
+FSM_STATES (AI_CANDLE_NEAR_SLEEPING,
+ AI_CANDLE_NEAR_PUNCHING)
+
+FSM_EVENTS (ai_candle_near_punch)
+
+FSM_START_WITH (AI_CANDLE_NEAR_SLEEPING)
+
+FSM_TRANS (AI_CANDLE_NEAR_SLEEPING, ai_candle_near_punch, AI_CANDLE_NEAR_PUNCHING)
+{
+ /* TODO: piston near OUT */
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_PUNCHING, 12, AI_CANDLE_NEAR_SLEEPING) //TODO timeout value
+{
+ /* TODO: piston near OUT */
+}
+
+/* Far analyse FSM */
+FSM_STATES (AI_CANDLE_FAR_ANALYSE_SLEEP,
+ AI_CANDLE_FAR_ANALYSING)
+
+FSM_EVENTS (ai_candle_far_analyse)
+
+FSM_START_WITH (AI_CANDLE_FAR_ANALYSE_SLEEP)
+
+FSM_TRANS (AI_CANDLE_FAR_ANALYSE_SLEEP,
+ ai_candle_far_analyse,
+ AI_CANDLE_FAR_ANALYSING)
+{
+ /* TODO: launch color analyse */
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_FAR_ANALYSING, 10, AI_CANDLE_FAR_ANALYSE_SLEEP) //TODO timeout value
+{
+ /* TODO Get color results and update table. */
+ /* ... */
+ /* Update color. */
+ robot->candles.color[robot->candles.actual_pos[FLOOR_FAR]] = CANDLE_RED; // TODO = color_result
+ /* Update whole colors. */
+ robot->candles.deduce ();
+ /* Send blow event. */
+ FSM_HANDLE (AI, ai_candle_blow);
+}
+
+/* Near analyse FSM */
+FSM_STATES (AI_CANDLE_NEAR_ANALYSE_SLEEP,
+ AI_CANDLE_NEAR_ANALYSING)
+
+FSM_EVENTS (ai_candle_near_analyse)
+
+FSM_START_WITH (AI_CANDLE_NEAR_ANALYSE_SLEEP)
+
+FSM_TRANS (AI_CANDLE_NEAR_ANALYSE_SLEEP,
+ ai_candle_near_analyse,
+ AI_CANDLE_NEAR_ANALYSING)
+{
+ /* TODO: Launch color analyse. */
+}
+
+FSM_TRANS_TIMEOUT (AI_CANDLE_NEAR_ANALYSING, 10, AI_CANDLE_NEAR_ANALYSE_SLEEP) //TODO timeout value
+{
+ /* TODO Get color results and update table. */
+ /* ... */
+ /* Update color. */
+ robot->candles.color[robot->candles.actual_pos[FLOOR_NEAR]] = CANDLE_RED; // TODO
+ /* Update whole colors. */
+ robot->candles.deduce ();
+ /* Send blow event. */
+ FSM_HANDLE (AI, ai_candle_blow);
+}
diff --git a/digital/io-hub/src/apbirthday/candles.hh b/digital/io-hub/src/apbirthday/candles.hh
new file mode 100644
index 00000000..04345400
--- /dev/null
+++ b/digital/io-hub/src/apbirthday/candles.hh
@@ -0,0 +1,82 @@
+#ifndef candles_hh
+#define candles_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.
+//
+// }}}
+
+#define CANDLE_FAR 8
+#define CANDLE_NEAR 12
+#define CANDLES_NB (CANDLE_NEAR + CANDLE_FAR)
+#define CANDLE_IS_FAR(c) ((c) < CANDLE_FAR)
+#define CANDLE_IS_NEAR(c) ((c) >= CANDLE_FAR)
+
+/* The cake is a lie !
+
+-----------------------------------------------------------
+- 8 0 7 19 -
+ - -
+ - 9 1 6 18 -
+ - -
+ - 10 2 5 17 -
+ - 3 4 -
+ - 11 16 -
+ - 12 13 14 15 -
+ - ______ -
+*/
+
+typedef enum
+{
+ CANDLE_UNPUNCHED,
+ CANDLE_PUNCHED,
+ CANDLE_STATE_NB
+} candle_state_t;
+
+typedef enum
+{
+ FLOOR_NEAR = 0,
+ FLOOR_FAR = 1,
+ CANDLE_FLOOR_NB
+} candle_floor_t;
+
+/* XXX use another common type for colors ? */
+typedef enum
+{
+ CANDLE_RED = TEAM_COLOR_RIGHT,
+ CANDLE_BLUE = TEAM_COLOR_LEFT,
+ CANDLE_WHITE,
+ CANDLE_UNKNOWN,
+} candle_color_t;
+
+class Candles
+{
+ public:
+ candle_state_t state[CANDLES_NB];
+ candle_color_t color[CANDLES_NB];
+ Candles (int calif_mode);
+ void blow (int candle);
+ void deduce ();
+ int actual_pos[CANDLE_FLOOR_NB];
+};
+
+#endif // candles_hh
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index d8b47387..8eebad49 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -31,7 +31,8 @@
Robot *robot;
Robot::Robot ()
- : main_i2c_queue_ (hardware.main_i2c),
+ : candles (1),
+ main_i2c_queue_ (hardware.main_i2c),
asserv (main_i2c_queue_, BOT_SCALE),
dev_proto (*this, hardware.dev_uart),
zb_proto (*this, hardware.zb_uart),
diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh
index 492b6e2d..11ab416a 100644
--- a/digital/io-hub/src/apbirthday/robot.hh
+++ b/digital/io-hub/src/apbirthday/robot.hh
@@ -28,6 +28,7 @@
#include "fsm_queue.hh"
#include "chrono.hh"
#include "outputs.hh"
+#include "candles.hh"
#include "ucoolib/base/proto/proto.hh"
@@ -45,6 +46,8 @@ class Robot : public ucoo::Proto::Handler
void proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size);
/// Send stats.
void proto_stats ();
+ /// Candles.
+ Candles candles;
public:
/// Public access to hardware class.
Hardware hardware;