✨ (libs): Add TouchSensorKit

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()

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

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

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);
+}

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();
+// }