From 8470259b1ff3e8ab17af54c2c95288e9fd838ecc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 30 Mar 2013 18:59:06 +0100 Subject: digital/io-hub/src/apbirthday: add US distance sensors control --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/hardware.hh | 2 ++ digital/io-hub/src/apbirthday/hardware.stm32.cc | 2 ++ digital/io-hub/src/apbirthday/robot.cc | 24 ++++++++++++++++++------ digital/io-hub/src/apbirthday/robot.hh | 7 +++++++ 5 files changed, 30 insertions(+), 7 deletions(-) diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 0b570851..17254322 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -12,7 +12,7 @@ apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ $(AVR_SOURCES) MODULES = utils hal/gpio hal/adc hal/uart hal/usb hal/i2c hal/spi \ - base/proto dev/avrisp + base/proto dev/avrisp dev/usdist # Hack mode, include old code from AVR modules. AVR_MODULES = math/geometry math/fixed diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 0cd4c7a5..55caea40 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -83,6 +83,8 @@ struct Hardware gift_out, gift_in, ballon_funny_action, pneum_open; + ucoo::Gpio + dist0_sync, dist1_sync, dist2_sync, dist3_sync; static const int inputs_nb = 9; static const int outputs_nb = 21; #ifdef TARGET_stm32 diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index a6938643..9a46f91e 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -55,6 +55,8 @@ Hardware::Hardware () gift_out (GPIOD, 4), gift_in (GPIOC, 11), ballon_funny_action (GPIOB, 7), pneum_open (GPIOD, 5), + dist0_sync (GPIOC, 0), dist1_sync (GPIOC, 1), + dist2_sync (GPIOC, 2), dist3_sync (GPIOC, 3), adc (0), adc_dist0 (adc, 0), adc_dist1 (adc, 1), adc_dist2 (adc, 2), adc_dist3 (adc, 3), diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 5eff14df..70b70e98 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -39,6 +39,11 @@ Robot::Robot () usb_proto (*this, hardware.usb), chrono (90000 - 1000), pressure (hardware.adc_pressure, hardware.pneum_open, mimot.motor0), + usdist_control_ (2), + usdist0_ (usdist_control_, hardware.adc_dist0, hardware.dist0_sync, 100, 700, 650), + usdist1_ (usdist_control_, hardware.adc_dist1, hardware.dist1_sync, 100, 700, 650), + usdist2_ (usdist_control_, hardware.adc_dist2, hardware.dist2_sync, 100, 700, 650), + usdist3_ (usdist_control_, hardware.adc_dist3, hardware.dist3_sync, 100, 700, 650), candles (1), fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), @@ -47,6 +52,7 @@ Robot::Robot () stats_inputs_ (0), stats_usdist_ (0), stats_pressure_ (0) { robot = this; + // Fill I/O arrays. unsigned i = 0; inputs_[i++] = &hardware.raw_jack; inputs_[i++] = &hardware.ihm_color; @@ -83,6 +89,15 @@ Robot::Robot () ucoo::assert (i == lengthof (outputs_)); for (i = 0; i < lengthof (outputs_); i++) outputs_[i]->output (); + // Set US dist sensors start state. + hardware.dist0_sync.output (); + hardware.dist1_sync.output (); + hardware.dist2_sync.output (); + hardware.dist3_sync.output (); + usdist0_.disable (); + usdist1_.disable (); + usdist2_.disable (); + usdist3_.disable (); } void @@ -93,6 +108,7 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Update IO modules. + usdist_control_.update (); pressure.update (); outputs_set_.update (); // Handle communications. @@ -269,12 +285,8 @@ Robot::proto_stats () } if (stats_usdist_ && !--stats_usdist_cpt_) { - // TODO: this is a hack, only work in simulation. - stats_proto_->send ('U', "HHHH", - hardware.adc_dist0.read (), - hardware.adc_dist1.read (), - hardware.adc_dist2.read (), - hardware.adc_dist3.read ()); + stats_proto_->send ('U', "HHHH", usdist0_.get (), usdist1_.get (), + usdist2_.get (), usdist3_.get ()); stats_usdist_cpt_ = stats_usdist_; } if (stats_pressure_ && !--stats_pressure_cpt_) diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index ceae26b0..aaac185b 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -32,6 +32,7 @@ #include "candles.hh" #include "ucoolib/base/proto/proto.hh" +#include "ucoolib/dev/usdist/usdist.hh" /// Main robot superclass. class Robot : public ucoo::Proto::Handler @@ -68,6 +69,12 @@ class Robot : public ucoo::Proto::Handler Chrono chrono; /// Public access to pressure handling. Pressure pressure; + private: + /// US distance sensors controller. + ucoo::UsDistControl usdist_control_; + /// US distance sensors. + ucoo::UsDist usdist0_, usdist1_, usdist2_, usdist3_; + public: /// Candles. Candles candles; private: -- cgit v1.2.3