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/robot.cc | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) (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 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_) -- cgit v1.2.3