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.cc21
1 files changed, 20 insertions, 1 deletions
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index a2fcfb89..4464013e 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -24,11 +24,14 @@
#include "robot.hh"
#include "ucoolib/arch/arch.hh"
+#include "ucoolib/utils/bytes.hh"
Robot *robot;
Robot::Robot ()
- : dev_proto (*this, hardware.dev_uart),
+ : main_i2c_queue_ (hardware.main_i2c),
+ asserv (main_i2c_queue_, scale),
+ dev_proto (*this, hardware.dev_uart),
zb_proto (*this, hardware.zb_uart),
usb_proto (*this, hardware.usb)
{
@@ -42,6 +45,8 @@ Robot::main_loop ()
{
// Wait until next cycle.
hardware.wait ();
+ // Handle communications.
+ main_i2c_queue_.sync ();
// Handle commands.
dev_proto.accept ();
zb_proto.accept ();
@@ -66,6 +71,20 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size
// Reset.
ucoo::arch_reset ();
break;
+ case c ('m', 5):
+ // Go to position.
+ // 2H: x, y.
+ // 1B: direction_consign.
+ {
+ vect_t pos = {
+ (int16_t) ucoo::bytes_pack (args[0], args[1]),
+ (int16_t) ucoo::bytes_pack (args[2], args[3]),
+ };
+ asserv.stop ();
+ // TODO: use move FSM.
+ asserv.goto_xy (pos, Asserv::DirectionConsign (args[4]));
+ }
+ break;
default:
proto.send ('?');
return;