From 1b0e39a3eeab73de59e5b6fe982bff67021afc78 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 3 Apr 2013 22:13:23 +0200 Subject: digital/io-hub/src/apbirthday: add communication with beacon board --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/hardware.hh | 4 ++-- digital/io-hub/src/apbirthday/hardware.host.cc | 2 +- digital/io-hub/src/apbirthday/hardware.stm32.cc | 8 ++++++- digital/io-hub/src/apbirthday/robot.cc | 31 +++++++++++++++++++++++-- digital/io-hub/src/apbirthday/robot.hh | 9 +++++-- 6 files changed, 47 insertions(+), 9 deletions(-) (limited to 'digital/io-hub/src/apbirthday') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 5611319d..910f1af3 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -4,7 +4,7 @@ TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ simu_report.host.cc zb_avrisp.stm32.cc \ - i2c_queue.cc asserv.cc mimot.cc \ + i2c_queue.cc asserv.cc mimot.cc beacon.cc \ pressure.cc chrono.host.cc chrono.stm32.cc \ radar.cc radar_2013.cc obstacles.cc \ outputs.cc \ diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index dbf98e62..577d5c03 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -58,10 +58,10 @@ struct Hardware #ifdef TARGET_stm32 ucoo::UsbStreamControl usb_control; ucoo::UsbStream usb, zb_usb_avrisp; - ucoo::I2cHard main_i2c; + ucoo::I2cHard main_i2c, zb_i2c; #else ucoo::HostStream usb; - ucoo::I2cHost main_i2c; + ucoo::I2cHost main_i2c, zb_i2c; #endif ucoo::Gpio raw_jack, diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index 2a906864..4a2afbb0 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -31,7 +31,7 @@ HardwareHost::HardwareHost () Hardware::Hardware () : dev_uart (), zb_uart ("zb_uart"), usb ("usb"), - main_i2c (host, 0), + main_i2c (host, 0), zb_i2c (host, 1), raw_jack (host, "raw_jack"), ihm_color (host, "ihm_color"), ihm_strat (host, "ihm_strat"), diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index be0c9729..abbfa477 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -33,7 +33,7 @@ Hardware::Hardware () : dev_uart (4), zb_uart (2), usb_control ("APBTeam", "APBirthday"), usb (usb_control, 0), zb_usb_avrisp (usb_control, 1), - main_i2c (2), + main_i2c (2), zb_i2c (1), raw_jack (GPIOD, 12), ihm_color (GPIOD, 14), ihm_strat (GPIOD, 13), @@ -91,6 +91,12 @@ Hardware::Hardware () gpio_set_af (GPIOA, GPIO_AF4, GPIO8); gpio_set_af (GPIOC, GPIO_AF4, GPIO9); main_i2c.enable (); + // zb_i2c + gpio_mode_setup (GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO10 | GPIO11); + gpio_set_output_options (GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, + GPIO10 | GPIO11); + gpio_set_af (GPIOB, GPIO_AF4, GPIO10 | GPIO11); + zb_i2c.enable (); // ADC. gpio_mode_setup (GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2 | GPIO3 | GPIO6 | GPIO7); diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index f76903f9..ced1b0c8 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -31,9 +31,10 @@ Robot *robot; Robot::Robot () - : main_i2c_queue_ (hardware.main_i2c), + : main_i2c_queue_ (hardware.main_i2c), zb_i2c_queue_ (hardware.zb_i2c), asserv (main_i2c_queue_, BOT_SCALE), mimot (main_i2c_queue_), + beacon (zb_i2c_queue_), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb), @@ -49,7 +50,8 @@ Robot::Robot () fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), - stats_asserv_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), + stats_asserv_ (0), stats_beacon_ (0), + stats_chrono_ (false), stats_chrono_last_s_ (-1), stats_inputs_ (0), stats_usdist_ (0), stats_cake_ (0), stats_pressure_ (0) { robot = this; @@ -109,6 +111,12 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Update IO modules. + if (zb_i2c_queue_.sync ()) + { + Position robot_pos; + asserv.get_position (robot_pos); + beacon.send_position (robot_pos.v); + } if (usdist_control_.update ()) { Position robot_pos; @@ -239,6 +247,14 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size stats_asserv_cpt_ = stats_asserv_ = args[0]; stats_proto_ = &proto; break; + case c ('B', 1): + // Beacon stats. + // 1B: stat interval. + stats_beacon_cpt_ = stats_beacon_ = args[0]; + // TODO: remove this test hack: + beacon.on (args[0] & 1); + stats_proto_ = &proto; + break; case c ('C', 1): // Chrono stats. // 1B: start chrono if non-zero. @@ -305,6 +321,17 @@ Robot::proto_stats () stats_proto_->send ('A', "hhH", pos.v.x, pos.v.y, pos.a); stats_asserv_cpt_ = stats_asserv_; } + if (stats_beacon_ && !--stats_beacon_cpt_) + { + for (int i = 0; i < beacon.pos_nb; i++) + { + vect_t pos; + int trust; + trust = beacon.get_position (i, pos); + stats_proto_->send ('B', "BhhB", i, pos.x, pos.y, trust); + } + stats_beacon_cpt_ = stats_beacon_; + } if (stats_chrono_) { int s = chrono.remaining_time_ms () / 1000; diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index fa2a2185..b9a6d8a6 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 "beacon.hh" #include "fsm_queue.hh" #include "chrono.hh" #include "pressure.hh" @@ -56,13 +57,15 @@ class Robot : public ucoo::Proto::Handler /// Public access to hardware class. Hardware hardware; private: - /// Main I2C queue. - I2cQueue main_i2c_queue_; + /// I2C queues. + I2cQueue main_i2c_queue_, zb_i2c_queue_; public: /// Public access to asserv class. Asserv asserv; /// Public access to mimot class. Mimot mimot; + /// Public access to beacon class. + Beacon beacon; private: /// Proto associated to each serial interface. ucoo::Proto dev_proto, zb_proto, usb_proto; @@ -111,6 +114,8 @@ class Robot : public ucoo::Proto::Handler ucoo::Proto *stats_proto_; /// Asserv stats interval and counter. int stats_asserv_, stats_asserv_cpt_; + /// Beacon stats interval and counter. + int stats_beacon_, stats_beacon_cpt_; /// Enable chrono stats. bool stats_chrono_; /// Last stated second. -- cgit v1.2.3