Skip to content

Commit

Permalink
🎨 (libs): Update TouchSensorKit
Browse files Browse the repository at this point in the history
  • Loading branch information
@MMyster
MMyster committed Jul 13, 2022
1 parent 0817599 commit fd4b484
Show file tree
Hide file tree
Showing 7 changed files with 81 additions and 174 deletions.
1 change: 1 addition & 0 deletions libs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
3 changes: 2 additions & 1 deletion libs/TouchSensorKit/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,8 @@ target_link_libraries(TouchSensorKit
IOKit
CoreI2C
CoreIOExpander
CoreDACTouch
CoreQDAC
CoreTouchSensor
)

if (${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
Expand Down
56 changes: 35 additions & 21 deletions libs/TouchSensorKit/include/TouchSensorKit.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {

Expand All @@ -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};
Expand All @@ -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
8 changes: 8 additions & 0 deletions libs/TouchSensorKit/include/external/TouchSensorSystem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#pragma once

#include "CoreIOExpander.h"
#include "CoreQDAC.h"

namespace leka::touch {

Expand All @@ -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 {
Expand Down
182 changes: 32 additions & 150 deletions libs/TouchSensorKit/source/TouchSensorKit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t, 2> {};

buffer.at(0) = static_cast<uint8_t>((0x0F00 & value) >> 8);
buffer.at(1) = static_cast<uint8_t>(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<uint8_t, 2> {};
auto buffer_right = std::array<uint8_t, 2> {};

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<uint8_t>((0x0F00 & value_left_calib) >> 8);
buffer_left.at(1) = static_cast<uint8_t>(0x00FF & value_left_calib);
dac_touch_left.writeToSpecificInputRegister(channel, buffer_left);
rtos::ThisThread::sleep_for(1ms);

buffer_right.at(0) = static_cast<uint8_t>((0x0F00 & value_right_calib) >> 8);
buffer_right.at(1) = static_cast<uint8_t>(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);
}
4 changes: 2 additions & 2 deletions libs/TouchSensorKit/tests/TouchSensorKit_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
1 change: 1 addition & 0 deletions tests/unit/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down

0 comments on commit fd4b484

Please sign in to comment.