From 0c5ba0b879baa5291d35dd0a5028ab9a5d52560d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 21 Mar 2013 21:20:18 +0100 Subject: digital/io-hub/src/apbirthday: add FSM queue --- digital/io-hub/src/apbirthday/Makefile | 4 +- digital/io-hub/src/apbirthday/robot.cc | 16 ++++++-- digital/io-hub/src/apbirthday/robot.hh | 6 ++- digital/io-hub/src/common-cc/fsm_queue.cc | 50 +++++++++++++++++++++++++ digital/io-hub/src/common-cc/fsm_queue.hh | 62 +++++++++++++++++++++++++++++++ 5 files changed, 131 insertions(+), 7 deletions(-) create mode 100644 digital/io-hub/src/common-cc/fsm_queue.cc create mode 100644 digital/io-hub/src/common-cc/fsm_queue.hh diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 89458a08..362e7476 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -5,7 +5,7 @@ PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ i2c_queue.cc asserv.cc \ top.cc \ - angfsm.host.c angfsm_gen_arm_AI.arm.c \ + angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ $(AVR_SOURCES) MODULES = utils hal/gpio hal/uart hal/usb hal/i2c base/proto @@ -21,7 +21,7 @@ AVR_SOURCES = $(foreach module,$(AVR_MODULES),$($(subst /,_,$(module))_SOURCES)) # FSM. INCLUDES += -I$(BASE)/../ai/src/fsm -I. -vpath %.c $(BASE)/../ai/src/fsm +vpath angfsm.host.c $(BASE)/../ai/src/fsm # Common files. INCLUDES += -I../common-cc diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 4464013e..83a9928d 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -46,19 +46,27 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Handle communications. - main_i2c_queue_.sync (); + bool sync = main_i2c_queue_.sync (); + // Handle events if synchronised. + if (sync) + fsm_gen_event (); // Handle commands. dev_proto.accept (); zb_proto.accept (); usb_proto.accept (); - // Handle events. - fsm_gen_event (); } } -void +bool Robot::fsm_gen_event () { + if (fsm_queue.poll ()) + { + FsmQueue::Event event = fsm_queue.pop (); + if (ANGFSM_HANDLE_VAR (AI, event)) + return true; + } + return false; } void diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index e0b50f41..d9590e59 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -25,6 +25,7 @@ // }}} #include "hardware.hh" #include "asserv.hh" +#include "fsm_queue.hh" #include "ucoolib/base/proto/proto.hh" @@ -40,7 +41,7 @@ class Robot : public ucoo::Proto::Handler /// Main program loop. void main_loop (); /// Generate events for the FSM. - void fsm_gen_event (); + bool fsm_gen_event (); /// Receive proto messages. void proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size); public: @@ -55,6 +56,9 @@ class Robot : public ucoo::Proto::Handler private: /// Proto associated to each serial interface. ucoo::Proto dev_proto, zb_proto, usb_proto; + public: + /// Public access to deferred events FSM queue. + FsmQueue fsm_queue; }; /// Global instance pointer. diff --git a/digital/io-hub/src/common-cc/fsm_queue.cc b/digital/io-hub/src/common-cc/fsm_queue.cc new file mode 100644 index 00000000..816d836d --- /dev/null +++ b/digital/io-hub/src/common-cc/fsm_queue.cc @@ -0,0 +1,50 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// 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 "fsm_queue.hh" + +FsmQueue::FsmQueue () + : head_ (0), nb_ (0) +{ +} + +void +FsmQueue::post (Event e) +{ + ucoo::assert (nb_ < queue_size_); + int tail = (head_ + nb_) % queue_size_; + events_[tail] = e; + nb_++; +} + +FsmQueue::Event +FsmQueue::pop () +{ + Event e; + ucoo::assert (nb_ > 0); + e = events_[head_]; + nb_--; + head_ = (head_ + 1) % queue_size_; + return e; +} + diff --git a/digital/io-hub/src/common-cc/fsm_queue.hh b/digital/io-hub/src/common-cc/fsm_queue.hh new file mode 100644 index 00000000..333bea80 --- /dev/null +++ b/digital/io-hub/src/common-cc/fsm_queue.hh @@ -0,0 +1,62 @@ +#ifndef fsm_queue_hh +#define fsm_queue_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// 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 "ucoolib/common.hh" + +extern "C" { +#define ANGFSM_NAME AI +#include "angfsm.h" +} + +/// Handle asynchronous FSM event queue. +class FsmQueue +{ + public: +#ifdef TARGET_host + typedef uint16_t Event; +#else + typedef angfsm_AI_event_t Event; +#endif + public: + /// Constructor. + FsmQueue (); + /// Post an event to be processed in the next main loop. + void post (Event e); + /// Poll for event (return true if there is an event pending). + bool poll () const { return nb_ != 0; } + /// Pop one event from the queue. + Event pop (); + private: + /// Maximum number of events in the queue. + static const int queue_size_ = 8; + /// Events to post to the FSM in next iteration. + Event events_[queue_size_]; + /// First event in the queue. + int head_; + /// Number of events in the queue. + int nb_; +}; + +#endif // fsm_queue_hh -- cgit v1.2.3