From fd4b484a4b8fe93c433e01ae096e9bd20efb9a29 Mon Sep 17 00:00:00 2001 From: Mourad Latoundji Date: Wed, 13 Jul 2022 17:38:10 +0200 Subject: [PATCH] :art: (libs): Update TouchSensorKit --- libs/CMakeLists.txt | 1 + libs/TouchSensorKit/CMakeLists.txt | 3 +- libs/TouchSensorKit/include/TouchSensorKit.h | 56 ++++-- .../include/external/TouchSensorSystem.h | 8 + libs/TouchSensorKit/source/TouchSensorKit.cpp | 182 +++--------------- .../tests/TouchSensorKit_test.cpp | 4 +- tests/unit/CMakeLists.txt | 1 + 7 files changed, 81 insertions(+), 174 deletions(-) diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt index cfb2e3c623..e010595676 100644 --- a/libs/CMakeLists.txt +++ b/libs/CMakeLists.txt @@ -17,6 +17,7 @@ add_subdirectory(${LIBS_DIR}/LedKit) add_subdirectory(${LIBS_DIR}/RobotKit) add_subdirectory(${LIBS_DIR}/RFIDKit) add_subdirectory(${LIBS_DIR}/SerialNumberKit) +add_subdirectory(${LIBS_DIR}/TouchSensorKit) add_subdirectory(${LIBS_DIR}/UIAnimationKit) add_subdirectory(${LIBS_DIR}/VideoKit) add_subdirectory(${LIBS_DIR}/WebKit) diff --git a/libs/TouchSensorKit/CMakeLists.txt b/libs/TouchSensorKit/CMakeLists.txt index 564b9f8931..8b09ddaf4b 100644 --- a/libs/TouchSensorKit/CMakeLists.txt +++ b/libs/TouchSensorKit/CMakeLists.txt @@ -19,7 +19,8 @@ target_link_libraries(TouchSensorKit IOKit CoreI2C CoreIOExpander - CoreDACTouch + CoreQDAC + CoreTouchSensor ) if (${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") diff --git a/libs/TouchSensorKit/include/TouchSensorKit.h b/libs/TouchSensorKit/include/TouchSensorKit.h index 5ad30f9da8..32bc5eeb01 100644 --- a/libs/TouchSensorKit/include/TouchSensorKit.h +++ b/libs/TouchSensorKit/include/TouchSensorKit.h @@ -4,14 +4,15 @@ #pragma once -#include "CoreDACTouch.h" #include "CoreI2C.h" #include "CoreIOExpander.h" +#include "CoreQDAC.h" +#include "CoreTouchSensor.h" #include "DigitalOut.h" #include "IOKit/DigitalIn.h" #include "IOKit/DigitalOut.h" #include "LogKit.h" -#include "internal/TouchSensorSystem.h" +#include "external/TouchSensorSystem.h" namespace leka { @@ -26,26 +27,21 @@ class TouchSensorKit void resetByPowerMode(); - void adjust_sensivity(uint16_t value); - void calibration(); + void adjustSensitivity(uint16_t value, bool saved = false); - private: - void setPullMode(PinMode mode); + auto ear_left_touched() const -> bool { return _state.ear_left_touched; }; + auto ear_right_touched() const -> bool { return _state.ear_right_touched; }; + auto belt_left_back_touched() const -> bool { return _state.belt_left_back_touched; }; + auto belt_left_front_touched() const -> bool { return _state.belt_left_front_touched; }; + auto belt_right_back_touched() const -> bool { return _state.belt_right_back_touched; }; + auto belt_right_front_touched() const -> bool { return _state.belt_right_front_touched; }; + private: void setPowerMode(int power_mode); - void initDACTouch(); - - void calibrateTwoSensors(bool &sensor_left, bool &sensor_right, uint8_t channel); - void calibrateEars(); - void calibrateBeltLBRF(); - void calibrateBeltRBLF(); - CoreI2C corei2c {PinName::SENSOR_PROXIMITY_MUX_I2C_SDA, PinName::SENSOR_PROXIMITY_MUX_I2C_SCL}; mbed::DigitalOut io_expander_reset {PinName::SENSOR_PROXIMITY_MUX_RESET, 0}; CoreIOExpanderMCP23017 io_expander {corei2c, io_expander_reset}; - CoreDACTouchMCP4728 dac_touch_left {corei2c, 0xC0}; - CoreDACTouchMCP4728 dac_touch_right {corei2c, 0xC2}; leka::io::expanded::DigitalIn<> _ear_left_input {io_expander, touch::pin::ear_left_input}; leka::io::expanded::DigitalIn<> _ear_right_input {io_expander, touch::pin::ear_right_input}; @@ -61,14 +57,32 @@ class TouchSensorKit leka::io::expanded::DigitalOut<> _belt_right_back_pm {io_expander, touch::pin::belt_right_back_power_mode}; leka::io::expanded::DigitalOut<> _belt_right_front_pm {io_expander, touch::pin::belt_right_front_power_mode}; - bool _ear_left_touched {false}; - bool _ear_right_touched {false}; - bool _belt_left_back_touched {false}; - bool _belt_left_front_touched {false}; - bool _belt_right_back_touched {false}; - bool _belt_right_front_touched {false}; + CoreQDACMCP4728 dac_touch_left {corei2c, 0xC0}; + CoreQDACMCP4728 dac_touch_right {corei2c, 0xC2}; + + CoreTouchSensor _sensor_ear_left {_ear_left_input, _ear_left_pm, dac_touch_left, touch::pin::ear_left_ch}; + CoreTouchSensor _sensor_ear_right {_ear_right_input, _ear_right_pm, dac_touch_right, touch::pin::ear_right_ch}; + CoreTouchSensor _sensor_belt_left_back {_belt_left_back_input, _belt_left_back_pm, dac_touch_left, + touch::pin::belt_left_back_ch}; + CoreTouchSensor _sensor_belt_left_front {_belt_left_front_input, _belt_left_front_pm, dac_touch_left, + touch::pin::belt_left_front_ch}; + CoreTouchSensor _sensor_belt_right_back {_belt_right_back_input, _belt_right_back_pm, dac_touch_right, + touch::pin::belt_right_back_ch}; + CoreTouchSensor _sensor_belt_right_front {_belt_right_back_input, _belt_right_back_pm, dac_touch_right, + touch::pin::belt_right_front_ch}; uint16_t default_max_value_calib {0x0FFF}; uint16_t default_min_value_calib {0x0000}; + + struct State { + bool ear_left_touched {false}; + bool ear_right_touched {false}; + bool belt_left_back_touched {false}; + bool belt_left_front_touched {false}; + bool belt_right_back_touched {false}; + bool belt_right_front_touched {false}; + }; + + State _state; }; } // namespace leka diff --git a/libs/TouchSensorKit/include/external/TouchSensorSystem.h b/libs/TouchSensorKit/include/external/TouchSensorSystem.h index cbb64be3b0..da145acd9b 100644 --- a/libs/TouchSensorKit/include/external/TouchSensorSystem.h +++ b/libs/TouchSensorKit/include/external/TouchSensorSystem.h @@ -5,6 +5,7 @@ #pragma once #include "CoreIOExpander.h" +#include "CoreQDAC.h" namespace leka::touch { @@ -22,6 +23,13 @@ namespace pin { inline constexpr auto belt_left_front_power_mode = uint16_t {leka::mcp23017::pin::PA2}; inline constexpr auto belt_right_back_power_mode = uint16_t {leka::mcp23017::pin::PA1}; inline constexpr auto belt_right_front_power_mode = uint16_t {leka::mcp23017::pin::PA0}; + + inline constexpr auto ear_left_ch = uint16_t {leka::mcp4728::channel::A}; + inline constexpr auto ear_right_ch = uint16_t {leka::mcp4728::channel::A}; + inline constexpr auto belt_left_back_ch = uint16_t {leka::mcp4728::channel::B}; + inline constexpr auto belt_left_front_ch = uint16_t {leka::mcp4728::channel::B}; + inline constexpr auto belt_right_back_ch = uint16_t {leka::mcp4728::channel::C}; + inline constexpr auto belt_right_front_ch = uint16_t {leka::mcp4728::channel::C}; } // namespace pin namespace power_mode { diff --git a/libs/TouchSensorKit/source/TouchSensorKit.cpp b/libs/TouchSensorKit/source/TouchSensorKit.cpp index ee34b1e30b..c6f56cbeff 100644 --- a/libs/TouchSensorKit/source/TouchSensorKit.cpp +++ b/libs/TouchSensorKit/source/TouchSensorKit.cpp @@ -11,175 +11,57 @@ using namespace std::chrono_literals; void TouchSensorKit::setup() { - setPullMode(PinMode::PullUp); setPowerMode(touch::power_mode::normal); - initDACTouch(); } void TouchSensorKit::updateState() { - _ear_left_touched = (0 == _ear_left_input.read()); - _ear_right_touched = (0 == _ear_right_input.read()); - _belt_left_back_touched = (0 == _belt_left_back_input.read()); - _belt_left_front_touched = (0 == _belt_left_front_input.read()); - _belt_right_back_touched = (0 == _belt_right_back_input.read()); - _belt_right_front_touched = (0 == _belt_right_front_input.read()); + _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()); } void TouchSensorKit::printState() { - log_info("Ear left touched: %s", _ear_left_touched ? "true" : "false"); - log_info("Ear right touched: %s", _ear_right_touched ? "true" : "false"); - log_info("Belt left front touched: %s", _belt_left_front_touched ? "true" : "false"); - log_info("Belt left back touched: %s", _belt_left_back_touched ? "true" : "false"); - log_info("Belt right front touched: %s", _belt_right_front_touched ? "true" : "false"); - log_info("Belt right back touched: %s", _belt_right_back_touched ? "true" : "false"); + 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"); } void TouchSensorKit::resetByPowerMode() { - setPowerMode(touch::power_mode::low); - rtos::ThisThread::sleep_for(5ms); - setPowerMode(touch::power_mode::normal); - rtos::ThisThread::sleep_for(5ms); -} - -void TouchSensorKit::setPullMode(PinMode mode) -{ - _ear_left_input.mode(mode); - _ear_right_input.mode(mode); - _belt_left_back_input.mode(mode); - _belt_left_front_input.mode(mode); - _belt_right_back_input.mode(mode); - _belt_right_front_input.mode(mode); + 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); + } } void TouchSensorKit::setPowerMode(int power_mode) { - _ear_left_pm.write(power_mode); - _ear_right_pm.write(power_mode); - _belt_left_back_pm.write(power_mode); - _belt_left_front_pm.write(power_mode); - _belt_right_back_pm.write(power_mode); - _belt_right_front_pm.write(power_mode); -} - -void TouchSensorKit::initDACTouch() -{ - dac_touch_left.writeVoltageReference(0x00); - dac_touch_left.writePowerMode(0x00); - dac_touch_left.writeGain(0x00); - dac_touch_right.writeVoltageReference(0x00); - dac_touch_right.writePowerMode(0x00); - dac_touch_right.writeGain(0x00); -} - -void TouchSensorKit::adjust_sensivity(uint16_t value) -{ - auto buffer = std::array {}; - - buffer.at(0) = static_cast((0x0F00 & value) >> 8); - buffer.at(1) = static_cast(0x00FF & value); - - dac_touch_left.writeToSpecificInputRegister(0, buffer); - dac_touch_right.writeToSpecificInputRegister(0, buffer); - - dac_touch_left.writeToSpecificInputRegister(1, buffer); - dac_touch_right.writeToSpecificInputRegister(1, buffer); - - dac_touch_left.writeToSpecificInputRegister(2, buffer); - dac_touch_right.writeToSpecificInputRegister(2, buffer); - rtos::ThisThread::sleep_for(1ms); + _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); } -void TouchSensorKit::calibrateTwoSensors(bool &sensor_left, bool &sensor_right, uint8_t channel) +void TouchSensorKit::adjustSensitivity(uint16_t value, bool saved) { - auto buffer_left = std::array {}; - auto buffer_right = std::array {}; - - dac_touch_left.writeToSpecificInputRegister(channel, buffer_left); - dac_touch_right.writeToSpecificInputRegister(channel, buffer_right); - rtos::ThisThread::sleep_for(1ms); - - auto value_left_calib = uint16_t {default_max_value_calib}; - auto value_right_calib = uint16_t {default_max_value_calib}; - auto step = uint8_t {0x00FF}; - - updateState(); - - while (!((sensor_left || value_left_calib <= value_left_calib % step) && - (sensor_right || value_right_calib <= value_right_calib % step))) { - if (!sensor_left) { - if (value_left_calib - step > default_max_value_calib) { - value_left_calib = default_max_value_calib; - } else { - value_left_calib -= step; - } - } - if (!sensor_right) { - if (value_right_calib - step > default_max_value_calib) { - value_right_calib = default_max_value_calib; - } else { - value_right_calib -= step; - } - } - - buffer_left.at(0) = static_cast((0x0F00 & value_left_calib) >> 8); - buffer_left.at(1) = static_cast(0x00FF & value_left_calib); - dac_touch_left.writeToSpecificInputRegister(channel, buffer_left); - rtos::ThisThread::sleep_for(1ms); - - buffer_right.at(0) = static_cast((0x0F00 & value_right_calib) >> 8); - buffer_right.at(1) = static_cast(0x00FF & value_right_calib); - dac_touch_right.writeToSpecificInputRegister(channel, buffer_right); - rtos::ThisThread::sleep_for(1ms); - - updateState(); - rtos::ThisThread::sleep_for(500ms); - } - - dac_touch_left.writeToSpecificMemoryRegister(channel, buffer_left); - dac_touch_right.writeToSpecificMemoryRegister(channel, buffer_right); - rtos::ThisThread::sleep_for(1ms); - log_info("Value Left Calibration : %d", value_left_calib); - log_info("Value Right Calibration : %d", value_right_calib); - log_info("CALIBRATED!"); + _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); } - -void TouchSensorKit::calibrateEars() -{ - log_info("Place hands on EAR LEFT and EAR RIGHT"); - log_info("Calibration will start in 5 seconds"); - rtos::ThisThread::sleep_for(5s); - calibrateTwoSensors(_ear_left_touched, _ear_right_touched, 2); -} - -void TouchSensorKit::calibrateBeltLBRF() -{ - log_info("Place hands on BELT LEFT BACK and BELT RIGHT FRONT"); - log_info("Calibration will start in 5 seconds"); - rtos::ThisThread::sleep_for(5s); - calibrateTwoSensors(_belt_left_back_touched, _belt_right_front_touched, 1); -} - -void TouchSensorKit::calibrateBeltRBLF() -{ - log_info("Place hands on BELT LEFT FRONT and BELT RIGHT BACK"); - log_info("Calibration will start in 5 seconds"); - rtos::ThisThread::sleep_for(5s); - calibrateTwoSensors(_belt_left_front_touched, _belt_right_back_touched, 0); -} - -void TouchSensorKit::calibration() -{ - log_info("Touch calibration"); - log_info("For each of 6 touch sensors, value of sensibility will change"); - log_info("Please keep your hands on 2 sensors until \"CALIBRATED !\" appears.\n"); - rtos::ThisThread::sleep_for(5s); - - calibrateEars(); - calibrateBeltLBRF(); - calibrateBeltRBLF(); - rtos::ThisThread::sleep_for(1s); -} diff --git a/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp index ca05ebd054..7cb5be0fd5 100644 --- a/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp +++ b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp @@ -43,7 +43,7 @@ TEST_F(TouchSensorTest, resetByPowerMode) touch_sensor_kit.resetByPowerMode(); } -TEST_F(TouchSensorTest, calibration) +TEST_F(TouchSensorTest, adjustSensitivity) { - touch_sensor_kit.caliration(); + touch_sensor_kit.adjustSensitivity(0x0ABC); } diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index 413fafea2e..497652c283 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -283,6 +283,7 @@ leka_register_unit_tests_for_library(LedKit) leka_register_unit_tests_for_library(RobotKit) leka_register_unit_tests_for_library(RFIDKit) leka_register_unit_tests_for_library(SerialNumberKit) +leka_register_unit_tests_for_library(TouchSensorKit) leka_register_unit_tests_for_library(UIAnimationKit) leka_register_unit_tests_for_library(VideoKit)