diff --git a/libs/TouchSensorKit/CMakeLists.txt b/libs/TouchSensorKit/CMakeLists.txt new file mode 100644 index 0000000000..ac8d98ef93 --- /dev/null +++ b/libs/TouchSensorKit/CMakeLists.txt @@ -0,0 +1,33 @@ +# Leka - LekaOS +# Copyright 2022 APF France handicap +# SPDX-License-Identifier: Apache-2.0 + +add_library(TouchSensorKit STATIC) + +target_include_directories(TouchSensorKit + PUBLIC + include + PRIVATE + internal +) + +target_sources(TouchSensorKit + PRIVATE + source/TouchSensorKit.cpp +) + +target_link_libraries(TouchSensorKit + mbed-os + IOKit + CoreI2C + CoreIOExpander + CoreDACExpander +) + +if (${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") + + leka_unit_tests_sources( + tests/TouchSensorKit_test.cpp + ) + +endif() diff --git a/libs/TouchSensorKit/include/TouchSensorKit.h b/libs/TouchSensorKit/include/TouchSensorKit.h new file mode 100644 index 0000000000..52af2072da --- /dev/null +++ b/libs/TouchSensorKit/include/TouchSensorKit.h @@ -0,0 +1,69 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "CoreDACExpander.h" +#include "CoreI2C.h" +#include "CoreIOExpander.h" +#include "DigitalOut.h" +#include "IOKit/DigitalIn.h" +#include "IOKit/DigitalOut.h" +#include "LogKit.h" +#include "internal/TouchSensorSystem.h" + +namespace leka { + +class TouchSensorKit +{ + public: + explicit TouchSensorKit() = default; + void setup(); + + void updateState(); + void printState(); + + void calibration(); + + private: + void set_pull_mode(PinMode mode); + + void set_power_mode(int power_mode); + void power_mode_reset(); + + void adjust_sensitivity_low(); + void adjust_sensitivity_high(); + + 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}; + CoreDACExpanderMCP4728 dac_expander {corei2c}; + + 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}; + leka::io::expanded::DigitalIn<> _belt_left_back_input {io_expander, touch::pin::belt_left_back_input}; + leka::io::expanded::DigitalIn<> _belt_left_front_input {io_expander, touch::pin::belt_left_front_input}; + leka::io::expanded::DigitalIn<> _belt_right_back_input {io_expander, touch::pin::belt_right_back_input}; + leka::io::expanded::DigitalIn<> _belt_right_front_input {io_expander, touch::pin::belt_right_front_input}; + + leka::io::expanded::DigitalOut<> _ear_left_pm {io_expander, touch::pin::ear_left_power_mode}; + leka::io::expanded::DigitalOut<> _ear_right_pm {io_expander, touch::pin::ear_right_power_mode}; + leka::io::expanded::DigitalOut<> _belt_left_back_pm {io_expander, touch::pin::belt_left_back_power_mode}; + leka::io::expanded::DigitalOut<> _belt_left_front_pm {io_expander, touch::pin::belt_left_front_power_mode}; + 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}; +}; +} // namespace leka diff --git a/libs/TouchSensorKit/include/internal/TouchSensorSystem.h b/libs/TouchSensorKit/include/internal/TouchSensorSystem.h new file mode 100644 index 0000000000..0ca544ba32 --- /dev/null +++ b/libs/TouchSensorKit/include/internal/TouchSensorSystem.h @@ -0,0 +1,33 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "CoreIOExpander.h" + +namespace leka::touch { + +namespace pin { + constexpr inline uint16_t ear_left_input = leka::mcp23017::pin::PB5; + constexpr inline uint16_t ear_right_input = leka::mcp23017::pin::PB4; + constexpr inline uint16_t belt_left_back_input = leka::mcp23017::pin::PB3; + constexpr inline uint16_t belt_left_front_input = leka::mcp23017::pin::PB2; + constexpr inline uint16_t belt_right_back_input = leka::mcp23017::pin::PB1; + constexpr inline uint16_t belt_right_front_input = leka::mcp23017::pin::PB0; + + constexpr inline uint16_t ear_left_power_mode = leka::mcp23017::pin::PA5; + constexpr inline uint16_t ear_right_power_mode = leka::mcp23017::pin::PA4; + constexpr inline uint16_t belt_left_back_power_mode = leka::mcp23017::pin::PA3; + constexpr inline uint16_t belt_left_front_power_mode = leka::mcp23017::pin::PA2; + constexpr inline uint16_t belt_right_back_power_mode = leka::mcp23017::pin::PA1; + constexpr inline uint16_t belt_right_front_power_mode = leka::mcp23017::pin::PA0; +} // namespace pin + +namespace power_mode { + constexpr inline uint16_t low = 0x00; + constexpr inline uint16_t normal = 0X01; + +} // namespace power_mode + +} // namespace leka::touch diff --git a/libs/TouchSensorKit/source/TouchSensorKit.cpp b/libs/TouchSensorKit/source/TouchSensorKit.cpp new file mode 100644 index 0000000000..b8e66e22a3 --- /dev/null +++ b/libs/TouchSensorKit/source/TouchSensorKit.cpp @@ -0,0 +1,175 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "TouchSensorKit.h" + +#include "rtos/ThisThread.h" + +using namespace leka; +using namespace std::chrono_literals; + +void TouchSensorKit::setup() +{ + set_pull_mode(PinMode::PullUp); + set_power_mode(touch::power_mode::normal); + dac_expander.write_vref(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 0x00); + dac_expander.write_vref(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 0x00); + dac_expander.write_power_down(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 0x00); + dac_expander.write_power_down(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 0x00); + dac_expander.write_gain(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 0x00); + dac_expander.write_gain(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 0x00); +} + +void TouchSensorKit::set_pull_mode(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); +} + +void TouchSensorKit::set_power_mode(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::power_mode_reset() +{ + set_power_mode(touch::power_mode::low); + rtos::ThisThread::sleep_for(5ms); + set_power_mode(touch::power_mode::normal); + rtos::ThisThread::sleep_for(5ms); +} + +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()); + power_mode_reset(); +} + +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"); +} + +void TouchSensorKit::adjust_sensitivity_low() +{ + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 0, 0x0FFF); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 0, 0x0FFF); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 1, 0x0FFF); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 1, 0x0FFF); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 2, 0x0FFF); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 2, 0x0FFF); +} + +void TouchSensorKit::adjust_sensitivity_high() +{ + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 0, 0x0000); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 0, 0x0000); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 1, 0x0000); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 1, 0x0000); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, 2, 0x0000); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, 2, 0x0000); +} + +void TouchSensorKit::calibrateTwoSensors(bool &sensor_left, bool &sensor_right, uint8_t channel) +{ + auto value_left_calib = uint16_t {0x0FFF}; + auto value_right_calib = uint16_t {0x0FFF}; + auto step = uint16_t {0x0001}; + + dac_expander.reset(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, channel); + dac_expander.reset(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, channel); + + rtos::ThisThread::sleep_for(1s); + updateState(); + + while (!(sensor_left && sensor_right)) { + if (!sensor_left) { + if (value_left_calib - step > 0x0FFF) { + value_left_calib = 0x0FFF; + } else { + value_left_calib -= step; + } + } + if (!sensor_right) { + if (value_right_calib - step > 0x0FFF) { + value_right_calib = 0x0FFF; + } else { + value_right_calib -= step; + } + } + + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, channel, + value_left_calib); + dac_expander.multiple_write_for_dac_input_registers(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, channel, + value_right_calib); + + rtos::ThisThread::sleep_for(1ms); + updateState(); + } + + dac_expander.single_write_for_dac_input_register_and_eeprom(CoreDACExpanderMCP4728::_I2C_ADDRESS_LEFT, channel, + value_left_calib); + dac_expander.single_write_for_dac_input_register_and_eeprom(CoreDACExpanderMCP4728::_I2C_ADDRESS_RIGHT, channel, + value_right_calib); + rtos::ThisThread::sleep_for(1ms); + + log_info("CALIBRATED!"); + rtos::ThisThread::sleep_for(100ms); +} + +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(15s); + + calibrateEars(); + calibrateBeltLBRF(); + calibrateBeltRBLF(); + rtos::ThisThread::sleep_for(1s); +} diff --git a/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp new file mode 100644 index 0000000000..6b1aceeb8d --- /dev/null +++ b/libs/TouchSensorKit/tests/TouchSensorKit_test.cpp @@ -0,0 +1,44 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "TouchSensorKit.h" + +#include "gtest/gtest.h" + +using namespace leka; +// using ::testing::_; + +class TouchSensorTest : public ::testing::Test +{ + protected: + // void SetUp() override {} + // void TearDown() override {} + + TouchSensorKit touch_sensor_kit = TouchSensorKit(); +}; + +TEST_F(TouchSensorTest, initializationDefault) +{ + ASSERT_NE(&touch_sensor_kit, nullptr); +} + +TEST_F(TouchSensorTest, setup) +{ + touch_sensor_kit.setup(); +} + +TEST_F(TouchSensorTest, updateState) +{ + touch_sensor_kit.updateState(); +} + +TEST_F(TouchSensorTest, printState) +{ + touch_sensor_kit.printState(); +} + +// TEST_F(TouchSensorTest, calibration) +// { +// touch_sensor_kit.calibration(); +// }