From a4ba86186d0888953a1a195f8c38571d40b7b6d1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 23 Mar 2013 08:21:17 +0100 Subject: digital/io-hub/src/apbirthday: add initialisation FSM and deps --- digital/io-hub/src/apbirthday/robot.cc | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src/apbirthday/robot.cc') diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 83a9928d..cfdc0c19 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -23,6 +23,8 @@ // }}} #include "robot.hh" +#include "bot.hh" + #include "ucoolib/arch/arch.hh" #include "ucoolib/utils/bytes.hh" @@ -30,7 +32,7 @@ Robot *robot; Robot::Robot () : main_i2c_queue_ (hardware.main_i2c), - asserv (main_i2c_queue_, scale), + asserv (main_i2c_queue_, BOT_SCALE), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb) @@ -60,6 +62,26 @@ Robot::main_loop () bool Robot::fsm_gen_event () { + // If an event is handled, stop generating any other event, because a + // transition may have invalidated the current robot state. +#define fsm_handle_and_return(event) \ + do { if (ANGFSM_HANDLE (AI, event)) return true; } while (0) + // FSM timeouts. + if (FSM_HANDLE_TIMEOUT (AI)) + return true; + // Motor status. + Motor::Status robot_move_status; + robot_move_status = asserv.get_status (); + if (robot_move_status == Motor::SUCCESS) + fsm_handle_and_return (robot_move_success); + else if (robot_move_status == Motor::FAILURE) + fsm_handle_and_return (robot_move_failure); + // Jack. TODO: bounce filter. + if (!hardware.raw_jack.get ()) + fsm_handle_and_return (jack_inserted); + else + fsm_handle_and_return (jack_removed); + // FSM queue. if (fsm_queue.poll ()) { FsmQueue::Event event = fsm_queue.pop (); -- cgit v1.2.3