From 0572f1537a8de90f45789c59e83dba7fcfa5fbb0 Mon Sep 17 00:00:00 2001 From: Mourad Latoundji Date: Thu, 21 Jul 2022 12:27:52 +0200 Subject: [PATCH] :art: (libs): Update TouchSensorKit --- libs/TouchSensorKit/include/TouchSensorKit.h | 44 ++++++----- .../include/internal/TouchSensorSystem.h | 57 +++++++------- libs/TouchSensorKit/source/TouchSensorKit.cpp | 76 ++++++++++--------- .../tests/TouchSensorKit_test.cpp | 7 +- 4 files changed, 94 insertions(+), 90 deletions(-) diff --git a/libs/TouchSensorKit/include/TouchSensorKit.h b/libs/TouchSensorKit/include/TouchSensorKit.h index 5cecab3072..de4d8208d0 100644 --- a/libs/TouchSensorKit/include/TouchSensorKit.h +++ b/libs/TouchSensorKit/include/TouchSensorKit.h @@ -14,37 +14,41 @@ enum class Position { ear_left, ear_right, - belt_front_left, - belt_front_right, - belt_back_left, - belt_back_right, + belt_left_back, + belt_left_front, + belt_right_back, + belt_right_front, +}; + +inline const auto positions = std::array { + Position::ear_left, Position::ear_right, Position::belt_left_back, + Position::belt_left_front, Position::belt_right_back, Position::belt_right_front, }; class TouchSensorKit { public: explicit TouchSensorKit() = default; - void setup(); + void init(); - void updateState(); - void printState(); + [[nodiscard]] auto isTouched(Position position) -> bool; + [[nodiscard]] auto isTouchedAny() -> bool; + void setPowerMode(Position position, PowerMode power_mode); void resetByPowerMode(); - void adjustSensitivity(uint16_t value, bool saved = false); + void setSensitivity(Position position, uint16_t value, bool saved = false); + + void registerOnSensorTouched(std::function const &on_sensor_touched_callback); + + static constexpr uint16_t default_max_sensitivity_value {0x0FFF}; + static constexpr uint16_t default_min_sensitivity_value {0x0000}; private: - CoreTouchSensor _sensor_ear_left {ear_left_in, _ear_left_pm, dac_touch_left, Channel::A}; - CoreTouchSensor _sensor_ear_right {ear_right_in, _ear_right_pm, dac_touch_right, ear_right_ch}; - CoreTouchSensor _sensor_belt_left_back {belt_left_back_in, _belt_left_back_pm, dac_touch_left, belt_left_back_ch}; - CoreTouchSensor _sensor_belt_left_front {belt_left_front_in, _belt_left_front_pm, dac_touch_left, - belt_left_front_ch}; - CoreTouchSensor _sensor_belt_right_back {belt_right_back_in, _belt_right_back_pm, dac_touch_right, - belt_right_back_ch}; - CoreTouchSensor _sensor_belt_right_front {belt_right_front_in, _belt_right_front_pm, dac_touch_right, - belt_right_front_ch}; - - uint16_t default_max_sensitivity_value {0x0FFF}; - uint16_t default_min_sensitivity_value {0x0000}; + std::array _sensors = {sensor_ear_left, sensor_ear_right, sensor_belt_left_back, + sensor_belt_left_front, sensor_belt_right_back, sensor_belt_right_front}; + + // struct State{} _state; + std::function _on_sensor_touched_callback {}; }; } // namespace leka diff --git a/libs/TouchSensorKit/include/internal/TouchSensorSystem.h b/libs/TouchSensorKit/include/internal/TouchSensorSystem.h index 04cd78c4b2..634bf3afcc 100644 --- a/libs/TouchSensorKit/include/internal/TouchSensorSystem.h +++ b/libs/TouchSensorKit/include/internal/TouchSensorSystem.h @@ -13,32 +13,35 @@ namespace leka { -auto corei2c = CoreI2C {PinName::SENSOR_PROXIMITY_MUX_I2C_SDA, PinName::SENSOR_PROXIMITY_MUX_I2C_SCL}; -auto io_expander_reset = mbed::DigitalOut {PinName::SENSOR_PROXIMITY_MUX_RESET, 0}; -auto io_expander = CoreIOExpanderMCP23017 {corei2c, io_expander_reset}; - -auto ear_left_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB5}; -auto ear_right_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB4}; -auto belt_left_back_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB3}; -auto belt_left_front_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB2}; -auto belt_right_back_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB1}; -auto belt_right_front_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB0}; - -auto ear_left_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA5}; -auto ear_right_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA4}; -auto belt_left_back_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA3}; -auto belt_left_front_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA2}; -auto belt_right_back_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA1}; -auto belt_right_front_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA0}; - -auto dac_touch_left = CoreQDACMCP4728 {corei2c, 0xC0}; -auto dac_touch_right = CoreQDACMCP4728 {corei2c, 0xC2}; - -auto ear_left_ch = Channel::A; -auto ear_right_ch = Channel::A; -auto belt_left_back_ch = Channel::B; -auto belt_left_front_ch = Channel::B; -auto belt_right_back_ch = Channel::C; -auto belt_right_front_ch = Channel::C; +inline auto corei2c = CoreI2C {PinName::SENSOR_PROXIMITY_MUX_I2C_SDA, PinName::SENSOR_PROXIMITY_MUX_I2C_SCL}; +inline auto io_expander_reset = mbed::DigitalOut {PinName::SENSOR_PROXIMITY_MUX_RESET, 0}; +inline auto io_expander = CoreIOExpanderMCP23017 {corei2c, io_expander_reset}; + +inline auto ear_left_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB5}; +inline auto ear_right_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB4}; +inline auto belt_left_back_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB3}; +inline auto belt_left_front_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB2}; +inline auto belt_right_back_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB1}; +inline auto belt_right_front_in = io::expanded::DigitalIn<> {io_expander, mcp23017::pin::PB0}; + +inline auto ear_left_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA5}; +inline auto ear_right_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA4}; +inline auto belt_left_back_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA3}; +inline auto belt_left_front_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA2}; +inline auto belt_right_back_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA1}; +inline auto belt_right_front_pm = io::expanded::DigitalOut<> {io_expander, mcp23017::pin::PA0}; + +inline auto dac_touch_left = CoreQDACMCP4728 {corei2c, 0xC0}; +inline auto dac_touch_right = CoreQDACMCP4728 {corei2c, 0xC2}; + +inline auto sensor_ear_left = CoreTouchSensor {ear_left_in, ear_left_pm, dac_touch_left, Channel::C}; +inline auto sensor_ear_right = CoreTouchSensor {ear_right_in, ear_right_pm, dac_touch_right, Channel::C}; +inline auto sensor_belt_left_back = CoreTouchSensor {belt_left_back_in, belt_left_back_pm, dac_touch_left, Channel::B}; +inline auto sensor_belt_left_front = + CoreTouchSensor {belt_left_front_in, belt_left_front_pm, dac_touch_left, Channel::A}; +inline auto sensor_belt_right_back = + CoreTouchSensor {belt_right_back_in, belt_right_back_pm, dac_touch_right, Channel::B}; +inline auto sensor_belt_right_front = + CoreTouchSensor {belt_right_front_in, belt_right_front_pm, dac_touch_right, Channel::A}; } // namespace leka diff --git a/libs/TouchSensorKit/source/TouchSensorKit.cpp b/libs/TouchSensorKit/source/TouchSensorKit.cpp index c6f56cbeff..8c9a4f4dad 100644 --- a/libs/TouchSensorKit/source/TouchSensorKit.cpp +++ b/libs/TouchSensorKit/source/TouchSensorKit.cpp @@ -9,59 +9,61 @@ using namespace leka; using namespace std::chrono_literals; -void TouchSensorKit::setup() +void TouchSensorKit::init() { - setPowerMode(touch::power_mode::normal); + for (Position position: positions) { + setPowerMode(position, PowerMode::normal); + } } -void TouchSensorKit::updateState() +auto TouchSensorKit::isTouched(Position position) -> bool { - _state.ear_left_touched = (0 == _sensor_ear_left.read()); - _state.ear_right_touched = (0 == _sensor_ear_right.read()); - _state.belt_left_back_touched = (0 == _sensor_belt_left_back.read()); - _state.belt_left_front_touched = (0 == _sensor_belt_left_front.read()); - _state.belt_right_back_touched = (0 == _sensor_belt_right_back.read()); - _state.belt_right_front_touched = (0 == _sensor_belt_right_front.read()); + const auto pos = static_cast(position); + + auto read = (1 == _sensors.at(pos).read()); + if (read && _on_sensor_touched_callback != nullptr) { + _on_sensor_touched_callback(); + } + return read; } -void TouchSensorKit::printState() +auto TouchSensorKit::isTouchedAny() -> bool { - log_info("Ear left touched: %s", _state.ear_left_touched ? "true" : "false"); - log_info("Ear right touched: %s", _state.ear_right_touched ? "true" : "false"); - log_info("Belt left front touched: %s", _state.belt_left_front_touched ? "true" : "false"); - log_info("Belt left back touched: %s", _state.belt_left_back_touched ? "true" : "false"); - log_info("Belt right front touched: %s", _state.belt_right_front_touched ? "true" : "false"); - log_info("Belt right back touched: %s", _state.belt_right_back_touched ? "true" : "false"); + for (Position position: positions) { + if (isTouched(position)) { + return true; + }; + }; + return false; +} + +void TouchSensorKit::setPowerMode(Position position, PowerMode power_mode) +{ + const auto pos = static_cast(position); + _sensors.at(pos).setPowerMode(power_mode); } void TouchSensorKit::resetByPowerMode() { - if (_state.ear_left_touched | _state.ear_right_touched | _state.belt_left_front_touched | - _state.belt_left_back_touched | _state.belt_right_front_touched | _state.belt_right_back_touched) { - setPowerMode(touch::power_mode::low); - rtos::ThisThread::sleep_for(5ms); - setPowerMode(touch::power_mode::normal); - rtos::ThisThread::sleep_for(5ms); + for (Position position: positions) { + if (isTouched(position)) { + setPowerMode(position, PowerMode::low); + rtos::ThisThread::sleep_for(20ms); + setPowerMode(position, PowerMode::normal); + rtos::ThisThread::sleep_for(20ms); + } } } -void TouchSensorKit::setPowerMode(int power_mode) +void TouchSensorKit::setSensitivity(Position position, uint16_t value, bool saved) { - _sensor_ear_left.setPowerMode(power_mode); - _sensor_ear_right.setPowerMode(power_mode); - _sensor_belt_left_back.setPowerMode(power_mode); - _sensor_belt_left_front.setPowerMode(power_mode); - _sensor_belt_right_back.setPowerMode(power_mode); - _sensor_belt_right_front.setPowerMode(power_mode); + const auto pos = static_cast(position); + _sensors.at(pos).setSensitivity(value, saved); + + rtos::ThisThread::sleep_for(10ms); } -void TouchSensorKit::adjustSensitivity(uint16_t value, bool saved) +void TouchSensorKit::registerOnSensorTouched(std::function const &on_sensor_touched_callback) { - _sensor_ear_left.adjustSensitivity(value, saved); - _sensor_ear_right.adjustSensitivity(value, saved); - _sensor_belt_left_back.adjustSensitivity(value, saved); - _sensor_belt_left_front.adjustSensitivity(value, saved); - _sensor_belt_right_back.adjustSensitivity(value, saved); - _sensor_belt_right_front.adjustSensitivity(value, saved); - rtos::ThisThread::sleep_for(1ms); + _on_sensor_touched_callback = on_sensor_touched_callback; } diff --git a/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp index 7cb5be0fd5..6dd7d294be 100644 --- a/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp +++ b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp @@ -25,7 +25,7 @@ TEST_F(TouchSensorTest, initializationDefault) TEST_F(TouchSensorTest, setup) { - touch_sensor_kit.setup(); + touch_sensor_kit.init(); } TEST_F(TouchSensorTest, updateState) @@ -33,11 +33,6 @@ TEST_F(TouchSensorTest, updateState) touch_sensor_kit.updateState(); } -TEST_F(TouchSensorTest, printState) -{ - touch_sensor_kit.printState(); -} - TEST_F(TouchSensorTest, resetByPowerMode) { touch_sensor_kit.resetByPowerMode();