From 143327bb7e9472691718572243c0f59bb0c02086 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 5 Apr 2013 23:06:50 +0200 Subject: digital/io-hub/src/apbirthday: add jack filtering --- digital/io-hub/src/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/robot.cc | 6 ++-- digital/io-hub/src/apbirthday/robot.hh | 3 ++ digital/io-hub/src/common-cc/debounce.cc | 46 ++++++++++++++++++++++++++++++ digital/io-hub/src/common-cc/debounce.hh | 49 ++++++++++++++++++++++++++++++++ 5 files changed, 103 insertions(+), 3 deletions(-) create mode 100644 digital/io-hub/src/common-cc/debounce.cc create mode 100644 digital/io-hub/src/common-cc/debounce.hh (limited to 'digital') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 9b0438ce..18f10fe2 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -5,7 +5,7 @@ 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 beacon.cc \ - pressure.cc chrono.host.cc chrono.stm32.cc \ + pressure.cc chrono.host.cc chrono.stm32.cc debounce.cc \ radar.cc radar_2013.cc obstacles.cc path.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc \ diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index ced1b0c8..bc0c3552 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -40,6 +40,7 @@ Robot::Robot () usb_proto (*this, hardware.usb), chrono (90000 - 1000), pressure (hardware.adc_pressure, hardware.pneum_open, mimot.motor0), + jack (hardware.raw_jack, 50), 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), @@ -125,6 +126,7 @@ Robot::main_loop () } obstacles.update (); pressure.update (); + jack.update (); outputs_set_.update (); // Handle communications. bool sync = main_i2c_queue_.sync (); @@ -163,8 +165,8 @@ Robot::fsm_gen_event () 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 ()) + // Jack. + if (!jack.get ()) fsm_handle_and_return (jack_inserted); else fsm_handle_and_return (jack_removed); diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index b9a6d8a6..67356694 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -29,6 +29,7 @@ #include "fsm_queue.hh" #include "chrono.hh" #include "pressure.hh" +#include "debounce.hh" #include "outputs.hh" #include "radar_2013.hh" #include "obstacles.hh" @@ -76,6 +77,8 @@ class Robot : public ucoo::Proto::Handler Chrono chrono; /// Public access to pressure handling. Pressure pressure; + /// Jack debouncing. + Debounce jack; private: /// US distance sensors controller. ucoo::UsDistControl usdist_control_; diff --git a/digital/io-hub/src/common-cc/debounce.cc b/digital/io-hub/src/common-cc/debounce.cc new file mode 100644 index 00000000..1ef06282 --- /dev/null +++ b/digital/io-hub/src/common-cc/debounce.cc @@ -0,0 +1,46 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "debounce.hh" + +Debounce::Debounce (ucoo::Io &io, int filter_depth) + : io_ (io), filter_depth_ (filter_depth), state_ (io.get ()), filter_ (0) +{ +} + +void +Debounce::update () +{ + bool now = io_.get (); + // State unchanged, reset filter. + if (now == state_) + filter_ = 0; + // State changed, should be kept stable for filter_depth_ updates until we + // accept it. + else if (filter_++ == filter_depth_) + { + state_ = now; + filter_ = 0; + } +} + diff --git a/digital/io-hub/src/common-cc/debounce.hh b/digital/io-hub/src/common-cc/debounce.hh new file mode 100644 index 00000000..6ce0c606 --- /dev/null +++ b/digital/io-hub/src/common-cc/debounce.hh @@ -0,0 +1,49 @@ +#ifndef debounce_hh +#define debounce_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 Nicolas Schodet +// +// APBTeam: +// Web: http://apbteam.org/ +// Email: team AT apbteam DOT org +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} +#include "ucoolib/intf/io.hh" + +/// Debounce a contact. +class Debounce +{ + public: + /// Constructor. + Debounce (ucoo::Io &io, int filter_depth); + /// To be called at each cycle. + void update (); + /// Get filtered state. + bool get () const { return state_; } + private: + /// Attached input. + ucoo::Io &io_; + /// Filter depth, number of updates for which input should be stable. + int filter_depth_; + /// Current output. + bool state_; + /// Current filter counter. + int filter_; +}; + +#endif // debounce_hh -- cgit v1.2.3