From 6277a0d5117767ac28e65568a8bde155b776c818 Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Mon, 18 Mar 2013 09:52:08 +0100 Subject: digital/io-hub/src/apbirthday: add candles FSM (need completion) --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/candles.cc | 246 +++++++++++++++++++++++++++++++ digital/io-hub/src/apbirthday/candles.hh | 82 +++++++++++ digital/io-hub/src/apbirthday/robot.cc | 3 +- digital/io-hub/src/apbirthday/robot.hh | 3 + 5 files changed, 334 insertions(+), 2 deletions(-) create mode 100644 digital/io-hub/src/apbirthday/candles.cc create mode 100644 digital/io-hub/src/apbirthday/candles.hh (limited to 'digital/io-hub/src') 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; -- cgit v1.2.3