From f7f01cbf53034fb656572758c22f2f392c01290f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 29 Mar 2013 23:29:57 +0100 Subject: digital/io-hub/src/apbirthday: add mimot & pressure sensor --- digital/io-hub/src/apbirthday/Makefile | 3 +- digital/io-hub/src/apbirthday/hardware.hh | 2 + digital/io-hub/src/apbirthday/hardware.host.cc | 3 +- digital/io-hub/src/apbirthday/hardware.stm32.cc | 4 +- digital/io-hub/src/apbirthday/robot.cc | 21 ++++- digital/io-hub/src/apbirthday/robot.hh | 7 ++ digital/io-hub/src/common-cc/mimot.cc | 76 +++++++++++++++++ digital/io-hub/src/common-cc/mimot.hh | 91 ++++++++++++++++++++ digital/io-hub/src/common-cc/pressure.cc | 107 ++++++++++++++++++++++++ digital/io-hub/src/common-cc/pressure.hh | 69 +++++++++++++++ 10 files changed, 379 insertions(+), 4 deletions(-) create mode 100644 digital/io-hub/src/common-cc/mimot.cc create mode 100644 digital/io-hub/src/common-cc/mimot.hh create mode 100644 digital/io-hub/src/common-cc/pressure.cc create mode 100644 digital/io-hub/src/common-cc/pressure.hh (limited to 'digital') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index f9aa26ed..a785d171 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -4,7 +4,8 @@ TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ zb_avrisp.stm32.cc \ - i2c_queue.cc asserv.cc chrono.host.cc chrono.stm32.cc \ + i2c_queue.cc asserv.cc mimot.cc \ + pressure.cc chrono.host.cc chrono.stm32.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 5713f921..0cd4c7a5 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -88,8 +88,10 @@ struct Hardware #ifdef TARGET_stm32 ucoo::AdcHard adc; ucoo::AdcHardChannel adc_dist0, adc_dist1, adc_dist2, adc_dist3; + ucoo::AdcHardChannel adc_pressure; #else ucoo::AdcHost adc_dist0, adc_dist1, adc_dist2, adc_dist3; + ucoo::AdcHost adc_pressure; #endif Hardware (); // Wait until next cycle. diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index 1cffaa7e..a53962b7 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -65,7 +65,8 @@ Hardware::Hardware () adc_dist0 (host, "dist0", 1 << 12), adc_dist1 (host, "dist1", 1 << 12), adc_dist2 (host, "dist2", 1 << 12), - adc_dist3 (host, "dist3", 1 << 12) + adc_dist3 (host, "dist3", 1 << 12), + adc_pressure (host, "pressure", 1 << 12) { dev_uart.block (false); zb_uart.block (false); diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index ed051a4a..a6938643 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -57,7 +57,8 @@ Hardware::Hardware () pneum_open (GPIOD, 5), adc (0), adc_dist0 (adc, 0), adc_dist1 (adc, 1), - adc_dist2 (adc, 2), adc_dist3 (adc, 3) + adc_dist2 (adc, 2), adc_dist3 (adc, 3), + adc_pressure (adc, 8) { rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPAEN); rcc_peripheral_enable_clock (&RCC_AHB1ENR, RCC_AHB1ENR_IOPBEN); @@ -90,6 +91,7 @@ Hardware::Hardware () // ADC. gpio_mode_setup (GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2 | GPIO3); + gpio_mode_setup (GPIOB, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO5); // Cycle timer, 4 ms period. rcc_peripheral_enable_clock (&RCC_APB1ENR, RCC_APB1ENR_TIM3EN); TIM3_PSC = 2 * rcc_ppre1_frequency / 1000000 - 1; // 1 µs prescaler diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 6e63b96a..5eff14df 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -33,16 +33,18 @@ Robot *robot; Robot::Robot () : main_i2c_queue_ (hardware.main_i2c), asserv (main_i2c_queue_, BOT_SCALE), + mimot (main_i2c_queue_), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb), chrono (90000 - 1000), + pressure (hardware.adc_pressure, hardware.pneum_open, mimot.motor0), candles (1), fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), - stats_inputs_ (0), stats_usdist_ (0) + stats_inputs_ (0), stats_usdist_ (0), stats_pressure_ (0) { robot = this; unsigned i = 0; @@ -91,6 +93,7 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Update IO modules. + pressure.update (); outputs_set_.update (); // Handle communications. bool sync = main_i2c_queue_.sync (); @@ -172,6 +175,11 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size asserv.goto_xy (pos, Asserv::DirectionConsign (args[4])); } break; + case c ('f', 2): + // Set low pressure threshold. + // 1w: sensor value, 4096 is full scale. + pressure.set (ucoo::bytes_pack (args[0], args[1])); + break; case c ('o', 5): // Set/clear/toggle outputs. // - 1d: mask. @@ -220,6 +228,12 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size stats_usdist_cpt_ = stats_usdist_ = args[0]; stats_proto_ = &proto; break; + case c ('F', 1): + // Pressure stats. + // 1B: stat interval. + stats_pressure_cpt_ = stats_pressure_ = args[0]; + stats_proto_ = &proto; + break; default: proto.send ('?'); return; @@ -263,5 +277,10 @@ Robot::proto_stats () hardware.adc_dist3.read ()); stats_usdist_cpt_ = stats_usdist_; } + if (stats_pressure_ && !--stats_pressure_cpt_) + { + stats_proto_->send ('F', "H", pressure.get ()); + stats_pressure_cpt_ = stats_pressure_; + } } diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index c447953e..ceae26b0 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -27,6 +27,7 @@ #include "asserv.hh" #include "fsm_queue.hh" #include "chrono.hh" +#include "pressure.hh" #include "outputs.hh" #include "candles.hh" @@ -55,6 +56,8 @@ class Robot : public ucoo::Proto::Handler public: /// Public access to asserv class. Asserv asserv; + /// Public access to mimot class. + Mimot mimot; private: /// Proto associated to each serial interface. ucoo::Proto dev_proto, zb_proto, usb_proto; @@ -63,6 +66,8 @@ class Robot : public ucoo::Proto::Handler FsmQueue fsm_queue; /// Public access to chrono. Chrono chrono; + /// Public access to pressure handling. + Pressure pressure; /// Candles. Candles candles; private: @@ -93,6 +98,8 @@ class Robot : public ucoo::Proto::Handler int stats_inputs_, stats_inputs_cpt_; /// US distance sensors stats interval and counter. int stats_usdist_, stats_usdist_cpt_; + /// Pressure stats interval and counter. + int stats_pressure_, stats_pressure_cpt_; }; /// Global instance pointer. diff --git a/digital/io-hub/src/common-cc/mimot.cc b/digital/io-hub/src/common-cc/mimot.cc new file mode 100644 index 00000000..febe3362 --- /dev/null +++ b/digital/io-hub/src/common-cc/mimot.cc @@ -0,0 +1,76 @@ +// 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 "mimot.hh" + +#include "ucoolib/utils/bytes.hh" + +Mimot::Mimot (I2cQueue &queue) + : I2cQueue::Slave (queue, 0x06, 6), + motor0 (*this, 0), motor1 (*this, 1), + status_flag_ (0), input_port_ (0) +{ +} + +void +Mimot::recv_status (const uint8_t *status) +{ + status_flag_ = status[0]; + input_port_ = status[1]; + motor0.position_ = ucoo::bytes_pack (status[2], status[3]); + motor1.position_ = ucoo::bytes_pack (status[4], status[5]); +} + +void +Mimot::Motor::move (uint16_t position, uint16_t speed) +{ + uint8_t buf[] = { (char) ('b' + index_), + ucoo::bytes_unpack (position, 1), + ucoo::bytes_unpack (position, 0), + ucoo::bytes_unpack (speed, 1), + ucoo::bytes_unpack (speed, 0), + }; + mimot_.send (buf, sizeof (buf)); +} + +void +Mimot::Motor::free () +{ + uint8_t buf[] = { 'w', + (uint8_t) index_, + 0, // brake + }; + mimot_.send (buf, sizeof (buf)); +} + +void +Mimot::Motor::output_set (int16_t pwm) +{ + uint8_t buf[] = { 'W', + (uint8_t) index_, + ucoo::bytes_unpack (pwm, 1), + ucoo::bytes_unpack (pwm, 0), + }; + mimot_.send (buf, sizeof (buf)); +} + diff --git a/digital/io-hub/src/common-cc/mimot.hh b/digital/io-hub/src/common-cc/mimot.hh new file mode 100644 index 00000000..f9703dda --- /dev/null +++ b/digital/io-hub/src/common-cc/mimot.hh @@ -0,0 +1,91 @@ +#ifndef mimot_hh +#define mimot_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 "i2c_queue.hh" +#include "motor.hh" + +/// Interface to mimot board. +class Mimot : public I2cQueue::Slave +{ + public: + static const int aux_nb = 2; + public: + /// Constructor. + Mimot (I2cQueue &queue); + /// See I2cQueue::Slave::recv_status. + void recv_status (const uint8_t *status); + public: + /// Mimot single output. + class Motor + { + public: + /// Maximum positive value for PWM. + static const int16_t pwm_max = 0x3ff; + public: + /// Contructor. + Motor (Mimot &mimot, int index) : mimot_ (mimot), index_ (index) { } + /// Get last command status. + ::Motor::Status get_status () const; + /// Get current shaft position. + uint16_t get_position () const; + /// Move to absolute position at given speed. + void move (uint16_t position, uint16_t speed); + /// Set motor to free running, no torque. + void free (); + /// Set raw output. + void output_set (int16_t pwm); + private: + Mimot &mimot_; + int index_; + uint16_t position_; + friend class Mimot; + }; + friend class Motor; + /// Motors public access. + Motor motor0, motor1; + private: + uint8_t status_flag_; + uint8_t input_port_; +}; + +inline Motor::Status +Mimot::Motor::get_status () const +{ + uint8_t status = mimot_.status_flag_ >> (index_ * 2); + if (status & 1) + return ::Motor::SUCCESS; + else if (status & 2) + return ::Motor::FAILURE; + else + return ::Motor::RUNNING; +} + +inline uint16_t +Mimot::Motor::get_position () const +{ + return position_; +} + +#endif // mimot_hh diff --git a/digital/io-hub/src/common-cc/pressure.cc b/digital/io-hub/src/common-cc/pressure.cc new file mode 100644 index 00000000..c258b165 --- /dev/null +++ b/digital/io-hub/src/common-cc/pressure.cc @@ -0,0 +1,107 @@ +// 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 "pressure.hh" + +#include + +Pressure::Pressure (ucoo::Adc &sensor, ucoo::Io &pneum_open, + Mimot::Motor &pump) + : sensor_ (sensor), pneum_open_ (pneum_open), pump_ (pump), + state_ (STATE_INITIAL), wait_ (0), low_ (0) +{ +} + +void +Pressure::set (int low) +{ + low_ = std::min (low, max_ - margin_); +} + +int +Pressure::get () const +{ + return sensor_.read (); +} + +void +Pressure::update () +{ + // Interval at which the sensor is read. + static const int interval = 25; + // Rest duration after a pumping session. + static const int rest = 250; + // Wait duration before the pump is started. + static const int open_wait = 125; + // Wait duration after the pump has been started before circuit is closed. + static const int start_wait = 50; + if (wait_) + { + wait_--; + } + else + { + switch (state_) + { + case STATE_INITIAL: + if (low_) + { + // Open circuit. + pneum_open_.set (); + state_ = STATE_IDLE; + wait_ = open_wait; + } + break; + case STATE_IDLE: + if (get () < low_) + { + // Start pump. + pump_.output_set (Mimot::Motor::pwm_max); + state_ = STATE_STARTING; + wait_ = start_wait; + } + else + wait_ = interval; + break; + case STATE_STARTING: + // Started, close circuit. + pneum_open_.reset (); + state_ = STATE_PUMPING; + break; + case STATE_PUMPING: + // Pump until high. + if (get () > low_ + margin_) + { + // Done. Stop. Open circuit. + pump_.free (); + pneum_open_.set (); + state_ = STATE_IDLE; + wait_ = rest; + } + else + wait_ = interval; + break; + } + } +} + diff --git a/digital/io-hub/src/common-cc/pressure.hh b/digital/io-hub/src/common-cc/pressure.hh new file mode 100644 index 00000000..32c10fc1 --- /dev/null +++ b/digital/io-hub/src/common-cc/pressure.hh @@ -0,0 +1,69 @@ +#ifndef pressure_hh +#define pressure_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/adc.hh" +#include "ucoolib/intf/io.hh" +#include "mimot.hh" + +/// Handle pressure sensor and pump. +class Pressure +{ + public: + /// Constructor. + Pressure (ucoo::Adc &sensor, ucoo::Io &pneum_open, Mimot::Motor &pump); + /// Set desired pressure (set low threshold). + void set (int low); + /// Get current pressure. + int get () const; + /// To be called at each cycle. + void update (); + private: + ucoo::Adc &sensor_; + ucoo::Io &pneum_open_; + Mimot::Motor &pump_; + /// Maximum pressure. + static const int max_ = 4000; + /// Hysteresis margin. + static const int margin_ = max_ / 16; + /// Pressure FSM state. + enum State + { + /// Start state, circuit is closed, no pressure. + STATE_INITIAL, + /// Idle, pressure is OK, circuit is open. + STATE_IDLE, + /// Starting the pump, with open circuit. + STATE_STARTING, + /// Pumping until the pressure is OK, circuit is closed. + STATE_PUMPING, + }; + State state_; + /// Wait counter before next action. + int wait_; + /// Pressure sensor low threshold. + int low_; +}; + +#endif // pressure_hh -- cgit v1.2.3