summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/apbirthday/robot.cc
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io-hub/src/apbirthday/robot.cc')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc24
1 files changed, 23 insertions, 1 deletions
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 ();