diff --git a/.github/scripts/get_firmware_name.sh b/.github/scripts/get_firmware_name.sh index 36b1bfb9..120fafe3 100755 --- a/.github/scripts/get_firmware_name.sh +++ b/.github/scripts/get_firmware_name.sh @@ -18,11 +18,6 @@ getBhapticsName() { target="$target+battery" fi - if [[ $flags =~ SENSESHIFT_SERIAL_PLOTTER=true ]]; then - echo "::debug::Serial Plotter is enabled, appending +serialplotter to the target" - target="$target+serialplotter" - fi - echo "firmware=$target" if [[ -n "$GITHUB_ACTIONS" ]]; then echo "firmware=$target" >> "$GITHUB_OUTPUT" diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bade01aa..85d5c815 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -16,12 +16,12 @@ on: jobs: build-bhaptics: - name: Build ${{ matrix.target }} ${{ matrix.coverage && 'with coverage' || 'without coverage' }}, -D ${{ matrix.battery_flag }} -D ${{ matrix.serial_plotter_flag }} -D ${{ matrix.nimble_flag }} + name: Build ${{ matrix.target }} ${{ matrix.coverage && 'with coverage' || 'without coverage' }}, -D ${{ matrix.battery_flag }} -D ${{ matrix.nimble_flag }} runs-on: ${{ matrix.os }} strategy: fail-fast: false matrix: - os: [ubuntu-latest] + os: [ ubuntu-latest ] target: - bhaptics_tactsuit_x16 - bhaptics_tactsuit_x16_pca9685 @@ -32,10 +32,9 @@ jobs: - bhaptics_tactal - bhaptics_tactvisor - bhaptics_tactglove_right - battery_flag: [SENSESHIFT_BATTERY_ENABLED=true] - serial_plotter_flag: [SENSESHIFT_SERIAL_PLOTTER=false] - nimble_flag: [SENSESHIFT_BLE_USE_NIMBLE=false] - coverage: [false] + battery_flag: [ SENSESHIFT_BATTERY_ENABLED=true ] + nimble_flag: [ SENSESHIFT_BLE_USE_NIMBLE=false ] + coverage: [ false ] include: # Extra tests for x40, as it uses the most features @@ -43,19 +42,16 @@ jobs: os: ubuntu-latest coverage: true battery_flag: SENSESHIFT_BATTERY_ENABLED=true - serial_plotter_flag: SENSESHIFT_SERIAL_PLOTTER=true nimble_flag: SENSESHIFT_BLE_USE_NIMBLE=true # - target: bhaptics_tactsuit_x40 # os: ubuntu-latest # coverage: true # battery_flag: SENSESHIFT_BATTERY_ENABLED=true - # serial_plotter_flag: SENSESHIFT_SERIAL_PLOTTER=true # nimble_flag: SENSESHIFT_BLE_USE_NIMBLE=false - target: bhaptics_tactsuit_x40 os: ubuntu-latest coverage: false battery_flag: SENSESHIFT_BATTERY_ENABLED=true - serial_plotter_flag: SENSESHIFT_SERIAL_PLOTTER=false nimble_flag: SENSESHIFT_BLE_USE_NIMBLE=true steps: @@ -66,7 +62,7 @@ jobs: - name: Get firmware name id: firmware_name run: | - ./.github/scripts/get_firmware_name.sh ${{ matrix.target }} ${{ matrix.serial_plotter_flag }} ${{ matrix.battery_flag }} ${{ matrix.nimble_flag }} + ./.github/scripts/get_firmware_name.sh ${{ matrix.target }} ${{ matrix.battery_flag }} ${{ matrix.nimble_flag }} - name: Enable coverage (non-macOS) if: runner.os != 'macOS' && matrix.coverage @@ -81,13 +77,11 @@ jobs: if: runner.os != 'macOS' run: | sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.battery_flag }}/' platformio.ini - sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.serial_plotter_flag }}/' platformio.ini sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.nimble_flag }}/' platformio.ini - name: Update build flags (macOS) if: runner.os == 'macOS' run: | sed -i '' '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.battery_flag }}/' platformio.ini - sed -i '' '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.serial_plotter_flag }}/' platformio.ini sed -i '' '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.nimble_flag }}/' platformio.ini - name: Speedup package installation @@ -165,7 +159,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-latest] + os: [ ubuntu-latest ] target: - lucidgloves-prototype3 - lucidgloves-prototype4 @@ -176,20 +170,20 @@ jobs: comm_flag: - COMMUNICATION_PROTOCOL=OPENGLOVES_COMM_SERIAL - COMMUNICATION_PROTOCOL=OPENGLOVES_COMM_BTSERIAL - coverage: [false] + coverage: [ false ] include: - os: ubuntu-latest target: lucidgloves-prototype4-ffb - curl_calibration_flag: CALIBRATION_CURL="::SenseShift::Calibration::MinMaxCalibrator" + curl_calibration_flag: CALIBRATION_CURL="new ::SenseShift::Input::Calibration::MinMaxCalibrator()" coverage: true - os: ubuntu-latest target: lucidgloves-prototype4-ffb - curl_calibration_flag: CALIBRATION_CURL="::SenseShift::Calibration::CenterPointDeviationCalibrator" + curl_calibration_flag: CALIBRATION_CURL="new ::SenseShift::Input::Calibration::CenterPointDeviationCalibrator(0.66F, 0.005F)" coverage: true - os: ubuntu-latest target: lucidgloves-prototype4-ffb - curl_calibration_flag: CALIBRATION_CURL="::SenseShift::Calibration::FixedCenterPointDeviationCalibrator" + curl_calibration_flag: CALIBRATION_CURL="new ::SenseShift::Input::Calibration::FixedCenterPointDeviationCalibrator(0.66F, 0.005F)" coverage: true - os: ubuntu-latest target: indexer-csf diff --git a/.github/workflows/codeql-analysis.yml b/.github/workflows/codeql-analysis.yml index ed8f6731..8cfbff2c 100644 --- a/.github/workflows/codeql-analysis.yml +++ b/.github/workflows/codeql-analysis.yml @@ -32,89 +32,86 @@ jobs: # Enabling all flags to test build for every feature battery_flag: - SENSESHIFT_BATTERY_ENABLED=true - serial_plotter_flag: - - SENSESHIFT_SERIAL_PLOTTER=true steps: - - name: Checkout repository - uses: actions/checkout@v3 - with: - # We must fetch at least the immediate parents so that if this is - # a pull request then we can checkout the head. - fetch-depth: 2 - submodules: 'recursive' - - # If this run was triggered by a pull request event, then checkout - # the head of the pull request instead of the merge commit. - # - run: git checkout HEAD^2 - # if: ${{ github.event_name == 'pull_request' }} - - # Initializes the CodeQL tools for scanning. - - name: Initialize CodeQL - uses: github/codeql-action/init@v2 - with: - languages: ${{ matrix.language }} - config-file: ./.github/codeql/codeql-config.yml - # If you wish to specify custom queries, you can do so here or in a config file. - # By default, queries listed here will override any specified in a config file. - # Prefix the list here with "+" to use these queries and those in the config file. - - # Details on CodeQL's query packs refer to : https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs - # queries: security-extended,security-and-quality - - - # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). - # If this step fails, then you should remove it and run the build manually (see below) - # - name: Autobuild - # uses: github/codeql-action/autobuild@v2 - - # ℹī¸ Command-line programs to run using the OS shell. - # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun - - # If the Autobuild fails above, remove it and uncomment the following three lines. - # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. - - # - run: | - # echo "Run, Build Application using script" - # ./location_of_script_within_repo/buildscript.sh - - - name: Cache pip - uses: actions/cache@v3 - with: - path: ~/.cache/pip - key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} - restore-keys: | - ${{ runner.os }}-pip- - - name: Cache PlatformIO - uses: actions/cache@v3 - with: - path: ~/.platformio - key: ${{ runner.os }}-pio-${{ hashFiles('**/lockfiles') }} - restore-keys: | - ${{ runner.os }}-pio- - - name: Set up Python - uses: actions/setup-python@v4 - with: - python-version: '3.9' - - - name: Update build flags (non-macOS) - if: runner.os != 'macOS' - run: | - sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.battery_flag }}/' platformio.ini - sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.serial_plotter_flag }}/' platformio.ini - - - name: Install PlatformIO - run: | - python -m pip install --upgrade pip - pip install --upgrade platformio - pio upgrade --dev - pio pkg update --global - - - name: Build - run: | - pio run --environment bhaptics_tactsuit_x40 - - - name: Perform CodeQL Analysis - uses: github/codeql-action/analyze@v2 - with: - category: "/language:${{matrix.language}}" + - name: Checkout repository + uses: actions/checkout@v3 + with: + # We must fetch at least the immediate parents so that if this is + # a pull request then we can checkout the head. + fetch-depth: 2 + submodules: 'recursive' + + # If this run was triggered by a pull request event, then checkout + # the head of the pull request instead of the merge commit. + # - run: git checkout HEAD^2 + # if: ${{ github.event_name == 'pull_request' }} + + # Initializes the CodeQL tools for scanning. + - name: Initialize CodeQL + uses: github/codeql-action/init@v2 + with: + languages: ${{ matrix.language }} + config-file: ./.github/codeql/codeql-config.yml + # If you wish to specify custom queries, you can do so here or in a config file. + # By default, queries listed here will override any specified in a config file. + # Prefix the list here with "+" to use these queries and those in the config file. + + # Details on CodeQL's query packs refer to : https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs + # queries: security-extended,security-and-quality + + + # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). + # If this step fails, then you should remove it and run the build manually (see below) + # - name: Autobuild + # uses: github/codeql-action/autobuild@v2 + + # ℹī¸ Command-line programs to run using the OS shell. + # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun + + # If the Autobuild fails above, remove it and uncomment the following three lines. + # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. + + # - run: | + # echo "Run, Build Application using script" + # ./location_of_script_within_repo/buildscript.sh + + - name: Cache pip + uses: actions/cache@v3 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + - name: Cache PlatformIO + uses: actions/cache@v3 + with: + path: ~/.platformio + key: ${{ runner.os }}-pio-${{ hashFiles('**/lockfiles') }} + restore-keys: | + ${{ runner.os }}-pio- + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: '3.9' + + - name: Update build flags (non-macOS) + if: runner.os != 'macOS' + run: | + sed -i '/__OH_FIRMWARE__/p; s/__OH_FIRMWARE__/${{ matrix.battery_flag }}/' platformio.ini + + - name: Install PlatformIO + run: | + python -m pip install --upgrade pip + pip install --upgrade platformio + pio upgrade --dev + pio pkg update --global + + - name: Build + run: | + pio run --environment bhaptics_tactsuit_x40 + + - name: Perform CodeQL Analysis + uses: github/codeql-action/analyze@v2 + with: + category: "/language:${{matrix.language}}" diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 6eb7910a..d2eecde2 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -132,8 +132,8 @@ jobs: - indexer-cs - indexer-csf comm_flag: - - OPENGLOVES_COMMUNCATION=OPENGLOVES_COMM_SERIAL - - OPENGLOVES_COMMUNCATION=OPENGLOVES_COMM_BTSERIAL + - OPENGLOVES_COMMUNICATION=OPENGLOVES_COMM_SERIAL + - OPENGLOVES_COMMUNICATION=OPENGLOVES_COMM_BTSERIAL steps: - uses: actions/checkout@v3 with: diff --git a/.wokwi/lucidgloves-prototype3+serial/test.yaml b/.wokwi/lucidgloves-prototype3+serial/test.yaml index 95f55e39..7dc4c210 100644 --- a/.wokwi/lucidgloves-prototype3+serial/test.yaml +++ b/.wokwi/lucidgloves-prototype3+serial/test.yaml @@ -45,32 +45,32 @@ steps: part-id: pot-thumb control: position value: 1 - - wait-serial: "A4095B4095C0D0E0F2047G2047IM" # M is for Pinch gesture + - wait-serial: "A4095B4095C0D0E0F2047G2047MI" # M is for Pinch gesture # Curl Middle finger - set-control: part-id: pot-middle control: position value: 1 - - wait-serial: "A4095B4095C4095D0E0F2047G2047IM" + - wait-serial: "A4095B4095C4095D0E0F2047G2047MI" # Curl Ring finger - set-control: part-id: pot-ring control: position value: 1 - - wait-serial: "A4095B4095C4095D4095E0F2047G2047IM" + - wait-serial: "A4095B4095C4095D4095E0F2047G2047MI" # Curl Pinky finger - set-control: part-id: pot-pinky control: position value: 1 - - wait-serial: "A4095B4095C4095D4095E4095F2047G2047ILM" # L is for the Grab gesture + - wait-serial: "A4095B4095C4095D4095E4095F2047G2047MIL" # L is for the Grab gesture # Partially Release Thumb finger - set-control: part-id: pot-thumb control: position value: 0.25 - - wait-serial: 'A1024B4095C4095D4095E4095F2047G2047IL' + - wait-serial: "A1024B4095C4095D4095E4095F2047G2047IL" diff --git a/.wokwi/lucidgloves-prototype4+serial/test.yaml b/.wokwi/lucidgloves-prototype4+serial/test.yaml index f89d70f4..d3a1f656 100644 --- a/.wokwi/lucidgloves-prototype4+serial/test.yaml +++ b/.wokwi/lucidgloves-prototype4+serial/test.yaml @@ -3,6 +3,21 @@ version: 1 author: Leonid Meleshin steps: + - delay: 2000ms + + # Press calibration button + - set-control: + part-id: btn3 + control: pressed + value: 1 + + - delay: 250ms + + - set-control: + part-id: btn3 + control: pressed + value: 0 + - wait-serial: "A0B0C0D0E0F2047G2047" # Press the 'A' button @@ -45,28 +60,28 @@ steps: part-id: pot-thumb control: position value: 1 - - wait-serial: "A4095B4095C0D0E0F2047G2047IM" # M is for Pinch gesture + - wait-serial: "A4095B4095C0D0E0F2047G2047MI" # M is for Pinch gesture # Curl Middle finger - set-control: part-id: pot-middle control: position value: 1 - - wait-serial: "A4095B4095C4095D0E0F2047G2047IM" + - wait-serial: "A4095B4095C4095D0E0F2047G2047MI" # Curl Ring finger - set-control: part-id: pot-ring control: position value: 1 - - wait-serial: "A4095B4095C4095D4095E0F2047G2047IM" + - wait-serial: "A4095B4095C4095D4095E0F2047G2047MI" # Curl Pinky finger - set-control: part-id: pot-pinky control: position value: 1 - - wait-serial: "A4095B4095C4095D4095E4095F2047G2047ILM" # L is for the Grab gesture + - wait-serial: "A4095B4095C4095D4095E4095F2047G2047MIL" # L is for the Grab gesture # Partially Release Thumb finger - set-control: diff --git a/docs/DEVELOPMENT.md b/docs/DEVELOPMENT.md index dafd8ccf..1a6e5409 100644 --- a/docs/DEVELOPMENT.md +++ b/docs/DEVELOPMENT.md @@ -1,3 +1,23 @@ +# Code Style + +## Member naming + +Local variable: `snake_case` +Global variable: `snake_case` + +Class: `UpperCamelCase` +Class member: `snake_case_` (**with trailing underscore**) +ClassFunction: `camelCase` +Function argument: `snake_case` + +## Useful Scripts + +### Fix `clang-format` + +```shell +find lib include firmware test examples -type f -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' ! -regex '^lib/frozen\(/.*\)' -exec clang-format-16 -style=file -i {} \; +``` + # Debugging ## Debugging in Wokwi @@ -9,7 +29,7 @@ Run and debug firmware in Wokwi Simulator pio debug -e lucidgloves-prototype3 ``` 2. `Ctrl+Shift+P` => `> Wokwi: Start Simulator and Wait for Debugger`, -3. Add launch option (this step is required until PlatformIO fixes this issue: [#3824](https://github.com/platformio/platformio-core/issues/3824)): +3. Add launch option (this step is required until PlatformIO fixes this issue: [#3824](https://github.com/platformio/platformio-core/issues/3824)): ```json ... @@ -24,12 +44,4 @@ Run and debug firmware in Wokwi Simulator "miDebuggerServerAddress": "localhost:3333", }, ... - ``` - -## Useful Scripts - -### Fix `clang-format` - -```shell -find lib include firmware test examples -type f -regex '.*\.\(cpp\|hpp\|cu\|c\|h\)' ! -regex '^lib/frozen\(/.*\)' -exec clang-format-16 -style=file -i {} \; -``` + ``` \ No newline at end of file diff --git a/firmware/firmware.cpp b/firmware/firmware.cpp index c30147c3..258921f2 100644 --- a/firmware/firmware.cpp +++ b/firmware/firmware.cpp @@ -4,14 +4,6 @@ #include #endif // ARDUINO -#if defined(SENSESHIFT_SERIAL_PLOTTER) && SENSESHIFT_SERIAL_PLOTTER == true -#include -#endif // SENSESHIFT_SERIAL_PLOTTER - -#ifndef SENSESHIFT_SERIAL_PLOTTER_PORT -#define SENSESHIFT_SERIAL_PLOTTER_PORT Serial -#endif // SENSESHIFT_SERIAL_PLOTTER_PORT - #ifndef PIO_UNIT_TESTING extern void setupMode(); @@ -19,19 +11,11 @@ extern void loopMode(); #if defined(ARDUINO) -SenseShift::SenseShift App; +SenseShift::Application App; void setup() { setupMode(); - -#if defined(SENSESHIFT_SERIAL_PLOTTER) && SENSESHIFT_SERIAL_PLOTTER == true - auto* serialOutputState = new ::SenseShift::Arduino::SerialPlotter_OutputStates( - SENSESHIFT_SERIAL_PLOTTER_PORT, - App.getHapticBody() - ); - serialOutputState->begin(); -#endif // SENSESHIFT_SERIAL_PLOTTER } void loop() diff --git a/firmware/mode_configs/bhaptics/tactal.cpp b/firmware/mode_configs/bhaptics/tactal.cpp index 54bd226f..ec8ea898 100644 --- a/firmware/mode_configs/bhaptics/tactal.cpp +++ b/firmware/mode_configs/bhaptics/tactal.cpp @@ -4,42 +4,43 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTAL_SIZE; -static const Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTAL; +static const std::array bhLayout = { BH_LAYOUT_TACTAL }; void setupMode() { // Configure PWM pins to their positions on the face - const auto faceOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + const auto faceOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32), new ActuatorPWM(33), new ActuatorPWM(25), new ActuatorPWM(26), new ActuatorPWM(27), new ActuatorPWM(14) }, + { new LedcOutput(32), new LedcOutput(33), new LedcOutput(25), new LedcOutput(26), new LedcOutput(27), new LedcOutput(14) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::FaceFront, new VibroPlane_Closest(faceOutputs)); + app->getVibroBody()->addTarget(Target::FaceFront, new FloatPlane_Closest(faceOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -48,19 +49,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro, Target::FaceFront); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro, Target::FaceFront); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactglove.cpp b/firmware/mode_configs/bhaptics/tactglove.cpp index 3091a5d6..d67f58e6 100644 --- a/firmware/mode_configs/bhaptics/tactglove.cpp +++ b/firmware/mode_configs/bhaptics/tactglove.cpp @@ -4,32 +4,33 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include #include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; static constexpr Body::Hands::HandSide handSide = Body::Hands::HandSide::SENSESHIFT_HAND_SIDE; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTGLOVE_SIZE; // clang-format off -static const OutputLayout (&bhLayout)[bhLayoutSize] = handSide == Body::Hands::HandSide::Left ? BH::TactGloveLeftLayout : BH::TactGloveRightLayout; +static const auto& bhLayout = handSide == Body::Hands::HandSide::Left ? BH::TactGloveLeftLayout : BH::TactGloveRightLayout; // clang-format on void setupMode() @@ -37,17 +38,17 @@ void setupMode() // Configure PWM pins to their positions on the glove // Replace `new PWMOutputWriter(...)` with `nullptr` to disable a specific actuator addTactGloveActuators( - app->getHapticBody(), + app->getVibroBody(), handSide, - new ActuatorPWM(32), // Thumb - new ActuatorPWM(33), // Index - new ActuatorPWM(25), // Middle - new ActuatorPWM(26), // Ring - new ActuatorPWM(27), // Little - new ActuatorPWM(14) // Wrist + new LedcOutput(32), // Thumb + new LedcOutput(33), // Index + new LedcOutput(25), // Middle + new LedcOutput(26), // Ring + new LedcOutput(27), // Little + new LedcOutput(14) // Wrist ); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -56,19 +57,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactosy2.cpp b/firmware/mode_configs/bhaptics/tactosy2.cpp index 20751691..c3053bd3 100644 --- a/firmware/mode_configs/bhaptics/tactosy2.cpp +++ b/firmware/mode_configs/bhaptics/tactosy2.cpp @@ -4,43 +4,44 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTOSY2_SIZE; -static const Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTOSY2; +static const std::array bhLayout = { BH_LAYOUT_TACTOSY2 }; void setupMode() { // Configure PWM pins to their positions on the forearm - auto forearmOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto forearmOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32), new ActuatorPWM(33), new ActuatorPWM(25) }, - { new ActuatorPWM(26), new ActuatorPWM(27), new ActuatorPWM(14) }, + { new LedcOutput(32), new LedcOutput(33), new LedcOutput(25) }, + { new LedcOutput(26), new LedcOutput(27), new LedcOutput(14) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::Accessory, new VibroPlane_Closest(forearmOutputs)); + app->getVibroBody()->addTarget(Target::Accessory, new FloatPlane_Closest(forearmOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -49,19 +50,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro, Target::Accessory); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro, Target::Accessory); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactosyf.cpp b/firmware/mode_configs/bhaptics/tactosyf.cpp index b7667f0f..b2c47e26 100644 --- a/firmware/mode_configs/bhaptics/tactosyf.cpp +++ b/firmware/mode_configs/bhaptics/tactosyf.cpp @@ -4,44 +4,45 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTOSYF_SIZE; -static const Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTOSYF; +static const std::array bhLayout = { BH_LAYOUT_TACTOSYF }; void setupMode() { // Configure PWM pins to their positions on the feet - auto footOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto footOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32) }, - { new ActuatorPWM(33) }, - { new ActuatorPWM(25) }, + { new LedcOutput(32) }, + { new LedcOutput(33) }, + { new LedcOutput(25) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::Accessory, new VibroPlane_Closest(footOutputs)); + app->getVibroBody()->addTarget(Target::Accessory, new FloatPlane_Closest(footOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -50,19 +51,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro, Target::Accessory); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro, Target::Accessory); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactosyh.cpp b/firmware/mode_configs/bhaptics/tactosyh.cpp index 50b46ec9..11bbe0a4 100644 --- a/firmware/mode_configs/bhaptics/tactosyh.cpp +++ b/firmware/mode_configs/bhaptics/tactosyh.cpp @@ -4,44 +4,45 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTOSYH_SIZE; -static const Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTOSYH; +static const std::array bhLayout = { BH_LAYOUT_TACTOSYH }; void setupMode() { // Configure PWM pins to their positions on the hands - auto handOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto handOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32) }, - { new ActuatorPWM(33) }, - { new ActuatorPWM(25) } + { new LedcOutput(32) }, + { new LedcOutput(33) }, + { new LedcOutput(25) } // clang-format on }); - app->getHapticBody()->addTarget(Target::Accessory, new VibroPlane_Closest(handOutputs)); + app->getVibroBody()->addTarget(Target::Accessory, new FloatPlane_Closest(handOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -50,19 +51,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro, Target::Accessory); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro, Target::Accessory); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactsuit_x16.cpp b/firmware/mode_configs/bhaptics/tactsuit_x16.cpp index 62fc1e45..ae6011f2 100644 --- a/firmware/mode_configs/bhaptics/tactsuit_x16.cpp +++ b/firmware/mode_configs/bhaptics/tactsuit_x16.cpp @@ -4,54 +4,54 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTSUITX16_SIZE; -static const OutputLayout bhLayout[BH_LAYOUT_TACTSUITX16_SIZE] = BH_LAYOUT_TACTSUITX16; +static const std::array bhLayout = { BH_LAYOUT_TACTSUITX16 }; // Ouput indices, responsible for x40 => x16 grouping -static constexpr size_t layoutGroupsSize = BH_LAYOUT_TACTSUITX16_GROUPS_SIZE; -static const uint8_t layoutGroups[layoutGroupsSize] = BH_LAYOUT_TACTSUITX16_GROUPS; +static const std::array layoutGroups = BH_LAYOUT_TACTSUITX16_GROUPS; void setupMode() { // Configure PWM pins to their positions on the vest - auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32), new ActuatorPWM(33), new ActuatorPWM(25), new ActuatorPWM(26) }, - { new ActuatorPWM(27), new ActuatorPWM(14), new ActuatorPWM(12), new ActuatorPWM(13) }, + { new LedcOutput(32), new LedcOutput(33), new LedcOutput(25), new LedcOutput(26) }, + { new LedcOutput(27), new LedcOutput(14), new LedcOutput(12), new LedcOutput(13) }, // clang-format on }); - auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(19), new ActuatorPWM(18), new ActuatorPWM(5), new ActuatorPWM(17) }, - { new ActuatorPWM(16), new ActuatorPWM(4), new ActuatorPWM(2), new ActuatorPWM(15) }, + { new LedcOutput(19), new LedcOutput(18), new LedcOutput(5), new LedcOutput(17) }, + { new LedcOutput(16), new LedcOutput(4), new LedcOutput(2), new LedcOutput(15) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::ChestFront, new VibroPlane_Closest(frontOutputs)); - app->getHapticBody()->addTarget(Target::ChestBack, new VibroPlane_Closest(backOutputs)); + app->getVibroBody()->addTarget(Target::ChestFront, new FloatPlane_Closest(frontOutputs)); + app->getVibroBody()->addTarget(Target::ChestBack, new FloatPlane_Closest(backOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -60,19 +60,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyVestGrouped(app->getHapticBody(), value, bhLayout, layoutGroups); + Decoder::applyVestGrouped(app->getVibroBody(), value, bhLayout, layoutGroups); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactsuit_x16_pca9685.cpp b/firmware/mode_configs/bhaptics/tactsuit_x16_pca9685.cpp index 8d3383e8..b8b05c77 100644 --- a/firmware/mode_configs/bhaptics/tactsuit_x16_pca9685.cpp +++ b/firmware/mode_configs/bhaptics/tactsuit_x16_pca9685.cpp @@ -4,59 +4,59 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTSUITX16_SIZE; -static const OutputLayout bhLayout[BH_LAYOUT_TACTSUITX16_SIZE] = BH_LAYOUT_TACTSUITX16; +static const std::array bhLayout = { BH_LAYOUT_TACTSUITX16 }; // Ouput indices, responsible for x40 => x16 grouping -static constexpr size_t layoutGroupsSize = BH_LAYOUT_TACTSUITX16_GROUPS_SIZE; -static const uint8_t layoutGroups[layoutGroupsSize] = BH_LAYOUT_TACTSUITX16_GROUPS; +static const std::array layoutGroups = BH_LAYOUT_TACTSUITX16_GROUPS; void setupMode() { // Configure the PCA9685 - auto pwm = new Adafruit_PWMServoDriver(0x40); + auto* pwm = new Adafruit_PWMServoDriver(0x40); pwm->begin(); pwm->setPWMFreq(PWM_FREQUENCY); // Assign the pins on the configured PCA9685 to positions on the vest - auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPCA9685(pwm, 0), new ActuatorPCA9685(pwm, 1), new ActuatorPCA9685(pwm, 2), new ActuatorPCA9685(pwm, 3) }, - { new ActuatorPCA9685(pwm, 4), new ActuatorPCA9685(pwm, 5), new ActuatorPCA9685(pwm, 6), new ActuatorPCA9685(pwm, 7) }, + { new PCA9685Output(pwm, 0), new PCA9685Output(pwm, 1), new PCA9685Output(pwm, 2), new PCA9685Output(pwm, 3) }, + { new PCA9685Output(pwm, 4), new PCA9685Output(pwm, 5), new PCA9685Output(pwm, 6), new PCA9685Output(pwm, 7) }, // clang-format on }); - auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPCA9685(pwm, 8), new ActuatorPCA9685(pwm, 9), new ActuatorPCA9685(pwm, 10), new ActuatorPCA9685(pwm, 11) }, - { new ActuatorPCA9685(pwm, 12), new ActuatorPCA9685(pwm, 13), new ActuatorPCA9685(pwm, 14), new ActuatorPCA9685(pwm, 15) }, + { new PCA9685Output(pwm, 8), new PCA9685Output(pwm, 9), new PCA9685Output(pwm, 10), new PCA9685Output(pwm, 11) }, + { new PCA9685Output(pwm, 12), new PCA9685Output(pwm, 13), new PCA9685Output(pwm, 14), new PCA9685Output(pwm, 15) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::ChestFront, new VibroPlane_Closest(frontOutputs)); - app->getHapticBody()->addTarget(Target::ChestBack, new VibroPlane_Closest(backOutputs)); + app->getVibroBody()->addTarget(Target::ChestFront, new FloatPlane_Closest(frontOutputs)); + app->getVibroBody()->addTarget(Target::ChestBack, new FloatPlane_Closest(backOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -65,19 +65,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyVestGrouped(app->getHapticBody(), value, bhLayout, layoutGroups); + Decoder::applyVestGrouped(app->getVibroBody(), value, bhLayout, layoutGroups); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactsuit_x40.cpp b/firmware/mode_configs/bhaptics/tactsuit_x40.cpp index 13fce314..a3d77b06 100644 --- a/firmware/mode_configs/bhaptics/tactsuit_x40.cpp +++ b/firmware/mode_configs/bhaptics/tactsuit_x40.cpp @@ -4,30 +4,31 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include -#include +#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTSUITX40_SIZE; -static const OutputLayout bhLayout[bhLayoutSize] = BH_LAYOUT_TACTSUITX40; +static const std::array bhLayout = { BH_LAYOUT_TACTSUITX40 }; void setupMode() { @@ -42,29 +43,29 @@ void setupMode() // Assign the pins on the configured PCA9685s and PWM pins to locations on the // vest - auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPCA9685(pwm0, 0), new ActuatorPCA9685(pwm0, 1), new ActuatorPCA9685(pwm0, 2), new ActuatorPCA9685(pwm0, 3) }, - { new ActuatorPCA9685(pwm0, 4), new ActuatorPCA9685(pwm0, 5), new ActuatorPCA9685(pwm0, 6), new ActuatorPCA9685(pwm0, 7) }, - { new ActuatorPCA9685(pwm0, 8), new ActuatorPCA9685(pwm0, 9), new ActuatorPCA9685(pwm0, 10), new ActuatorPCA9685(pwm0, 11) }, - { new ActuatorPCA9685(pwm0, 12), new ActuatorPCA9685(pwm0, 13), new ActuatorPCA9685(pwm0, 14), new ActuatorPCA9685(pwm0, 15) }, - { new ActuatorPWM(32), new ActuatorPWM(33), new ActuatorPWM(25), new ActuatorPWM(26) }, + { new PCA9685Output(pwm0, 0), new PCA9685Output(pwm0, 1), new PCA9685Output(pwm0, 2), new PCA9685Output(pwm0, 3) }, + { new PCA9685Output(pwm0, 4), new PCA9685Output(pwm0, 5), new PCA9685Output(pwm0, 6), new PCA9685Output(pwm0, 7) }, + { new PCA9685Output(pwm0, 8), new PCA9685Output(pwm0, 9), new PCA9685Output(pwm0, 10), new PCA9685Output(pwm0, 11) }, + { new PCA9685Output(pwm0, 12), new PCA9685Output(pwm0, 13), new PCA9685Output(pwm0, 14), new PCA9685Output(pwm0, 15) }, + { new LedcOutput(32), new LedcOutput(33), new LedcOutput(25), new LedcOutput(26) }, // clang-format on }); - auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPCA9685(pwm1, 0), new ActuatorPCA9685(pwm1, 1), new ActuatorPCA9685(pwm1, 2), new ActuatorPCA9685(pwm1, 3) }, - { new ActuatorPCA9685(pwm1, 4), new ActuatorPCA9685(pwm1, 5), new ActuatorPCA9685(pwm1, 6), new ActuatorPCA9685(pwm1, 7) }, - { new ActuatorPCA9685(pwm1, 8), new ActuatorPCA9685(pwm1, 9), new ActuatorPCA9685(pwm1, 10), new ActuatorPCA9685(pwm1, 11) }, - { new ActuatorPCA9685(pwm1, 12), new ActuatorPCA9685(pwm1, 13), new ActuatorPCA9685(pwm1, 14), new ActuatorPCA9685(pwm1, 15) }, - { new ActuatorPWM(27), new ActuatorPWM(14), new ActuatorPWM(12), new ActuatorPWM(13) }, + { new PCA9685Output(pwm1, 0), new PCA9685Output(pwm1, 1), new PCA9685Output(pwm1, 2), new PCA9685Output(pwm1, 3) }, + { new PCA9685Output(pwm1, 4), new PCA9685Output(pwm1, 5), new PCA9685Output(pwm1, 6), new PCA9685Output(pwm1, 7) }, + { new PCA9685Output(pwm1, 8), new PCA9685Output(pwm1, 9), new PCA9685Output(pwm1, 10), new PCA9685Output(pwm1, 11) }, + { new PCA9685Output(pwm1, 12), new PCA9685Output(pwm1, 13), new PCA9685Output(pwm1, 14), new PCA9685Output(pwm1, 15) }, + { new LedcOutput(27), new LedcOutput(14), new LedcOutput(12), new LedcOutput(13) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::ChestFront, new VibroPlane_Closest(frontOutputs)); - app->getHapticBody()->addTarget(Target::ChestBack, new VibroPlane_Closest(backOutputs)); + app->getVibroBody()->addTarget(Target::ChestFront, new FloatPlane_Closest(frontOutputs)); + app->getVibroBody()->addTarget(Target::ChestBack, new FloatPlane_Closest(backOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -73,19 +74,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyVest(app->getHapticBody(), value, bhLayout); + Decoder::applyVest(app->getVibroBody(), value, bhLayout); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/bhaptics/tactvisor.cpp b/firmware/mode_configs/bhaptics/tactvisor.cpp index f7e7ab44..38ece6af 100644 --- a/firmware/mode_configs/bhaptics/tactvisor.cpp +++ b/firmware/mode_configs/bhaptics/tactvisor.cpp @@ -4,42 +4,43 @@ #include #include -#include "senseshift.h" +#include #include -#include -#include +#include +#include #include #include #include -#include +#include using namespace SenseShift; +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; using namespace SenseShift::Arduino::Output; using namespace SenseShift::Arduino::Input; -using namespace SenseShift::FreeRTOS::Input; using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; -extern SenseShift::SenseShift App; -SenseShift::SenseShift* app = &App; +extern Application App; +Application* app = &App; -static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTVISOR_SIZE; -static const Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTVISOR; +static const std::array bhLayout = { BH_LAYOUT_TACTVISOR }; void setupMode() { // Configure PWM pins to their positions on the face - auto faceOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto faceOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ // clang-format off - { new ActuatorPWM(32), new ActuatorPWM(33), new ActuatorPWM(25), new ActuatorPWM(26) }, + { new LedcOutput(32), new LedcOutput(33), new LedcOutput(25), new LedcOutput(26) }, // clang-format on }); - app->getHapticBody()->addTarget(Target::FaceFront, new VibroPlane_Closest(faceOutputs)); + app->getVibroBody()->addTarget(Target::FaceFront, new FloatPlane_Closest(faceOutputs)); - app->getHapticBody()->setup(); + app->getVibroBody()->setup(); auto* bhBleConnection = new BLE::Connection( { @@ -48,19 +49,33 @@ void setupMode() .serialNumber = BH_SERIAL_NUMBER, }, [](std::string& value) -> void { - Decoder::applyPlain(app->getHapticBody(), value, bhLayout, Effect::Vibro, Target::FaceFront); + Decoder::applyPlain(app->getVibroBody(), value, bhLayout, Effect::Vibro, Target::FaceFront); }, app ); bhBleConnection->begin(); #if defined(SENSESHIFT_BATTERY_ENABLED) && SENSESHIFT_BATTERY_ENABLED == true - auto* battery = new TaskedSensor( - new BatterySensor(new NaiveBatterySensor(new AnalogSensor(36)), app), + auto* batteryVoltageSensor = new SimpleSensorDecorator(new AnalogSimpleSensor(36)); + batteryVoltageSensor->addFilters({ + new MultiplyFilter(3.3F), // Convert to raw pin voltage + new VoltageDividerFilter(27000.0F, 100000.0F), // Convert to voltage divider voltage + }); + auto* batteryTask = new ::SenseShift::FreeRTOS::ComponentUpdateTask>( + batteryVoltageSensor, SENSESHIFT_BATTERY_SAMPLE_RATE, { "ADC Battery", 4096, SENSESHIFT_BATTERY_TASK_PRIORITY, tskNO_AFFINITY } ); - battery->begin(); + batteryTask->begin(); + + auto* batterySensor = new LookupTableInterpolateBatterySensor>( + batteryVoltageSensor, + &VoltageMap::LiPO_1S_42 + ); + batterySensor->addValueCallback([](BatteryState value) -> void { + app->postEvent(new BatteryLevelEvent(value)); + }); + batterySensor->init(); #endif } diff --git a/firmware/mode_configs/opengloves/opengloves.cpp b/firmware/mode_configs/opengloves/opengloves.cpp index 3ebfca56..382ad083 100644 --- a/firmware/mode_configs/opengloves/opengloves.cpp +++ b/firmware/mode_configs/opengloves/opengloves.cpp @@ -1,225 +1,45 @@ -#include -#include -#include -#include -#include -#include -#include -#include +#include #include -#include -#include -#include -#include +#include -using namespace OpenGloves; using namespace SenseShift::OpenGloves; -HandSensors handSensors = { -#if FINGER_THUMB_SPLAY - .thumb = FINGER_SPLAY_CLASS( - IEncodedInput::Type::THUMB, - PIN_FINGER_THUMB, - FINGER_THUMB_INVERT, - CALIBRATION_CURL, - PIN_FINGER_THUMB_SPLAY, - FINGER_THUMB_SPLAY_INVERT, - CALIBRATION_SPLAY - ), -#elif FINGER_THUMB_ENABLED - .thumb = FINGER_CLASS(IEncodedInput::Type::THUMB, PIN_FINGER_THUMB, FINGER_THUMB_INVERT, CALIBRATION_CURL), -#endif - -#if FINGER_INDEX_SPLAY - .index = FINGER_SPLAY_CLASS( - IEncodedInput::Type::INDEX, - PIN_FINGER_INDEX, - FINGER_INDEX_INVERT, - CALIBRATION_CURL, - PIN_FINGER_INDEX_SPLAY, - FINGER_INDEX_SPLAY_INVERT, - CALIBRATION_SPLAY - ), -#elif FINGER_INDEX_ENABLED - .index = FINGER_CLASS(IEncodedInput::Type::INDEX, PIN_FINGER_INDEX, FINGER_INDEX_INVERT, CALIBRATION_CURL), -#endif - -#if FINGER_MIDDLE_SPLAY - .middle = FINGER_SPLAY_CLASS( - IEncodedInput::Type::MIDDLE, - PIN_FINGER_MIDDLE, - FINGER_MIDDLE_INVERT, - CALIBRATION_CURL, - PIN_FINGER_MIDDLE_SPLAY, - FINGER_MIDDLE_SPLAY_INVERT, - CALIBRATION_SPLAY - ), -#elif FINGER_MIDDLE_ENABLED - .middle = FINGER_CLASS(IEncodedInput::Type::MIDDLE, PIN_FINGER_MIDDLE, FINGER_MIDDLE_INVERT, CALIBRATION_CURL), -#endif - -#if FINGER_RING_SPLAY - .ring = FINGER_SPLAY_CLASS( - IEncodedInput::Type::RING, - PIN_FINGER_RING, - FINGER_RING_INVERT, - CALIBRATION_CURL, - PIN_FINGER_RING_SPLAY, - FINGER_RING_SPLAY_INVERT, - CALIBRATION_SPLAY - ), -#elif FINGER_RING_ENABLED - .ring = FINGER_CLASS(IEncodedInput::Type::RING, PIN_FINGER_RING, FINGER_RING_INVERT, CALIBRATION_CURL), -#endif - -#if FINGER_PINKY_SPLAY - .pinky = FINGER_SPLAY_CLASS( - IEncodedInput::Type::PINKY, - PIN_FINGER_PINKY, - FINGER_PINKY_INVERT, - CALIBRATION_CURL, - PIN_FINGER_PINKY_SPLAY, - FINGER_PINKY_SPLAY_INVERT, - CALIBRATION_SPLAY - ), -#elif FINGER_PINKY_ENABLED - .pinky = FINGER_CLASS(IEncodedInput::Type::PINKY, PIN_FINGER_PINKY, FINGER_PINKY_INVERT, CALIBRATION_CURL), -#endif -}; - -#if BUTTON_CALIBRATE_ENABLED -std::optional> calibrateButton = - BUTTON_CLASS(IEncodedInput::Type::CALIBRATE, PIN_BUTTON_CALIBRATE, BUTTON_CALIBRATE_INVERT); -#else -std::optional> calibrateButton = std::nullopt; -#endif - -std::vector*> buttons = std::vector*> +void setupMode() { - // https://github.com/llvm/llvm-project/issues/63776 - // clang-format off -#if BUTTON_A_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::A_BTN, PIN_BUTTON_A, BUTTON_A_INVERT), -#endif - -#if BUTTON_B_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::B_BTN, PIN_BUTTON_B, BUTTON_B_INVERT), -#endif + auto* communication = AutoConfig::createTransport(); + auto* encoding = new og::AlphaEncoder(); -#if BUTTON_JOYSTICK_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::JOY_BTN, PIN_BUTTON_JOYSTICK, BUTTON_JOYSTICK_INVERT), -#endif + auto input_sensors = AutoConfig::createInput(); -#if BUTTON_MENU_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::MENU, PIN_BUTTON_MENU, BUTTON_MENU_INVERT), -#endif + OpenGlovesTrackingComponent::Config tracking_config(CALIBRATION_DURATION, CALIBRATION_ALWAYS_CALIBRATE); + auto* og_tracking = new OpenGlovesTrackingComponent(tracking_config, input_sensors, communication, encoding); -#if GESTURE_TRIGGER_ENABLED && FINGER_INDEX_ENABLED - new GESTURE_CLASS( - IEncodedInput::Type::TRIGGER, - new TriggerGesture(handSensors.index.value(), GESTURE_TRIGGER_THRESHOLD) - ), -#elif BUTTON_TRIGGER_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::TRIGGER, PIN_BUTTON_TRIGGER, BUTTON_TRIGGER_INVERT), -#endif - -#if GESTURE_GRAB_ENABLED && FINGER_INDEX_ENABLED && FINGER_MIDDLE_ENABLED && FINGER_RING_ENABLED && FINGER_PINKY_ENABLED - new GESTURE_CLASS( - IEncodedInput::Type::GRAB, - new GrabGesture( - handSensors.index.value(), - handSensors.middle.value(), - handSensors.ring.value(), - handSensors.pinky.value(), - GESTURE_GRAB_THRESHOLD - ) - ), -#elif BUTTON_GRAB_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::GRAB, PIN_BUTTON_GRAB, BUTTON_GRAB_INVERT), -#endif - -#if GESTURE_PINCH_ENABLED && FINGER_THUMB_ENABLED && FINGER_INDEX_ENABLED - new GESTURE_CLASS( - IEncodedInput::Type::PINCH, - new PinchGesture(handSensors.thumb.value(), handSensors.index.value(), GESTURE_PINCH_THRESHOLD) - ), -#elif BUTTON_PINCH_ENABLED - new BUTTON_CLASS(IEncodedInput::Type::PINCH, PIN_BUTTON_PINCH, BUTTON_PINCH_INVERT), -#endif - // clang-format on -}; - -std::vector*> joysticks = { -#if JOYSTICK_ENABLED - new JOYSTICK_CLASS(IEncodedInput::Type::JOY_X, PIN_JOYSTICK_X, JOYSTICK_X_INVERT, JOYSTICK_DEADZONE), - new JOYSTICK_CLASS(IEncodedInput::Type::JOY_Y, PIN_JOYSTICK_Y, JOYSTICK_Y_INVERT, JOYSTICK_DEADZONE), -#endif - // todo: get joystick movement from Thumb finger movement? -}; - -std::vector otherSensors = std::vector(); - -OpenGlovesTrackingTaskConfig config = - OpenGlovesTrackingTaskConfig(UPDATE_RATE, CALIBRATION_DURATION, CALIBRATION_ALWAYS_CALIBRATE); -OpenGlovesTrackingTask* trackingTask; - -#if FFB_ENABLED -HandActuators handActuators = { -#if FFB_THUMB_ENABLED - .thumb = new SenseShift::Arduino::Output::ActuatorServo(PIN_FFB_THUMB), -#endif - -#if FFB_INDEX_ENABLED - .index = new SenseShift::Arduino::Output::ActuatorServo(PIN_FFB_INDEX), -#endif - -#if FFB_MIDDLE_ENABLED - .middle = new SenseShift::Arduino::Output::ActuatorServo(PIN_FFB_MIDDLE), -#endif - -#if FFB_RING_ENABLED - .ring = new SenseShift::Arduino::Output::ActuatorServo(PIN_FFB_RING), -#endif - -#if FFB_PINKY_ENABLED - .pinky = new SenseShift::Arduino::Output::ActuatorServo(PIN_FFB_PINKY), -#endif -}; -OpenGlovesForceFeedbackTask* ffbTask; -#endif - -void setupMode() -{ - auto* communication = AutoConfig::setupTransport(); - - trackingTask = new OpenGlovesTrackingTask( - config, - *communication, - handSensors, - buttons, - joysticks, - otherSensors, - calibrateButton, + auto* og_tracking_task = new SenseShift::FreeRTOS::ComponentUpdateTask( + og_tracking, + 1000 / UPDATE_RATE, { - .name = "OpenGlovesSensorTask", + .name = "OG_TRACKING", .stackDepth = 8192, - .priority = OPENGLOVES_FINGERS_TASK_PRIORITY, + .priority = 1, } ); - trackingTask->begin(); + og_tracking_task->begin(); #if FFB_ENABLED - ffbTask = new OpenGlovesForceFeedbackTask( - *communication, - handActuators, - UPDATE_RATE, + auto output_writers = AutoConfig::createFfbOutputs(); + + auto* og_ffb = new OpenGlovesForceFeedbackComponent(output_writers, communication, encoding); + + auto* og_ffb_task = new ::SenseShift::FreeRTOS::ComponentUpdateTask( + og_ffb, + 1000 / UPDATE_RATE, { - .name = "OpenGlovesForceFeedbackTask", + .name = "OG_FFB", .stackDepth = 8192, - .priority = OPENGLOVES_FINGERS_TASK_PRIORITY, + .priority = 1, } ); - ffbTask->begin(); + og_ffb_task->begin(); #endif } diff --git a/firmware/senseshift.cpp b/firmware/senseshift.cpp index 3ba2b1f7..a4f7ab8b 100644 --- a/firmware/senseshift.cpp +++ b/firmware/senseshift.cpp @@ -6,27 +6,29 @@ #include #endif -#include +#include namespace SenseShift { - SenseShift::SenseShift() + static const char* const TAG = "application"; + + Application::Application() { - this->pHapticBody = new Body::Haptics::HapticBody(); + this->vibro_body_ = new Body::Haptics::FloatBody(); } - void SenseShift::postEvent(const IEvent* event) + void Application::postEvent(const IEvent* event) { - log_i("Event dispatched at %u: %s (%p)", millis(), event->eventName.c_str(), event); + LOG_I(TAG, "Event dispatched at %u: %s (%p)", millis(), event->eventName.c_str(), event); - for (auto* listener : this->eventListeners) { + for (const auto* listener : this->event_listeners_) { listener->handleEvent(event); } delete event; } - void SenseShift::addEventListener(const IEventListener* listener) + void Application::addEventListener(const IEventListener* listener) { - this->eventListeners.push_back(listener); + this->event_listeners_.push_back(listener); } } // namespace SenseShift diff --git a/include/senseshift.h b/include/senseshift.h index 95c5603e..d3d8629d 100644 --- a/include/senseshift.h +++ b/include/senseshift.h @@ -2,25 +2,25 @@ #include "config/all.h" -#include +#include "senseshift/battery/input/battery_sensor.hpp" #include #include #include namespace SenseShift { - class SenseShift final : public IEventDispatcher { - private: - std::vector eventListeners{}; - Body::Haptics::HapticBody* pHapticBody; - ::SenseShift::Battery::IBatterySensor* battery; - + class Application final : public IEventDispatcher { public: - SenseShift(); + Application(); - Body::Haptics::HapticBody* getHapticBody() { return this->pHapticBody; }; + [[nodiscard]] auto getVibroBody() const -> Body::Haptics::FloatBody* { return this->vibro_body_; }; void postEvent(const IEvent* event) override; void addEventListener(const IEventListener* listener) override; + + private: + std::vector event_listeners_{}; + Body::Haptics::FloatBody* vibro_body_; + Battery::Input::IBatterySensor* battery_ = nullptr; }; } // namespace SenseShift diff --git a/ini/bhaptics.ini b/ini/bhaptics.ini index ac624ea8..0d3c4343 100644 --- a/ini/bhaptics.ini +++ b/ini/bhaptics.ini @@ -1,71 +1,78 @@ [bhaptics] -platform = platformio/espressif32@^6.1.0 -platform_packages = - platformio/framework-arduinoespressif32@^3.20007.0 -framework = arduino -board = esp32doit-devkit-v1 -upload_speed = 921600 -monitor_speed = 115200 +platform = platformio/espressif32@^6.1.0 +platform_packages = + platformio/framework-arduinoespressif32@^3.20014.231204 +framework = arduino +board = esp32doit-devkit-v1 +upload_speed = 921600 +monitor_speed = 115200 -build_flags = ${common.build_flags} - -D BHAPTICS -build_unflags = ${common.build_unflags} -build_src_filter = ${common.build_src_filter} -lib_deps = ${common.lib_deps} +build_flags = + ${common.build_flags} + -D BHAPTICS +build_unflags = ${common.build_unflags} +build_src_filter = ${common.build_src_filter} +lib_deps = ${common.lib_deps} [env:bhaptics_tactsuit_x16] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTSUITX16 - -D BH_BLE_APPEARANCE=510 - '-D BLUETOOTH_NAME="TactSuitX16"' - '-D BH_SERIAL_NUMBER={ 0x0d, 0x3a, 0xeb, 0x77, 0xbe, 0xf8, 0x7a, 0x1e, 0x3b, 0x2a }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTSUITX16 + -D BH_BLE_APPEARANCE=510 + '-D BLUETOOTH_NAME="TactSuitX16"' + '-D BH_SERIAL_NUMBER={ 0x0d, 0x3a, 0xeb, 0x77, 0xbe, 0xf8, 0x7a, 0x1e, 0x3b, 0x2a }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactsuit_x16_pca9685] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTSUITX16 - -D BH_BLE_APPEARANCE=510 - '-D BLUETOOTH_NAME="TactSuitX16"' - '-D BH_SERIAL_NUMBER={ 0x0d, 0x3a, 0xeb, 0x77, 0xbe, 0xf8, 0x7a, 0x1e, 0x3b, 0x2a }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTSUITX16 + -D BH_BLE_APPEARANCE=510 + '-D BLUETOOTH_NAME="TactSuitX16"' + '-D BH_SERIAL_NUMBER={ 0x0d, 0x3a, 0xeb, 0x77, 0xbe, 0xf8, 0x7a, 0x1e, 0x3b, 0x2a }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactsuit_x40] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTSUITX40 - -D BH_BLE_APPEARANCE=509 - '-D BLUETOOTH_NAME="TactSuitX40"' - '-D BH_SERIAL_NUMBER={ 0xcf, 0xcb, 0x0d, 0x95, 0x5f, 0xf6, 0xee, 0x2c, 0xbd, 0x73 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTSUITX40 + -D BH_BLE_APPEARANCE=509 + '-D BLUETOOTH_NAME="TactSuitX40"' + '-D BH_SERIAL_NUMBER={ 0xcf, 0xcb, 0x0d, 0x95, 0x5f, 0xf6, 0xee, 0x2c, 0xbd, 0x73 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} ; [env:bhaptics_tactbelt] ; platform = ${bhaptics.platform} @@ -86,183 +93,203 @@ lib_deps = ${bhaptics.lib_deps} ; lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosy2_forearm_left] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSY2 - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="Tactosy2_L"' - '-D BH_SERIAL_NUMBER={ 0xa0, 0xba, 0x0a, 0xd1, 0xbf, 0x36, 0x11, 0x30, 0xa4, 0xff }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSY2 + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="Tactosy2_L"' + '-D BH_SERIAL_NUMBER={ 0xa0, 0xba, 0x0a, 0xd1, 0xbf, 0x36, 0x11, 0x30, 0xa4, 0xff }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosy2_forearm_right] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSY2 - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="Tactosy2_R"' - '-D BH_SERIAL_NUMBER={ 0xb0, 0x1c, 0xc1, 0xf8, 0xec, 0x12, 0x18, 0x4e, 0x09, 0x77 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSY2 + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="Tactosy2_R"' + '-D BH_SERIAL_NUMBER={ 0xb0, 0x1c, 0xc1, 0xf8, 0xec, 0x12, 0x18, 0x4e, 0x09, 0x77 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosyh_hand_left] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSYH - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactosyH_L"' - '-D BH_SERIAL_NUMBER={ 0xc1, 0x36, 0xdc, 0x21, 0xc9, 0xd4, 0x17, 0x85, 0xbb, 0x90 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSYH + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactosyH_L"' + '-D BH_SERIAL_NUMBER={ 0xc1, 0x36, 0xdc, 0x21, 0xc9, 0xd4, 0x17, 0x85, 0xbb, 0x90 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosyh_hand_right] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSYH - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactosyH_R"' - '-D BH_SERIAL_NUMBER={ 0xc7, 0x5f, 0x3b, 0x06, 0x38, 0xba, 0x34, 0xfa, 0x36, 0xc1 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSYH + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactosyH_R"' + '-D BH_SERIAL_NUMBER={ 0xc7, 0x5f, 0x3b, 0x06, 0x38, 0xba, 0x34, 0xfa, 0x36, 0xc1 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosyf_foot_left] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSYF - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactosyF_L"' - '-D BH_SERIAL_NUMBER={ 0x1a, 0x45, 0x83, 0x44, 0x03, 0xc5, 0xf3, 0xc3, 0xf3, 0xb8 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSYF + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactosyF_L"' + '-D BH_SERIAL_NUMBER={ 0x1a, 0x45, 0x83, 0x44, 0x03, 0xc5, 0xf3, 0xc3, 0xf3, 0xb8 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactosyf_foot_right] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTOSYF - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactosyF_R"' - '-D BH_SERIAL_NUMBER={ 0x14, 0xb9, 0x02, 0x62, 0x41, 0xe4, 0x04, 0xb2, 0xc5, 0x11 }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTOSYF + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactosyF_R"' + '-D BH_SERIAL_NUMBER={ 0x14, 0xb9, 0x02, 0x62, 0x41, 0xe4, 0x04, 0xb2, 0xc5, 0x11 }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactal] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTAL - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="Tactal_"' - '-D BH_SERIAL_NUMBER={ 0xed, 0xcb, 0x55, 0x7c, 0xd7, 0xb9, 0x16, 0xc5, 0x18, 0x2a }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTAL + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="Tactal_"' + '-D BH_SERIAL_NUMBER={ 0xed, 0xcb, 0x55, 0x7c, 0xd7, 0xb9, 0x16, 0xc5, 0x18, 0x2a }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactvisor] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTAL - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactVisor_V____"' - '-D BH_SERIAL_NUMBER={ 0x5e, 0xa3, 0xdd, 0x12, 0x00, 0x01, 0x43, 0xc1, 0x26, 0x8a }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTAL + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactVisor_V____"' + '-D BH_SERIAL_NUMBER={ 0x5e, 0xa3, 0xdd, 0x12, 0x00, 0x01, 0x43, 0xc1, 0x26, 0x8a }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactglove_left] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTGLOVE - -D SENSESHIFT_HAND_SIDE=Left - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactGlove (L"' - '-D BH_SERIAL_NUMBER={ 0xcd, 0x0b, 0x81, 0x45, 0x85, 0xf9, 0x2b, 0x6c, 0xed, 0x5b }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTGLOVE + -D SENSESHIFT_HAND_SIDE=Left + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactGlove (L"' + '-D BH_SERIAL_NUMBER={ 0xcd, 0x0b, 0x81, 0x45, 0x85, 0xf9, 0x2b, 0x6c, 0xed, 0x5b }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} [env:bhaptics_tactglove_right] -platform = ${bhaptics.platform} -platform_packages = ${bhaptics.platform_packages} -framework = ${bhaptics.framework} -board = ${bhaptics.board} -upload_speed = ${bhaptics.upload_speed} -monitor_speed = ${bhaptics.monitor_speed} +platform = ${bhaptics.platform} +platform_packages = ${bhaptics.platform_packages} +framework = ${bhaptics.framework} +board = ${bhaptics.board} +upload_speed = ${bhaptics.upload_speed} +monitor_speed = ${bhaptics.monitor_speed} -build_flags = ${bhaptics.build_flags} - -D BH_DEVICE_TACTGLOVE - -D SENSESHIFT_HAND_SIDE=Right - -D BH_BLE_APPEARANCE=508 - '-D BLUETOOTH_NAME="TactGlove (R"' - '-D BH_SERIAL_NUMBER={ 0x12, 0x0b, 0xae, 0xbf, 0xbc, 0x90, 0x3b, 0x0d, 0x84, 0xdd }' -build_unflags = ${bhaptics.build_unflags} -build_src_filter = ${bhaptics.build_src_filter} - + -lib_deps = ${bhaptics.lib_deps} +build_flags = + ${bhaptics.build_flags} + -D BH_DEVICE_TACTGLOVE + -D SENSESHIFT_HAND_SIDE=Right + -D BH_BLE_APPEARANCE=508 + '-D BLUETOOTH_NAME="TactGlove (R"' + '-D BH_SERIAL_NUMBER={ 0x12, 0x0b, 0xae, 0xbf, 0xbc, 0x90, 0x3b, 0x0d, 0x84, 0xdd }' +build_unflags = ${bhaptics.build_unflags} +build_src_filter = + ${bhaptics.build_src_filter} + + +lib_deps = ${bhaptics.lib_deps} diff --git a/ini/opengloves-indexer.ini b/ini/opengloves-indexer.ini index 7d5cd163..08048903 100644 --- a/ini/opengloves-indexer.ini +++ b/ini/opengloves-indexer.ini @@ -3,173 +3,183 @@ ; Wiring Diagram: https://github.com/Valsvirtuals/Indexer/wiki/wiring-and-pinout ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:indexer-c] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = wemos_d1_mini32 -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=25 - -D PIN_FINGER_INDEX=14 - -D PIN_FINGER_MIDDLE=33 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_JOYSTICK_X=12 - -D PIN_JOYSTICK_Y=4 - -D PIN_BUTTON_JOYSTICK=0 - - -D PIN_BUTTON_A=2 - -D PIN_BUTTON_B=11 - ; -D PIN_BUTTON_MENU=5 - -D PIN_BUTTON_CALIBRATE=27 - ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = wemos_d1_mini32 +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + -D OG_ENCODE_FAST + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=25 + -D PIN_FINGER_INDEX=14 + -D PIN_FINGER_MIDDLE=33 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_JOYSTICK_X=12 + -D PIN_JOYSTICK_Y=4 + -D PIN_BUTTON_JOYSTICK=0 + + -D PIN_BUTTON_A=2 + -D PIN_BUTTON_B=11 + ; -D PIN_BUTTON_MENU=5 + -D PIN_BUTTON_CALIBRATE=27 + ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; Indexer CF ; Wiring Diagram: https://github.com/Valsvirtuals/Indexer/wiki/wiring-and-pinout ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:indexer-cf] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = wemos_d1_mini32 -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=25 - -D PIN_FINGER_INDEX=14 - -D PIN_FINGER_MIDDLE=33 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_JOYSTICK_X=12 - -D PIN_JOYSTICK_Y=4 - -D PIN_BUTTON_JOYSTICK=0 - - -D PIN_BUTTON_A=2 - -D PIN_BUTTON_B=11 - ; -D PIN_BUTTON_MENU=5 - -D PIN_BUTTON_CALIBRATE=27 - ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - - -D PIN_FFB_THUMB=16 - -D PIN_FFB_INDEX=17 - -D PIN_FFB_MIDDLE=21 - -D PIN_FFB_RING=22 - -D PIN_FFB_PINKY=1 - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = wemos_d1_mini32 +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + -D OG_ENCODE_FAST + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=25 + -D PIN_FINGER_INDEX=14 + -D PIN_FINGER_MIDDLE=33 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_JOYSTICK_X=12 + -D PIN_JOYSTICK_Y=4 + -D PIN_BUTTON_JOYSTICK=0 + + -D PIN_BUTTON_A=2 + -D PIN_BUTTON_B=11 + ; -D PIN_BUTTON_MENU=5 + -D PIN_BUTTON_CALIBRATE=27 + ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + + -D PIN_FFB_THUMB=16 + -D PIN_FFB_INDEX=17 + -D PIN_FFB_MIDDLE=21 + -D PIN_FFB_RING=22 + -D PIN_FFB_PINKY=1 + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; Indexer CS ; Wiring Diagram: https://github.com/Valsvirtuals/Indexer/wiki/wiring-and-pinout ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:indexer-cs] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = wemos_d1_mini32 -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=25 - -D PIN_FINGER_INDEX=14 - -D PIN_FINGER_MIDDLE=33 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_FINGER_THUMB_SPLAY=32 - -D PIN_FINGER_INDEX_SPLAY=13 - -D PIN_FINGER_MIDDLE_SPLAY=34 - -D PIN_FINGER_RING_SPLAY=35 - -D PIN_FINGER_PINKY_SPLAY=26 - - -D PIN_JOYSTICK_X=12 - -D PIN_JOYSTICK_Y=4 - -D PIN_BUTTON_JOYSTICK=0 - - -D PIN_BUTTON_A=2 - -D PIN_BUTTON_B=11 - ; -D PIN_BUTTON_MENU=5 - -D PIN_BUTTON_CALIBRATE=27 - ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = wemos_d1_mini32 +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=25 + -D PIN_FINGER_INDEX=14 + -D PIN_FINGER_MIDDLE=33 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_FINGER_THUMB_SPLAY=32 + -D PIN_FINGER_INDEX_SPLAY=13 + -D PIN_FINGER_MIDDLE_SPLAY=34 + -D PIN_FINGER_RING_SPLAY=35 + -D PIN_FINGER_PINKY_SPLAY=26 + + -D PIN_JOYSTICK_X=12 + -D PIN_JOYSTICK_Y=4 + -D PIN_BUTTON_JOYSTICK=0 + + -D PIN_BUTTON_A=2 + -D PIN_BUTTON_B=11 + ; -D PIN_BUTTON_MENU=5 + -D PIN_BUTTON_CALIBRATE=27 + ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; Indexer CSF ; Wiring Diagram: https://github.com/Valsvirtuals/Indexer/wiki/wiring-and-pinout ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:indexer-csf] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = wemos_d1_mini32 -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=25 - -D PIN_FINGER_INDEX=14 - -D PIN_FINGER_MIDDLE=33 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_FINGER_THUMB_SPLAY=32 - -D PIN_FINGER_INDEX_SPLAY=13 - -D PIN_FINGER_MIDDLE_SPLAY=34 - -D PIN_FINGER_RING_SPLAY=35 - -D PIN_FINGER_PINKY_SPLAY=26 - - -D PIN_JOYSTICK_X=12 - -D PIN_JOYSTICK_Y=4 - -D PIN_BUTTON_JOYSTICK=0 - - -D PIN_BUTTON_A=2 - -D PIN_BUTTON_B=11 - ; -D PIN_BUTTON_MENU=5 - -D PIN_BUTTON_CALIBRATE=27 - ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - - -D PIN_FFB_THUMB=16 - -D PIN_FFB_INDEX=17 - -D PIN_FFB_MIDDLE=21 - -D PIN_FFB_RING=22 - -D PIN_FFB_PINKY=1 - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = wemos_d1_mini32 +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=25 + -D PIN_FINGER_INDEX=14 + -D PIN_FINGER_MIDDLE=33 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_FINGER_THUMB_SPLAY=32 + -D PIN_FINGER_INDEX_SPLAY=13 + -D PIN_FINGER_MIDDLE_SPLAY=34 + -D PIN_FINGER_RING_SPLAY=35 + -D PIN_FINGER_PINKY_SPLAY=26 + + -D PIN_JOYSTICK_X=12 + -D PIN_JOYSTICK_Y=4 + -D PIN_BUTTON_JOYSTICK=0 + + -D PIN_BUTTON_A=2 + -D PIN_BUTTON_B=11 + ; -D PIN_BUTTON_MENU=5 + -D PIN_BUTTON_CALIBRATE=27 + ; -D PIN_BUTTON_TRIGGER=19 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=18 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + + -D PIN_FFB_THUMB=16 + -D PIN_FFB_INDEX=17 + -D PIN_FFB_MIDDLE=21 + -D PIN_FFB_RING=22 + -D PIN_FFB_PINKY=1 + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} diff --git a/ini/opengloves-lucidgloves.ini b/ini/opengloves-lucidgloves.ini index f4047f55..b777cee8 100644 --- a/ini/opengloves-lucidgloves.ini +++ b/ini/opengloves-lucidgloves.ini @@ -3,114 +3,124 @@ ; Wiring Diagram: https://github.com/LucidVR/lucidgloves/wiki/Prototype-3-Wiring-Diagram ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:lucidgloves-prototype3] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = ${opengloves.board} -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=32 - -D PIN_FINGER_INDEX=35 - -D PIN_FINGER_MIDDLE=34 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_JOYSTICK_X=33 - -D PIN_JOYSTICK_Y=25 - -D PIN_BUTTON_JOYSTICK=26 - - -D PIN_BUTTON_A=27 - -D PIN_BUTTON_B=14 - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = ${opengloves.board} +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + -D OG_ENCODE_FAST + + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=32 + -D PIN_FINGER_INDEX=35 + -D PIN_FINGER_MIDDLE=34 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_JOYSTICK_X=33 + -D PIN_JOYSTICK_Y=25 + -D PIN_BUTTON_JOYSTICK=26 + + -D PIN_BUTTON_A=27 + -D PIN_BUTTON_B=14 + + -D CALIBRATION_ALWAYS_CALIBRATE=true + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; LucidGloves Prototype 4 ; Wiring Diagram: https://github.com/LucidVR/lucidgloves/wiki/Prototype-4-Wiring-Diagram ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:lucidgloves-prototype4] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = ${opengloves.board} -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=32 - -D PIN_FINGER_INDEX=35 - -D PIN_FINGER_MIDDLE=34 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_JOYSTICK_X=33 - -D PIN_JOYSTICK_Y=25 - -D PIN_BUTTON_JOYSTICK=26 - - -D PIN_BUTTON_A=27 - -D PIN_BUTTON_B=14 - ; -D PIN_BUTTON_MENU=27 - -D PIN_BUTTON_CALIBRATE=12 - ; -D PIN_BUTTON_TRIGGER=12 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=13 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - - ; todo: add servo pins - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = ${opengloves.board} +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + -D OG_ENCODE_FAST + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=32 + -D PIN_FINGER_INDEX=35 + -D PIN_FINGER_MIDDLE=34 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_JOYSTICK_X=33 + -D PIN_JOYSTICK_Y=25 + -D PIN_BUTTON_JOYSTICK=26 + + -D PIN_BUTTON_A=27 + -D PIN_BUTTON_B=14 + ; -D PIN_BUTTON_MENU=27 + -D PIN_BUTTON_CALIBRATE=12 + ; -D PIN_BUTTON_TRIGGER=12 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=13 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ; LucidGloves Prototype 4 + Force Feedback ; Wiring Diagram: https://github.com/LucidVR/lucidgloves/wiki/Prototype-4-Wiring-Diagram ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; [env:lucidgloves-prototype4-ffb] -platform = ${opengloves.platform} -platform_packages = ${opengloves.platform_packages} -framework = ${opengloves.framework} -board = ${opengloves.board} -upload_speed = ${opengloves.upload_speed} -monitor_speed = ${opengloves.monitor_speed} - -build_flags = ${opengloves.build_flags} - ; Pins configuration - ; Comment out to disable - -D PIN_FINGER_THUMB=32 - -D PIN_FINGER_INDEX=35 - -D PIN_FINGER_MIDDLE=34 - -D PIN_FINGER_RING=39 - -D PIN_FINGER_PINKY=36 - - -D PIN_JOYSTICK_X=33 - -D PIN_JOYSTICK_Y=25 - -D PIN_BUTTON_JOYSTICK=26 - - -D PIN_BUTTON_A=27 - -D PIN_BUTTON_B=14 - ; -D PIN_BUTTON_MENU=27 - -D PIN_BUTTON_CALIBRATE=12 - ; -D PIN_BUTTON_TRIGGER=12 ; unused if GESTURE_TRIGGER is true - ; -D PIN_BUTTON_GRAB=13 ; unused if GESTURE_GRAB is true - ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true - - -D PIN_FFB_THUMB=17 - -D PIN_FFB_INDEX=21 - -D PIN_FFB_MIDDLE=19 - -D PIN_FFB_RING=18 - -D PIN_FFB_PINKY=5 - -build_unflags = ${opengloves.build_unflags} -build_src_filter = ${opengloves.build_src_filter} - + -lib_deps = ${opengloves.lib_deps} +platform = ${opengloves.platform} +platform_packages = ${opengloves.platform_packages} +framework = ${opengloves.framework} +board = ${opengloves.board} +upload_speed = ${opengloves.upload_speed} +monitor_speed = ${opengloves.monitor_speed} + +build_flags = + ${opengloves.build_flags} + -D OG_ENCODE_FAST + ;;;; Pins configuration + ;;;; Comment out to disable + -D PIN_FINGER_THUMB=32 + -D PIN_FINGER_INDEX=35 + -D PIN_FINGER_MIDDLE=34 + -D PIN_FINGER_RING=39 + -D PIN_FINGER_PINKY=36 + + -D PIN_JOYSTICK_X=33 + -D PIN_JOYSTICK_Y=25 + -D PIN_BUTTON_JOYSTICK=26 + + -D PIN_BUTTON_A=27 + -D PIN_BUTTON_B=14 + ; -D PIN_BUTTON_MENU=27 + -D PIN_BUTTON_CALIBRATE=12 + ; -D PIN_BUTTON_TRIGGER=12 ; unused if GESTURE_TRIGGER is true + ; -D PIN_BUTTON_GRAB=13 ; unused if GESTURE_GRAB is true + ; -D PIN_BUTTON_PINCH=23 ; unused if GESTURE_PINCH is true + + -D PIN_FFB_THUMB=17 + -D PIN_FFB_INDEX=21 + -D PIN_FFB_MIDDLE=19 + -D PIN_FFB_RING=18 + -D PIN_FFB_PINKY=5 + +build_unflags = ${opengloves.build_unflags} +build_src_filter = + ${opengloves.build_src_filter} + + +lib_deps = ${opengloves.lib_deps} diff --git a/ini/opengloves.ini b/ini/opengloves.ini index b7bb70cc..ab5c8333 100644 --- a/ini/opengloves.ini +++ b/ini/opengloves.ini @@ -1,60 +1,62 @@ [opengloves] -platform = platformio/espressif32@^6.1.0 -platform_packages = - platformio/framework-arduinoespressif32@^3.20007.0 -framework = arduino -board = esp32doit-devkit-v1 -upload_speed = 921600 -monitor_speed = 115200 - -build_flags = ${common.build_flags} - -D OPENGLOVES - - ; Communication - ; -D OPENGLOVES_COMMUNCATION=OPENGLOVES_COMM_SERIAL ; OPENGLOVES_COMM_SERIAL, OPENGLOVES_COMM_BTSERIAL, OPENGLOVES_COMM_BLESERIAL - ; Serial - -D SERIAL_BAUDRATE=115200 - -D SERIAL_PORT=Serial ; Serial, Serial1, Serial2, Serial3 - ; BTSerial - '-D BTSERIAL_PREFIX="SenseShift_OG"' - ; '-D BTSERIAL_NAME="SenseShift_OG_Left"' - - ; Sensors - -D FINGER_THUMB_INVERT=false - -D FINGER_INDEX_INVERT=false - -D FINGER_MIDDLE_INVERT=false - -D FINGER_RING_INVERT=false - -D FINGER_PINKY_INVERT=false - - -D FINGER_THUMB_SPLAY_INVERT=false - -D FINGER_INDEX_SPLAY_INVERT=false - -D FINGER_MIDDLE_SPLAY_INVERT=false - -D FINGER_RING_SPLAY_INVERT=false - -D FINGER_PINKY_SPLAY_INVERT=false - - -D JOYSTICK_X_INVERT=false - -D JOYSTICK_Y_INVERT=false - -D JOYSTICK_DEADZONE=0.1 - -D BUTTON_JOYSTICK_INVERT=false - - -D BUTTON_A_INVERT=false - -D BUTTON_B_INVERT=false - -D BUTTON_MENU_INVERT=false - -D BUTTON_CALIBRATE_INVERT=false - -D BUTTON_TRIGGER_INVERT=false - -D BUTTON_GRAB_INVERT=false - -D BUTTON_PINCH_INVERT=false - - -D GESTURE_TRIGGER_ENABLED=true - -D GESTURE_GRAB_ENABLED=true - -D GESTURE_PINCH_ENABLED=true - - ; Calibration - -D CALIBRATION_ALWAYS_CALIBRATE=true - -D CALIBRATION_DURATION=2000 ; in ms - - -D UPDATE_RATE=90 ; sensors update rate in Hz - -build_unflags = ${common.build_unflags} -build_src_filter = ${common.build_src_filter} -lib_deps = ${common.lib_deps} +platform = platformio/espressif32@^6.1.0 +platform_packages = + platformio/framework-arduinoespressif32@^3.20014.231204 +framework = arduino +board = esp32doit-devkit-v1 +upload_speed = 921600 +monitor_speed = 115200 + +build_flags = + ${common.build_flags} + -D OPENGLOVES + + ;;;; Communication + ; -D OPENGLOVES_COMMUNICATION=OPENGLOVES_COMM_BLESERIAL ; OPENGLOVES_COMM_SERIAL, OPENGLOVES_COMM_BTSERIAL, OPENGLOVES_COMM_BLESERIAL + ;;;; Serial + -D SERIAL_BAUDRATE=115200 + ; Options: Serial, Serial1, Serial2, Serial3 + -D SERIAL_PORT=Serial + ;;;; BTSerial + '-D BTSERIAL_PREFIX="SenseShift_OG"' + ; '-D BTSERIAL_NAME="SenseShift_OG_Left"' + + ; Sensors + -D FINGER_THUMB_INVERT=false + -D FINGER_INDEX_INVERT=false + -D FINGER_MIDDLE_INVERT=false + -D FINGER_RING_INVERT=false + -D FINGER_PINKY_INVERT=false + + -D FINGER_THUMB_SPLAY_INVERT=false + -D FINGER_INDEX_SPLAY_INVERT=false + -D FINGER_MIDDLE_SPLAY_INVERT=false + -D FINGER_RING_SPLAY_INVERT=false + -D FINGER_PINKY_SPLAY_INVERT=false + + -D JOYSTICK_X_INVERT=false + -D JOYSTICK_Y_INVERT=false + -D JOYSTICK_DEADZONE=0.1 + -D BUTTON_JOYSTICK_INVERT=false + + -D BUTTON_A_INVERT=false + -D BUTTON_B_INVERT=false + -D BUTTON_MENU_INVERT=false + -D BUTTON_CALIBRATE_INVERT=false + -D BUTTON_TRIGGER_INVERT=false + -D BUTTON_GRAB_INVERT=false + -D BUTTON_PINCH_INVERT=false + + -D GESTURE_TRIGGER_ENABLED=true + -D GESTURE_GRAB_ENABLED=true + -D GESTURE_PINCH_ENABLED=true + + ;;;; Calibration + -D CALIBRATION_ALWAYS_CALIBRATE=false + -D CALIBRATION_DURATION=2000 ; in ms + + -D UPDATE_RATE=90 ; sensors update rate in Hz + +build_unflags = ${common.build_unflags} +build_src_filter = ${common.build_src_filter} +lib_deps = ${common.lib_deps} diff --git a/lib/arduino/library.json b/lib/arduino/library.json index 8601c003..b906817b 100644 --- a/lib/arduino/library.json +++ b/lib/arduino/library.json @@ -1,13 +1,13 @@ { - "$schema": "https://raw.githubusercontent.com/platformio/platformio-core/develop/platformio/assets/schema/library.json", - "frameworks": "arduino", - "platforms": "*", - "dependencies": { - "adafruit/Adafruit BusIO": "^1.14.1", - "adafruit/Adafruit Unified Sensor": "^1.1.4", - "adafruit/Adafruit PWM Servo Driver Library": "^2.4.0", - "adafruit/Adafruit INA219": "^1.2.1", - "sparkfun/SparkFun MAX1704x Fuel Gauge Arduino Library": "^1.0.4", - "madhephaestus/ESP32Servo": "^0.13.0" - } + "$schema": "https://raw.githubusercontent.com/platformio/platformio-core/develop/platformio/assets/schema/library.json", + "frameworks": "arduino", + "platforms": "*", + "dependencies": { + "adafruit/Adafruit BusIO": "^1.14.1", + "adafruit/Adafruit Unified Sensor": "^1.1.4", + "adafruit/Adafruit PWM Servo Driver Library": "^2.4.0", + "adafruit/Adafruit INA219": "^1.2.1", + "sparkfun/SparkFun MAX1704x Fuel Gauge Arduino Library": "^1.0.4", + "madhephaestus/ESP32Servo": "^1.1.2" + } } diff --git a/lib/arduino/senseshift/arduino/battery/ina219.hpp b/lib/arduino/senseshift/arduino/battery/ina219.hpp deleted file mode 100644 index a343488d..00000000 --- a/lib/arduino/senseshift/arduino/battery/ina219.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include - -#include - -namespace SenseShift::Arduino::Battery { - class INA219_Battery : public ::SenseShift::Battery::IBatterySensor { - private: - bool active = false; - Adafruit_INA219* sensor; - - public: - INA219_Battery(Adafruit_INA219* sensor) : sensor(sensor){}; - - void init() override { this->active = this->sensor->begin(); } - - ::SenseShift::Battery::BatteryState getValue() override - { - if (!this->active) { - return { 0 }; - } - - auto batteryVoltage = this->sensor->getBusVoltage_V(); - // TODO: change this linear transformation to smth more useful - auto batteryPercentage = (batteryVoltage - 3.0) / 0.96; - - return { - simpleMap(batteryPercentage, 1.0f, 255.0f), - }; - } - }; -} // namespace SenseShift::Arduino::Battery diff --git a/lib/arduino/senseshift/arduino/battery/max17048.hpp b/lib/arduino/senseshift/arduino/battery/max17048.hpp deleted file mode 100644 index dc886cc2..00000000 --- a/lib/arduino/senseshift/arduino/battery/max17048.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include - -#include - -namespace SenseShift::Arduino::Battery { - class MAX1704_Battery : public ::SenseShift::Battery::IBatterySensor { - public: - MAX1704_Battery(SFE_MAX1704X* gauge) : gauge(gauge){}; - - /** - * @see - * https://github.com/sparkfun/SparkFun_MAX1704x_Fuel_Gauge_Arduino_Library/blob/main/examples/Example1_Simple/Example1_Simple.ino - */ - void init() override - { - // Set up the MAX17043 LiPo fuel gauge: - this->active = this->gauge->begin(); - - if (this->active) { - // Quick start restarts the MAX17043 in hopes of getting a more accurate - // guess for the SOC. - this->gauge->quickStart(); - - // We can set an interrupt to alert when the battery SoC gets too low. - // We can alert at anywhere between 1% - 32%: - // this->gauge->setThreshold(SENSESHIFT_BATTERY_THRESHOLD_PERCENTAGE); - } - } - - ::SenseShift::Battery::BatteryState getValue() override - { - if (!this->active) { - return { 0 }; - } - - return { .level = simpleMap(this->gauge->getSOC(), 1.0f, 255.0f) }; - } - - private: - bool active = false; - SFE_MAX1704X* gauge; - }; -} // namespace SenseShift::Arduino::Battery diff --git a/lib/arduino/senseshift/arduino/components/serial_plotter.cpp b/lib/arduino/senseshift/arduino/components/serial_plotter.cpp deleted file mode 100644 index 8cd48978..00000000 --- a/lib/arduino/senseshift/arduino/components/serial_plotter.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include "senseshift/arduino/components/serial_plotter.hpp" - -namespace SenseShift::Arduino { - struct PlaneVisitor { - const SenseShift::Body::Haptics::Target target; - HardwareSerial* serial; - - void operator()(const SenseShift::Body::Haptics::VibroPlane* plane) const - { - for (const auto& [position, state] : *(plane->getActuatorStates())) { - this->serial->printf("Output[%u][%ux%u]:%u, ", this->target, position.x, position.y, state.intensity); - } - } - }; - - template - void SerialPlotter_OutputStates<_Tp>::run() - { - while (true) { - for (const auto& [target, plane] : *output->getTargets()) { - std::visit(PlaneVisitor{ target, this->serial }, plane); - } - this->serial->println(); - - delay(this->sampleRate); - } - } -} // namespace SenseShift::Arduino diff --git a/lib/arduino/senseshift/arduino/components/serial_plotter.hpp b/lib/arduino/senseshift/arduino/components/serial_plotter.hpp deleted file mode 100644 index 2cc04d92..00000000 --- a/lib/arduino/senseshift/arduino/components/serial_plotter.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include -#include - -#include - -#ifndef SERIAL_PLOTTER_BAUD -#define SERIAL_PLOTTER_BAUD 115200 -#endif // SERIAL_PLOTTER_BAUD - -namespace SenseShift::Arduino { - /** - * Component, that prints the current state of the output to the serial port in Arduino's Serial Plotter format - * - * @tparam _Tp the type of the serial port - */ - template - class SerialPlotter_OutputStates : public ::SenseShift::FreeRTOS::Task> { - static_assert( - std::is_base_of::value, - "SerialPlotter_OutputStates only can be used with types, that inherit from Print" - ); - friend class ::SenseShift::FreeRTOS::Task>; - - private: - _Tp* serial; - ::SenseShift::Body::Haptics::HapticBody* output; - uint32_t sampleRate; - - void setup(void){}; - void run(void); - - public: - SerialPlotter_OutputStates( - _Tp& serial, - ::SenseShift::Body::Haptics::HapticBody* output, - uint32_t sampleRate, - ::SenseShift::FreeRTOS::TaskConfig taskConfig = { "Serial Plotter", 2048, 1, tskNO_AFFINITY } - ) : - ::SenseShift::FreeRTOS::Task>(taskConfig), - serial(&serial), - output(output), - sampleRate(sampleRate){}; - SerialPlotter_OutputStates(_Tp& serial, ::SenseShift::Body::Haptics::HapticBody* output) : - SerialPlotter_OutputStates(serial, output, 100){}; - - void begin() override - { - this->setup(); - ::SenseShift::FreeRTOS::Task>::begin(); - }; - }; - - /** - * Specialized setup for HardwareSerial - */ - template<> - inline void SerialPlotter_OutputStates::setup() - { - this->serial->begin(SERIAL_PLOTTER_BAUD); - } - - template class SerialPlotter_OutputStates; -} // namespace SenseShift::Arduino diff --git a/lib/arduino/senseshift/arduino/input/sensor/analog.hpp b/lib/arduino/senseshift/arduino/input/sensor/analog.hpp index 4dabd41b..dbef7c39 100644 --- a/lib/arduino/senseshift/arduino/input/sensor/analog.hpp +++ b/lib/arduino/senseshift/arduino/input/sensor/analog.hpp @@ -1,32 +1,46 @@ #pragma once +#include + #include #include +#if defined(__AVR__) +#define ANALOG_MAX 1023.0F +#elif defined(ESP32) +#define ANALOG_MAX 4095.0F +#elif !defined(ANALOG_MAX) +#warning "This board doesn't have an auto ANALOG_MAX assignment, please set it manually" +#define ANALOG_MAX static_assert(false, "ANALOG_MAX is not defined") +// Uncomment and set as needed (only touch if you know what you are doing) +// #define ANALOG_MAX 4095.0F +#endif + namespace SenseShift::Arduino::Input { - template - class AnalogSensor : public ::SenseShift::Input::ISimpleSensor { - private: - uint8_t pin; + template + class AnalogSimpleSensor : public ::SenseShift::Input::IFloatSimpleSensor { + std::uint8_t pin_; public: - AnalogSensor(uint8_t pin) : pin(pin) {} + explicit AnalogSimpleSensor(const std::uint8_t pin) : pin_(pin) {} - void init() override { pinMode(this->pin, INPUT); }; + void init() override { pinMode(this->pin_, INPUT); }; - uint16_t getValue() override; + [[nodiscard]] auto getValue() -> float override; }; template<> - uint16_t AnalogSensor::getValue() + [[nodiscard]] inline auto AnalogSimpleSensor::getValue() -> float { - return analogRead(this->pin); + const std::uint16_t raw = analogRead(this->pin_); + return static_cast(raw) / ANALOG_MAX; } template<> - uint16_t AnalogSensor::getValue() + [[nodiscard]] inline auto AnalogSimpleSensor::getValue() -> float { - return ANALOG_MAX - analogRead(this->pin); + const std::uint16_t raw = ANALOG_MAX - analogRead(this->pin_); + return static_cast(raw) / ANALOG_MAX; } } // namespace SenseShift::Arduino::Input diff --git a/lib/arduino/senseshift/arduino/input/sensor/digital.hpp b/lib/arduino/senseshift/arduino/input/sensor/digital.hpp index 3cb68b3f..cc3e7573 100644 --- a/lib/arduino/senseshift/arduino/input/sensor/digital.hpp +++ b/lib/arduino/senseshift/arduino/input/sensor/digital.hpp @@ -5,30 +5,27 @@ #include namespace SenseShift::Arduino::Input { - using IDigitalSensor = ::SenseShift::Input::ISimpleSensor; - - template - class DigitalSensor : public IDigitalSensor { - private: - uint8_t pin; + template + class DigitalSimpleSensor : public ::SenseShift::Input::IBinarySimpleSensor { + uint8_t pin_; public: - DigitalSensor(uint8_t pin) : pin(pin) {} + DigitalSimpleSensor(const uint8_t pin) : pin_(pin) {} - void init() override { pinMode(this->pin, INPUT_PULLUP); }; + void init() override { pinMode(this->pin_, INPUT_PULLUP); }; - bool getValue() override; + [[nodiscard]] auto getValue() -> bool override; }; template<> - bool DigitalSensor::getValue() + [[nodiscard]] inline auto DigitalSimpleSensor::getValue() -> bool { - return digitalRead(this->pin) == LOW; + return digitalRead(this->pin_) == LOW; } template<> - bool DigitalSensor::getValue() + [[nodiscard]] inline auto DigitalSimpleSensor::getValue() -> bool { - return digitalRead(this->pin) == HIGH; + return digitalRead(this->pin_) == HIGH; } } // namespace SenseShift::Arduino::Input diff --git a/lib/arduino/senseshift/arduino/input/sensor/ina219.hpp b/lib/arduino/senseshift/arduino/input/sensor/ina219.hpp new file mode 100644 index 00000000..a2f4e6eb --- /dev/null +++ b/lib/arduino/senseshift/arduino/input/sensor/ina219.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include + +#include + +namespace SenseShift::Arduino::Input { + /// INA219 Current sensor + /// TODO: refactor to component, that updates the value in multiple sensors (ESPHome style) + class INA219CurrentSimpleSensor : public ::SenseShift::Input::IFloatSimpleSensor { + Adafruit_INA219 ina219_; + + public: + void init() override + { + if (!ina219_.success()) { + log_e("Failed to find INA219 sensor"); + } + } + + [[nodiscard]] inline auto getValue() -> float override { return ina219_.getCurrent_mA(); } + }; +} // namespace SenseShift::Arduino::Input \ No newline at end of file diff --git a/lib/arduino/senseshift/arduino/output/actuator/pca9685.hpp b/lib/arduino/senseshift/arduino/output/actuator/pca9685.hpp deleted file mode 100644 index 4d28f92d..00000000 --- a/lib/arduino/senseshift/arduino/output/actuator/pca9685.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include - -namespace SenseShift::Arduino::Output { - class ActuatorPCA9685 : public ::SenseShift::Output::IActuator { - public: - static inline constexpr const std::uint16_t MAX_INTENSITY = 4095; - - ActuatorPCA9685(Adafruit_PWMServoDriver* driver, const std::uint8_t num) : driver(driver), num(num){}; - - void writeOutput(std::uint16_t intensity) override - { - this->driver->setPin(this->num, ::SenseShift::simpleMap(intensity, MAX_INTENSITY, 4095)); - } - - private: - Adafruit_PWMServoDriver* driver; - std::uint8_t num; - }; -} // namespace SenseShift::Arduino::Output diff --git a/lib/arduino/senseshift/arduino/output/actuator/pwm.hpp b/lib/arduino/senseshift/arduino/output/actuator/pwm.hpp deleted file mode 100644 index 5f2a595e..00000000 --- a/lib/arduino/senseshift/arduino/output/actuator/pwm.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace SenseShift::Arduino::Output { - class ActuatorPWM : public ::SenseShift::Output::IActuator { - public: - static inline constexpr const std::uint16_t MAX_INTENSITY = 4095; - - ActuatorPWM(const std::uint8_t pin, const double freq = 60, const std::uint8_t resolution = 12) : - pin(pin), freq(freq), resolution(resolution){}; - - void setup() override - { - this->chan = CHANNELS++; - -#if defined(ESP32) - ledcSetup(this->chan, this->freq, this->resolution); - ledcAttachPin(this->pin, this->chan); -#else - pinMode(this->pin, OUTPUT); -#endif - } - - void writeOutput(std::uint16_t intensity) override - { -#if defined(ESP32) - ledcWrite( - chan, - ::SenseShift::simpleMap(intensity, MAX_INTENSITY, (1 << this->resolution) - 1) - ); -#else - // Arduino only supports 8-bit PWM - analogWrite(this->pin, ::SenseShift::simpleMap(intensity, MAX_INTENSITY, (1 << 8) - 1)); -#endif - } - - private: - static inline std::uint8_t CHANNELS = 0; - std::uint8_t pin, chan; - double freq; - std::uint8_t resolution; - }; -}; // namespace SenseShift::Arduino::Output diff --git a/lib/arduino/senseshift/arduino/output/actuator/servo.hpp b/lib/arduino/senseshift/arduino/output/actuator/servo.hpp deleted file mode 100644 index 29bfa5e1..00000000 --- a/lib/arduino/senseshift/arduino/output/actuator/servo.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace SenseShift::Arduino::Output { - class ActuatorServo : public ::SenseShift::Output::IActuator { - public: - static inline constexpr const std::uint16_t MAX_INTENSITY = 4095; - - ActuatorServo(const uint8_t pin, const std::uint16_t min = 500, const std::uint16_t max = 2400) : - pin(pin), min(min), max(max){}; - - void setup() override - { - servo.attach(this->pin, this->min, this->max); - this->writeOutput(0); - }; - - void writeOutput(std::uint16_t intensity) override - { - servo.writeMicroseconds( - ::SenseShift::accurateMap(intensity, 0, MAX_INTENSITY, this->min, this->max) - ); - }; - - private: - Servo servo = Servo(); - uint8_t pin; - std::uint16_t min, max; - }; -} // namespace SenseShift::Arduino::Output diff --git a/lib/arduino/senseshift/arduino/output/analog.hpp b/lib/arduino/senseshift/arduino/output/analog.hpp new file mode 100644 index 00000000..28358ac3 --- /dev/null +++ b/lib/arduino/senseshift/arduino/output/analog.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include + +#include + +#include + +namespace SenseShift::Arduino::Output { + /// Arduino analog output + class AnalogOutput : public ::SenseShift::Output::IFloatOutput { + public: + static inline constexpr std::uint16_t MAX_INTENSITY = 255; + + explicit AnalogOutput(const std::uint8_t pin) : pin_(pin) {} + + void init() override { pinMode(this->pin_, OUTPUT); } + + void writeState(const float value) override + { + const auto duty = static_cast(value * MAX_INTENSITY); + analogWrite(this->pin_, duty); + } + + private: + uint8_t pin_; + }; +} // namespace SenseShift::Arduino::Output diff --git a/lib/arduino/senseshift/arduino/output/pca9685.hpp b/lib/arduino/senseshift/arduino/output/pca9685.hpp new file mode 100644 index 00000000..1bbffd38 --- /dev/null +++ b/lib/arduino/senseshift/arduino/output/pca9685.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include +#include + +#include +#include +#include + +namespace SenseShift::Arduino::Output { + class PCA9685Output : public ::SenseShift::Output::IFloatOutput { + public: + static inline constexpr std::uint16_t MAX_INTENSITY = 4095; + + PCA9685Output(Adafruit_PWMServoDriver* driver, const std::uint8_t num) : driver_(driver), channel_(num){}; + + void init() override { this->driver_->begin(); } + + void writeState(const ValueType value) override + { + const auto duty = static_cast(value * MAX_INTENSITY); + this->driver_->setPin(this->channel_, duty); + } + + private: + Adafruit_PWMServoDriver* driver_; + std::uint8_t channel_; + }; +} // namespace SenseShift::Arduino::Output diff --git a/lib/arduino/senseshift/arduino/output/servo.hpp b/lib/arduino/senseshift/arduino/output/servo.hpp new file mode 100644 index 00000000..81f238eb --- /dev/null +++ b/lib/arduino/senseshift/arduino/output/servo.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include + +#ifdef ESP32 +#include +#else +#include +#endif + +#include + +namespace SenseShift::Arduino::Output { + class ServoOutput : public ::SenseShift::Output::IFloatOutput { + public: + ServoOutput(size_t pin) : pin_(pin){}; + + void init() override { this->servo_.attach(this->pin_); } + + void writeState(const ValueType value) override + { + const auto duty = static_cast(value * 180); + this->servo_.write(duty); + } + + private: + Servo servo_; + size_t pin_; + }; +} // namespace SenseShift::Arduino::Output \ No newline at end of file diff --git a/lib/arduino_esp32/library.json b/lib/arduino_esp32/library.json new file mode 100644 index 00000000..d97e8e1b --- /dev/null +++ b/lib/arduino_esp32/library.json @@ -0,0 +1,7 @@ +{ + "$schema": "https://raw.githubusercontent.com/platformio/platformio-core/develop/platformio/assets/schema/library.json", + "frameworks": "arduino", + "platforms": [ + "espressif32" + ] +} diff --git a/lib/arduino_esp32/senseshift/arduino/output/ledc.hpp b/lib/arduino_esp32/senseshift/arduino/output/ledc.hpp new file mode 100644 index 00000000..6c29efed --- /dev/null +++ b/lib/arduino_esp32/senseshift/arduino/output/ledc.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include + +#include + +#include +#include + +namespace SenseShift::Arduino::Output { + static const char* const TAG = "output.ledc"; + + /// Arduino analog output + class LedcOutput : public ::SenseShift::Output::IFloatOutput { + public: + explicit LedcOutput( + const std::uint8_t pin, const std::uint8_t analog_resolution = 12, const std::uint32_t analog_frequency = 60 + ) : + pin_(pin), analog_resolution_(analog_resolution), analog_frequency_(analog_frequency) + { + } + + void init() override + { + pinMode(this->pin_, OUTPUT); + this->channel_ = findChannel(this->pin_); + + LOG_D(TAG, "GPIO %d - Setting up Channel %d", this->pin_, this->channel_); + if (ledcSetup(this->channel_, this->analog_frequency_, this->analog_resolution_) == 0) { + LOG_E( + TAG, + "setup failed (freq = %u, resolution = %u). Try setting different resolution or frequency", + this->analog_frequency_, + this->analog_resolution_ + ); + return; + } + ledcAttachPin(this->pin_, this->channel_); + } + + [[nodiscard]] auto getMaxValue() const -> std::uint32_t { return (1 << analog_resolution_) - 1; } + + void writeState(const float value) override + { + const auto duty = static_cast(value * this->getMaxValue()); + LOG_V(TAG, "GPIO %d - Writing %d to Channel %d", this->pin_, duty, this->channel_); + ledcWrite(this->channel_, duty); + }; + + protected: + static auto findChannel(std::uint8_t /*pin*/) -> std::int8_t { return CHANNELS++; } + + private: + static inline std::uint8_t CHANNELS = 0; + + std::uint8_t pin_; + std::int8_t channel_ = -1; + std::uint8_t analog_resolution_; + std::uint32_t analog_frequency_; + }; +} // namespace SenseShift::Arduino::Output diff --git a/lib/battery/senseshift/battery.hpp b/lib/battery/senseshift/battery.hpp deleted file mode 100644 index a89455fb..00000000 --- a/lib/battery/senseshift/battery.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include - -#include - -namespace SenseShift::Battery { - struct BatteryState { - uint8_t level; - }; - - class BatteryLevelEvent : public IEvent { - public: - const BatteryState& state; - BatteryLevelEvent(const BatteryState& state) : IEvent(OH_EVENT_BATTERY_LEVEL), state(state){}; - }; -} // namespace SenseShift::Battery diff --git a/lib/battery/senseshift/battery/battery.hpp b/lib/battery/senseshift/battery/battery.hpp new file mode 100644 index 00000000..0f4e587c --- /dev/null +++ b/lib/battery/senseshift/battery/battery.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +#include "frozen/map.h" + +#include "senseshift/events.hpp" + +namespace SenseShift::Battery { + + namespace VoltageMap { + /// Lookup table for LiPO 1S 4.2V batteries + /// \see Source + [[maybe_unused]] inline static const auto LiPO_1S_42 = frozen::make_map({ + { 4.2, 1.0 }, { 4.15, 0.95 }, { 4.11, 0.9 }, { 4.08, 0.85 }, { 4.02, 0.8 }, { 3.98, 0.75 }, + { 3.95, 0.7 }, { 3.91, 0.65 }, { 3.87, 0.6 }, { 3.85, 0.55 }, { 3.84, 0.5 }, { 3.82, 0.45 }, + { 3.8, 0.4 }, { 3.79, 0.35 }, { 3.77, 0.3 }, { 3.75, 0.25 }, { 3.73, 0.2 }, { 3.71, 0.15 }, + { 3.69, 0.1 }, { 3.61, 0.05 }, { 3.27, 0.0 }, + }); + } // namespace VoltageMap + + struct BatteryState { + using VoltageType = float; + using LevelType = std::uint8_t; + + static constexpr LevelType MAX_LEVEL = std::numeric_limits::max(); + LevelType level; + }; + + class BatteryLevelEvent : public IEvent { + public: + const BatteryState& state; + BatteryLevelEvent(const BatteryState& state) : IEvent(OH_EVENT_BATTERY_LEVEL), state(state){}; + }; +} // namespace SenseShift::Battery diff --git a/lib/battery/senseshift/battery/input/battery_sensor.hpp b/lib/battery/senseshift/battery/input/battery_sensor.hpp new file mode 100644 index 00000000..d0314a03 --- /dev/null +++ b/lib/battery/senseshift/battery/input/battery_sensor.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include + +#include "senseshift/battery/battery.hpp" + +#include "senseshift/core/helpers.hpp" +#include "senseshift/input/sensor.hpp" + +namespace SenseShift::Battery::Input { + /// Abstract battery sensor + using IBatterySensor = ::SenseShift::Input::Sensor; + + /// Interpolate voltage according to a lookup table. + template + class LookupTableInterpolateBatterySensor : public IBatterySensor { + static_assert(std::is_same_v); + static_assert(std::is_same_v); + + public: + using VoltageType = typename BatteryState::VoltageType; + using VoltageSource = ::SenseShift::Input::Sensor; + + LookupTableInterpolateBatterySensor(VoltageSource* voltage_source, Container* lookup_table) : + IBatterySensor(), voltage_source_(voltage_source), lookup_table_(lookup_table) + { + } + + void init() override + { + this->voltage_source_->init(); + this->voltage_source_->addValueCallback([this](VoltageType voltage) { + // Current level in % (0.0 - 1.0) + auto level = this->lookupInterpolateLevel(voltage); + + LOG_D("battery.sensor", "voltage=%f, level=%f", voltage, level); + + const BatteryState value = { + .level = static_cast(level * BatteryState::MAX_LEVEL), + }; + + this->publishState(value); + }); + } + + protected: + [[nodiscard]] auto lookupInterpolateLevel(VoltageType voltage) -> float + { + return ::SenseShift::lookup_table_interpolate(*this->lookup_table_, voltage); + } + + private: + VoltageSource* voltage_source_; + Container* lookup_table_; + }; +} // namespace SenseShift::Battery::Input diff --git a/lib/battery/senseshift/battery/sensor.hpp b/lib/battery/senseshift/battery/sensor.hpp deleted file mode 100644 index 01b1ae96..00000000 --- a/lib/battery/senseshift/battery/sensor.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include "senseshift/battery.hpp" - -#include -#include -#include - -#ifndef SENSESHIFT_BATTERY_TASK_PRIORITY -#define SENSESHIFT_BATTERY_TASK_PRIORITY 1 -#endif - -namespace SenseShift::Battery { - /** - * Abstract battery sensor - */ - using IBatterySensor = ::SenseShift::Input::ISimpleSensor; - - class NaiveBatterySensor : public IBatterySensor { - public: - NaiveBatterySensor(::SenseShift::Input::ISimpleSensor* sensor) : sensor(sensor){}; - - BatteryState getValue() override - { - return { .level = - static_cast(::SenseShift::simpleMap(this->sensor->getValue(), 4095, 255)) }; - }; - void init() override { this->sensor->init(); } - - private: - ::SenseShift::Input::ISimpleSensor* sensor; - }; - - class BatterySensor : public ::SenseShift::Input::MemoizedSensor<::SenseShift::Battery::BatteryState> { - public: - BatterySensor(::SenseShift::Battery::IBatterySensor* sensor, ::SenseShift::IEventDispatcher* eventDispatcher) : - ::SenseShift::Input::MemoizedSensor<::SenseShift::Battery::BatteryState>(sensor), - eventDispatcher(eventDispatcher){}; - - void tick() override - { - this->::SenseShift::Input::MemoizedSensor<::SenseShift::Battery::BatteryState>::tick(); - this->eventDispatcher->postEvent(new ::SenseShift::Battery::BatteryLevelEvent(this->value)); - } - - private: - ::SenseShift::IEventDispatcher* eventDispatcher; - }; -} // namespace SenseShift::Battery diff --git a/lib/bhaptics/senseshift/bh/devices.hpp b/lib/bhaptics/senseshift/bh/devices.hpp index f4c168de..5cd2b438 100644 --- a/lib/bhaptics/senseshift/bh/devices.hpp +++ b/lib/bhaptics/senseshift/bh/devices.hpp @@ -1,7 +1,12 @@ #pragma once -#include +#include +#include + +#include "senseshift/body/hands/hands_interface.hpp" + #include +#include #pragma region BH_DEVICE_TACTSUITX40 @@ -18,8 +23,8 @@ // X * Y for front and back #define BH_LAYOUT_TACTSUITX40_SIZE 40 // clang-format off -#define BH_LAYOUT_TACTSUITX40 { \ - /* Front, left part */ \ +#define BH_LAYOUT_TACTSUITX40 { \ + /* Front, left part */ \ /* 0 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 0) }, \ /* 1 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 0) }, \ /* 2 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 1) }, \ @@ -30,8 +35,8 @@ /* 7 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 3) }, \ /* 8 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 4) }, \ /* 9 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 4) }, \ - \ - /* Back */ \ + \ + /* Back */ \ /* 11 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 0) }, \ /* 11 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 0) }, \ /* 12 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 1) }, \ @@ -42,7 +47,7 @@ /* 17 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 3) }, \ /* 18 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(0, 4) }, \ /* 19 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(1, 4) }, \ - \ + \ /* 20 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(2, 0) }, \ /* 21 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(3, 0) }, \ /* 22 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(2, 1) }, \ @@ -53,8 +58,8 @@ /* 27 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(3, 3) }, \ /* 28 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(2, 4) }, \ /* 29 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX40_MAKE_POINT(3, 4) }, \ - \ - /* Front, again... Now right part */ \ + \ + /* Front, again... Now right part */ \ /* 30 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(2, 0) }, \ /* 31 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(3, 0) }, \ /* 32 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX40_MAKE_POINT(2, 1) }, \ @@ -85,51 +90,51 @@ // X16 suit uses the same packets structure as x40 suit and performs motor grouping in firmware #define BH_LAYOUT_TACTSUITX16_SIZE 40 // clang-format off -#define BH_LAYOUT_TACTSUITX16 { \ - /* Front, left part */ \ +#define BH_LAYOUT_TACTSUITX16 { \ + /* Front, left part */ \ /* 0 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 0) }, /* 0 */ \ /* 1 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 0) }, /* 1 */ \ /* 2 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 0) }, /* 4 */ \ /* 3 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 0) }, /* 5 */ \ - \ + \ /* 4 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 8 */ \ /* 5 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 9 */ \ /* 6 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 12 */ \ /* 7 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 13 */ \ /* 8 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 16 */ \ /* 9 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 17 */ \ - \ - /* Back */ \ + \ + /* Back */ \ /* 10 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 0) }, /* 0 */ \ /* 11 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 0) }, /* 1 */ \ /* 12 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 0) }, /* 4 */ \ /* 13 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 0) }, /* 5 */ \ - \ + \ /* 14 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 8 */ \ /* 15 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 9 */ \ /* 16 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 12 */ \ /* 17 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 13 */ \ /* 18 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(0, 1) }, /* 16 */ \ /* 19 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(1, 1) }, /* 17 */ \ - \ + \ /* 20 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 0) }, /* 2 */ \ /* 21 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 0) }, /* 3 */ \ /* 22 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 0) }, /* 4 */ \ /* 23 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 0) }, /* 7 */ \ - \ + \ /* 24 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 1) }, /* 10 */ \ /* 25 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 1) }, /* 11 */ \ /* 26 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 1) }, /* 14 */ \ /* 27 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 1) }, /* 15 */ \ /* 28 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 1) }, /* 18 */ \ /* 29 */ { Target::ChestBack, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 1) }, /* 19 */ \ - \ - /* Front, again... Now right part */ \ + \ + /* Front, again... Now right part */ \ /* 30 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 0) }, /* 2 */ \ /* 31 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 0) }, /* 3 */ \ /* 32 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 0) }, /* 4 */ \ /* 33 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 0) }, /* 7 */ \ - \ + \ /* 34 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 1) }, /* 10 */ \ /* 35 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(3, 1) }, /* 11 */ \ /* 36 */ { Target::ChestFront, BH_LAYOUT_TACTSUITX16_MAKE_POINT(2, 1) }, /* 14 */ \ @@ -290,63 +295,52 @@ namespace SenseShift::BH { using HandSide = ::SenseShift::Body::Hands::HandSide; - using OutputLayout = std::tuple; + using OutputLayout = std::tuple<::SenseShift::Body::Haptics::Target, SenseShift::Body::Haptics::Position>; // TactGlove Wrist motor position static constexpr const Position WRIST_MOTOR_POSITION(127, 191); - static constexpr const OutputLayout TactGloveLeftLayout[] = BH_LAYOUT_TACTGLOVE_LEFT; - static constexpr const OutputLayout TactGloveRightLayout[] = BH_LAYOUT_TACTGLOVE_RIGHT; + static constexpr const std::array TactGloveLeftLayout = { + BH_LAYOUT_TACTGLOVE_LEFT + }; + static constexpr const std::array TactGloveRightLayout = { + BH_LAYOUT_TACTGLOVE_RIGHT + }; inline void addTactGloveActuators( - HapticBody* hapticBody, + FloatBody* hapticBody, const HandSide side, - VibroPlane::Actuator* const thumbActuator, - VibroPlane::Actuator* const indexActuator, - VibroPlane::Actuator* const middleActuator, - VibroPlane::Actuator* const ringActuator, - VibroPlane::Actuator* const littleActuator, - VibroPlane::Actuator* const wristActuator + FloatBody::Plane::Actuator* const thumb, + FloatBody::Plane::Actuator* const index, + FloatBody::Plane::Actuator* const middle, + FloatBody::Plane::Actuator* const ring, + FloatBody::Plane::Actuator* const little, + FloatBody::Plane::Actuator* const wrist ) { - const OutputLayout(&layout)[6] = (side == HandSide::Left) ? TactGloveLeftLayout : TactGloveRightLayout; + const auto& layout = (side == HandSide::Left) ? TactGloveLeftLayout : TactGloveRightLayout; - if (thumbActuator != nullptr) { - hapticBody->addTarget( - std::get<0>(layout[0]), - new VibroPlane({ { std::get<1>(layout[0]), thumbActuator } }) - ); + if (thumb != nullptr) { + hapticBody->addTarget(std::get<0>(layout[0]), new FloatPlane({ { std::get<1>(layout[0]), thumb } })); } - if (indexActuator != nullptr) { - hapticBody->addTarget( - std::get<0>(layout[1]), - new VibroPlane({ { std::get<1>(layout[1]), indexActuator } }) - ); + if (index != nullptr) { + hapticBody->addTarget(std::get<0>(layout[1]), new FloatPlane({ { std::get<1>(layout[1]), index } })); } - if (middleActuator != nullptr) { - hapticBody->addTarget( - std::get<0>(layout[2]), - new VibroPlane({ { std::get<1>(layout[2]), middleActuator } }) - ); + if (middle != nullptr) { + hapticBody->addTarget(std::get<0>(layout[2]), new FloatPlane({ { std::get<1>(layout[2]), middle } })); } - if (ringActuator != nullptr) { - hapticBody->addTarget(std::get<0>(layout[3]), new VibroPlane({ { std::get<1>(layout[3]), ringActuator } })); + if (ring != nullptr) { + hapticBody->addTarget(std::get<0>(layout[3]), new FloatPlane({ { std::get<1>(layout[3]), ring } })); } - if (littleActuator != nullptr) { - hapticBody->addTarget( - std::get<0>(layout[4]), - new VibroPlane({ { std::get<1>(layout[4]), littleActuator } }) - ); + if (little != nullptr) { + hapticBody->addTarget(std::get<0>(layout[4]), new FloatPlane({ { std::get<1>(layout[4]), little } })); } - if (wristActuator != nullptr) { - hapticBody->addTarget( - std::get<0>(layout[5]), - new VibroPlane({ { std::get<1>(layout[5]), wristActuator } }) - ); + if (wrist != nullptr) { + hapticBody->addTarget(std::get<0>(layout[5]), new FloatPlane({ { std::get<1>(layout[5]), wrist } })); } } } // namespace SenseShift::BH diff --git a/lib/bhaptics/senseshift/bh/encoding.hpp b/lib/bhaptics/senseshift/bh/encoding.hpp index 273b98c7..e23a55f3 100644 --- a/lib/bhaptics/senseshift/bh/encoding.hpp +++ b/lib/bhaptics/senseshift/bh/encoding.hpp @@ -1,20 +1,24 @@ #pragma once +#include +#include +#include #include +#include +#include + #include +#include namespace SenseShift::BH { class Decoder { public: using VibroEffectData = ::SenseShift::Body::Haptics::VibroEffectData; - using EffectData = ::SenseShift::Body::Haptics::EffectData; using Effect = ::SenseShift::Body::Haptics::Effect; using Target = ::SenseShift::Body::Haptics::Target; using Position = ::SenseShift::Body::Haptics::Position; - using HapticBody = ::SenseShift::Body::Haptics::HapticBody; - using OutputLayout = std::tuple; static constexpr size_t VEST_LAYOUT_SIZE = 40; @@ -22,29 +26,28 @@ namespace SenseShift::BH { template static void applyPlain( - HapticBody* output, const uint8_t (&value)[N], const OutputLayout (&layout)[N], const Effect effect + FloatBody* output, + const std::array& value, + const std::array& layout, + const Effect effect ) { for (size_t i = 0; i < N; i++) { const auto [target, position] = layout[i]; - const uint8_t byte = value[i]; + const std::uint8_t byte = value[i]; - output->effect({ - .effect = effect, - .target = target, - .position = position, - .data = effectDataFromByte(effect, byte), - }); + output->effect(target, position, static_cast(effectDataFromByte(byte))); } } template - static void - applyPlain(HapticBody* output, std::string& value, const OutputLayout (&layout)[N], const Effect effect) + static void applyPlain( + FloatBody* output, std::string& value, const std::array& layout, const Effect effect + ) { - std::uint8_t buf[N]; + std::array buf{}; std::size_t copyLength = std::min(value.size(), sizeof(buf)); - std::memcpy(buf, value.c_str(), copyLength); + std::memcpy(buf.data(), value.c_str(), copyLength); applyPlain(output, buf, layout, effect); } @@ -54,9 +57,9 @@ namespace SenseShift::BH { */ template static void applyPlain( - HapticBody* output, - const uint8_t (&value)[N], - const Position (&layout)[N], + FloatBody* output, + const std::array& value, + const std::array& layout, const Effect effect, const Target target ) @@ -65,23 +68,22 @@ namespace SenseShift::BH { const auto position = layout[i]; const uint8_t byte = value[i]; - output->effect({ - .effect = effect, - .target = target, - .position = position, - .data = effectDataFromByte(effect, byte), - }); + output->effect(target, position, static_cast(effectDataFromByte(byte))); } } template static void applyPlain( - HapticBody* output, std::string& value, const Position (&layout)[N], const Effect effect, const Target target + FloatBody* output, + std::string& value, + const std::array& layout, + const Effect effect, + const Target target ) { - std::uint8_t buf[N]; + std::array buf{}; std::size_t copyLength = std::min(value.size(), sizeof(buf)); - std::memcpy(buf, value.c_str(), copyLength); + std::memcpy(buf.data(), value.c_str(), copyLength); applyPlain(output, buf, layout, effect, target); } @@ -90,32 +92,34 @@ namespace SenseShift::BH { * Apply vest-encoded data to the output. */ static void applyVest( - HapticBody* output, const uint8_t (&value)[VEST_PAYLOAD_SIZE], const OutputLayout (&layout)[VEST_LAYOUT_SIZE] + FloatBody* output, + const std::array& value, + const std::array& layout ) { for (size_t i = 0; i < VEST_PAYLOAD_SIZE; i++) { - uint8_t byte = value[i]; - uint actIndex = i * 2; - output->effect({ - .effect = Effect::Vibro, - .target = std::get<0>(layout[actIndex]), - .position = std::get<1>(layout[actIndex]), - .data = effectDataFromByte(Effect::Vibro, ((byte >> 4) & 0xf), 15), - }); - output->effect({ - .effect = Effect::Vibro, - .target = std::get<0>(layout[actIndex + 1]), - .position = std::get<1>(layout[actIndex + 1]), - .data = effectDataFromByte(Effect::Vibro, (byte & 0xf), 15), - }); + const std::uint8_t byte = value[i]; + const size_t actIndex = i * 2; + + output->effect( + std::get<0>(layout[actIndex]), + std::get<1>(layout[actIndex]), + static_cast(effectDataFromByte(((byte >> 4) & 0xf), 15)) + ); + output->effect( + std::get<0>(layout[actIndex + 1]), + std::get<1>(layout[actIndex + 1]), + static_cast(effectDataFromByte((byte & 0xf), 15).getIntensity()) + ); } } - static void applyVest(HapticBody* output, std::string& value, const OutputLayout (&layout)[VEST_LAYOUT_SIZE]) + static void + applyVest(FloatBody* output, std::string& value, const std::array& layout) { - std::uint8_t buf[VEST_PAYLOAD_SIZE]; - std::size_t copyLength = std::min(value.size(), sizeof(buf)); - std::memcpy(buf, value.c_str(), copyLength); + std::array buf{}; + const size_t copyLength = std::min(value.size(), sizeof(buf)); + std::memcpy(buf.data(), value.c_str(), copyLength); applyVest(output, buf, layout); } @@ -125,18 +129,18 @@ namespace SenseShift::BH { */ template static void applyVestGrouped( - HapticBody* output, - const uint8_t (&value)[VEST_PAYLOAD_SIZE], - const OutputLayout (&layout)[VEST_LAYOUT_SIZE], - const uint8_t (&layoutGroups)[N] + FloatBody* output, + const std::array& value, + const std::array& layout, + const std::array& layoutGroups ) { - uint8_t result[VEST_LAYOUT_SIZE]; + std::array result{}; // Unpack values for (auto i = 0; i < VEST_PAYLOAD_SIZE; i++) { - uint8_t byte = value[i]; - uint actIndex = i * 2; + const std::uint8_t byte = value[i]; + const size_t actIndex = i * 2; result[actIndex] = (byte >> 4) & 0xf; result[actIndex + 1] = (byte & 0xf); @@ -164,51 +168,38 @@ namespace SenseShift::BH { for (uint8_t i = 0; i < VEST_LAYOUT_SIZE; i++) { // take only meaningful values - if (!::SenseShift::contains(layoutGroups, VEST_LAYOUT_SIZE, i)) { + if (std::find(layoutGroups.begin(), layoutGroups.end(), i) == layoutGroups.end()) { continue; } const auto target = std::get<0>(layout[i]); const auto position = std::get<1>(layout[i]); - output->effect({ - .effect = Effect::Vibro, - .target = target, - .position = position, - .data = effectDataFromByte(Effect::Vibro, result[i], 15), - }); + output + ->effect(target, position, static_cast(effectDataFromByte(result[i], 15))); } } template static void applyVestGrouped( - HapticBody* output, + FloatBody* output, std::string& value, - const OutputLayout (&layout)[VEST_LAYOUT_SIZE], - const uint8_t (&layoutGroups)[N] + const std::array& layout, + const std::array& layoutGroups ) { - std::uint8_t buf[VEST_PAYLOAD_SIZE]; - std::size_t copyLength = std::min(value.size(), sizeof(buf)); - std::memcpy(buf, value.c_str(), copyLength); + std::array buf{}; + const size_t copyLength = std::min(value.size(), sizeof(buf)); + std::memcpy(buf.data(), value.c_str(), copyLength); applyVestGrouped(output, buf, layout, layoutGroups); } private: - static const EffectData - effectDataFromByte(const Effect effect, const uint8_t byte, const uint8_t maxValue = 100) + static auto effectDataFromByte(const uint8_t byte, const uint8_t maxValue = 100) -> VibroEffectData { - switch (effect) { - case Effect::Vibro: - return VibroEffectData(::SenseShift::simpleMap( - byte, - maxValue, - VibroEffectData::INTENSITY_MAX - )); - default: - throw std::runtime_error("Unknown effect"); - } + const auto value = static_cast(byte) / static_cast(maxValue); + return VibroEffectData(value); } }; } // namespace SenseShift::BH diff --git a/lib/bhaptics_ble/senseshift/bh/ble/connection.hpp b/lib/bhaptics_ble/senseshift/bh/ble/connection.hpp index a0265f74..e5033fb1 100644 --- a/lib/bhaptics_ble/senseshift/bh/ble/connection.hpp +++ b/lib/bhaptics_ble/senseshift/bh/ble/connection.hpp @@ -1,6 +1,7 @@ #pragma once -#include +#include "senseshift/battery/battery.hpp" +#include "senseshift/core/helpers.hpp" #include #include #include @@ -44,9 +45,9 @@ namespace SenseShift::BH::BLE { void handleEvent(const IEvent* event) const override { if (event->eventName == OH_EVENT_BATTERY_LEVEL) { - uint16_t level = simpleMap( + uint16_t level = remap_simple( static_cast(event)->state.level, - 255, + ::SenseShift::Battery::BatteryState::MAX_LEVEL, 100 ); diff --git a/lib/ble_serial/BLESerial.hpp b/lib/ble_serial/BLESerial.hpp index b239eec2..090ecb75 100644 --- a/lib/ble_serial/BLESerial.hpp +++ b/lib/ble_serial/BLESerial.hpp @@ -42,7 +42,7 @@ class BLESerial : public Stream { } this->m_pTxCharacteristic->setValue(const_cast(buffer), bufferSize); - this->flush(); + // this->flush(); return bufferSize; } @@ -54,7 +54,7 @@ class BLESerial : public Stream { } this->m_pTxCharacteristic->setValue(&byte, 1); - this->flush(); + // this->flush(); return 1; } diff --git a/lib/core/senseshift/core/component.hpp b/lib/core/senseshift/core/component.hpp new file mode 100644 index 00000000..8ead9f04 --- /dev/null +++ b/lib/core/senseshift/core/component.hpp @@ -0,0 +1,45 @@ +#pragma once + +#define SS_INIT_NOT_NULL(ptr) \ + if ((ptr) != nullptr) { \ + (ptr)->init(); \ + } +#define SS_TICK_NOT_NULL(ptr) \ + if ((ptr) != nullptr) { \ + (ptr)->tick(); \ + } + +namespace SenseShift { + class IInitializable { + public: + virtual ~IInitializable() = default; + + virtual void init() = 0; + }; + + class ITickable { + public: + virtual ~ITickable() = default; + + virtual void tick() = 0; + }; + + class Component : public virtual IInitializable, public virtual ITickable { + public: + /// @brief Initialize the component. + /// + /// Where the component's initialization should happen. + /// Analogous to Arduino's setup(). This method is guaranteed to only be called once. + /// + /// Defaults to doing nothing. + void init() override {} + + /// @brief Tick the component. + /// + /// This method will be called repeatedly. + /// Analogous to Arduino's loop(). init() is guaranteed to be called before this. + /// + /// Defaults to doing nothing. + void tick() override {} + }; +} // namespace SenseShift diff --git a/lib/core/senseshift/core/helpers.hpp b/lib/core/senseshift/core/helpers.hpp new file mode 100644 index 00000000..1d4fb8b1 --- /dev/null +++ b/lib/core/senseshift/core/helpers.hpp @@ -0,0 +1,131 @@ +#pragma once + +#include +#include +#include + +#include + +namespace SenseShift { + /// Linearly interpolate between \p start and \p end by \p completion (between 0 and 1). + template + constexpr auto lerp(const float completion, const Tp start, const Tp end) -> Tp + { + static_assert(std::is_arithmetic_v, "Type must be arithmetic"); + + return start + (end - start) * completion; + } + + /// Remap \p value from the range (\p min, \p max) to (\p min_out, \p max_out). + /// + /// \tparam Tp The output type. + /// \tparam Up The input type. + /// + /// \param value The value to remap. + /// \param min The minimum value of the input range. + /// \param max The maximum value of the input range. + /// \param min_out The minimum value of the output range. + /// \param max_out The maximum value of the output range. + /// + /// \return The remapped value. + template + constexpr auto remap(Up value, Up min, Up max, Tp min_out, Tp max_out) -> Tp + { + static_assert(std::is_arithmetic_v, "Type must be arithmetic"); + static_assert(std::is_arithmetic_v, "Type must be arithmetic"); + + if (max <= min) { + LOG_E("util.remap", "Invalid input range, min <= max"); + return (min_out + max_out) / 2; + } + + return (value - min) * (max_out - min_out) / (max - min) + min_out; + } + + /// Remap \p value from the range (0, \p max) to (0, \p max_out). + /// + /// \tparam Tp The output type. + /// \tparam Up The input type. + /// + /// \param value The value to remap. + /// \param max The maximum value of the input range. + /// \param max_out The maximum value of the output range. + /// + /// \return The remapped value. + /// + /// \note This is a simplified version of remap() where the minimum values are 0. + template + constexpr auto remap_simple(Up value, Up max, Tp max_out) noexcept -> Tp + { + static_assert(std::is_arithmetic_v, "Type must be arithmetic"); + static_assert(std::is_arithmetic_v, "Type must be arithmetic"); + + return value * max_out / max; + } + + /// Lookup a value in a table and interpolate between the two closest values. + /// + /// \tparam Tp + /// \tparam To + /// \tparam Container + /// + /// \param lookup_table Lookup table to use in the format of std::map in descending order. + /// \param value + /// + /// \return + template + auto lookup_table_interpolate(Container const& lookup_table, Tp value) -> To + { + static_assert(std::is_same_v); + static_assert(std::is_same_v); + static_assert(std::is_arithmetic_v, "lookup_table_interpolate only supports arithmetic types"); + static_assert(std::is_arithmetic_v, "lookup_table_interpolate only supports arithmetic types"); + + // If the value is outside the range of the lookup table, return the closest value + if (value <= lookup_table.begin()->first) { + return lookup_table.begin()->second; + } + if (value >= lookup_table.rbegin()->first) { + return lookup_table.rbegin()->second; + } + + // Find the two closest values in the lookup table + auto const upper = lookup_table.lower_bound(value); + auto const lower = std::prev(upper); + + // Interpolate between the two closest values + float const completion = (value - lower->first) / (upper->first - lower->first); + + return lerp(completion, lower->second, upper->second); + } + + template + class CallbackManager; + + /// Helper class to allow having multiple subscribers to a callback. + /// + /// \tparam Ts The arguments for the callbacks, wrapped in void(). + template + class CallbackManager { + public: + using CallbackType = std::function; + + /// Add a callback to the list. + void add(std::function&& callback) { this->callbacks_.push_back(std::move(callback)); } + + /// Call all callbacks in this manager. + void call(Ts... args) + { + for (auto& callback : this->callbacks_) { + callback(args...); + } + } + [[nodiscard]] auto size() const -> size_t { return this->callbacks_.size(); } + + /// Call all callbacks in this manager. + void operator()(Ts... args) { call(args...); } + + private: + std::vector> callbacks_; + }; +} // namespace SenseShift diff --git a/lib/core/senseshift/core/logging.hpp b/lib/core/senseshift/core/logging.hpp new file mode 100644 index 00000000..647965ef --- /dev/null +++ b/lib/core/senseshift/core/logging.hpp @@ -0,0 +1,40 @@ +#pragma once + +#if defined(ESP32) + +#include +#define LOG_E(tag, format, ...) ESP_LOGE(tag, format, ##__VA_ARGS__) +#define LOG_W(tag, format, ...) ESP_LOGW(tag, format, ##__VA_ARGS__) +#define LOG_I(tag, format, ...) ESP_LOGI(tag, format, ##__VA_ARGS__) +#define LOG_D(tag, format, ...) ESP_LOGD(tag, format, ##__VA_ARGS__) +#define LOG_V(tag, format, ...) ESP_LOGV(tag, format, ##__VA_ARGS__) + +#elif defined(UNITY_INCLUDE_PRINT_FORMATTED) + +#define log_e(...) TEST_PRINTF(__VA_ARGS__) +#define log_w(...) TEST_PRINTF(__VA_ARGS__) +#define log_i(...) TEST_PRINTF(__VA_ARGS__) +#define log_d(...) TEST_PRINTF(__VA_ARGS__) +#define log_v(...) TEST_PRINTF(__VA_ARGS__) + +#define LOG_E(tag, format, ...) log_e("[%s] " format, tag, ##__VA_ARGS__) +#define LOG_W(tag, format, ...) log_w("[%s] " format, tag, ##__VA_ARGS__) +#define LOG_I(tag, format, ...) log_i("[%s] " format, tag, ##__VA_ARGS__) +#define LOG_D(tag, format, ...) log_d("[%s] " format, tag, ##__VA_ARGS__) +#define LOG_V(tag, format, ...) log_v("[%s] " format, tag, ##__VA_ARGS__) + +#else + +#define log_e(...) +#define log_w(...) +#define log_i(...) +#define log_d(...) +#define log_v(...) + +#define LOG_E(tag, format, ...) +#define LOG_W(tag, format, ...) +#define LOG_I(tag, format, ...) +#define LOG_D(tag, format, ...) +#define LOG_V(tag, format, ...) + +#endif diff --git a/lib/core/senseshift/core/macros.hpp b/lib/core/senseshift/core/macros.hpp new file mode 100644 index 00000000..1087e76f --- /dev/null +++ b/lib/core/senseshift/core/macros.hpp @@ -0,0 +1,4 @@ +#pragma once + +// Helper macro to define a version code, whose value can be compared against other version codes. +#define VERSION_CODE(major, minor, patch) ((major) << 16 | (minor) << 8 | (patch)) \ No newline at end of file diff --git a/lib/core/senseshift/interface.hpp b/lib/core/senseshift/interface.hpp deleted file mode 100644 index af428cc6..00000000 --- a/lib/core/senseshift/interface.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -namespace SenseShift { - struct IInitializable { - virtual void init() = 0; - }; - - struct ITickable { - virtual void tick() = 0; - }; -} // namespace SenseShift diff --git a/lib/core/senseshift/logging.hpp b/lib/core/senseshift/logging.hpp deleted file mode 100644 index 9cf54f37..00000000 --- a/lib/core/senseshift/logging.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#if defined(ESP32) - -#include - -#elif defined(UNITY_INCLUDE_PRINT_FORMATTED) - -#define log_e(...) TEST_PRINTF(__VA_ARGS__) -#define log_w(...) TEST_PRINTF(__VA_ARGS__) -#define log_i(...) TEST_PRINTF(__VA_ARGS__) -#define log_d(...) TEST_PRINTF(__VA_ARGS__) -#define log_t(...) TEST_PRINTF(__VA_ARGS__) - -#else - -#define log_e(...) -#define log_w(...) -#define log_i(...) -#define log_d(...) -#define log_t(...) - -#endif diff --git a/lib/freertos/senseshift/freertos/input/sensor.hpp b/lib/freertos/senseshift/freertos/input/sensor.hpp deleted file mode 100644 index 8209aa3f..00000000 --- a/lib/freertos/senseshift/freertos/input/sensor.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include -#include - -#include - -namespace SenseShift::FreeRTOS::Input { - /** - * Sensor update task - */ - class SensorUpdateTask : public Task { - friend class Task; - - private: - using Sensor = ::SenseShift::ITickable; - - public: - SensorUpdateTask(Sensor* sensor, std::uint32_t updateDelay, TaskConfig taskConfig) : - Task(taskConfig), updateDelay(updateDelay){}; - - protected: - Sensor* sensor; - std::uint32_t updateDelay; - - void run() - { - while (true) { - this->sensor->tick(); - ::delay(this->updateDelay); - } - } - }; - - template - class TaskedSensor : public SensorUpdateTask, public ::SenseShift::Input::ISimpleSensor<_Tp> { - friend class SensorUpdateTask; - - private: - using Sensor = ::SenseShift::Input::MemoizedSensor<_Tp>; - - public: - TaskedSensor(Sensor* sensor, std::uint32_t updateDelay, TaskConfig taskConfig) : - SensorUpdateTask(sensor, updateDelay, taskConfig), sensor(sensor){}; - - void begin() override - { - this->init(); - SensorUpdateTask::begin(); - }; - - void init() override { this->sensor->init(); }; - - _Tp getValue() override { return this->sensor->getValue(); }; - - private: - Sensor* sensor; - }; -} // namespace SenseShift::FreeRTOS::Input diff --git a/lib/freertos/senseshift/freertos/task.hpp b/lib/freertos/senseshift/freertos/task.hpp index d41355ec..ee4f3028 100644 --- a/lib/freertos/senseshift/freertos/task.hpp +++ b/lib/freertos/senseshift/freertos/task.hpp @@ -1,10 +1,15 @@ #pragma once -#include +#include +#include + +#include +#include extern "C" void delay(uint32_t ms); #if defined(ESP32) +#include #include // Include the base FreeRTOS definitions. #include // Include the task definitions. @@ -39,11 +44,10 @@ namespace SenseShift::FreeRTOS { friend class Task; public: - Task(const char* name, uint32_t stackDepth, UBaseType_t priority, const BaseType_t coreId = tskNO_AFFINITY) + explicit Task(TaskConfig& config) : taskConfig(config) { - this->taskConfig = { name, stackDepth, priority, coreId }; + log_i("creating ComponentUpdateTask: %s", taskConfig.name); }; - Task(TaskConfig& config) : taskConfig(config){}; virtual ~Task() { if (taskHandle) { @@ -56,21 +60,52 @@ namespace SenseShift::FreeRTOS { virtual void begin() { BaseType_t result = xTaskCreateUniversal( - taskFunction, // pvTaskCode - this->taskConfig.name, // pcName - this->taskConfig.stackDepth, // usStackDepth - this, // pvParameters - this->taskConfig.priority, // uxPriority - &taskHandle, // pvCreatedTask - this->taskConfig.coreId // xCoreID + taskFunction, //< pvTaskCode + this->taskConfig.name, //< pcName + this->taskConfig.stackDepth, //< usStackDepth + this, //< pvParameters + this->taskConfig.priority, //< uxPriority + &taskHandle, //< pvCreatedTask + this->taskConfig.coreId //< xCoreID ); + log_i("Created task %s, result %i", this->taskConfig.name, result); assert("Failed to create task" && result == pdPASS); if (!taskHandle) { log_e("Failed to create task %s", this->taskConfig.name); } }; + protected: + inline void monitorTask() + { + log_i("----------------------------------------\nFree Heap: %d\n", xPortGetFreeHeapSize()); + volatile UBaseType_t uxArraySize = uxTaskGetNumberOfTasks(); + TaskStatus_t* pxTaskStatusArray = (TaskStatus_t*) pvPortMalloc(uxArraySize * sizeof(TaskStatus_t)); + if (!pxTaskStatusArray) { + log_e("Failed to allocate memory for task status array!"); + return; + } + uxArraySize = uxTaskGetSystemState(pxTaskStatusArray, uxArraySize, NULL); + for (UBaseType_t i = 0; i < uxArraySize; i++) { + log_i( + "Task: %s\n\tStack High Watermark: %d\n\tState: %d\n", + pxTaskStatusArray[i].pcTaskName, + pxTaskStatusArray[i].usStackHighWaterMark, + pxTaskStatusArray[i].eCurrentState + ); + + if (pxTaskStatusArray[i].usStackHighWaterMark < 20) { + log_w( + "Warning: Task %s has low stack space, only %dB awailable!", + pxTaskStatusArray[i].pcTaskName, + pxTaskStatusArray[i].usStackHighWaterMark * 4 + ); + } + } + vPortFree(pxTaskStatusArray); + } + private: const TaskConfig& taskConfig; TaskHandle_t taskHandle = nullptr; @@ -81,4 +116,49 @@ namespace SenseShift::FreeRTOS { task->run(); } }; + + template + class ComponentUpdateTask : public Task> { + static_assert(std::is_same_v); + static_assert(std::is_same_v); + + public: + ComponentUpdateTask(Tp* component, std::uint32_t updateDelay, TaskConfig taskConfig) : + Task(taskConfig), component_(component), updateDelay_(updateDelay) + { + log_i("creating ComponentUpdateTask: %s", taskConfig.name); + }; + + void begin() override + { + this->component_->init(); + this->Task::begin(); + } + + protected: + [[noreturn]] void run() + { + auto now = millis(); + auto targetHz = 1000 / this->updateDelay_; + + while (true) { + now = millis(); + + this->component_->tick(); + + const auto elapsed = millis() - now; + + log_d("T: %d, Fmax: %dHz, Ft: %dHz", elapsed, 1000 / elapsed, targetHz); + if (elapsed < this->updateDelay_) { + delay(this->updateDelay_ - elapsed); + } + } + } + + private: + friend class Task; + + Tp* component_; + std::uint32_t updateDelay_; + }; } // namespace SenseShift::FreeRTOS diff --git a/lib/hands/hand_interface.hpp b/lib/hands/hand_interface.hpp deleted file mode 100644 index 907a553b..00000000 --- a/lib/hands/hand_interface.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include - -namespace SenseShift::Body { - namespace Hands { - using HandSideIndex = std::uint8_t; - enum class HandSide : HandSideIndex { Left, Right }; - using FingerIndex = std::uint8_t; - enum class Finger : FingerIndex { - Thumb, - Index, - Middle, - Ring, - Little, - }; - - namespace Haptics { - /** - * @brief Helper with position of the haptic device on the fingertip. - * Distal phalanx of the volar surface of the any finger. - */ - static constexpr const ::SenseShift::Body::Haptics::Position FINGERTIP_POSITION(127, 16); - } // namespace Haptics - } // namespace Hands -} // namespace SenseShift::Body diff --git a/lib/hands/senseshift/body/hands/hands_interface.hpp b/lib/hands/senseshift/body/hands/hands_interface.hpp new file mode 100644 index 00000000..cee4877e --- /dev/null +++ b/lib/hands/senseshift/body/hands/hands_interface.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +#include + +namespace SenseShift::Body::Hands { + using HandLateralityIndex = std::uint8_t; + enum class HandSide : HandLateralityIndex { Left, Right }; + using FingerIndex = std::uint8_t; + enum class Finger : FingerIndex { + Thumb, + Index, + Middle, + Ring, + Little, + }; + + namespace Haptics { + /// @brief Helper with position of the haptic device on the fingertip. + /// Distal phalanx of the volar surface of the any finger. + static constexpr const ::SenseShift::Body::Haptics::Position FINGERTIP_POSITION(127, 16); + } // namespace Haptics +} // namespace SenseShift::Body::Hands diff --git a/lib/hands_input/senseshift/body/hands/input/gesture.hpp b/lib/hands_input/senseshift/body/hands/input/gesture.hpp new file mode 100644 index 00000000..eccb911f --- /dev/null +++ b/lib/hands_input/senseshift/body/hands/input/gesture.hpp @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include + +namespace SenseShift::Body::Hands::Input { + using Gesture = ::SenseShift::Input::BinarySensor; + + /// An alias for semantic consistency. + using TriggerGesture = ::SenseShift::Input::AnalogThresholdSensor; + + class GrabGesture : public Gesture { + public: + struct Fingers { + ::SenseShift::Input::FloatSensor* index; + ::SenseShift::Input::FloatSensor* middle; + ::SenseShift::Input::FloatSensor* ring; + ::SenseShift::Input::FloatSensor* pinky; + }; + + explicit GrabGesture(Fingers fingers, float threshold = 0.5F, bool attach_callbacks = false) : + fingers_(fingers), threshold_(threshold), attach_callbacks_(attach_callbacks) + { + } + + void init() override + { + SS_SUBSENSOR_INIT(this->fingers_.index, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + SS_SUBSENSOR_INIT(this->fingers_.middle, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + SS_SUBSENSOR_INIT(this->fingers_.ring, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + SS_SUBSENSOR_INIT(this->fingers_.pinky, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + } + + void tick() override + { + if (this->attach_callbacks_) { + LOG_E("gesture.grab", "tick() called when attach_callbacks_ is true, infinite loop go wroom-wroom!"); + } + this->recalculateState(); + } + + void recalculateState() + { + return this->publishState( + this->fingers_.index->getValue() > this->threshold_ + && this->fingers_.middle->getValue() > this->threshold_ + && this->fingers_.ring->getValue() > this->threshold_ + && this->fingers_.pinky->getValue() > this->threshold_ + ); + } + + private: + Fingers fingers_; + float threshold_; + bool attach_callbacks_ = false; + }; + + class PinchGesture : public Gesture { + public: + struct Fingers { + ::SenseShift::Input::FloatSensor* thumb; + ::SenseShift::Input::FloatSensor* index; + }; + + explicit PinchGesture(Fingers fingers, float threshold = 0.5F, bool attach_callbacks = false) : + fingers_(fingers), threshold_(threshold), attach_callbacks_(attach_callbacks) + { + } + + void init() override + { + SS_SUBSENSOR_INIT(this->fingers_.thumb, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + SS_SUBSENSOR_INIT(this->fingers_.index, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + } + + void tick() override + { + if (this->attach_callbacks_) { + LOG_E("gesture.pinch", "tick() called when attach_callbacks_ is true, infinite loop go wroom-wroom!"); + } + this->recalculateState(); + } + + void recalculateState() + { + return this->publishState( + this->fingers_.thumb->getValue() > this->threshold_ && this->fingers_.index->getValue() > this->threshold_ + ); + } + + private: + Fingers fingers_; + float threshold_; + bool attach_callbacks_ = false; + }; +} // namespace SenseShift::Body::Hands::Input \ No newline at end of file diff --git a/lib/hands_input/senseshift/body/hands/input/total_curl.hpp b/lib/hands_input/senseshift/body/hands/input/total_curl.hpp new file mode 100644 index 00000000..e086b894 --- /dev/null +++ b/lib/hands_input/senseshift/body/hands/input/total_curl.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace SenseShift::Body::Hands::Input { + class TotalCurl : public ::SenseShift::Input::FloatSensor { + public: + /// \param joints The joints to calculate the total curl from. + /// \param attach_callbacks Whether to attach callbacks to the joints to update the total curl when they update. + /// If false, the total curl will only be recalculated when the tick() method is called. + /// Setting this to true is not recommended, as it will cause the total curl to + /// be recalculated multiple times per tick (the same as number of joints). + explicit TotalCurl(std::vector<::SenseShift::Input::FloatSensor> joints, bool attach_callbacks = false) : + joints_(std::move(joints)), attach_callbacks_(attach_callbacks) + { + } + + void init() override + { + for (auto& joint : this->joints_) { + SS_SUBSENSOR_INIT(&joint, this->attach_callbacks_, [this](float /*value*/) { + this->recalculateState(); + }); + } + } + + void tick() override + { + if (this->attach_callbacks_) { + LOG_E("total_curl", "tick() called when attach_callbacks_ is true, infinite loop go wroom-wroom!"); + } + this->recalculateState(); + } + + void recalculateState() + { + float total = 0.0F; + + for (auto& joint : this->joints_) { + total += joint.getValue(); + } + + if (!this->joints_.empty()) { + this->publishState(total / static_cast(this->joints_.size())); + } + } + + private: + std::vector<::SenseShift::Input::FloatSensor> joints_; + + bool attach_callbacks_ = false; + }; +} // namespace SenseShift::Body::Hands::Input diff --git a/lib/haptics/senseshift/body/haptics/body.cpp b/lib/haptics/senseshift/body/haptics/body.cpp index edd3ee6e..eac297c1 100644 --- a/lib/haptics/senseshift/body/haptics/body.cpp +++ b/lib/haptics/senseshift/body/haptics/body.cpp @@ -1,33 +1,23 @@ #include "senseshift/body/haptics/body.hpp" +#include "senseshift/body/haptics/interface.hpp" -#include +#include +#include namespace SenseShift::Body::Haptics { - void HapticBody::setup() - { - for (auto& [target, plane] : this->vibroTargets) { - plane->setup(); - } - } + static const char* const TAG = "haptic.body"; - void HapticBody::effect(const EffectRequest& effect) + template + void OutputBody::effect(const Target& target, const Position& pos, const typename Plane::Value& val) { - if (effect.effect == Effect::Vibro && std::holds_alternative(effect.data)) { - auto it = this->vibroTargets.find(effect.target); - if (it == this->vibroTargets.end()) { - log_w("No target found for effect: %d", effect.target); - return; - } - - it->second->effect(effect.position, std::get(effect.data)); - } else { - log_w("Non-supported effect type: %d", effect.effect); + auto plane = this->getTarget(target); + if (!plane.has_value()) { + LOG_W(TAG, "No target found for effect: %d", target); + return; } - } - void HapticBody::addTarget(const Target target, VibroPlane* plane) - { - this->vibroTargets[target] = plane; - this->allTargets.insert({ target, plane }); + plane.value()->effect(pos, val); } + + template class OutputBody; } // namespace SenseShift::Body::Haptics diff --git a/lib/haptics/senseshift/body/haptics/body.hpp b/lib/haptics/senseshift/body/haptics/body.hpp index b0c9068e..d381348a 100644 --- a/lib/haptics/senseshift/body/haptics/body.hpp +++ b/lib/haptics/senseshift/body/haptics/body.hpp @@ -3,29 +3,52 @@ #include "senseshift/body/haptics/interface.hpp" #include "senseshift/body/haptics/plane.hpp" -#include - #include +#include + +#include namespace SenseShift::Body::Haptics { - class HapticBody { + /// IOutput body, contains all the output planes. + /// + /// \tparam Tc The type of the coordinate. + /// \tparam To The type of the output value. + template + class OutputBody { public: - using AuctiativePlane = std::variant; - using PlaneTargetMap = std::multimap; - using VibroTargetMap = std::map; + /// The type of the output plane for the given target. + using Plane = OutputPlane; + /// The type of the target to output plane map (e.g. Chest -> OutputPlane). + using TargetPlaneMap = std::map; + + OutputBody() = default; - HapticBody(){}; + void setup() + { + for (auto& [target, plane] : this->targets_) { + plane->setup(); + } + } - void setup(); + void addTarget(Target target, Plane* plane) { this->targets_[target] = plane; } - void effect(const EffectRequest&); + auto getTarget(Target target) -> std::optional + { + auto find = this->targets_.find(target); + if (find == this->targets_.end()) { + return std::nullopt; + } - void addTarget(const Target, VibroPlane* plane); + return find->second; + } - [[nodiscard]] const PlaneTargetMap* getTargets() const { return &allTargets; } + void effect(const Target& target, const Position& pos, const typename Plane::Value& val); + + [[nodiscard]] auto getTargets() const -> const TargetPlaneMap* { return &targets_; } private: - PlaneTargetMap allTargets{}; - VibroTargetMap vibroTargets{}; + TargetPlaneMap targets_{}; }; + + using FloatBody = OutputBody; } // namespace SenseShift::Body::Haptics diff --git a/lib/haptics/senseshift/body/haptics/interface.hpp b/lib/haptics/senseshift/body/haptics/interface.hpp index c4084531..f0952fd4 100644 --- a/lib/haptics/senseshift/body/haptics/interface.hpp +++ b/lib/haptics/senseshift/body/haptics/interface.hpp @@ -6,9 +6,9 @@ #include namespace SenseShift::Body::Haptics { - using EffectIntex = std::uint8_t; - static constexpr EffectIntex EFFECT_INVALID = 0xFF; - enum class Effect : EffectIntex { + using EffectIndex = std::uint8_t; + static constexpr EffectIndex EFFECT_INVALID = 0xFF; + enum class Effect : EffectIndex { Invalid = EFFECT_INVALID, Vibro = 0x00, // TODO: thermal, etc. @@ -22,7 +22,7 @@ namespace SenseShift::Body::Haptics { ChestFront = 0x00, ChestBack = 0x01, - // Legacy backword compatibility + // Legacy backward compatibility Accessory [[deprecated]] = 0x02, FaceFront, @@ -47,31 +47,36 @@ namespace SenseShift::Body::Haptics { }; using Coordinate = std::uint8_t; - using Position = ::SenseShift::Math::Point2; + using Position = Math::Point2; - // Vibration intensity. - struct VibroEffectData { - using Intensity = std::uint16_t; - static constexpr Intensity INTENSITY_MIN = 0; - static constexpr Intensity INTENSITY_MAX = 4095; + // Vibration intensity_. + class VibroEffectData { + public: + using Intensity = float; - Intensity intensity = 0; + static constexpr Intensity INTENSITY_MIN = 0.0F; + static constexpr Intensity INTENSITY_MAX = 1.0F; - inline constexpr VibroEffectData() = default; - inline constexpr VibroEffectData(const Intensity intensity) : intensity(intensity) {} - inline constexpr VibroEffectData(const VibroEffectData& other) = default; + constexpr VibroEffectData() = default; + constexpr explicit VibroEffectData(const Intensity intensity) : intensity_(intensity) {} + constexpr VibroEffectData(const VibroEffectData& other) = default; - inline constexpr operator std::uint16_t() const { return intensity; } + constexpr inline explicit operator float() const { return this->intensity_; } + + [[nodiscard]] constexpr inline auto getIntensity() const -> Intensity { return intensity_; }; + + private: + Intensity intensity_ = 0; }; // TODO: thermal, etc. - using EffectData = std::variant; struct EffectRequest { Effect effect = Effect::Invalid; Target target = Target::Invalid; Position position = Position(0, 0); + EffectData data; }; } // namespace SenseShift::Body::Haptics diff --git a/lib/haptics/senseshift/body/haptics/plane.cpp b/lib/haptics/senseshift/body/haptics/plane.cpp index ab11c81b..85fb184b 100644 --- a/lib/haptics/senseshift/body/haptics/plane.cpp +++ b/lib/haptics/senseshift/body/haptics/plane.cpp @@ -1,78 +1,83 @@ #include "senseshift/body/haptics/plane.hpp" +#include "senseshift/body/haptics/interface.hpp" #include -#include -#include +#include + +#include +#include namespace SenseShift::Body::Haptics { - template - void ActuativePlane<_Tp, _Ta>::setActuators(const ActuatorMap& actuators) + static const char* const TAG = "haptic.plane"; + + template + void OutputPlane::setActuators(const ActuatorMap& actuators) { - this->actuators.clear(); + this->actuators_.clear(); for (const auto& [point, actuator] : actuators) { - this->actuators[point] = actuator; + this->actuators_[point] = actuator; } - this->points.clear(); + this->points_.clear(); for (const auto& [point, _] : actuators) { - this->points.insert(point); + this->points_.insert(point); } - this->states.clear(); + this->states_.clear(); for (const auto& [point, _] : actuators) { - this->states[point] = 0; + this->states_[point] = static_cast(0); } } - template - void ActuativePlane<_Tp, _Ta>::setup() + template + void OutputPlane::setup() { - for (const auto& [point, actuator] : this->actuators) { - actuator->setup(); + for (const auto& [point, actuator] : this->actuators_) { + actuator->init(); } } - template - void ActuativePlane<_Tp, _Ta>::effect(const Position& pos, const Value& val) + template + void OutputPlane::effect(const Position& pos, const Value& val) { - auto it = this->actuators.find(pos); - if (it == this->actuators.end()) { - log_w("No actuator for point (%u, %u)", pos.x, pos.y); + auto find = this->actuators_.find(pos); + if (find == this->actuators_.end()) { + LOG_W(TAG, "No actuator for point (%u, %u)", pos.x, pos.y); return; } - it->second->writeOutput(val); - this->states[pos] = val; + find->second->writeState(val); + this->states_[pos] = val; } - template - void ActuativePlane_Closest<_Tp, _Ta>::effect(const Position& pos, const Value& val) + template + void OutputPlane_Closest::effect(const Position& pos, const Value& val) { auto& closest = this->findClosestPoint(*this->getAvailablePoints(), pos); - ActuativePlane<_Tp, _Ta>::effect(closest, val); + OutputPlane::effect(closest, val); } - template - const Position& - ActuativePlane_Closest<_Tp, _Ta>::findClosestPoint(const PositionSet& pts, const Position& target) const + template + [[nodiscard]] auto OutputPlane_Closest::findClosestPoint(const PositionSet& pts, const Position& target) + -> const Position& { // check if exact point exists - auto it = pts.find(target); - if (it != pts.end()) { - return *it; + const auto find = pts.find(target); + if (find != pts.end()) { + return *find; } - // find closest point by square distance - std::multimap mp = {}; - for (const auto& _p : pts) { - mp.insert({ (target - _p), _p }); + // find the closest point by square distance + std::multimap distance_map = {}; + for (const auto& point : pts) { + distance_map.insert({ (target - point), point }); } - auto nearest = std::min_element(mp.begin(), mp.end()); + const auto nearest = std::min_element(distance_map.begin(), distance_map.end()); return nearest->second; } - template class ActuativePlane>; - template class ActuativePlane_Closest>; + template class OutputPlane; + template class OutputPlane_Closest; } // namespace SenseShift::Body::Haptics diff --git a/lib/haptics/senseshift/body/haptics/plane.hpp b/lib/haptics/senseshift/body/haptics/plane.hpp index b8ac0727..5971ee32 100644 --- a/lib/haptics/senseshift/body/haptics/plane.hpp +++ b/lib/haptics/senseshift/body/haptics/plane.hpp @@ -2,96 +2,102 @@ #include "senseshift/body/haptics/interface.hpp" -#include -#include - -#include +#include #include #include #include -namespace SenseShift::Body::Haptics { - using PositionSet = std::set; - - /** - * Output "plane" (e.g. Chest, Palm, Finger, etc.) - * - * @tparam _Tp The type of the output value. - */ - template - class ActuativePlane { - static_assert(std::is_same<_Tp, VibroEffectData>()); +#include +#include +#include +namespace SenseShift::Body::Haptics { + /// Output "plane" (e.g. Chest, Palm, Finger, etc.). + /// + /// \tparam Tc The type of the coordinate. + /// \tparam To The type of the output value. + template + class OutputPlane { public: - using Value = _Tp; - using Actuator = _Ta; + /// The type of the coordinate (e.g. std::uint8_t) for the plane. + using Coordinate = Tc; + /// The type of the position (e.g. Point2) for the plane. + using Position = Math::Point2; + using PositionSet = std::set; + + /// The type of the output value (e.g. float) for the plane. + using Value = To; + /// The type of the actuator for the plane. + using Actuator = Output::IOutput; using ActuatorMap = std::map; using PositionStateMap = std::map; - ActuativePlane() = default; + OutputPlane() = default; - ActuativePlane(const ActuatorMap& actuators) { this->setActuators(actuators); } + explicit OutputPlane(const ActuatorMap& actuators) { this->setActuators(actuators); } void setup(); virtual void effect(const Position&, const Value&); - [[nodiscard]] const PositionSet* getAvailablePoints() const { return &points; } - [[nodiscard]] const PositionStateMap* getActuatorStates() const { return &states; } + [[nodiscard]] auto getAvailablePoints() const -> const PositionSet* { return &points_; } + [[nodiscard]] auto getActuatorStates() const -> const PositionStateMap* { return &states_; } - private: - PositionSet points; - ActuatorMap actuators{}; - PositionStateMap states{}; + protected: + void setActuators(const ActuatorMap& actuators); - void setActuators(const ActuatorMap&); + private: + PositionSet points_; + ActuatorMap actuators_{}; + PositionStateMap states_{}; }; - using VibroPlane = ActuativePlane>; - - /** - * Output plane, finds the closest actuator for the given point. - * @deprecated We should guarantee on the driver level, that the actuator is always exists - */ - template - class ActuativePlane_Closest : public ActuativePlane<_Tp, _Ta> { + /// Output plane, finds the closest actuator for the given point. + /// \deprecated We should guarantee on the driver level, that the actuator is always exists. + /// + /// \tparam Tc The type of the coordinate. + /// \tparam To The type of the output value. + template + class OutputPlane_Closest : public OutputPlane { public: - using Value = _Tp; + using Value = To; + using PositionSet = typename OutputPlane::PositionSet; - ActuativePlane_Closest(const typename ActuativePlane<_Tp, _Ta>::ActuatorMap& actuators) : - ActuativePlane<_Tp, _Ta>(actuators) + explicit OutputPlane_Closest(const typename OutputPlane::ActuatorMap& actuators) : + OutputPlane(actuators) { } void effect(const Position&, const Value&) override; private: - [[nodiscard]] const Position& findClosestPoint(const PositionSet&, const Position&) const; + [[nodiscard]] static auto findClosestPoint(const PositionSet&, const Position&) -> const Position&; }; - using VibroPlane_Closest = - ActuativePlane_Closest>; + using FloatPlane = OutputPlane; + using FloatPlane_Closest = OutputPlane_Closest; // TODO: configurable margin class PlaneMapper_Margin { public: - template - [[nodiscard]] static constexpr inline std::map - mapMatrixCoordinates(std::vector> map2d) + /// Maps a 2D matrix into a 1D (coord, object) map. + template + [[nodiscard]] static constexpr auto mapMatrixCoordinates(std::vector> map2d) + -> std::map { - std::map points{}; + std::map points{}; - size_t y_size = map2d.size(); - size_t y_max = y_size - 1; + const size_t y_size = map2d.size(); + const size_t y_max = y_size - 1; for (size_t y = 0; y < y_size; ++y) { auto row = map2d.at(y); - size_t x_size = row.size(); - size_t x_max = x_size - 1; + const size_t x_size = row.size(); + const size_t x_max = x_size - 1; for (size_t x = 0; x < x_size; ++x) { auto* wr = row.at(x); - Position coord = PlaneMapper_Margin::mapPoint(x, y, x_max, y_max); + Position coord = mapPoint(x, y, x_max, y_max); points[coord] = wr; } @@ -100,19 +106,16 @@ namespace SenseShift::Body::Haptics { return points; } - /** - * Re-maps a point index to output coordinate. - * @tparam _Tp The type of the point index. - */ - template - [[nodiscard]] static constexpr inline ::SenseShift::Math::Point2<_Tp> - mapPoint(_Tp x, _Tp y, _Tp x_max, _Tp y_max) + /// Re-maps a point index to output coordinate. + /// \tparam Tp The type of the point index. + template + [[nodiscard]] static constexpr auto mapPoint(Tp x, Tp y, Tp x_max, Tp y_max) -> Math::Point2 { - using Point = ::SenseShift::Math::Point2<_Tp>; + using LocalPointType = Math::Point2; - return Point( - ::SenseShift::accurateMap<_Tp>(x + 1, 0, x_max + 2, Point::MIN, Point::MAX), - ::SenseShift::accurateMap<_Tp>(y + 1, 0, y_max + 2, Point::MIN, Point::MAX) + return LocalPointType( + ::SenseShift::remap(x + 1, 0, x_max + 2, LocalPointType::MIN, LocalPointType::MAX), + ::SenseShift::remap(y + 1, 0, y_max + 2, LocalPointType::MIN, LocalPointType::MAX) ); } }; diff --git a/lib/io/senseshift/input/analog_threshold.hpp b/lib/io/senseshift/input/analog_threshold.hpp new file mode 100644 index 00000000..48397ef4 --- /dev/null +++ b/lib/io/senseshift/input/analog_threshold.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include "senseshift/input/sensor.hpp" +#include + +namespace SenseShift::Input { + template + class AnalogThresholdSensor : public BinarySensor { + public: + /// Analog threshold sensor with hysteresis. + /// + /// \param source The source sensor. + /// \param threshold_upper Upper threshold, that needs to be crossed to transition from `low` to `high` states. + /// \param threshold_lower Lower threshold, that needs to be crossed to transition from `high` to `low` states. + explicit AnalogThresholdSensor( + ::SenseShift::Input::Sensor* source, Tp threshold_upper, Tp threshold_lower, bool attach_callbacks = false + ) : + source_(source), + threshold_upper_(threshold_upper), + threshold_lower_(threshold_lower), + attach_callbacks_(attach_callbacks) + { + } + + /// \param source The source sensor. + /// \param threshold Threshold, that will be used for both upper and lower thresholds. + template, int> = 0> + explicit AnalogThresholdSensor( + ::SenseShift::Input::Sensor* source, float threshold = 0.5f, bool attach_callbacks = false + ) : + AnalogThresholdSensor(source, threshold, threshold, attach_callbacks) + { + } + + void init() override + { + SS_SUBSENSOR_INIT(this->source_, this->attach_callbacks_, [this](Tp /*value*/) { + this->recalculateState(); + }); + } + + void tick() override + { + if (this->attach_callbacks_) { + LOG_E( + "sensor.analog_threshold", + "tick() called when attach_callbacks_ is true, infinite loop go wroom-wroom!" + ); + } + this->recalculateState(); + } + + void recalculateState() + { + const auto sensor_value = this->source_->getValue(); + this->publishState(sensor_value >= (this->getValue() ? this->threshold_lower_ : this->threshold_upper_)); + } + + private: + ::SenseShift::Input::Sensor* source_; + + Tp threshold_lower_, threshold_upper_; + + bool attach_callbacks_ = false; + }; +} // namespace SenseShift::Input diff --git a/lib/io/senseshift/input/calibration.hpp b/lib/io/senseshift/input/calibration.hpp new file mode 100644 index 00000000..fd6ec99e --- /dev/null +++ b/lib/io/senseshift/input/calibration.hpp @@ -0,0 +1,238 @@ +/// Calibrated input +/// Credit: https://github.com/JohnRThomas/OpenGloves-Firmware/blob/main/open-gloves/Calibration.hpp + +#pragma once + +#include "senseshift/utility.hpp" + +#include + +namespace SenseShift::Input::Calibration { + struct ICalibrated { + virtual void startCalibration() = 0; + virtual void stopCalibration() = 0; + virtual void reselCalibration() = 0; + }; + + template + struct ICalibrator { + /// Reset the calibration. + virtual void reset() = 0; + + /// Update the calibration with a new input value. + virtual void update(Tp input) = 0; + + /// Calibrate the input value. + [[nodiscard]] virtual auto calibrate(Tp input) const -> Tp = 0; + }; + + template + class MinMaxCalibrator : public ICalibrator { + static_assert(std::is_arithmetic_v, "MinMaxCalibrator only can be used with arithmetic types"); + + public: + using ValueType = Tp; + + explicit MinMaxCalibrator(Tp output_min, Tp output_max) : + output_min_(output_min), output_max_(output_max), value_min_(output_max), value_max_(output_min) + { + } + + template, int> = 0> + explicit MinMaxCalibrator(Tp output_min = 0.0F, Tp output_max = 1.0F) : + output_min_(output_min), output_max_(output_max), value_min_(output_max), value_max_(output_min) + { + } + + void reset() override + { + value_min_ = output_max_; + value_max_ = output_min_; + } + + void update(ValueType input) override + { + // Update the min and the max. + if (input < value_min_) { + value_min_ = input; + } + if (input > value_max_) { + value_max_ = input; + } + } + + auto calibrate(ValueType input) const -> ValueType override + { + // This means we haven't had any calibration data yet. + // Return a neutral value right in the middle of the output range. + if (value_min_ > value_max_) { + return (output_min_ + output_max_) / 2.0F; + } + + if (input <= value_min_) { + return output_min_; + } + + if (input >= value_max_) { + return output_max_; + } + + // Map the input range to the output range. + ValueType output = + ::SenseShift::remap(input, value_min_, value_max_, output_min_, output_max_); + + // Lock the range to the output. + return std::clamp(output, output_min_, output_max_); + } + + private: + const ValueType output_min_; + const ValueType output_max_; + + ValueType value_min_; + ValueType value_max_; + }; + + template + class CenterPointDeviationCalibrator : public ICalibrator { + static_assert( + std::is_arithmetic_v, "CenterPointDeviationCalibrator only can be used with arithmetic types" + ); + + public: + using ValueType = Tp; + + CenterPointDeviationCalibrator(Tp sensor_max, Tp driver_max_deviation, Tp output_min, Tp output_max) : + sensor_max_(sensor_max), + driver_max_deviation_(driver_max_deviation), + output_min_(output_min), + output_max_(output_max), + range_min_(sensor_max), + range_max_(0) + { + } + + template, int> = 0> + CenterPointDeviationCalibrator( + Tp sensor_max, Tp driver_max_deviation, Tp output_min = 0.0F, Tp output_max = 1.0F + ) : + sensor_max_(sensor_max), + driver_max_deviation_(driver_max_deviation), + output_min_(output_min), + output_max_(output_max), + range_min_(sensor_max), + range_max_(0) + { + } + + void reset() override + { + this->range_min_ = this->sensor_max_; + this->range_max_ = 0; + } + + void update(Tp input) override + { + // Update the min and the max. + if (input < this->range_min_) { + this->range_min_ = + ::SenseShift::remap(input, this->output_min_, this->output_max_, 0, this->sensor_max_); + } + if (input > this->range_max_) { + this->range_max_ = + ::SenseShift::remap(input, this->output_min_, this->output_max_, 0, this->sensor_max_); + } + } + + auto calibrate(ValueType input) const -> ValueType override + { + // Find the center point of the sensor, so we know how much we have deviated from it. + Tp center = (this->range_min_ + this->range_max_) / 2.0F; + + // Map the input to the sensor range of motion. + int output = ::SenseShift::remap(input, this->output_min_, this->output_max_, 0, this->sensor_max_); + + // Find the deviation from the center and clamp it to the maximum that the driver supports. + output = std::clamp(output - center, -(this->driver_max_deviation_), this->driver_max_deviation_); + + // Finally map the deviation from the center back to the output range. + return SenseShift::remap( + output, + -(this->driver_max_deviation_), + this->driver_max_deviation_, + this->output_min_, + this->output_max_ + ); + } + + private: + const Tp sensor_max_; + const Tp driver_max_deviation_; + const Tp output_min_; + const Tp output_max_; + + Tp range_min_; + Tp range_max_; + }; + + template + class FixedCenterPointDeviationCalibrator : public ICalibrator { + static_assert( + std::is_arithmetic_v, "FixedCenterPointDeviationCalibrator only can be used with arithmetic types" + ); + + public: + using ValueType = Tp; + + explicit FixedCenterPointDeviationCalibrator( + Tp sensor_max, Tp driver_max_deviation, Tp output_min, Tp output_max + ) : + sensor_max_(sensor_max), + driver_max_deviation_(driver_max_deviation), + output_min_(output_min), + output_max_(output_max) + { + } + + template, int> = 0> + explicit FixedCenterPointDeviationCalibrator( + Tp sensor_max, Tp driver_max_deviation, Tp output_min = 0.0F, Tp output_max = 1.0F + ) : + sensor_max_(sensor_max), + driver_max_deviation_(driver_max_deviation), + output_min_(output_min), + output_max_(output_max) + { + } + + void reset() override {} + void update(ValueType input) override {} + + auto calibrate(ValueType input) const -> ValueType override + { + // Find the center point of the sensor, so we know how much we have deviated from it. + Tp center = this->sensor_max_ / 2.0F; + + // Map the input to the sensor range of motion. + int output = ::SenseShift::remap(input, this->output_min_, this->output_max_, 0, this->sensor_max_); + + // Find the deviation from the center and clamp it to the maximum that the driver supports. + output = std::clamp(output - center, -(this->driver_max_deviation_), this->driver_max_deviation_); + + // Finally map the deviation from the center back to the output range. + return SenseShift::remap( + output, + -(this->driver_max_deviation_), + this->driver_max_deviation_, + this->output_min_, + this->output_max_ + ); + } + + private: + const Tp sensor_max_; + const Tp driver_max_deviation_; + const Tp output_min_; + const Tp output_max_; + }; +} // namespace SenseShift::Input::Calibration diff --git a/lib/io/senseshift/input/filter.hpp b/lib/io/senseshift/input/filter.hpp new file mode 100644 index 00000000..b13bc857 --- /dev/null +++ b/lib/io/senseshift/input/filter.hpp @@ -0,0 +1,303 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace SenseShift::Input::Filter { + template + class ISimpleSensor { + public: + using ValueType = Tp; + + virtual auto getValue() -> ValueType = 0; + }; + + template + class Sensor; + + template + class IFilter { + public: + using ValueType = Tp; + + virtual auto filter(ISimpleSensor* sensor, ValueType value) -> ValueType = 0; + }; + + template + class IFiltered { + public: + using ValueType = Tp; + + virtual void addFilter(IFilter* filter) = 0; + + virtual void addFilters(std::vector*> filters) = 0; + + virtual void setFilters(std::vector*> filters) = 0; + + void clearFilters() = 0; + }; + + template + class AddFilter : public IFilter { + public: + explicit AddFilter(Tp offset) : offset_(offset){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override { return value + this->offset_; } + + private: + Tp offset_; + }; + + template + class SubtractFilter : public IFilter { + public: + explicit SubtractFilter(Tp offset) : offset_(offset){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override { return value - this->offset_; } + + private: + Tp offset_; + }; + + template + class MultiplyFilter : public IFilter { + public: + explicit MultiplyFilter(Tp factor) : factor_(factor){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override { return value * this->factor_; } + + private: + Tp factor_; + }; + + class VoltageDividerFilter : public MultiplyFilter { + public: + /// Calculates the original voltage from the voltage divider. + /// + /// \param r1 The resistance in Ohms of the first resistor in the voltage divider. + /// Example: 27000.0F. + /// \param r2 The resistance in Ohms of the second resistor in the voltage divider. + /// Example: 100000.0F. + /// + /// \example + /// \code + /// new VoltageDividerFilter(27000.0F, 100000.0F); + /// \endcode + explicit VoltageDividerFilter(float r1, float r2) : MultiplyFilter((r1 + r2) / r2){}; + }; + + template + class ClampFilter : public IFilter { + public: + ClampFilter(Tp min, Tp max) : min_(min), max_(max){}; + + auto filter(ISimpleSensor* /*source*/, Tp value) -> Tp override + { + return std::clamp(value, this->min_, this->max_); + } + + private: + Tp min_; + Tp max_; + }; + + /// An alias for ClampFilter. + template + using MinMaxFilter = ClampFilter; + + /// An alias for ClampFilter. + template + using RangeFilter = ClampFilter; + + template + class LambdaFilter : public IFilter { + public: + using Lambda = std::function; + + explicit LambdaFilter(Lambda filter) : filter_(std::move(filter)){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override { return this->filter_(value); } + + private: + Lambda filter_; + }; + + /// Average filter. Reads the value from the sensor and returns the average of the N values. + template + class SampleAverageFilter : public IFilter { + static_assert(std::is_arithmetic_v, "SampleAverageFilter only supports arithmetic types"); + static_assert(std::is_same_v, "Sensor type must match filter type"); + static_assert( + std::is_same_v, + "Can only use sensors with readRawValue()" + ); + + public: + explicit SampleAverageFilter(std::size_t size) : size_(size){}; + + auto filter(ISimpleSensor* sensor, Tp value) -> Tp override + { + auto sum = value; + + // Read the acc_ from the sensor N-1 times and sum them up. + // We read it N-1 times because we already have the first acc_. + for (std::size_t i = 0; i < this->size_ - 1; i++) { + sum += sensor->readRawValue(); + } + + // Return the average of the values. + return sum / this->size_; + } + + private: + std::size_t size_; + }; + + template + class SampleMedianFilter : public IFilter { + static_assert(std::is_same_v, "Sensor type must match filter type"); + // static_assert(std::is_same_v, "Can only use sensors with + // readRawValue()"); + + public: + explicit SampleMedianFilter(std::size_t size_) : size_(size_) + { + // allocate the array + this->values = new Tp[size_]; + }; + + auto filter(ISimpleSensor* sensor, Tp value) -> Tp override + { + if (sensor == nullptr) { + LOG_E("filter.sampling_median", "Source sensor is null"); + return value; + } + + this->values[0] = value; + + // Read the acc_ from the sensor N-1 times and put them in the array. + // We read it N-1 times because we already have the first acc_. + for (std::size_t i = 1; i <= this->size_ - 1; i++) { + this->values[i] = sensor->getValue(); + } + + // Sort the array. + std::sort(this->values, this->values + this->size_); + + // Return the median of the values. + return this->values[this->size_ / 2]; + } + + private: + std::size_t size_; + Tp* values; + }; + + template + class SlidingWindowMovingAverageFilter : public IFilter { + static_assert(std::is_arithmetic_v, "SlidingWindowAverageFilter only supports arithmetic types"); + + public: + explicit SlidingWindowMovingAverageFilter(size_t window_size) : window_size_(window_size){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override + { + while (this->queue_.size() >= this->window_size_) { + this->queue_.pop_front(); + } + this->queue_.push_back(value); + + return this->getAverage(); + } + + private: + std::size_t window_size_; + std::deque queue_; + + [[nodiscard]] auto getAverage() const -> Tp + { + Tp sum = 0; + for (auto value : this->queue_) { + sum += value; + } + return sum / this->queue_.size(); + } + }; + + template + class ExponentialMovingAverageFilter : public IFilter { + static_assert(std::is_arithmetic_v, "ExponentialMovingAverageFilter only supports arithmetic types"); + + public: + explicit ExponentialMovingAverageFilter(float alpha) : alpha_(alpha){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override + { + if (this->is_first_) { + this->is_first_ = false; + + this->acc_ = value; + return this->acc_; + } + + this->acc_ = (this->alpha_ * value) + ((1 - this->alpha_) * this->acc_); + return this->acc_; + } + + private: + bool is_first_ = true; + float alpha_; + Tp acc_; + }; + + /// Deadzone filter. Clamps acc_ to center if it is within the deadzone. + /// Usually used to filter out noise in the joystick. + class CenterDeadzoneFilter : public IFilter { + public: + explicit CenterDeadzoneFilter(float deadzone, float center = 0.5f) : deadzone_(deadzone), center_(center){}; + + auto filter(ISimpleSensor* /*sensor*/, float value) -> float override + { + float const deviation = std::abs(value - this->center_); + return deviation < deadzone_ ? this->center_ : value; + } + + private: + const float center_; + + float deadzone_; + }; + + /// Interpolates the value from the lookup table. + /// Can be used to determine battery level from the voltage. + /// + /// \tparam Tp Type of the lookup table values. + /// \tparam Container Type of the lookup table container. + template + class LookupTableInterpolationFilter : public IFilter { + static_assert(std::is_same_v); + static_assert(std::is_arithmetic_v, "LookupTableInterpolationFilter only supports arithmetic types"); + + public: + explicit LookupTableInterpolationFilter(Container const& lookup_table) : lookup_table_(lookup_table){}; + + auto filter(ISimpleSensor* /*sensor*/, Tp value) -> Tp override + { + return SenseShift::lookup_table_interpolate(this->lookup_table_, value); + } + + private: + Container const& lookup_table_; + }; +} // namespace SenseShift::Input::Filter diff --git a/lib/io/senseshift/input/sensor.hpp b/lib/io/senseshift/input/sensor.hpp index 2520cd76..3d6e1b1f 100644 --- a/lib/io/senseshift/input/sensor.hpp +++ b/lib/io/senseshift/input/sensor.hpp @@ -1,162 +1,213 @@ #pragma once +#include #include +#include -#include -#include -#include - -#if defined(__AVR__) -#define ANALOG_MAX 1023 -#elif defined(ESP32) -#define ANALOG_MAX 4095 -#elif !defined(ANALOG_MAX) -#warning "This board doesn't have an auto ANALOG_MAX assignment, please set it manually" -#define ANALOG_MAX static_assert(false, "ANALOG_MAX is not defined") -// Uncomment and set as needed (only touch if you know what you are doing) -// #define ANALOG_MAX 4095 -#endif +#include "senseshift/input/calibration.hpp" +#include "senseshift/input/filter.hpp" + +#include +#include + +#define SS_SUBSENSOR_INIT(SENSOR, ATTACH_CALLBACK, CALLBACK) \ + (SENSOR)->init(); \ + if (ATTACH_CALLBACK) { \ + (SENSOR)->addValueCallback(CALLBACK); \ + } namespace SenseShift::Input { - /** - * Abstract hardware sensor (e.g. potentiometer, flex sensor, etc.) - * @tparam _Tp Type of the sensor value - */ - template - struct ISimpleSensor : public virtual IInitializable { - using ValueType = _Tp; - - /** - * Get the current sensor value - */ - virtual ValueType getValue() = 0; + /// Abstract hardware sensor (e.g. potentiometer, flex sensor, etc.) + /// \tparam Tp Type of the sensor value + template + class ISimpleSensor : public virtual IInitializable { + public: + using ValueType = Tp; + + explicit ISimpleSensor() = default; + + /// Get the current sensor value. + [[nodiscard]] virtual auto getValue() -> ValueType = 0; }; - template - struct ISensor : public virtual ISimpleSensor<_Tp>, ITickable {}; + using IBinarySimpleSensor = ISimpleSensor; + using IFloatSimpleSensor = ISimpleSensor; - /** - * Memoized sensor decorator - * @tparam _Tp Type of the sensor value - */ - template - class MemoizedSensor : public ISensor<_Tp> { - protected: - ISimpleSensor<_Tp>* sensor; - _Tp value; + template + class ISensor : public virtual ISimpleSensor, public Calibration::ICalibrated {}; + template + class Sensor : public ISensor, public Component { public: - /** - * @param sensor Sensor to be decorated - */ - MemoizedSensor(ISimpleSensor<_Tp>* sensor) : sensor(sensor){}; - - /** - * Setup the sensor hardware - */ - void init() override { this->sensor->init(); }; - - /** - * Read actual value from the hardware and memoize it - */ - void tick() override { this->value = this->sensor->getValue(); }; - - /** - * Get the current memoized value - */ - _Tp getValue() override { return this->value; }; - }; + using ValueType = Tp; + using CallbackManagerType = CallbackManager; + using CallbackType = typename CallbackManagerType::CallbackType; - template - class ICalibratedSimpleSensor : public ISimpleSensor<_Tp>, public Calibration::ICalibrated {}; + explicit Sensor() = default; - /** - * Calibrated sensor decorator - * - * @tparam _Tp Type of the sensor value - */ - template - class CalibratedSimpleSensor : public ICalibratedSimpleSensor<_Tp> { - public: - /** - * @param sensor Sensor to be decorated - * @param calibrator ICalibrator algorithm to be used - */ - CalibratedSimpleSensor(ISimpleSensor<_Tp>* sensor, Calibration::ICalibrator<_Tp>* calibrator) : - sensor(sensor), calibrator(calibrator){}; + template, int> = 0> + explicit Sensor(float value = 0.0f) : raw_value_(value) + { + this->value_ = this->applyFilters(value); + } - void init() override { this->sensor->init(); }; - _Tp getValue() override { return this->getCalibratedValue(); }; + /// Appends a filter to the sensor's filter chain. + /// + /// \param filter The filter to add. + /// + /// \see addFilters for adding multiple filters. + void addFilter(Filter::IFilter* filter) { this->filters_.push_back(filter); } + + /// Adds multiple filters to the sensor's filter chain. Appends to the end of the chain. + /// + /// \param filters The chain of filters to add. + /// + /// \example + /// \code + /// sensor->addFilters({ + /// new MinMaxFilter(0.1f, 0.9f), + /// new CenterDeadzoneFilter(0.1f), + /// }); + /// \endcode + void addFilters(std::vector*> filters) + { + this->filters_.insert(this->filters_.end(), filters.begin(), filters.end()); + } - void resetCalibration() override { this->calibrator->reset(); }; - void enableCalibration() override { calibrating = true; } - void disableCalibration() override { calibrating = false; } + /// Replaces the sensor's filter chain with the given filters. + /// + /// \param filters New filter chain. + /// + /// \example + /// \code + /// sensor->setFilters({ + /// new MinMaxFilter(0.1f, 0.9f), + /// new CenterDeadzoneFilter(0.1f), + /// }); + /// \endcode + void setFilters(std::vector*> filters) { this->filters_ = filters; } - protected: - ISimpleSensor<_Tp>* sensor; - Calibration::ICalibrator<_Tp>* calibrator; - bool calibrating = false; + /// Removes everything from the sensor's filter chain. + void clearFilters() { this->filters_.clear(); } - _Tp getCalibratedValue() - { - auto value = this->sensor->getValue(); + void setCalibrator(Calibration::ICalibrator* calibrator) { this->calibrator_ = calibrator; } - if (this->calibrating) { - this->calibrator->update(value); - } + void clearCalibrator() { this->calibrator_ = std::nullopt; } + + void startCalibration() override { this->is_calibrating_ = true; } - return this->calibrator->calibrate(value); + void stopCalibration() override { this->is_calibrating_ = false; } + + void reselCalibration() override + { + if (this->calibrator_.has_value()) { + this->calibrator_.value()->reset(); + } } - }; - template - class AverageSensor : public ISimpleSensor<_Tp> { - static_assert(std::is_arithmetic<_Tp>::value, "AverageSensor only supports arithmetic types"); + void addValueCallback(CallbackType&& callback) { this->callbacks_.add(std::move(callback)); } - public: - AverageSensor(ISimpleSensor<_Tp>* sensor, size_t samples) : sensor(sensor), samples(samples) {} + void addRawValueCallback(CallbackType&& callback) { this->raw_callbacks_.add(std::move(callback)); } - void init() override { this->sensor->init(); }; + void init() override {} - _Tp getValue() override + /// Publish the given state to the sensor. + /// + /// Firstly, the given state will be assigned to the sensor's raw_value_. + /// Then, the raw_value_ will be passed through the sensor's filter chain. + /// Finally, the filtered value will be assigned to the sensor's .value_. + /// + /// \param rawValue The new .raw_value_. + void publishState(ValueType rawValue) { - // TODO: another type for sum? - double sum = 0; - for (size_t i = 0; i < this->samples; i++) { - sum += this->sensor->getValue(); + this->raw_value_ = rawValue; + this->raw_callbacks_.call(this->raw_value_); + + this->value_ = this->applyFilters(rawValue); + this->callbacks_.call(this->value_); + } + + /// Get the current sensor .value_. + [[nodiscard]] auto getValue() -> ValueType override { return this->value_; } + + /// Get the current raw sensor .raw_value_. + [[nodiscard]] auto getRawValue() -> ValueType { return this->raw_value_; } + + protected: + /// Apply current filters to value. + [[nodiscard]] auto applyFilters(ValueType value) -> ValueType + { + /// Apply calibration + if (this->calibrator_.has_value()) { + if (this->is_calibrating_) { + this->calibrator_.value()->update(value); + } + + value = this->calibrator_.value()->calibrate(value); } - return sum / this->samples; + /// Apply filters + for (auto filter : this->filters_) { + value = filter->filter(nullptr, value); + } + + return value; } private: - ISimpleSensor<_Tp>* sensor; - size_t samples; + friend class Filter::IFilter; + friend class Calibration::ICalibrator; + + /// The sensor's filter chain. + std::vector*> filters_ = std::vector*>(); + + bool is_calibrating_ = false; + std::optional*> calibrator_ = std::nullopt; + + ValueType raw_value_; + ValueType value_; + + /// Storage for raw state callbacks. + CallbackManagerType raw_callbacks_; + /// Storage for filtered state callbacks. + CallbackManagerType callbacks_; }; - template - class StaticMedianSensor : public ISimpleSensor<_Tp> { - static_assert(std::is_arithmetic<_Tp>::value, "StaticMedianSensor only supports arithmetic types"); - static_assert(_Samples % 2 == 1, "StaticMedianSensor only supports odd sample sizes"); + using FloatSensor = Sensor; + // todo: support double/triple/N-times/long click and so on + using BinarySensor = Sensor; + + template + class SimpleSensorDecorator : public Sensor { public: - StaticMedianSensor(ISimpleSensor<_Tp>* sensor) : sensor(sensor) {} + using ValueType = Tp; + using SourceType = ISimpleSensor; + + explicit SimpleSensorDecorator(SourceType* source) : source_(source) {} - void init() override { this->sensor->init(); }; + void init() override { this->source_->init(); } - _Tp getValue() override + void tick() override { this->updateValue(); } + + auto updateValue() -> ValueType { - for (size_t i = 0; i < _Samples; i++) { - this->values[i] = this->sensor->getValue(); - } + auto const raw_value = this->readRawValue(); + this->publishState(raw_value); - std::sort(this->values, this->values + _Samples); + LOG_D("decorator.simple", " raw_value=%f, value=%f", raw_value, this->getValue()); - return this->values[_Samples / 2]; + return this->getValue(); } + [[nodiscard]] auto readRawValue() -> ValueType { return this->source_->getValue(); } + + protected: private: - _Tp values[_Samples]; - ISimpleSensor<_Tp>* sensor; + SourceType* source_; }; + + namespace _private { + class TheFloatSensor : public Sensor {}; + } // namespace _private } // namespace SenseShift::Input diff --git a/lib/io/senseshift/input/sensor/joystick.hpp b/lib/io/senseshift/input/sensor/joystick.hpp deleted file mode 100644 index a03af6f7..00000000 --- a/lib/io/senseshift/input/sensor/joystick.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "senseshift/input/sensor.hpp" - -namespace SenseShift::Input { - /** - * Joystick axis sensor decorator - */ - template - class JoystickAxisSensor : public ISimpleSensor<_Tp> { - private: - ISimpleSensor<_Tp>* sensor; - float dead_zone; - - int filterDeadZone(int in) - { - // This function clamps the input to the center of the range if - // the value is within the threshold. This is to eliminate at-rest - // noise of the joystick. - int center = ANALOG_MAX / 2; - return abs(center - in) < dead_zone * ANALOG_MAX ? center : in; - } - - public: - JoystickAxisSensor(ISimpleSensor<_Tp>* sensor, float dead_zone) : sensor(sensor), dead_zone(dead_zone){}; - - void init() override { this->sensor->init(); }; - - uint16_t getValue(void) override - { - auto value = this->sensor->getValue(); - value = this->filterDeadZone(value); - return value; - } - }; -}; // namespace SenseShift::Input diff --git a/lib/io/senseshift/output/actuator.hpp b/lib/io/senseshift/output/actuator.hpp deleted file mode 100644 index 3938c65f..00000000 --- a/lib/io/senseshift/output/actuator.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include - -#include - -namespace SenseShift::Output { - // Singular output point (e.g. vibration motor) - template - class IActuator { - public: - virtual void setup(){}; - virtual void writeOutput(T) = 0; - }; - - template<> - class IActuator { - public: - virtual void setup(){}; - virtual void writeOutput(std::uint16_t) = 0; - }; -} // namespace SenseShift::Output diff --git a/lib/io/senseshift/output/output.hpp b/lib/io/senseshift/output/output.hpp new file mode 100644 index 00000000..581770d7 --- /dev/null +++ b/lib/io/senseshift/output/output.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +namespace SenseShift::Output { + template + class IOutput : public IInitializable { + public: + using ValueType = Tp; + + virtual void writeState(ValueType value) = 0; + }; + + using IBinaryOutput = IOutput; + using IFloatOutput = IOutput; +} // namespace SenseShift::Output \ No newline at end of file diff --git a/lib/math/senseshift/math/point2.hpp b/lib/math/senseshift/math/point2.hpp index b91b096b..57d3f1e2 100644 --- a/lib/math/senseshift/math/point2.hpp +++ b/lib/math/senseshift/math/point2.hpp @@ -1,33 +1,36 @@ #pragma once #include +#include #include +#include namespace SenseShift::Math { - template + template struct Point2 { - static_assert( - std::is_arithmetic<_Tp>::value, "::SenseShift::Math::Point2 only can be used with arithmetic types" - ); + static_assert(std::is_arithmetic_v, "Point2 only can be used with arithmetic types"); - using Value = _Tp; + using Value = Tp; - static constexpr _Tp MIN = std::numeric_limits<_Tp>::min(); - static constexpr _Tp MAX = std::numeric_limits<_Tp>::max(); + static constexpr Tp MIN = std::numeric_limits::min(); + static constexpr Tp MAX = std::numeric_limits::max(); - _Tp x, y; + Tp x, y; - constexpr Point2() : x((_Tp) 0), y((_Tp) 0){}; - constexpr Point2(_Tp x, _Tp y) : x(x), y(y){}; - constexpr Point2(const Point2<_Tp>& v) : x((_Tp) v.x), y((_Tp) v.y){}; + constexpr Point2() : x(static_cast(0)), y(static_cast(0)){}; + constexpr Point2(Tp x, Tp y) : x(x), y(y){}; + constexpr Point2(const Point2& v) : x((Tp) v.x), y((Tp) v.y){}; - constexpr inline bool operator==(const Point2<_Tp>& rhs) const { return x == rhs.x && y == rhs.y; } + constexpr inline auto operator==(const Point2& rhs) const -> bool { return x == rhs.x && y == rhs.y; } - constexpr inline bool operator!=(const Point2<_Tp>& rhs) const { return !(*this == rhs); } + constexpr inline auto operator!=(const Point2& rhs) const -> bool { return !(*this == rhs); } - constexpr bool operator<(const Point2<_Tp>& rhs) const { return std::tie(x, y) < std::tie(rhs.x, rhs.y); } + constexpr auto operator<(const Point2& rhs) const -> bool + { + return std::tie(x, y) < std::tie(rhs.x, rhs.y); + } - constexpr float operator-(const Point2<_Tp>& rhs) const + constexpr auto operator-(const Point2& rhs) const -> float { return std::sqrt(std::pow(x - rhs.x, 2) + std::pow(y - rhs.y, 2)); } diff --git a/lib/opengloves/og_constants.hpp b/lib/opengloves/og_constants.hpp deleted file mode 100644 index df163940..00000000 --- a/lib/opengloves/og_constants.hpp +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -#define OPENGLOVES_FINGERS_TASK_PRIORITY 1 - -#define OPENGLOVES_COMM_SERIAL 0x1 -#define OPENGLOVES_COMM_BTSERIAL 0x2 -#define OPENGLOVES_COMM_BLESERIAL 0x3 diff --git a/lib/opengloves/og_ffb.hpp b/lib/opengloves/og_ffb.hpp deleted file mode 100644 index ca2c5b9d..00000000 --- a/lib/opengloves/og_ffb.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -#include - -namespace OpenGloves { - struct HandActuators { - std::optional<::SenseShift::Output::IActuator*> thumb = std::nullopt; - std::optional<::SenseShift::Output::IActuator*> index = std::nullopt; - std::optional<::SenseShift::Output::IActuator*> middle = std::nullopt; - std::optional<::SenseShift::Output::IActuator*> ring = std::nullopt; - std::optional<::SenseShift::Output::IActuator*> pinky = std::nullopt; - }; -} // namespace OpenGloves diff --git a/lib/opengloves/og_protocol.hpp b/lib/opengloves/og_protocol.hpp deleted file mode 100644 index a8f19e2e..00000000 --- a/lib/opengloves/og_protocol.hpp +++ /dev/null @@ -1,79 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace OpenGloves { - struct IEncodedInput { - public: - enum Type : char { - THUMB = 'A', - INDEX = 'B', - MIDDLE = 'C', - RING = 'D', - PINKY = 'E', - JOY_X = 'F', - JOY_Y = 'G', - JOY_BTN = 'H', - TRIGGER = 'I', - A_BTN = 'J', - B_BTN = 'K', - GRAB = 'L', - PINCH = 'M', - MENU = 'N', - CALIBRATE = 'O' - }; - - IEncodedInput(Type type) : type(type){}; - - constexpr Type getType() const { return this->type; } - - protected: - Type type; - }; - - class IStringEncoded : public IEncodedInput { - public: - IStringEncoded(Type type) : IEncodedInput(type){}; - - virtual size_t getEncodedLength() const = 0; - virtual size_t encodeString(char* buffer) const = 0; - }; - - class IStringEncodedMemoizedSensor : public IStringEncoded { - public: - IStringEncodedMemoizedSensor(Type type) : IStringEncoded(type){}; - - virtual void init() = 0; - virtual void updateValue() = 0; - }; - - class ICommunication { - public: - virtual void setup() = 0; - virtual void send(std::vector& sensors) = 0; - virtual bool hasData() = 0; - virtual size_t readCommand(char* buffer, size_t length) = 0; - }; - - using CommandIndex = uint16_t; - enum Command : CommandIndex { - ThumbCurl, - ThumbSplay, - - IndexCurl, - IndexSplay, - - MiddleCurl, - MiddleSplay, - - RingCurl, - RingSplay, - - PinkyCurl, - PinkySplay, - }; - - using CommandCallback = std::function; -} // namespace OpenGloves diff --git a/lib/opengloves/opengloves/opengloves.cpp b/lib/opengloves/opengloves/opengloves.cpp new file mode 100644 index 00000000..8f3f0c45 --- /dev/null +++ b/lib/opengloves/opengloves/opengloves.cpp @@ -0,0 +1,272 @@ +#include "opengloves/opengloves.hpp" + +#include +#include +#include +#include +#include +#include + +namespace og { + +#ifdef OG_ENCODE_FASTER + inline auto ifloor(float d) -> int + { + union Cast { + double d; + long l; + }; + volatile Cast c; + c.d = d + 6755399441055743.5; + return c.l; + } +#elifdef OG_ENCODE_FAST + /// Source: https://stackoverflow.com/questions/429632/429812#429812 + inline int float2int(double d) + { + union Cast { + double d; + long l; + }; + volatile Cast c; + c.d = d + 6755399441055744; + return c.l; + } + + inline auto ifloor(float x) -> int + { + return float2int(x) - (x < float2int(x)); + } +#else + inline auto ifloor(float x) -> int + { + return static_cast(std::floor(x)); + } +#endif + + auto AlphaEncoder::encode_input(const InputData& input, char* buffer, size_t length) const -> size_t + { + if (std::holds_alternative(input)) { + const auto& info = std::get(input); + + const auto& keyFirmwareVersion = AlphaEncoder::INFO_FIRMWARE_VERSION_KEY; + const auto& keyDeviceType = AlphaEncoder::INFO_DEVICE_TYPE_KEY; + const auto& keyHand = AlphaEncoder::INFO_HAND_KEY; + + return snprintf( + buffer, + length, + "%s%u%s%u%s%u\n", + keyFirmwareVersion, + info.firmware_version, + keyDeviceType, + info.device_type, + keyHand, + info.hand + ); + } + + if (std::holds_alternative(input)) { + const auto peripheral = std::get(input); + auto written = 0; + + const auto& curls = peripheral.curl.fingers; + const auto& splays = peripheral.splay.fingers; +#ifdef OG_ENCODE_FAST + for (auto i = 0; i < curls.size(); i++) { + const auto& finger = curls[i]; + const auto& finger_curl = finger.curl_total; + + written += + snprintf(buffer + written, length - written, "%c%u", 'A' + i, ifloor(finger_curl * MAX_ANALOG_VALUE)); + } +#else + for (auto i = 0; i < curls.size(); i++) { + const auto& finger_curl = curls[i]; + const auto& finger_splay = splays[i]; + const auto finger_alpha_key = 'A' + i; + + written += snprintf( + buffer + written, + length - written, + "%c%u", + finger_alpha_key, + ifloor(finger_curl.curl_total * MAX_ANALOG_VALUE) + ); + + if (finger_splay > 0.0F) { + written += snprintf( + buffer + written, + length - written, + "(%cB)%u", + finger_alpha_key, + ifloor(finger_splay * MAX_ANALOG_VALUE) + ); + } + + const auto& joints = finger_curl.curl; + for (auto j = 1; j < joints.size(); j++) { + const auto& joint = joints[j]; + const auto joint_alpha_key = 'A' + j; + + if (joint == 0.0F) { + continue; + } + + written += snprintf( + buffer + written, + length - written, + "(%cA%c)%u", + finger_alpha_key, + joint_alpha_key, + ifloor(joint * MAX_ANALOG_VALUE) + ); + } + } +#endif + if (peripheral.joystick.x != 0.0F) { + written += + snprintf(buffer + written, length - written, "F%u", ifloor(peripheral.joystick.x * MAX_ANALOG_VALUE)); + } + if (peripheral.joystick.y != 0.0F) { + written += + snprintf(buffer + written, length - written, "G%u", ifloor(peripheral.joystick.y * MAX_ANALOG_VALUE)); + } + if (peripheral.joystick.press) { + written += snprintf(buffer + written, length - written, "H"); + } + + const auto& buttons = peripheral.buttons; + for (auto i = 0; i < buttons.size(); i++) { + const auto& button = buttons[i]; + if (button.press) { + const auto& buttonKey = AlphaEncoder::BUTTON_ALPHA_KEY[i]; + written += snprintf(buffer + written, length - written, "%c", buttonKey); + } + } + + const auto& analog_buttons = peripheral.analog_buttons; + for (auto i = 0; i < analog_buttons.size(); i++) { + const auto& button = analog_buttons[i]; + if (button.press) { + const auto& buttonKey = AlphaEncoder::ANALOG_BUTTON_ALPHA_KEY[i]; + written += snprintf(buffer + written, length - written, "%c", buttonKey); + } + } + + // Add newline and null terminator + written += snprintf(buffer + written, length - written, "\n"); + + return written; + } + + return 0; + } + + auto AlphaEncoder::decode_output(const char* data, size_t length) const -> OutputData + { + if (length == 0) { + return OutputInvalid{}; + } + + const auto commands = split_to_map(data, length); + if (commands.empty()) { + return OutputInvalid{}; + } + + // We assume all commands are for ffb, if there is any ffb command + const auto& thumb_curl = commands.find("A"); + const auto& index_curl = commands.find("B"); + const auto& middle_curl = commands.find("C"); + const auto& ring_curl = commands.find("D"); + const auto& pinky_curl = commands.find("E"); + + if (thumb_curl != commands.end() || index_curl != commands.end() || middle_curl != commands.end() || + ring_curl != commands.end() || pinky_curl != commands.end() + ) { + OutputForceFeedbackData ffb{}; + + if (thumb_curl != commands.end()) { + ffb.thumb = std::stof(thumb_curl->second) / MAX_ANALOG_VALUE; + } + + if (index_curl != commands.end()) { + ffb.index = std::stof(index_curl->second) / MAX_ANALOG_VALUE; + } + + if (middle_curl != commands.end()) { + ffb.middle = std::stof(middle_curl->second) / MAX_ANALOG_VALUE; + } + + if (ring_curl != commands.end()) { + ffb.ring = std::stof(ring_curl->second) / MAX_ANALOG_VALUE; + } + + if (pinky_curl != commands.end()) { + ffb.pinky = std::stof(pinky_curl->second) / MAX_ANALOG_VALUE; + } + + return ffb; + } + + // const auto& haptics_frequency = commands.find("F"); + // if (haptics_frequency != commands.end()) { + // OutputHaptics haptics{}; + // return haptics; + // } + + return OutputInvalid{}; + } + + /// Splits the input data into a map of commands and their respective values. + /// + /// Example: `A100(AB)200B300(BB)400C500\n` -> `{"A": "100", "(AB)": "200", "B": "300", "(BB)": "400", "C": "500"}` + auto AlphaEncoder::split_to_map(const char* data, size_t length) const -> std::map + { + std::map result{}; + + // Start at the beginning of the data + size_t command_start = 0; + for (size_t i = 0; i < length; i++) { + const auto& current_char = data[i]; + + // Start a new command if the character is non-numeric or an opening parenthesis + // and previous character is a numeric character + const bool is_command_start = ((isdigit(current_char)) == 0) || current_char == '('; + const bool prev_is_digit = isdigit(data[i - 1]) != 0; + if (is_command_start && i > 0 && prev_is_digit) { + split_command(data, command_start, i, result); + command_start = i; + } + } + + // Add the last command + split_command(data, command_start, length, result); + + return result; + } + + void AlphaEncoder::split_command( + const char* data, size_t start, size_t length, std::map& commands + ) const + { + const std::string current_command = std::string(data + start, length - start); + + if (current_command.empty()) { + return; + } + + const size_t split_index = current_command.find_first_of("0123456789"); + + // If there is no numeric value, the command is empty (likely a binary command) + if (split_index == std::string::npos) { + commands[current_command] = ""; + return; + } + + const std::string command = current_command.substr(0, split_index); + const std::string value = current_command.substr(split_index, current_command.length() - split_index); + + commands[command] = value; + } +} // namespace og \ No newline at end of file diff --git a/lib/opengloves/opengloves/opengloves.hpp b/lib/opengloves/opengloves/opengloves.hpp new file mode 100644 index 00000000..e6d708c7 --- /dev/null +++ b/lib/opengloves/opengloves/opengloves.hpp @@ -0,0 +1,462 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef ARDUINO +#include +#endif // ARDUINO + +#ifdef OG_USE_FROZEN +#include +#include +#endif + +#ifndef OG_BUFFER_SIZE +#define OG_BUFFER_SIZE 256 +#endif // OG_BUFFER_SIZE + +namespace og { + using HandIndex = std::uint8_t; + enum Hand : HandIndex { + Hand_Left, + Hand_Right, + }; + + using DeviceTypeIndex = std::uint8_t; + enum DeviceType : DeviceTypeIndex { + DeviceType_LucidGloves, + }; + + template + union InputFingerCurl { + /// Access the curl as an array. + std::array curl; // NOLINT(*-magic-numbers): I'm sure our finger aren't changing anytime soon + + union { + /// The total curl of the finger. + /// Only use it if you do not use per-joint tracking. + Tf curl_total; + + /// Access the individual curl joints. + struct { + Tf curl_joint0; + Tf curl_joint1; + Tf curl_joint2; + Tf curl_joint3; + }; + }; + + auto operator==(const InputFingerCurl& other) const -> bool + { + return this->curl_joint0 == other.curl_joint0 && this->curl_joint1 == other.curl_joint1 + && this->curl_joint2 == other.curl_joint2 && this->curl_joint3 == other.curl_joint3; + } + }; + using InputFingerCurlData = InputFingerCurl; + + template + union InputFinger { + std::array fingers; // NOLINT(*-magic-numbers): We aren't going to grow any new fingers soon tbh + struct { + Tp thumb; + Tp index; + Tp middle; + Tp ring; + + union { + Tp pinky; + Tp little; + }; + }; + + auto operator==(const InputFinger& other) const -> bool + { + return this->thumb == other.thumb && this->index == other.index && this->middle == other.middle + && this->ring == other.ring && this->pinky == other.pinky; + } + }; + using InputFingerData = InputFinger; + + template + struct InputJoystick { + Tf x, y; + Tb press; + }; + using InputJoystickData = InputJoystick; + + template + struct InputButton { + Tb press; + // bool touch; + }; + using InputButtonData = InputButton; + + template + struct InputAnalogButton : public InputButton { + Tf value; + }; + using InputAnalogButtonData = InputAnalogButton; + + /// Input data structure. + /// + /// I know, it is not the prettiest one, but we need this type of punning to efficiently encode/decode the data + template + struct InputPeripheral { + template< + typename U = Tf, + typename V = Tb, + std::enable_if_t && std::is_same_v, bool> = true> + InputPeripheral() + { + this->curl.fingers = { { + { 0.0F, 0.0F, 0.0F, 0.0F }, + { 0.0F, 0.0F, 0.0F, 0.0F }, + { 0.0F, 0.0F, 0.0F, 0.0F }, + { 0.0F, 0.0F, 0.0F, 0.0F }, + { 0.0F, 0.0F, 0.0F, 0.0F }, + } }; + this->splay.fingers = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F }; + this->joystick = { 0.0F, 0.0F, false }; + this->buttons = { { false, false, false, false, false } }; + this->analog_buttons = { { { false, 0.0F }, { false, 0.0F } } }; + } + + template< + typename U = Tf, + typename V = Tb, + std::enable_if_t && std::is_pointer_v, bool> = true> + InputPeripheral() + { + this->curl.fingers = { { + { nullptr, nullptr, nullptr, nullptr }, + { nullptr, nullptr, nullptr, nullptr }, + { nullptr, nullptr, nullptr, nullptr }, + { nullptr, nullptr, nullptr, nullptr }, + { nullptr, nullptr, nullptr, nullptr }, + } }; + this->splay.fingers = { nullptr, nullptr, nullptr, nullptr, nullptr }; + this->joystick = { nullptr, nullptr, nullptr }; + this->buttons = { { nullptr, nullptr, nullptr, nullptr, nullptr } }; + this->analog_buttons = { { { nullptr, nullptr }, { nullptr, nullptr } } }; + } + + InputFinger> curl; + InputFinger splay; + + InputJoystick joystick; + + union { + /// Buttons as array. + /// MUST be the same length as the struct below + std::array, 5> buttons; // NOLINT(*-magic-numbers) We keep it here for clarity + + struct { + InputButton button_a; + InputButton button_b; + InputButton button_menu; + InputButton button_calibrate; + InputButton pinch; + }; + }; + + union { + std::array, 2> analog_buttons; + struct { + InputAnalogButton trigger; + InputAnalogButton grab; + }; + }; + }; + using InputPeripheralData = InputPeripheral; + + struct InputInfoData { + Hand hand; + DeviceType device_type; + + unsigned int firmware_version; + }; + + using InputData = std::variant; + + template + using OutputForceFeedback = InputFinger; + using OutputForceFeedbackData = OutputForceFeedback; + + template + struct OutputHaptics { + Tf frequency; + Tf duration; + Tf amplitude; + + auto operator==(const OutputHaptics& other) const -> bool + { + return frequency == other.frequency && duration == other.duration && amplitude == other.amplitude; + } + }; + using OutputHapticsData = OutputHaptics; + + class OutputInvalid { + auto operator==(const OutputInvalid& /*unused*/) const -> bool { return true; } + }; + using OutputData = std::variant; + + class IEncoder { + public: + [[nodiscard]] virtual auto encode_input(const InputData& input, char* buffer, size_t length) const + -> size_t = 0; + + [[nodiscard]] auto encode_input(const InputData& input) const -> std::string + { + std::string buffer; + buffer.resize(OG_BUFFER_SIZE); + + const auto length = this->encode_input(input, buffer.data(), buffer.length()); + buffer.resize(length); + + return buffer; + } + + [[nodiscard]] virtual auto decode_output(const char* data, size_t length) const -> OutputData = 0; + + [[nodiscard]] inline auto decode_output(const std::vector& data) const -> OutputData + { + return this->decode_output(data.data(), data.size()); + } + + [[nodiscard]] inline auto decode_output(const std::string& data) const -> OutputData + { + return this->decode_output(data.data(), data.length()); + } + +#ifdef ARDUINO + [[nodiscard]] inline auto decode_output(const String& data) const -> OutputData + { + return this->decode_output(data.c_str(), data.length()); + } +#endif // ARDUINO + }; + + class AlphaEncoder : public IEncoder { + public: + using CommandIndex = std::uint8_t; + enum Command : CommandIndex { + FingerThumb_CurlTotal, + FingerThumb_Splay, + FingerThumb_CurlJoint0, + FingerThumb_CurlJoint1, + FingerThumb_CurlJoint2, + FingerThumb_CurlJoint3 [[maybe_unused]], + + FingerIndex_CurlTotal, + FingerIndex_Splay, + FingerIndex_CurlJoint0, + FingerIndex_CurlJoint1, + FingerIndex_CurlJoint2, + FingerIndex_CurlJoint3, + + FingerMiddle_CurlTotal, + FingerMiddle_Splay, + FingerMiddle_CurlJoint0, + FingerMiddle_CurlJoint1, + FingerMiddle_CurlJoint2, + FingerMiddle_CurlJoint3, + + FingerRing_CurlTotal, + FingerRing_Splay, + FingerRing_CurlJoint0, + FingerRing_CurlJoint1, + FingerRing_CurlJoint2, + FingerRing_CurlJoint3, + + FingerPinky_CurlTotal, + FingerPinky_Splay, + FingerPinky_CurlJoint0, + FingerPinky_CurlJoint1, + FingerPinky_CurlJoint2, + FingerPinky_CurlJoint3, + + MainJoystick_X, + MainJoystick_Y, + MainJoystick_Click, + + ButtonA_Click, + ButtonB_Click, + ButtonMenu_Click, + ButtonCalibrate_Click, + + GestureTrigger_Value, + GestureTrigger_Click, + + GestureGrab_Click, + + GesturePinch_Click, + + Info, + Info_StartStreaming, + Info_StopStreaming, + Info_FirmwareVersion, + Info_DeviceType, + Info_Hand, + + Haptics_Frequency, + Haptics_Duration, + Haptics_Amplitude, + }; + + inline static constexpr const char* INFO_FIRMWARE_VERSION_KEY = "(ZV)"; + inline static constexpr const char* INFO_DEVICE_TYPE_KEY = "(ZG)"; + inline static constexpr const char* INFO_HAND_KEY = "(ZH)"; + + inline static constexpr const uint16_t MAX_ANALOG_VALUE = 4095; + +#ifdef OG_USE_FROZEN + inline static constexpr const auto ALPHA_KEYS_TO_COMMAND = frozen::make_map +#else + inline static const auto ALPHA_KEYS_TO_COMMAND = std::map +#endif + + ({ + { "A", Command::FingerThumb_CurlTotal }, // Thumb finger force feedback + { "B", Command::FingerIndex_CurlTotal }, // Index finger force feedback + { "C", Command::FingerMiddle_CurlTotal }, // Middle finger force feedback + { "D", Command::FingerRing_CurlTotal }, // Ring finger force feedback + { "E", Command::FingerPinky_CurlTotal }, // Pinky finger force feedback + + { "F", Command::Haptics_Frequency }, // + { "G", Command::Haptics_Duration }, // + { "H", Command::Haptics_Amplitude }, // + + { "Z", Command::Info }, + }); + +#ifdef OG_USE_FROZEN + inline static constexpr const auto COMMAND_TO_ALPHA_KEY = frozen::make_map +#else + inline static const auto COMMAND_TO_ALPHA_KEY = std::map +#endif + ({ + { Command::FingerThumb_CurlTotal, "A" }, // Whole thumb curl + { Command::FingerThumb_Splay, "(AB)" }, // Whole thumb splay + { Command::FingerThumb_CurlJoint0, "(AAA)" }, // + { Command::FingerThumb_CurlJoint1, "(AAB)" }, // + { Command::FingerThumb_CurlJoint2, "(AAC)" }, // + // { Command::FingerThumb_CurlJoint3, "(AAD)" }, // not used + + { Command::FingerIndex_CurlTotal, "B" }, // + { Command::FingerIndex_Splay, "(BB)" }, // + { Command::FingerIndex_CurlJoint0, "(BAA)" }, // + { Command::FingerIndex_CurlJoint1, "(BAB)" }, // + { Command::FingerIndex_CurlJoint2, "(BAC)" }, // + { Command::FingerIndex_CurlJoint3, "(BAD)" }, // + + { Command::FingerMiddle_CurlTotal, "C" }, // + { Command::FingerMiddle_Splay, "(CB)" }, // + { Command::FingerMiddle_CurlJoint0, "(CAA)" }, // + { Command::FingerMiddle_CurlJoint1, "(CAB)" }, // + { Command::FingerMiddle_CurlJoint2, "(CAC)" }, // + { Command::FingerMiddle_CurlJoint3, "(CAD)" }, // + + { Command::FingerRing_CurlTotal, "D" }, // + { Command::FingerRing_Splay, "(DB)" }, // + { Command::FingerRing_CurlJoint0, "(DAA)" }, // + { Command::FingerRing_CurlJoint1, "(DAB)" }, // + { Command::FingerRing_CurlJoint2, "(DAC)" }, // + { Command::FingerRing_CurlJoint3, "(DAD)" }, // + + { Command::FingerPinky_CurlTotal, "E" }, // + { Command::FingerPinky_Splay, "(EB)" }, // + { Command::FingerPinky_CurlJoint0, "(EAA)" }, // + { Command::FingerPinky_CurlJoint1, "(EAB)" }, // + { Command::FingerPinky_CurlJoint2, "(EAC)" }, // + { Command::FingerPinky_CurlJoint3, "(EAD)" }, // + + { Command::MainJoystick_X, "F" }, // + { Command::MainJoystick_Y, "G" }, // + { Command::MainJoystick_Click, "H" }, // + + { Command::GestureTrigger_Click, "I" }, // Binary trigger button/gesture + + { Command::ButtonA_Click, "J" }, // + { Command::ButtonB_Click, "K" }, // + + { Command::GestureGrab_Click, "L" }, // + { Command::GesturePinch_Click, "M" }, // + + { Command::ButtonMenu_Click, "N" }, // + { Command::ButtonCalibrate_Click, "O" }, // + + { Command::GestureTrigger_Value, "P" }, // Analog trigger button/gesture + + { Command::Info, "Z" }, +#ifdef OG_USE_FROZEN + { Command::Info_FirmwareVersion, frozen::string(INFO_FIRMWARE_VERSION_KEY) }, + { Command::Info_DeviceType, frozen::string(INFO_DEVICE_TYPE_KEY) }, + { Command::Info_Hand, frozen::string(INFO_HAND_KEY) }, +#else + { Command::Info_FirmwareVersion, INFO_FIRMWARE_VERSION_KEY }, + { Command::Info_DeviceType, INFO_DEVICE_TYPE_KEY }, + { Command::Info_Hand, INFO_HAND_KEY }, +#endif + }); + + /// Alpha keys for fingers. + /// MUST be in the same order as the `InputFingerData` struct. + inline static constexpr const std::array FINGER_ALPHA_KEY = { { + 'A', // Thumb + 'B', // Index + 'C', // Middle + 'D', // Ring + 'E', // Pinky + } }; + + /// Alpha keys for finger curl joints. Top level is the finger, second level is the joint. + /// The top level MUST be in the same order as the `InputFingerData` struct. + /// Second level array MUST be in the same order as the `InputFingerCurlData` struct. +#ifdef OG_USE_FROZEN + inline static constexpr const std::array, 5> FINGER_CURL_JOINT_ALPHA_KEY = +#else + inline static const std::array, 5> FINGER_CURL_JOINT_ALPHA_KEY = +#endif + { { + { "A", "(AAB)", "(AAC)", "(AAD)" }, // Thumb (total, joint1, joint2, joint3) + { "B", "(BAB)", "(BAC)", "(BAD)" }, // Index (total, joint1, joint2, joint3) + { "C", "(CAB)", "(CAC)", "(CAD)" }, // Middle (total, joint1, joint2, joint3) + { "D", "(DAB)", "(DAC)", "(DAD)" }, // Ring (total, joint1, joint2, joint3) + { "E", "(EAB)", "(EAC)", "(EAD)" }, // Pinky (total, joint1, joint2, joint3) + } }; + + /// Alpha keys for buttons. + /// MUST be in the same order as the `InputPeripheralData` struct. + inline static constexpr const std::array BUTTON_ALPHA_KEY = { { + 'J', // Button A + 'K', // Button B + 'N', // Button Menu + 'O', // Button Calibrate + 'M', // Button Pinch + } }; + + /// Alpha keys for analog buttons. + /// MUST be in the same order as the `InputPeripheralData` struct. + inline static constexpr const std::array ANALOG_BUTTON_ALPHA_KEY = { { + 'I', // Trigger + 'L', // Grab + } }; + + [[nodiscard]] auto encode_input(const InputData& input, char* buffer, size_t length) const -> size_t override; + + [[nodiscard]] auto decode_output(const char* data, size_t length) const -> OutputData override; + + protected: + [[nodiscard]] auto split_to_map(const char* data, size_t length) const -> std::map; + void split_command(const char* data, size_t start, size_t length, std::map& commands) + const; + }; +} // namespace og diff --git a/lib/opengloves/senseshift/opengloves/autoconfig.hpp b/lib/opengloves/senseshift/opengloves/autoconfig.hpp index 191a8c06..de6eeaf2 100644 --- a/lib/opengloves/senseshift/opengloves/autoconfig.hpp +++ b/lib/opengloves/senseshift/opengloves/autoconfig.hpp @@ -1,6 +1,15 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifdef ARDUINO #include @@ -8,8 +17,8 @@ #pragma region Communication -#ifndef OPENGLOVES_COMMUNCATION -#define OPENGLOVES_COMMUNCATION OPENGLOVES_COMM_SERIAL +#ifndef OPENGLOVES_COMMUNICATION +#define OPENGLOVES_COMMUNICATION OPENGLOVES_COMM_SERIAL #endif #ifndef SERIAL_PORT @@ -29,10 +38,10 @@ #pragma region Calibration #ifndef CALIBRATION_CURL -#define CALIBRATION_CURL ::SenseShift::Calibration::MinMaxCalibrator +#define CALIBRATION_CURL new ::SenseShift::Input::Calibration::MinMaxCalibrator() #endif #ifndef CALIBRATION_SPLAY -#define CALIBRATION_SPLAY ::SenseShift::Calibration::MinMaxCalibrator +#define CALIBRATION_SPLAY new ::SenseShift::Input::Calibration::CenterPointDeviationCalibrator(0.66F, 0.005F) #endif #ifndef CALIBRATION_DURATION @@ -43,61 +52,93 @@ #pragma region Fingers -#define FINGER_THUMB_ENABLED (defined(PIN_FINGER_THUMB) && (PIN_FINGER_THUMB != -1)) -#define FINGER_INDEX_ENABLED (defined(PIN_FINGER_INDEX) && (PIN_FINGER_INDEX != -1)) -#define FINGER_MIDDLE_ENABLED (defined(PIN_FINGER_MIDDLE) && (PIN_FINGER_MIDDLE != -1)) -#define FINGER_RING_ENABLED (defined(PIN_FINGER_RING) && (PIN_FINGER_RING != -1)) -#define FINGER_PINKY_ENABLED (defined(PIN_FINGER_PINKY) && (PIN_FINGER_PINKY != -1)) -#define FINGER_CLASS(type, curl_pin, curl_invert, curl_calib) \ - FingerSensor( \ - new ::SenseShift::Input::CalibratedSimpleSensor( \ - new ::SenseShift::Input::StaticMedianSensor( \ - new ::SenseShift::Arduino::Input::AnalogSensor(curl_pin) \ - ), \ - new curl_calib() \ - ), \ - type \ - ) -#define FINGER_THUMB_SPLAY (FINGER_THUMB_ENABLED && defined(PIN_FINGER_THUMB_SPLAY) && (PIN_FINGER_THUMB_SPLAY != -1)) -#define FINGER_INDEX_SPLAY (FINGER_INDEX_ENABLED && defined(PIN_FINGER_INDEX_SPLAY) && (PIN_FINGER_INDEX_SPLAY != -1)) -#define FINGER_MIDDLE_SPLAY \ - (FINGER_MIDDLE_ENABLED && defined(PIN_FINGER_MIDDLE_SPLAY) && (PIN_FINGER_MIDDLE_SPLAY != -1)) -#define FINGER_RING_SPLAY (FINGER_RING_ENABLED && defined(PIN_FINGER_RING_SPLAY) && (PIN_FINGER_RING_SPLAY != -1)) -#define FINGER_PINKY_SPLAY (FINGER_PINKY_ENABLED && defined(PIN_FINGER_PINKY_SPLAY) && (PIN_FINGER_PINKY_SPLAY != -1)) -#define FINGER_SPLAY_CLASS(type, curl_pin, curl_invert, curl_calib, splay_pin, splay_invert, splay_calib) \ - FingerSensor( \ - new ::SenseShift::Input::CalibratedSimpleSensor( \ - new ::SenseShift::Input::StaticMedianSensor( \ - new ::SenseShift::Arduino::Input::AnalogSensor(curl_pin) \ - ), \ - new curl_calib() \ - ), \ - new ::SenseShift::Input::CalibratedSimpleSensor( \ - new ::SenseShift::Input::StaticMedianSensor( \ - new ::SenseShift::Arduino::Input::AnalogSensor(splay_pin) \ - ), \ - new splay_calib() \ - ), \ - type \ - ) +#ifdef PIN_FINGER_THUMB +#define FINGER_THUMB_ENABLED (PIN_FINGER_THUMB != -1) +#else +#define FINGER_THUMB_ENABLED false +#endif + +#ifdef PIN_FINGER_INDEX +#define FINGER_INDEX_ENABLED (PIN_FINGER_INDEX != -1) +#else +#define FINGER_INDEX_ENABLED false +#endif + +#ifdef PIN_FINGER_MIDDLE +#define FINGER_MIDDLE_ENABLED (PIN_FINGER_MIDDLE != -1) +#else +#define FINGER_MIDDLE_ENABLED false +#endif + +#ifdef PIN_FINGER_RING +#define FINGER_RING_ENABLED (PIN_FINGER_RING != -1) +#else +#define FINGER_RING_ENABLED false +#endif + +#ifdef PIN_FINGER_PINKY +#define FINGER_PINKY_ENABLED (PIN_FINGER_PINKY != -1) +#else +#define FINGER_PINKY_ENABLED false +#endif + +#define DEFINE_FINGER(NAME, CURL_PIN, CURL_INVERT, CURL_CALIB) \ + auto* NAME##_sensor = new ::SenseShift::Input::SimpleSensorDecorator( \ + new ::SenseShift::Arduino::Input::AnalogSimpleSensor(CURL_PIN) \ + ); \ + NAME##_sensor->addFilters({ new ::SenseShift::Input::Filter::ExponentialMovingAverageFilter(0.8F) }); \ + NAME##_sensor->setCalibrator((CURL_CALIB)); + +#ifdef PIN_FINGER_THUMB_SPLAY +#define FINGER_THUMB_SPLAY (FINGER_THUMB_ENABLED && (PIN_FINGER_THUMB_SPLAY != -1)) +#else +#define FINGER_THUMB_SPLAY false +#endif + +#ifdef PIN_FINGER_INDEX_SPLAY +#define FINGER_INDEX_SPLAY (FINGER_INDEX_ENABLED && (PIN_FINGER_INDEX_SPLAY != -1)) +#else +#define FINGER_INDEX_SPLAY false +#endif + +#ifdef PIN_FINGER_MIDDLE_SPLAY +#define FINGER_MIDDLE_SPLAY (FINGER_MIDDLE_ENABLED && (PIN_FINGER_MIDDLE_SPLAY != -1)) +#else +#define FINGER_MIDDLE_SPLAY false +#endif + +#ifdef PIN_FINGER_RING_SPLAY +#define FINGER_RING_SPLAY (FINGER_RING_ENABLED && (PIN_FINGER_RING_SPLAY != -1)) +#else +#define FINGER_RING_SPLAY false +#endif + +#ifdef PIN_FINGER_PINKY_SPLAY +#define FINGER_PINKY_SPLAY (FINGER_PINKY_ENABLED && (PIN_FINGER_PINKY_SPLAY != -1)) +#else +#define FINGER_PINKY_SPLAY false +#endif + +#define DEFINE_FINGER_SPLAY(NAME, CURL_PIN, CURL_INVERT, CURL_CALIB, SPLAY_PIN, SPLAY_INVERT, SPLAY_CALIB) \ + DEFINE_FINGER(NAME##_curl, CURL_PIN, CURL_INVERT, CURL_CALIB); \ + DEFINE_FINGER(NAME##_splay, SPLAY_PIN, SPLAY_INVERT, SPLAY_CALIB); #pragma endregion #pragma region Joysticks -#define JOYSTICK_ENABLED \ - (defined(PIN_JOYSTICK_X) && defined(PIN_JOYSTICK_Y) && (PIN_JOYSTICK_X != -1) && (PIN_JOYSTICK_Y != -1)) +#if defined(PIN_JOYSTICK_X) && defined(PIN_JOYSTICK_Y) +#define JOYSTICK_ENABLED (PIN_JOYSTICK_X != -1) && (PIN_JOYSTICK_Y != -1) +#else +#define JOYSTICK_ENABLED false +#endif -#define JOYSTICK_CLASS(type, pin, invert, deadzone) \ - StringEncodedMemoizedSensor( \ - new ::SenseShift::Input::JoystickAxisSensor( \ - new ::SenseShift::Input::StaticMedianSensor( \ - new ::SenseShift::Arduino::Input::AnalogSensor(pin) \ - ), \ - deadzone \ - ), \ - type \ - ) +#define DEFINE_JOYSTICK_AXIS(NAME, PIN, INVERT, DEADZONE) \ + auto NAME##_sensor = \ + new ::SenseShift::Input::SimpleSensorDecorator(new ::SenseShift::Arduino::Input::AnalogSimpleSensor(PIN) \ + ); \ + NAME##_sensor->addFilters({ new ::SenseShift::Input::Filter::ExponentialMovingAverageFilter(0.8F), \ + new ::SenseShift::Input::Filter::CenterDeadzoneFilter(DEADZONE) }); #pragma endregion @@ -112,27 +153,25 @@ #define BUTTON_GRAB_ENABLED (!GESTURE_GRAB_ENABLED && defined(PIN_BUTTON_GRAB) && (PIN_BUTTON_GRAB != -1)) #define BUTTON_PINCH_ENABLED (!GESTURE_PINCH_ENABLED && defined(PIN_BUTTON_PINCH) && (PIN_BUTTON_PINCH != -1)) -#define BUTTON_CLASS(_type, _pin, _invert) \ - StringEncodedMemoizedSensor(new ::SenseShift::Arduino::Input::DigitalSensor<_invert>(_pin), _type) +#define BUTTON_CLASS(PIN, INVERT) \ + ::SenseShift::Input::SimpleSensorDecorator(new ::SenseShift::Arduino::Input::DigitalSimpleSensor(PIN)); #pragma endregion #pragma region Gestures #ifndef GESTURE_TRIGGER_THRESHOLD -#define GESTURE_TRIGGER_THRESHOLD (ANALOG_MAX / 2) +#define GESTURE_TRIGGER_THRESHOLD (0.5F) #endif #ifndef GESTURE_GRAB_THRESHOLD -#define GESTURE_GRAB_THRESHOLD (ANALOG_MAX / 2) +#define GESTURE_GRAB_THRESHOLD (0.5F) #endif #ifndef GESTURE_PINCH_THRESHOLD -#define GESTURE_PINCH_THRESHOLD (ANALOG_MAX / 2) +#define GESTURE_PINCH_THRESHOLD (0.5F) #endif -#define GESTURE_CLASS(_type, _sensor) StringEncodedMemoizedSensor(_sensor, _type) - #pragma endregion #define FFB_THUMB_ENABLED (defined(PIN_FFB_THUMB) && (PIN_FFB_THUMB != -1)) @@ -144,23 +183,199 @@ #define FFB_ENABLED \ (FFB_THUMB_ENABLED || FFB_INDEX_ENABLED || FFB_MIDDLE_ENABLED || FFB_RING_ENABLED || FFB_PINKY_ENABLED) +#ifndef CALIBRATION_ALWAYS_CALIBRATE +#define CALIBRATION_ALWAYS_CALIBRATE false +#endif + #ifndef UPDATE_RATE #define UPDATE_RATE 90 #endif namespace SenseShift::OpenGloves::AutoConfig { + [[nodiscard]] auto createInput() -> InputSensors + { + InputSensors input_sensors; + +#if FINGER_THUMB_SPLAY + DEFINE_FINGER_SPLAY( + thumb, + PIN_FINGER_THUMB, + FINGER_THUMB_INVERT, + CALIBRATION_CURL, + PIN_FINGER_THUMB_SPLAY, + FINGER_THUMB_SPLAY_INVERT, + CALIBRATION_SPLAY + ); +#elif FINGER_THUMB_ENABLED + DEFINE_FINGER(thumb_curl, PIN_FINGER_THUMB, FINGER_THUMB_INVERT, CALIBRATION_CURL); + input_sensors.curl.thumb.curl_total = thumb_curl_sensor; +#endif + +#if FINGER_INDEX_SPLAY + DEFINE_FINGER_SPLAY( + index, + PIN_FINGER_INDEX, + FINGER_INDEX_INVERT, + CALIBRATION_CURL, + PIN_FINGER_INDEX_SPLAY, + FINGER_INDEX_SPLAY_INVERT, + CALIBRATION_SPLAY + ); +#elif FINGER_INDEX_ENABLED + DEFINE_FINGER(index_curl, PIN_FINGER_INDEX, FINGER_INDEX_INVERT, CALIBRATION_CURL); + input_sensors.curl.index.curl_total = index_curl_sensor; +#endif + +#if FINGER_MIDDLE_SPLAY + DEFINE_FINGER_SPLAY( + middle, + PIN_FINGER_MIDDLE, + FINGER_MIDDLE_INVERT, + CALIBRATION_CURL, + PIN_FINGER_MIDDLE_SPLAY, + FINGER_MIDDLE_SPLAY_INVERT, + CALIBRATION_SPLAY + ); +#elif FINGER_MIDDLE_ENABLED + DEFINE_FINGER(middle_curl, PIN_FINGER_MIDDLE, FINGER_MIDDLE_INVERT, CALIBRATION_CURL); + input_sensors.curl.middle.curl_total = middle_curl_sensor; +#endif + +#if FINGER_RING_SPLAY + DEFINE_FINGER_SPLAY( + ring, + PIN_FINGER_RING, + FINGER_RING_INVERT, + CALIBRATION_CURL, + PIN_FINGER_RING_SPLAY, + FINGER_RING_SPLAY_INVERT, + CALIBRATION_SPLAY + ); +#elif FINGER_RING_ENABLED + DEFINE_FINGER(ring_curl, PIN_FINGER_RING, FINGER_RING_INVERT, CALIBRATION_CURL); + input_sensors.curl.ring.curl_total = ring_curl_sensor; +#endif + +#if FINGER_PINKY_SPLAY + DEFINE_FINGER_SPLAY( + pinky, + PIN_FINGER_PINKY, + FINGER_PINKY_INVERT, + CALIBRATION_CURL, + PIN_FINGER_PINKY_SPLAY, + FINGER_PINKY_SPLAY_INVERT, + CALIBRATION_SPLAY + ); +#elif FINGER_PINKY_ENABLED + DEFINE_FINGER(pinky_curl, PIN_FINGER_PINKY, FINGER_PINKY_INVERT, CALIBRATION_CURL); + input_sensors.curl.pinky.curl_total = pinky_curl_sensor; +#endif + +#if JOYSTICK_ENABLED + DEFINE_JOYSTICK_AXIS(joystick_x, PIN_JOYSTICK_X, JOYSTICK_X_INVERT, JOYSTICK_DEADZONE); + DEFINE_JOYSTICK_AXIS(joystick_y, PIN_JOYSTICK_Y, JOYSTICK_Y_INVERT, JOYSTICK_DEADZONE); + + input_sensors.joystick.x = joystick_x_sensor; + input_sensors.joystick.y = joystick_y_sensor; +#endif + +#if BUTTON_A_ENABLED + auto* button_a = new BUTTON_CLASS(PIN_BUTTON_A, BUTTON_A_INVERT); + input_sensors.button_a.press = button_a; +#endif +#if BUTTON_B_ENABLED + auto* button_b = new BUTTON_CLASS(PIN_BUTTON_B, BUTTON_B_INVERT); + input_sensors.button_b.press = button_b; +#endif +#if BUTTON_JOYSTICK_ENABLED + auto* button_joystick = new BUTTON_CLASS(PIN_BUTTON_JOYSTICK, BUTTON_JOYSTICK_INVERT); + input_sensors.joystick.press = button_joystick; +#endif +#if BUTTON_MENU_ENABLED + auto* button_menu = new BUTTON_CLASS(PIN_BUTTON_MENU, BUTTON_MENU_INVERT); +#endif +#if BUTTON_CALIBRATE_ENABLED + auto* button_calibrate = new BUTTON_CLASS(PIN_BUTTON_CALIBRATE, BUTTON_CALIBRATE_INVERT); + input_sensors.button_calibrate.press = button_calibrate; +#endif + +#if GESTURE_TRIGGER_ENABLED + auto* trigger = new Body::Hands::Input::TriggerGesture(index_curl_sensor, GESTURE_TRIGGER_THRESHOLD); + input_sensors.trigger.press = trigger; +#elif BUTTON_TRIGGER_ENABLED + auto trigger = new BUTTON_CLASS(PIN_BUTTON_TRIGGER, BUTTON_TRIGGER_INVERT); +#endif + +#if GESTURE_GRAB_ENABLED + auto* grab = new Body::Hands::Input::GrabGesture( + Body::Hands::Input::GrabGesture::Fingers{ .index = index_curl_sensor, + .middle = middle_curl_sensor, + .ring = ring_curl_sensor, + .pinky = pinky_curl_sensor }, + GESTURE_GRAB_THRESHOLD + ); + input_sensors.grab.press = grab; +#elif BUTTON_GRAB_ENABLED + auto* grab = new BUTTON_CLASS(PIN_BUTTON_GRAB, BUTTON_GRAB_INVERT); +#endif + +#if GESTURE_PINCH_ENABLED + auto* pinch = new Body::Hands::Input::PinchGesture( + Body::Hands::Input::PinchGesture::Fingers{ .thumb = thumb_curl_sensor, .index = index_curl_sensor }, + GESTURE_PINCH_THRESHOLD + ); + input_sensors.pinch.press = pinch; +#elif BUTTON_PINCH_ENABLED + auto* pinch = new BUTTON_CLASS(PIN_BUTTON_PINCH, BUTTON_PINCH_INVERT); +#endif + + return input_sensors; + } + + [[nodiscard]] auto createFfbOutputs() -> OutputWriters + { + OutputWriters output_writers; + +#if FFB_THUMB_ENABLED + auto* thumb_ffb_output = new ::SenseShift::Arduino::Output::ServoOutput(PIN_FFB_THUMB); + output_writers.ffb.thumb = thumb_ffb_output; +#endif + +#if FFB_INDEX_ENABLED + auto* index_ffb_output = new ::SenseShift::Arduino::Output::ServoOutput(PIN_FFB_INDEX); + output_writers.ffb.index = index_ffb_output; +#endif + +#if FFB_MIDDLE_ENABLED + auto* middle_ffb_output = new ::SenseShift::Arduino::Output::ServoOutput(PIN_FFB_MIDDLE); + output_writers.ffb.middle = middle_ffb_output; +#endif + +#if FFB_RING_ENABLED + auto* ring_ffb_output = new ::SenseShift::Arduino::Output::ServoOutput(PIN_FFB_RING); + output_writers.ffb.ring = ring_ffb_output; +#endif + +#if FFB_PINKY_ENABLED + auto* pinky_ffb_output = new ::SenseShift::Arduino::Output::ServoOutput(PIN_FFB_PINKY); + output_writers.ffb.pinky = pinky_ffb_output; +#endif + + return output_writers; + } + /** * Setup the transport for the OpenGloves interface. */ - [[nodiscard]] ITransport* setupTransport(void) + [[nodiscard]] auto createTransport() -> ITransport* { -#if OPENGLOVES_COMMUNCATION == OPENGLOVES_COMM_SERIAL // Serial +#if OPENGLOVES_COMMUNICATION == OPENGLOVES_COMM_SERIAL // Serial auto* pSerial = &SERIAL_PORT; pSerial->begin(SERIAL_BAUDRATE); return new StreamTransport(pSerial); -#elif (OPENGLOVES_COMMUNCATION == OPENGLOVES_COMM_BTSERIAL) \ - || (OPENGLOVES_COMMUNCATION == OPENGLOVES_COMM_BLESERIAL) // Bluetooth Serial +#elif (OPENGLOVES_COMMUNICATION == OPENGLOVES_COMM_BTSERIAL) \ + || (OPENGLOVES_COMMUNICATION == OPENGLOVES_COMM_BLESERIAL) // Bluetooth Serial std::string name; #ifdef BTSERIAL_NAME name = BTSERIAL_NAME; @@ -172,11 +387,11 @@ namespace SenseShift::OpenGloves::AutoConfig { log_i("Generated Bluetooth name: %s", name.c_str()); #endif -#if OPENGLOVES_COMMUNCATION == OPENGLOVES_COMM_BTSERIAL // Bluetooth Classic +#if OPENGLOVES_COMMUNICATION == OPENGLOVES_COMM_BTSERIAL // Bluetooth Classic BluetoothSerial* pBtSerial = new BluetoothSerial(); pBtSerial->begin(name.c_str()); return new BluetoothSerialTransport(*pBtSerial); -#elif OPENGLOVES_COMMUNCATION == OPENGLOVES_COMM_BLESERIAL // Bluetooth Low Energy +#elif OPENGLOVES_COMMUNICATION == OPENGLOVES_COMM_BLESERIAL // Bluetooth Low Energy BLESerial* pBleSerial = new BLESerial(); pBleSerial->begin(name.c_str()); return new BLESerialTransport(*pBleSerial); diff --git a/lib/opengloves/senseshift/opengloves/constants.hpp b/lib/opengloves/senseshift/opengloves/constants.hpp new file mode 100644 index 00000000..b9de70c9 --- /dev/null +++ b/lib/opengloves/senseshift/opengloves/constants.hpp @@ -0,0 +1,7 @@ +#pragma once + +#define OPENGLOVES_FINGERS_TASK_PRIORITY 1 + +#define OPENGLOVES_COMM_SERIAL 1 +#define OPENGLOVES_COMM_BTSERIAL 2 +#define OPENGLOVES_COMM_BLESERIAL 3 diff --git a/lib/opengloves/senseshift/opengloves/encoding/alpha.cpp b/lib/opengloves/senseshift/opengloves/encoding/alpha.cpp deleted file mode 100644 index c9dc738d..00000000 --- a/lib/opengloves/senseshift/opengloves/encoding/alpha.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "senseshift/opengloves/encoding/alpha.hpp" - -#include - -namespace SenseShift::OpenGloves { - size_t AlphaEncodingService::serialize( - const std::vector<::OpenGloves::IStringEncodedMemoizedSensor*>& sensors, char* buffer - ) const - { - buffer[0] = '\0'; - size_t offset = 0; - - for (size_t i = 0; i < sensors.size(); i++) { - auto* sensor = sensors[i]; - offset += sensor->encodeString(buffer + offset); - } - - buffer[offset++] = '\n'; - buffer[offset] = '\0'; - - return offset; - } - - bool AlphaEncodingService::deserialize( - const char* buffer, const size_t length, std::map<::OpenGloves::Command, uint16_t>& commands - ) const - { - std::string input_string(buffer, length); - - size_t start = 0; // Start of the current command - for (size_t i = 0; i < input_string.length(); i++) { - const char ch = input_string[i]; - - // Start a new command if the character is non-numeric or an opening parenthesis - // and previous character is a numeric character - if ((!(isdigit(ch)) || ch == '(') && i > 0 && isdigit(input_string[i - 1])) { - AlphaEncodingService::splitCommand(input_string, start, i, commands); - start = i; - } - } - - AlphaEncodingService::splitCommand(input_string, start, input_string.size(), commands); - - return true; - } - - void AlphaEncodingService::splitCommand( - const std::string& input_string, size_t start, size_t end, std::map& commands - ) - { - std::string current_command = input_string.substr(start, end - start); - - if (current_command.empty()) { - return; - } - - // Split the command into prefix and number - size_t split_index = current_command.find_last_not_of(valueSymbols.data()) + 1; - - if (split_index >= current_command.size()) { - log_w("Invalid command: %s", current_command.c_str()); - return; - } - - std::string prefix = current_command.substr(0, split_index); - int number = std::stoi(current_command.substr(split_index)); - - // Check if the command prefix is in commandMap - auto it = commandMap.find(prefix); - if (it == commandMap.end()) { - log_w("Unknown command prefix: %s", prefix.c_str()); - return; - } - - // Look up the Command enum value for the prefix in the commandMap - Command command = it->second; - commands[command] = number; - } -} // namespace SenseShift::OpenGloves diff --git a/lib/opengloves/senseshift/opengloves/encoding/alpha.hpp b/lib/opengloves/senseshift/opengloves/encoding/alpha.hpp deleted file mode 100644 index 49d223cf..00000000 --- a/lib/opengloves/senseshift/opengloves/encoding/alpha.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#define SENSESHIFT_OPENGLOVES_ALPHA_ENCODING_BUFFER_SIZE 256 - -namespace SenseShift::OpenGloves { - class AlphaEncodingService : public IEncoding { - using Command = ::OpenGloves::Command; - - public: - inline static constexpr frozen::string valueSymbols = "0123456789"; - inline static const auto commandMap = frozen::make_map({ - // clang-format off - { "A", Command::ThumbCurl }, - { "(AB)", Command::ThumbSplay }, - { "B", Command::IndexCurl }, - { "(BB)", Command::IndexSplay }, - { "C", Command::MiddleCurl }, - { "(CB)", Command::MiddleSplay }, - { "D", Command::RingCurl }, - { "(DB)", Command::RingSplay }, - { "E", Command::PinkyCurl }, - { "(EB)", Command::PinkySplay }, - // clang-format on - }); - - AlphaEncodingService(){}; - - virtual size_t serialize(const std::vector<::OpenGloves::IStringEncodedMemoizedSensor*>& sensors, char* buffer) - const override; - - virtual bool deserialize( - const char* buffer, const size_t length, std::map<::OpenGloves::Command, uint16_t>& commands - ) const override; - - private: - static void splitCommand( - const std::string& input_string, size_t start, size_t end, std::map& commands - ); - }; -} // namespace SenseShift::OpenGloves diff --git a/lib/opengloves/senseshift/opengloves/interface.hpp b/lib/opengloves/senseshift/opengloves/interface.hpp deleted file mode 100644 index 5e94b1e9..00000000 --- a/lib/opengloves/senseshift/opengloves/interface.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "og_protocol.hpp" - -namespace SenseShift::OpenGloves { - struct ITransport { - virtual void setup(){}; - virtual size_t send(const char* buffer, size_t length) = 0; - virtual bool hasData() = 0; - virtual size_t read(char* buffer, size_t length) = 0; - }; - - struct IEncoding { - virtual size_t - serialize(const std::vector<::OpenGloves::IStringEncodedMemoizedSensor*>& sensors, char* buffer) const = 0; - - virtual bool deserialize( - const char* buffer, const size_t length, std::map<::OpenGloves::Command, uint16_t>& commands - ) const = 0; - }; -} // namespace SenseShift::OpenGloves diff --git a/lib/opengloves/senseshift/opengloves/opengloves.hpp b/lib/opengloves/senseshift/opengloves/opengloves.hpp new file mode 100644 index 00000000..cd4cbd0e --- /dev/null +++ b/lib/opengloves/senseshift/opengloves/opengloves.hpp @@ -0,0 +1,193 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace SenseShift::OpenGloves { + class ITransport : public IInitializable { + public: + virtual auto send(const char* buffer, size_t length) -> size_t = 0; + virtual auto hasData() -> bool = 0; + virtual auto read(char* buffer, size_t length) -> size_t = 0; + }; + + using FloatSensor = ::SenseShift::Input::FloatSensor; + using BinarySensor = ::SenseShift::Input::BinarySensor; + + class InputSensors : + public og::InputPeripheral, + public Component, + public ::SenseShift::Input::Calibration::ICalibrated { + public: + void init() override + { + for (auto& finger_curl : this->curl.fingers) { + for (auto& joint_sensor : finger_curl.curl) { + if (joint_sensor != nullptr) { + joint_sensor->init(); + this->calibrated_inputs_.insert(joint_sensor); + } + } + } + + for (auto& finger_splay : this->splay.fingers) { + if (finger_splay != nullptr) { + finger_splay->init(); + this->calibrated_inputs_.insert(finger_splay); + } + } + + SS_INIT_NOT_NULL(this->joystick.x); + SS_INIT_NOT_NULL(this->joystick.y); + SS_INIT_NOT_NULL(this->joystick.press); + + for (auto& button : this->buttons) { + SS_INIT_NOT_NULL(button.press); + } + + for (auto& analog_button : this->analog_buttons) { + SS_INIT_NOT_NULL(analog_button.press); + SS_INIT_NOT_NULL(analog_button.value); + } + } + + void tick() override + { + for (auto& finger_curl : this->curl.fingers) { + for (auto& joint_sensor : finger_curl.curl) { + SS_TICK_NOT_NULL(joint_sensor); + } + } + + for (auto& finger_splay : this->splay.fingers) { + SS_TICK_NOT_NULL(finger_splay); + } + + SS_TICK_NOT_NULL(this->joystick.x); + SS_TICK_NOT_NULL(this->joystick.y); + SS_TICK_NOT_NULL(this->joystick.press); + + for (auto& button : this->buttons) { + SS_TICK_NOT_NULL(button.press); + } + + for (auto& analog_button : this->analog_buttons) { + SS_TICK_NOT_NULL(analog_button.press); + SS_TICK_NOT_NULL(analog_button.value); + } + } + + auto collectData() -> og::InputPeripheralData + { + og::InputPeripheralData data{}; + + const auto& curls = this->curl.fingers; + const auto& splays = this->splay.fingers; + for (auto i = 0; i < curls.size(); i++) { + const auto& finger_curl = curls[i].curl; + for (auto j = 0; j < finger_curl.size(); j++) { + auto* joint_sensor = finger_curl[j]; + if (joint_sensor != nullptr) { + data.curl.fingers[i].curl[j] = joint_sensor->getValue(); + } + } + + auto* finger_splay = splays[i]; + if (finger_splay != nullptr) { + data.splay.fingers[i] = finger_splay->getValue(); + } + } + + if (this->joystick.x != nullptr) { + data.joystick.x = this->joystick.x->getValue(); + } + if (this->joystick.y != nullptr) { + data.joystick.y = this->joystick.y->getValue(); + } + if (this->joystick.press != nullptr) { + data.joystick.press = this->joystick.press->getValue(); + } + + for (auto i = 0; i < this->buttons.size(); i++) { + auto* button = this->buttons[i].press; + if (button != nullptr) { + data.buttons[i].press = button->getValue(); + } + } + + for (auto i = 0; i < this->analog_buttons.size(); i++) { + auto* button = this->analog_buttons[i].press; + if (button != nullptr) { + data.analog_buttons[i].press = button->getValue(); + } + auto* value = this->analog_buttons[i].value; + if (value != nullptr) { + data.analog_buttons[i].value = value->getValue(); + } + } + + return data; + } + + void reselCalibration() override + { + for (const auto& calibrated_input : this->calibrated_inputs_) { + calibrated_input->reselCalibration(); + } + } + + void startCalibration() override + { + for (const auto& calibrated_input : this->calibrated_inputs_) { + calibrated_input->startCalibration(); + } + } + + void stopCalibration() override + { + for (const auto& calibrated_input : this->calibrated_inputs_) { + calibrated_input->stopCalibration(); + } + } + + private: + std::set calibrated_inputs_{}; + }; + + using FloatOutput = ::SenseShift::Output::IFloatOutput; + + class OutputWriters : public IInitializable { + public: + og::OutputForceFeedback ffb; + + void init() override + { + for (auto& finger : this->ffb.fingers) { + if (finger != nullptr) { + finger->init(); + } + } + } + + void apply(const og::OutputData& data) + { + if (std::holds_alternative(data)) { + const auto& ffb_data = std::get(data); + for (auto i = 0; i < this->ffb.fingers.size(); i++) { + auto* finger = this->ffb.fingers[i]; + if (finger != nullptr) { + finger->writeState(ffb_data.fingers[i]); + } + } + } + } + }; +} // namespace SenseShift::OpenGloves diff --git a/lib/opengloves/sensor/og_finger.hpp b/lib/opengloves/sensor/og_finger.hpp deleted file mode 100644 index 542d981e..00000000 --- a/lib/opengloves/sensor/og_finger.hpp +++ /dev/null @@ -1,156 +0,0 @@ -#pragma once - -#include - -#include "og_sensor.hpp" - -namespace OpenGloves { - struct FingerSensors { - std::vector*> curl = - std::vector*>(); - std::optional*> splay = std::nullopt; - - FingerSensors( - std::vector*> curl, - std::optional*> splay = std::nullopt - ) : - curl(curl), splay(splay){}; - - FingerSensors( - SenseShift::Input::CalibratedSimpleSensor* curl1, - std::optional*> splay = std::nullopt - ) : - curl({ curl1 }), splay(splay){}; - }; - - class ICurl { - public: - /** - * Get finger curl value. - */ - virtual uint16_t getCurl() = 0; - }; - - using IFingerSensor = SenseShift::Input::ISimpleSensor; - - class SimpleFingerSensor : public IFingerSensor, public ICurl { - public: - SimpleFingerSensor(FingerSensors sensors) : sensors(sensors){}; - - SimpleFingerSensor( - SenseShift::Input::CalibratedSimpleSensor* curl1, - std::optional*> splay = std::nullopt - ) : - sensors(curl1, splay){}; - - void init() override - { - for (auto sensor : sensors.curl) { - sensor->init(); - } - if (sensors.splay.has_value()) { - sensors.splay.value()->init(); - } - } - - FingerValue getValue() override - { - FingerValue value{ - .curl = std::vector(), - .splay = std::nullopt, - }; - for (auto sensor : sensors.curl) { - value.curl.push_back(sensor->getValue()); - } - if (sensors.splay.has_value()) { - value.splay = sensors.splay.value()->getValue(); - } - return value; - } - - uint16_t getCurl() override { return this->getValue().getTotalCurl(); } - - protected: - FingerSensors sensors; - }; - - /** - * Simple finger sensor that only provides calibration. - */ - class CalibratedFingerSensor : public SimpleFingerSensor, public SenseShift::Calibration::ICalibrated { - public: - CalibratedFingerSensor(FingerSensors sensors) : SimpleFingerSensor(sensors){}; - - CalibratedFingerSensor( - SenseShift::Input::CalibratedSimpleSensor* curl1, - std::optional*> splay = std::nullopt - ) : - SimpleFingerSensor(curl1, splay){}; - - void resetCalibration() override - { - for (auto sensor : this->sensors.curl) { - sensor->resetCalibration(); - } - if (this->sensors.splay.has_value()) { - this->sensors.splay.value()->resetCalibration(); - } - } - - void enableCalibration() override - { - for (auto sensor : this->sensors.curl) { - sensor->enableCalibration(); - } - if (this->sensors.splay.has_value()) { - this->sensors.splay.value()->enableCalibration(); - } - } - - void disableCalibration() override - { - for (auto sensor : this->sensors.curl) { - sensor->disableCalibration(); - } - if (this->sensors.splay.has_value()) { - this->sensors.splay.value()->disableCalibration(); - } - } - }; - - class FingerSensor : - public StringEncodedMemoizedSensor, - public SenseShift::Calibration::ICalibrated, - public ICurl { - public: - FingerSensor(CalibratedFingerSensor* sensor, IEncodedInput::Type type) : - StringEncodedMemoizedSensor(sensor, type){}; - - FingerSensor( - SenseShift::Input::CalibratedSimpleSensor* curl1, - std::optional*> splay, - IEncodedInput::Type type - ) : - StringEncodedMemoizedSensor(new CalibratedFingerSensor(curl1, splay), type){}; - - FingerSensor(SenseShift::Input::CalibratedSimpleSensor* curl1, IEncodedInput::Type type) : - StringEncodedMemoizedSensor(new CalibratedFingerSensor(curl1, std::nullopt), type){}; - - void resetCalibration() override { static_cast(this->sensor)->resetCalibration(); } - - void enableCalibration() override { static_cast(this->sensor)->enableCalibration(); } - - void disableCalibration() override { static_cast(this->sensor)->disableCalibration(); } - - uint16_t getCurl() override { return this->getValue().getTotalCurl(); } - }; - - struct HandSensors { - std::optional thumb = std::nullopt; - std::optional index = std::nullopt; - std::optional middle = std::nullopt; - std::optional ring = std::nullopt; - std::optional pinky = std::nullopt; - }; - -} // namespace OpenGloves diff --git a/lib/opengloves/sensor/og_gesture.hpp b/lib/opengloves/sensor/og_gesture.hpp deleted file mode 100644 index 9bb0e375..00000000 --- a/lib/opengloves/sensor/og_gesture.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include - -namespace OpenGloves { - class Gesture : public SenseShift::Input::ISimpleSensor {}; - - class GrabGesture : public Gesture { - private: - ICurl& index; - ICurl& middle; - ICurl& ring; - ICurl& pinky; - uint16_t threshold; - - public: - GrabGesture(ICurl& index, ICurl& middle, ICurl& ring, ICurl& pinky, uint16_t threshold) : - index(index), middle(middle), ring(ring), pinky(pinky), threshold(threshold){}; - - void init() override{}; - - bool getValue() override - { - return this->index.getCurl() > this->threshold && this->middle.getCurl() > this->threshold - && this->ring.getCurl() > this->threshold && this->pinky.getCurl() > this->threshold; - } - }; - - class TriggerGesture : public Gesture { - private: - ICurl& index; - uint16_t threshold; - - public: - TriggerGesture(ICurl& index, uint16_t threshold) : index(index), threshold(threshold){}; - - void init() override{}; - - bool getValue() override { return this->index.getCurl() > this->threshold; } - }; - - class PinchGesture : public Gesture { - private: - ICurl& index; - ICurl& thumb; - uint16_t threshold; - - public: - PinchGesture(ICurl& index, ICurl& thumb, uint16_t threshold) : - index(index), thumb(thumb), threshold(threshold){}; - - void init() override{}; - - bool getValue() override - { - return this->index.getCurl() > this->threshold && this->thumb.getCurl() > this->threshold; - } - }; -} // namespace OpenGloves diff --git a/lib/opengloves/sensor/og_sensor.hpp b/lib/opengloves/sensor/og_sensor.hpp deleted file mode 100644 index caa4a9c8..00000000 --- a/lib/opengloves/sensor/og_sensor.hpp +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -namespace OpenGloves { - struct FingerValue { - std::vector curl = std::vector({ 0 }); - std::optional splay = std::nullopt; - - uint16_t getTotalCurl() const - { - if (this->curl.size() == 0) { - return 0; - } - - uint16_t total = 0; - for (auto curl : this->curl) { - total += curl; - } - return total / this->curl.size(); - } - }; - - template - class StringEncodedMemoizedSensor : - public IStringEncodedMemoizedSensor, - public SenseShift::Input::MemoizedSensor<_Tp> { - public: - StringEncodedMemoizedSensor(SenseShift::Input::ISimpleSensor<_Tp>* sensor, IEncodedInput::Type type) : - IStringEncodedMemoizedSensor(type), SenseShift::Input::MemoizedSensor<_Tp>(sensor){}; - - void init() override { this->sensor->init(); } - - void updateValue() override { this->value = this->sensor->getValue(); } - - size_t getEncodedLength() const override; - - size_t encodeString(char* buffer) const override; - }; - - template<> - size_t StringEncodedMemoizedSensor::getEncodedLength() const - { - return 6; - } - - template<> - size_t StringEncodedMemoizedSensor::encodeString(char* buffer) const - { - // Format as "Axxxx", where A is the type and xxxxx is the value without - // leading zeros. - return snprintf(buffer, this->getEncodedLength(), "%c%d", this->getType(), this->value); - } - - template<> - size_t StringEncodedMemoizedSensor::getEncodedLength() const - { - return 1; - } - - template<> - size_t StringEncodedMemoizedSensor::encodeString(char* buffer) const - { - if (value) { - buffer[0] = this->getType(); - } - return value ? this->getEncodedLength() : 0; - } - - template<> - size_t StringEncodedMemoizedSensor::getEncodedLength() const - { - // curl + splay + (3 * knuckle) - return 6 + 9 + (3 * 10); - } - - template<> - size_t StringEncodedMemoizedSensor::encodeString(char* buffer) const - { - size_t offset = 0; - offset += snprintf(buffer + offset, 6, "%c%d", this->type, this->value.getTotalCurl()); - - if (this->value.curl.size() > 1) { - for (size_t i = 0; i < this->value.curl.size(); i++) { - char knuckle = 'A' + i; - offset += snprintf(buffer + offset, 10, "(%cA%c)%d", this->type, knuckle, this->value.curl[i]); - } - } - - if (this->value.splay.has_value()) { - offset += snprintf(buffer + offset, 9, "(%cB)%d", this->type, this->value.splay.value()); - } - - return offset; - } - -} // namespace OpenGloves diff --git a/lib/opengloves_arduino/senseshift/opengloves/transport/stream.hpp b/lib/opengloves_arduino/senseshift/opengloves/transport/stream.hpp index f249f954..12d413e2 100644 --- a/lib/opengloves_arduino/senseshift/opengloves/transport/stream.hpp +++ b/lib/opengloves_arduino/senseshift/opengloves/transport/stream.hpp @@ -1,26 +1,27 @@ #pragma once +#include + +#include + #include #include #include #include #include -#include -#include +#include namespace SenseShift::OpenGloves { class IStreamTransport : public ITransport { - using IStringEncodedMemoizedSensor = ::OpenGloves::IStringEncodedMemoizedSensor; - protected: Stream* channel; - char* buffer = new char[256]; + std::array buffer_{}; public: IStreamTransport(Stream* channel) : channel(channel){}; - size_t send(const char* buffer, size_t length) override + auto send(const char* buffer, size_t length) -> size_t override { if (!this->isReady()) { return 0; @@ -32,17 +33,17 @@ namespace SenseShift::OpenGloves { return written; } - virtual bool isReady() = 0; + virtual auto isReady() -> bool = 0; - virtual bool hasData() override + auto hasData() -> bool override { return this->isReady() && this->channel != nullptr && this->channel->available() > 0; } - virtual size_t read(char* buffer, size_t length) + auto read(char* buffer, size_t length) -> size_t override { if (!this->hasData()) { - return false; + return 0U; } size_t bytesRead = this->channel->readBytesUntil('\n', buffer, length); @@ -54,12 +55,12 @@ namespace SenseShift::OpenGloves { class StreamTransport : public IStreamTransport { public: - StreamTransport(Stream& channel) : IStreamTransport(&channel){}; - StreamTransport(Stream* channel) : IStreamTransport(channel){}; + explicit StreamTransport(Stream& channel) : IStreamTransport(&channel){}; + explicit StreamTransport(Stream* channel) : IStreamTransport(channel){}; - void setup() override { this->mReady = true; } + void init() override { this->mReady = true; } - bool isReady() override { return this->channel != nullptr && this->mReady; } + auto isReady() -> bool override { return this->channel != nullptr && this->mReady; } private: bool mReady = false; @@ -67,15 +68,17 @@ namespace SenseShift::OpenGloves { class BluetoothSerialTransport : public IStreamTransport { public: - BluetoothSerialTransport(BluetoothSerial& channel) : IStreamTransport(&channel){}; + explicit BluetoothSerialTransport(BluetoothSerial& channel) : IStreamTransport(&channel){}; - bool isReady() override + auto isReady() -> bool override { auto* serial = static_cast(this->channel); return serial->isReady() && serial->hasClient(); } - virtual size_t send(const char* buffer, size_t length) override + void init() override {} + + auto send(const char* buffer, size_t length) -> size_t override { auto written = this->channel->write(buffer, length); @@ -88,9 +91,11 @@ namespace SenseShift::OpenGloves { class BLESerialTransport : public IStreamTransport { public: - BLESerialTransport(BLESerial& channel) : IStreamTransport(&channel){}; + explicit BLESerialTransport(BLESerial& channel) : IStreamTransport(&channel){}; + + void init() override {} - bool isReady() override + auto isReady() -> bool override { auto* serial = static_cast(this->channel); return serial->connected(); @@ -101,11 +106,11 @@ namespace SenseShift::OpenGloves { public: WiFiSerialTransport(WiFiServer& server) : IStreamTransport(nullptr), m_server(server){}; - void setup() override + void init() override { auto* client = static_cast(this->channel); if (client != nullptr) { - if (client->connected()) { + if (client->connected() != 0U) { return; } } @@ -114,14 +119,14 @@ namespace SenseShift::OpenGloves { this->channel = new WiFiClient(this->m_server.available()); } - bool isReady() override + auto isReady() -> bool override { if (this->channel == nullptr) { return false; } auto* client = static_cast(this->channel); - return client->connected(); + return client->connected() != 0U; } private: diff --git a/lib/opengloves_freertos/senseshift/opengloves/opengloves_task.hpp b/lib/opengloves_freertos/senseshift/opengloves/opengloves_task.hpp new file mode 100644 index 00000000..f8509f3d --- /dev/null +++ b/lib/opengloves_freertos/senseshift/opengloves/opengloves_task.hpp @@ -0,0 +1,163 @@ +#pragma once + +#include + +#include +#include +#include + +#include "senseshift/arduino/input/sensor/analog.hpp" +#include "senseshift/arduino/input/sensor/digital.hpp" +#include "senseshift/core/component.hpp" +#include "senseshift/freertos/task.hpp" +#include "senseshift/input/sensor.hpp" +#include "senseshift/opengloves/opengloves.hpp" +#include "senseshift/utility.hpp" +#include +#include +#include + +namespace SenseShift::OpenGloves { + class OpenGlovesTrackingComponent : public SenseShift::Component { + public: + class Config { + friend class OpenGlovesTrackingComponent; + size_t calibration_duration_ms_; + bool always_calibrate_; + + public: + /// \param updateRate The rate at which the sensors should be updated in Hz. + /// \param calibrationDuration The duration in milliseconds that the calibration button should be held for. + Config(size_t calibration_duration_ms, bool always_calibrate) : + calibration_duration_ms_(calibration_duration_ms), always_calibrate_(always_calibrate) + { + } + }; + + OpenGlovesTrackingComponent( + Config& config, + InputSensors& input_sensors, + ITransport* communication, + og::IEncoder* encoder = new og::AlphaEncoder() + ) : + config_(config), input_sensors_(std::move(input_sensors)), communication_(communication), encoder_(encoder) + { + } + + void init() override + { + this->communication_->init(); + this->input_sensors_.init(); + + // If the calibration button is not present, start calibration immediately. + if (this->config_.always_calibrate_ || this->input_sensors_.button_calibrate.press == nullptr) { + this->startCalibration(); + } + } + + void tick() override + { + // const auto start = micros(); + + // auto now = micros(); + this->input_sensors_.tick(); + // const auto tickTime = micros() - now; + + // now = micros(); + const auto data = this->input_sensors_.collectData(); + // const auto collectTime = micros() - now; + + bool const calibrate_pressed = data.button_calibrate.press; + if (calibrate_pressed && this->calibration_start_time_ == 0) { + this->startCalibration(); + } + + // now = micros(); + const auto length = this->encoder_->encode_input(data, buffer.data(), buffer.size()); + // const auto encodeTime = micros() - now; + + // now = micros(); + this->communication_->send(buffer.data(), length); + // const auto sendTime = micros() - now; + + if (!this->config_.always_calibrate_ && this->calibration_start_time_ != 0) { + const auto calibration_elapsed = millis() - this->calibration_start_time_; + const bool calibration_done = calibration_elapsed >= this->config_.calibration_duration_ms_; + + if (calibration_done) { + this->stopCalibration(); + } + } + + // const auto total = micros() - start; + // log_i( + // "total: %d, tick: %d, collect: %d, encode: %d, send: %d, c/s: %.2f", + // total, + // tickTime, + // collectTime, + // encodeTime, + // sendTime, + // 1000000.0 / total + // ); + } + + protected: + void startCalibration() + { + log_i("Starting calibration"); + this->input_sensors_.reselCalibration(); + this->input_sensors_.startCalibration(); + this->calibration_start_time_ = millis(); + } + + void stopCalibration() + { + log_i("Stopping calibration"); + this->input_sensors_.stopCalibration(); + this->calibration_start_time_ = 0; + } + + private: + std::array buffer = {}; + + unsigned long long calibration_start_time_ = 0; + + Config& config_; + InputSensors input_sensors_; + ITransport* communication_; + og::IEncoder* encoder_; + }; + + class OpenGlovesForceFeedbackComponent : public SenseShift::Component { + public: + OpenGlovesForceFeedbackComponent( + OutputWriters& output_writers, + ::SenseShift::OpenGloves::ITransport* communication, + og::IEncoder* encoder = new og::AlphaEncoder() + ) : + output_writers_(output_writers), communication_(communication), encoder_(encoder){}; + + void init() override + { + log_d("Setting up OpenGloves force feedback task: %p", this); + this->communication_->init(); + this->output_writers_.init(); + } + + void tick() override + { + if (this->communication_->hasData()) { + const auto length = this->communication_->read(this->buffer.data(), this->buffer.size()); + const auto output = this->encoder_->decode_output(this->buffer.data(), length); + this->output_writers_.apply(output); + } + } + + private: + std::array buffer = {}; + + OutputWriters output_writers_; + ::SenseShift::OpenGloves::ITransport* communication_; + og::IEncoder* encoder_; + }; +} // namespace SenseShift::OpenGloves diff --git a/lib/opengloves_task/opengloves_task.hpp b/lib/opengloves_task/opengloves_task.hpp deleted file mode 100644 index 2f7bc581..00000000 --- a/lib/opengloves_task/opengloves_task.hpp +++ /dev/null @@ -1,351 +0,0 @@ -#pragma once - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace OpenGloves { - struct OpenGlovesTrackingTaskConfig { - size_t updateIntervalMs; - size_t calibrationDuration; - bool alwaysCalibrate; - - /** - * @param updateRate The rate at which the sensors should be updated in Hz. - * @param calibrationDuration The duration in milliseconds that the - * calibration button should be held for. - */ - OpenGlovesTrackingTaskConfig(size_t updateRate, size_t calibrationDuration, bool alwaysCalibrate) : - calibrationDuration(calibrationDuration), alwaysCalibrate(alwaysCalibrate) - { - // Convert the update rate to an interval in milliseconds. - this->updateIntervalMs = 1000 / updateRate; - } - }; - - class OpenGlovesTrackingTask : public SenseShift::FreeRTOS::Task { - friend class SenseShift::FreeRTOS::Task; - - public: - /** - * @param config The configuration for the OpenGloves tracking task. - * @param communication The communication interface. - * @param fingers The finger sensors. - * @param buttons The button sensors. - * @param joysticks The joystick sensors. - * @param otherSensors Other sensors that should be updated. - * @param calibrationButton The calibration button sensor, optional. - * @param taskConfig The task configuration. - */ - OpenGlovesTrackingTask( - OpenGlovesTrackingTaskConfig& config, - ::SenseShift::OpenGloves::ITransport& communication, - HandSensors& fingers, - std::vector*>& buttons, - std::vector*>& joysticks, - std::vector& otherSensors, - std::optional>& calibrationButton, - SenseShift::FreeRTOS::TaskConfig taskConfig - ) : - config(config), - communication(communication), - fingers(fingers), - buttons(buttons), - joysticks(joysticks), - otherSensors(otherSensors), - calibrationButton(calibrationButton), - allSensors(std::vector()), - SenseShift::FreeRTOS::Task(taskConfig) - { - if (fingers.thumb.has_value()) { - auto* thumb = &fingers.thumb.value(); - this->calibrated.push_back(thumb); - this->allSensors.push_back(thumb); - } - if (fingers.index.has_value()) { - auto* index = &fingers.index.value(); - this->calibrated.push_back(index); - this->allSensors.push_back(index); - } - if (fingers.middle.has_value()) { - auto* middle = &fingers.middle.value(); - this->calibrated.push_back(middle); - this->allSensors.push_back(middle); - } - if (fingers.ring.has_value()) { - auto* ring = &fingers.ring.value(); - this->calibrated.push_back(ring); - this->allSensors.push_back(ring); - } - if (fingers.pinky.has_value()) { - auto* pinky = &fingers.pinky.value(); - this->calibrated.push_back(pinky); - this->allSensors.push_back(pinky); - } - - for (auto* button : buttons) { - this->allSensors.push_back(button); - } - - if (calibrationButton.has_value()) { - this->allSensors.push_back(&calibrationButton.value()); - } - - for (auto* joystick : joysticks) { - this->allSensors.push_back(joystick); - } - - for (auto* otherSensor : otherSensors) { - this->allSensors.push_back(otherSensor); - } - - // sort all sensors by type for easier debugging - std::sort( - this->allSensors.begin(), - this->allSensors.end(), - [](IStringEncodedMemoizedSensor* a, IStringEncodedMemoizedSensor* b) { - return a->getType() < b->getType(); - } - ); - } - - void begin() override - { - log_d("Starting OpenGloves tracking task: %p", this); - this->setup(); - this->SenseShift::FreeRTOS::Task::begin(); - }; - - private: - OpenGlovesTrackingTaskConfig& config; - - HandSensors& fingers; - ::SenseShift::OpenGloves::ITransport& communication; - ::SenseShift::OpenGloves::AlphaEncodingService encodingService = - ::SenseShift::OpenGloves::AlphaEncodingService(); - - std::vector*>& buttons; - std::vector*>& joysticks; - std::vector& otherSensors; - - std::vector allSensors; - - std::optional>& calibrationButton; - std::vector calibrated = - std::vector(); - unsigned long long calibrationStarted = 0; - - void startCalibration() - { - for (auto* input : this->calibrated) { - input->resetCalibration(); - input->enableCalibration(); - } - this->calibrationStarted = millis(); - } - - void setup() - { - log_d("Setting up OpenGloves tracking task: %p", this); - log_d("There is a total of %d sensors", this->allSensors.size()); - for (auto* input : this->allSensors) { - log_d("Setting up sensor: %c", input->getType()); - input->init(); - } - - // Start calibration if no calibration button is present or if configured to always calibrate. - if (!this->calibrationButton.has_value() || this->config.alwaysCalibrate) { - log_d("Starting calibration on startup"); - this->startCalibration(); - } - - this->communication.setup(); - } - - void run() - { - while (true) { - auto now = millis(); - - // Update the sensors. - for (auto* input : this->allSensors) { - input->updateValue(); - } - - // Update the calibration if calibration has not started, calibration is not configured to always run, - // and the calibration button is present and pressed. - if (calibrationStarted == 0 && - !this->config.alwaysCalibrate && - this->calibrationButton.has_value() && - this->calibrationButton.value().getValue() - ) { - log_d("Calibration started"); - this->startCalibration(); - } - - // Send the sensor values. - char command[256]; - size_t length = this->encodingService.serialize(this->allSensors, command); - this->communication.send(command, length); - - // Check if the calibration has finished. - if (!(this->config.alwaysCalibrate) && calibrationStarted > 0 && (now - calibrationStarted) > CALIBRATION_DURATION) { - log_d("Calibration finished"); - for (size_t i = 0; i < calibrated.size(); i++) { - auto* input = calibrated[i]; - input->disableCalibration(); - } - calibrationStarted = 0; - } - - // Delay until the next update. - auto elapsed = millis() - now; - - log_d( - "Update took %d ms, theoretical max rate is %dHz (target is %dHz)", - elapsed, - 1000 / elapsed, - 1000 / this->config.updateIntervalMs - ); - - if (elapsed < this->config.updateIntervalMs) { - delay(this->config.updateIntervalMs - elapsed); - } - } - }; - }; - - class OpenGlovesForceFeedbackTask : public SenseShift::FreeRTOS::Task { - friend class SenseShift::FreeRTOS::Task; - - public: - OpenGlovesForceFeedbackTask( - ::SenseShift::OpenGloves::ITransport& communication, - HandActuators& actuators, - size_t updateRate, - SenseShift::FreeRTOS::TaskConfig taskConfig - ) : - communication(communication), - actuators(actuators), - SenseShift::FreeRTOS::Task(taskConfig) - { - this->updateIntervalMs = 1000 / updateRate; - }; - - void begin() override - { - log_d("Starting OpenGloves force feedback task: %p", this); - this->setup(); - this->SenseShift::FreeRTOS::Task::begin(); - }; - - private: - ::SenseShift::OpenGloves::ITransport& communication; - HandActuators& actuators; - size_t updateIntervalMs; - ::SenseShift::OpenGloves::AlphaEncodingService encodingService = - ::SenseShift::OpenGloves::AlphaEncodingService(); - - void setup() - { - log_d("Setting up OpenGloves force feedback task: %p", this); - this->communication.setup(); - - if (this->actuators.thumb.has_value()) { - this->actuators.thumb.value()->setup(); - } - - if (this->actuators.index.has_value()) { - this->actuators.index.value()->setup(); - } - - if (this->actuators.middle.has_value()) { - this->actuators.middle.value()->setup(); - } - - if (this->actuators.ring.has_value()) { - this->actuators.ring.value()->setup(); - } - - if (this->actuators.pinky.has_value()) { - this->actuators.pinky.value()->setup(); - } - } - - void run() - { - char commandBuffer[256]; - std::string command; - while (true) { - auto now = millis(); - - if (this->communication.hasData()) { - auto bytesRead = this->communication.read(commandBuffer, sizeof(commandBuffer)); - if (bytesRead == 0) { - continue; - } - - std::map<::OpenGloves::Command, uint16_t> commands = {}; - this->encodingService.deserialize(commandBuffer, bytesRead, commands); - - for (auto& [command, value] : commands) { - this->handleCommand(command, value); - } - } - - // Delay until the next update. - auto elapsed = millis() - now; - if (elapsed < this->updateIntervalMs) { - delay(this->updateIntervalMs - elapsed); - } - } - } - - void handleCommand(Command command, uint16_t value) - { - switch (command) { - case Command::ThumbCurl: - if (this->actuators.thumb.has_value()) { - this->actuators.thumb.value()->writeOutput(value); - } - break; - case Command::IndexCurl: - if (this->actuators.index.has_value()) { - this->actuators.index.value()->writeOutput(value); - } - break; - case Command::MiddleCurl: - if (this->actuators.middle.has_value()) { - this->actuators.middle.value()->writeOutput(value); - } - break; - case Command::RingCurl: - if (this->actuators.ring.has_value()) { - this->actuators.ring.value()->writeOutput(value); - } - break; - case Command::PinkyCurl: - if (this->actuators.pinky.has_value()) { - this->actuators.pinky.value()->writeOutput(value); - } - break; - default: - log_w("Unhandled command: %d", command); - break; - } - } - }; -} // namespace OpenGloves diff --git a/lib/util/senseshift/calibration.hpp b/lib/util/senseshift/calibration.hpp deleted file mode 100644 index 53411728..00000000 --- a/lib/util/senseshift/calibration.hpp +++ /dev/null @@ -1,157 +0,0 @@ -/** - * Calibrated input - * Credit: https://github.com/JohnRThomas/OpenGloves-Firmware/blob/main/open-gloves/Calibration.hpp - */ - -#pragma once - -#include - -namespace SenseShift::Calibration { - struct ICalibrated { - virtual void resetCalibration() = 0; - virtual void enableCalibration() = 0; - virtual void disableCalibration() = 0; - }; - - class Calibrated : public virtual ICalibrated { - protected: - bool calibrate = false; - - public: - virtual void resetCalibration() = 0; - void enableCalibration() override { calibrate = true; } - void disableCalibration() override { calibrate = false; } - }; - - template - struct ICalibrator { - static_assert(std::is_arithmetic<_Tp>::value, "ICalibrator only can be used with arithmetic types"); - - virtual void reset() = 0; - virtual void update(_Tp input) = 0; - virtual _Tp calibrate(_Tp input) const = 0; - }; - - template - class MinMaxCalibrator : public ICalibrator<_Tp> { - public: - MinMaxCalibrator() : value_min(output_max), value_max(output_min) {} - - void reset() - { - value_min = output_max; - value_max = output_min; - } - - void update(_Tp input) - { - // Update the min and the max. - if (input < value_min) - value_min = input; - if (input > value_max) - value_max = input; - } - - _Tp calibrate(_Tp input) const - { - // This means we haven't had any calibration data yet. - // Return a neutral value right in the middle of the output range. - if (value_min > value_max) { - return (output_min + output_max) / 2.0f; - } - - if (input <= value_min) { - return output_min; - } - - if (input >= value_max) { - return output_max; - } - - // Map the input range to the output range. - _Tp output = ::SenseShift::accurateMap<_Tp>(input, value_min, value_max, output_min, output_max); - - // Lock the range to the output. - return std::clamp(output, output_min, output_max); - } - - private: - _Tp value_min; - _Tp value_max; - }; - - template - class CenterPointDeviationCalibrator : public ICalibrator<_Tp> { - public: - CenterPointDeviationCalibrator() : range_min(sensor_max), range_max(0) {} - - void reset() - { - range_min = sensor_max; - range_max = 0; - } - - void update(_Tp input) - { - // Update the min and the max. - if (input < range_min) - range_min = ::SenseShift::accurateMap<_Tp>(input, output_min, output_max, 0, sensor_max); - if (input > range_max) - range_max = ::SenseShift::accurateMap<_Tp>(input, output_min, output_max, 0, sensor_max); - } - - _Tp calibrate(_Tp input) const - { - // Find the center point of the sensor so we know how much we have deviated from it. - _Tp center = (range_min + range_max) / 2.0f; - - // Map the input to the sensor range of motion. - int output = ::SenseShift::accurateMap<_Tp>(input, output_min, output_max, 0, sensor_max); - - // Find the deviation from the center and clamp it to the maximum that the driver supports. - output = std::clamp(output - center, -driver_max_deviation, driver_max_deviation); - - // Finally map the deviation from the center back to the output range. - return (_Tp)::SenseShift::accurateMap( - output, - -driver_max_deviation, - driver_max_deviation, - output_min, - output_max - ); - } - - private: - _Tp range_min; - _Tp range_max; - }; - - template - class FixedCenterPointDeviationCalibrator : public ICalibrator<_Tp> { - public: - void reset() {} - void update(_Tp input) {} - - _Tp calibrate(_Tp input) const - { - // Find the center point of the sensor so we know how much we have deviated from it. - _Tp center = sensor_max / 2.0f; - - // Map the input to the sensor range of motion. - int output = ::SenseShift::accurateMap<_Tp>(input, output_min, output_max, 0, sensor_max); - - // Find the deviation from the center and clamp it to the maximum that the driver supports. - output = std::clamp(output - center, -driver_max_deviation, driver_max_deviation); - - // Finally map the deviation from the center back to the output range. - return (_Tp)::SenseShift::accurateMap( - output, - -driver_max_deviation, - driver_max_deviation, - output_min, - output_max - ); - } - }; -} // namespace SenseShift::Calibration diff --git a/lib/util/senseshift/container.hpp b/lib/util/senseshift/container.hpp index 7f0ab894..613b962b 100644 --- a/lib/util/senseshift/container.hpp +++ b/lib/util/senseshift/container.hpp @@ -2,6 +2,7 @@ #include #include +#include #include namespace SenseShift { @@ -11,29 +12,29 @@ namespace SenseShift { * @tparam _Cp The type of the container. * @tparam _Tp The type of the value. */ - template - inline bool contains(_Cp&& c, _Tp val) + template + inline auto contains(Cp&& c, Tp val) -> bool { return std::find(std::begin(c), std::end(c), val) != std::end(c); }; template<> - inline bool contains(std::string& s, char val) + inline auto contains(std::string& s, char val) -> bool { return s.find(val) != std::string::npos; }; - template - inline bool contains(_Tp* begin, _Tp* end, const _Tp& val) + template + inline auto contains(Tp* begin, Tp* end, const Tp& val) -> bool { return std::find(begin, end, val) != end; }; - template - constexpr inline bool contains(const _Tp* arr, const std::size_t size, const _Tp& val) + template + constexpr inline auto contains(const Tp* arr, const std::size_t size, const Tp& val) -> bool { static_assert( - std::is_same<_Tp, typename std::iterator_traits<_Tp*>::value_type>::value, + std::is_same_v::value_type>, "Container and value must be of the same type" ); return std::find(arr, arr + size, val) != arr + size; diff --git a/lib/util/senseshift/range.hpp b/lib/util/senseshift/range.hpp deleted file mode 100644 index 6dac8598..00000000 --- a/lib/util/senseshift/range.hpp +++ /dev/null @@ -1,30 +0,0 @@ -#include -#include - -#include - -namespace SenseShift { - template - constexpr _Tp accurateMap(_Tp x, _Tp in_min, _Tp in_max, _Tp out_min, _Tp out_max) noexcept - { - static_assert(std::is_arithmetic<_Tp>::value, "Type must be arithmetic"); - - const _Tp run = in_max - in_min; - if (run == 0) { - log_e("map(): Invalid input range, min == max"); - return (out_min + out_max) / 2; - } - const _Tp rise = out_max - out_min; - const _Tp delta = x - in_min; - return (delta * rise) / run + out_min; - } - - // Same as the above, but both mins are 0. - template - constexpr inline _Tp simpleMap(_Tp x, _Tp in_max, _Tp out_max) noexcept - { - static_assert(std::is_arithmetic<_Tp>::value, "Type must be arithmetic"); - - return x * out_max / in_max; - } -} // namespace SenseShift diff --git a/lib/util/senseshift/utility.hpp b/lib/util/senseshift/utility.hpp index 5c95fd01..595944a9 100644 --- a/lib/util/senseshift/utility.hpp +++ b/lib/util/senseshift/utility.hpp @@ -5,4 +5,4 @@ #include #include "senseshift/container.hpp" -#include "senseshift/range.hpp" +#include "senseshift/core/helpers.hpp" diff --git a/platformio.ini b/platformio.ini index ce93b43b..e6d2c7c4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,27 +14,26 @@ lib_dir = ./lib src_dir = ./firmware extra_configs = ini/bhaptics.ini - ini/opengloves.ini - ini/opengloves-lucidgloves.ini - ini/opengloves-indexer.ini + ini/opengloves.ini + ini/opengloves-lucidgloves.ini + ini/opengloves-indexer.ini default_envs = bhaptics_tactsuit_x16, bhaptics_tactal, bhaptics_tactglove_left, lucidgloves-prototype3 [common] -build_unflags = - -std=gnu++11 -build_flags = - -std=gnu++17 - -D __OH_FIRMWARE__ - -D CORE_DEBUG_LEVEL=3 -; -D DEBUG_MODE=0 -; -D DEBUG_ESP_PORT=Serial -; -D SENSESHIFT_SERIAL_PLOTTER=true -; -D SENSESHIFT_BATTERY_ENABLED=true -; -D SENSESHIFT_BLE_USE_NIMBLE=true +build_unflags = + -std=gnu++11 +build_flags = + -std=gnu++17 + -D __OH_FIRMWARE__ + -D CORE_DEBUG_LEVEL=3 +; -D DEBUG_MODE=0 +; -D DEBUG_ESP_PORT=Serial +; -D SENSESHIFT_BATTERY_ENABLED=true +; -D SENSESHIFT_BLE_USE_NIMBLE=true -build_src_filter = - +<*> - - +build_src_filter = + +<*> + - lib_deps = https://github.com/senseshift/frozen.git#feature/platformio @@ -48,20 +47,23 @@ lib_ldf_mode = deep+ check_tool = clangtidy check_flags = - clangtidy: --config-file=./.clang-tidy --fix + clangtidy: --config-file=./.clang-tidy --fix debug_build_flags = -Os [env:native] -platform = native +platform = native build_unflags = ${common.build_unflags} -build_flags = ${common.build_flags} - -lgcov - --coverage -build_src_filter = ${common.build_src_filter} - + -lib_deps = ${common.lib_deps} - fabiobatsilva/ArduinoFake@^0.4 +build_flags = + ${common.build_flags} + -lgcov + --coverage +build_src_filter = + ${common.build_src_filter} + + +lib_deps = + ${common.lib_deps} + fabiobatsilva/ArduinoFake@^0.4 -test_ignore = test_embedded +test_ignore = test_embedded diff --git a/test/test_battery/main.cpp b/test/test_battery/main.cpp new file mode 100644 index 00000000..c563d37c --- /dev/null +++ b/test/test_battery/main.cpp @@ -0,0 +1,53 @@ +#include +#include +#include + +using namespace SenseShift::Input; +using namespace SenseShift::Battery; +using namespace SenseShift::Battery::Input; + +void test_battery_sensor(void) +{ + auto* source = new FloatSensor(); + + auto* battery = new LookupTableInterpolateBatterySensor(source, &VoltageMap::LiPO_1S_42); + battery->init(); + + source->publishState(0.f); + TEST_ASSERT_EQUAL_INT(0, battery->getValue().level); + + source->publishState(4.2f); + TEST_ASSERT_EQUAL_INT(255, battery->getValue().level); + + source->publishState(3.7f); + TEST_ASSERT_EQUAL_INT(31, battery->getValue().level); +} + +int process(void) +{ + UNITY_BEGIN(); + + RUN_TEST(test_battery_sensor); + + return UNITY_END(); +} + +#ifdef ARDUINO + +#include + +void setup(void) +{ + process(); +} + +void loop(void) {} + +#else + +int main(int argc, char** argv) +{ + return process(); +} + +#endif \ No newline at end of file diff --git a/test/test_bhaptics_encoding/main.cpp b/test/test_bhaptics_encoding/main.cpp index 0ff45557..0d171a91 100644 --- a/test/test_bhaptics_encoding/main.cpp +++ b/test/test_bhaptics_encoding/main.cpp @@ -6,25 +6,29 @@ using namespace SenseShift::BH; using namespace SenseShift::Body::Haptics; using namespace SenseShift::Output; -class TestActuator : public IActuator { +class TestActuator : public IOutput { public: bool isSetup = false; - uint16_t intensity = 0; + float intensity = 0; - TestActuator() : IActuator() {} - void setup() override { this->isSetup = true; } - void writeOutput(uint16_t intensity) override { this->intensity = intensity; } + TestActuator() : IFloatOutput() {} + void init() override { this->isSetup = true; } + void writeState(float newIntensity) override { this->intensity = newIntensity; } }; +#define ASSERT_EQUAL_FLOAT_ROUNDED(expected, actual, precision) \ + TEST_ASSERT_EQUAL_FLOAT( \ + std::round(expected* std::pow(10, precision)) / std::pow(10, precision), \ + std::round(actual * std::pow(10, precision)) / std::pow(10, precision) \ + ) + void test_layout_tactsuitx16(void) { - static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTSUITX16_SIZE; - static const OutputLayout bhLayout[bhLayoutSize] = BH_LAYOUT_TACTSUITX16; - - static constexpr size_t layoutGroupsSize = BH_LAYOUT_TACTSUITX16_GROUPS_SIZE; - static const uint8_t layoutGroups[layoutGroupsSize] = BH_LAYOUT_TACTSUITX16_GROUPS; + static const std::array bhLayout = { BH_LAYOUT_TACTSUITX16 }; + static const std::array layoutGroups = + BH_LAYOUT_TACTSUITX16_GROUPS; - auto body = new HapticBody(); + auto body = new FloatBody(); TestActuator* actuator0 = new TestActuator(); TestActuator* actuator1 = new TestActuator(); @@ -43,62 +47,61 @@ void test_layout_tactsuitx16(void) TestActuator* actuator14 = new TestActuator(); TestActuator* actuator15 = new TestActuator(); - auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ { actuator0, actuator1, actuator2, actuator3 }, { actuator4, actuator5, actuator6, actuator7 }, }); - auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates({ { actuator8, actuator9, actuator10, actuator11 }, { actuator12, actuator13, actuator14, actuator15 }, }); - auto frontPlane = new VibroPlane(frontOutputs); - auto backPlane = new VibroPlane(backOutputs); + auto frontPlane = new FloatPlane(frontOutputs); + auto backPlane = new FloatPlane(backOutputs); body->addTarget(Target::ChestFront, frontPlane); body->addTarget(Target::ChestBack, backPlane); // body->setup(); - const uint8_t values[] = { + const std::array values = { 0x01, 0x00, 0x23, 0x00, 0x00, 0x45, 0x00, 0x67, 0x00, 0x00, 0x89, 0x00, 0xab, 0x00, 0x00, 0xcd, 0x00, 0xef, 0x00, 0x00, }; Decoder::applyVestGrouped(body, values, bhLayout, layoutGroups); - TEST_ASSERT_EQUAL_INT(0, actuator0->intensity); - TEST_ASSERT_EQUAL_INT(273, actuator1->intensity); - TEST_ASSERT_EQUAL_INT(3276, actuator2->intensity); - TEST_ASSERT_EQUAL_INT(3549, actuator3->intensity); - TEST_ASSERT_EQUAL_INT(546, actuator4->intensity); - TEST_ASSERT_EQUAL_INT(819, actuator5->intensity); - TEST_ASSERT_EQUAL_INT(3822, actuator6->intensity); - TEST_ASSERT_EQUAL_INT(4095, actuator7->intensity); - - TEST_ASSERT_EQUAL_INT(1092, actuator8->intensity); - TEST_ASSERT_EQUAL_INT(1365, actuator9->intensity); - TEST_ASSERT_EQUAL_INT(2184, actuator10->intensity); - TEST_ASSERT_EQUAL_INT(2457, actuator11->intensity); - TEST_ASSERT_EQUAL_INT(1638, actuator12->intensity); - TEST_ASSERT_EQUAL_INT(1911, actuator13->intensity); - TEST_ASSERT_EQUAL_INT(2730, actuator14->intensity); - TEST_ASSERT_EQUAL_INT(3003, actuator15->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator0->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(273.0F / 4095.0F, actuator1->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3276.0F / 4095.0F, actuator2->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3549.0F / 4095.0F, actuator3->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(546.0F / 4095.0F, actuator4->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(819.0F / 4095.0F, actuator5->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3822.0F / 4095.0F, actuator6->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(4095.0F / 4095.0F, actuator7->intensity, 2); + + ASSERT_EQUAL_FLOAT_ROUNDED(1092.0F / 4095.0F, actuator8->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1365.0F / 4095.0F, actuator9->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2184.0F / 4095.0F, actuator10->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2457.0F / 4095.0F, actuator11->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1638.0F / 4095.0F, actuator12->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1911.0F / 4095.0F, actuator13->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2730.0F / 4095.0F, actuator14->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3003.0F / 4095.0F, actuator15->intensity, 2); } void test_layout_tactsuitx40(void) { - static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTSUITX40_SIZE; - static const OutputLayout bhLayout[bhLayoutSize] = BH_LAYOUT_TACTSUITX40; + static const std::array bhLayout = { BH_LAYOUT_TACTSUITX40 }; - auto body = new HapticBody(); + auto body = new FloatBody(); - std::vector> frontMatrix = { + std::vector> frontMatrix = { { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, }; - std::vector> backMatrix = { + std::vector> backMatrix = { { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, @@ -106,11 +109,11 @@ void test_layout_tactsuitx40(void) { new TestActuator(), new TestActuator(), new TestActuator(), new TestActuator() }, }; - auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates(frontMatrix); - auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates(backMatrix); + auto frontOutputs = PlaneMapper_Margin::mapMatrixCoordinates(frontMatrix); + auto backOutputs = PlaneMapper_Margin::mapMatrixCoordinates(backMatrix); - auto frontPlane = new VibroPlane(frontOutputs); - auto backPlane = new VibroPlane(backOutputs); + auto frontPlane = new FloatPlane(frontOutputs); + auto backPlane = new FloatPlane(backOutputs); body->addTarget(Target::ChestFront, frontPlane); body->addTarget(Target::ChestBack, backPlane); @@ -124,55 +127,54 @@ void test_layout_tactsuitx40(void) }, bhLayout ); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[0][0])->intensity); - TEST_ASSERT_EQUAL_INT(273, static_cast(frontMatrix[0][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[0][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[0][3])->intensity); - TEST_ASSERT_EQUAL_INT(546, static_cast(frontMatrix[1][0])->intensity); - TEST_ASSERT_EQUAL_INT(819, static_cast(frontMatrix[1][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[1][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[1][3])->intensity); - TEST_ASSERT_EQUAL_INT(1092, static_cast(frontMatrix[2][0])->intensity); - TEST_ASSERT_EQUAL_INT(1365, static_cast(frontMatrix[2][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[2][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[2][3])->intensity); - TEST_ASSERT_EQUAL_INT(1638, static_cast(frontMatrix[3][0])->intensity); - TEST_ASSERT_EQUAL_INT(1911, static_cast(frontMatrix[3][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[3][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[3][3])->intensity); - TEST_ASSERT_EQUAL_INT(2184, static_cast(frontMatrix[4][0])->intensity); - TEST_ASSERT_EQUAL_INT(2457, static_cast(frontMatrix[4][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[4][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(frontMatrix[4][3])->intensity); - - TEST_ASSERT_EQUAL_INT(2730, static_cast(backMatrix[0][0])->intensity); - TEST_ASSERT_EQUAL_INT(3003, static_cast(backMatrix[0][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[0][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[0][3])->intensity); - TEST_ASSERT_EQUAL_INT(3276, static_cast(backMatrix[1][0])->intensity); - TEST_ASSERT_EQUAL_INT(3549, static_cast(backMatrix[1][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[1][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[1][3])->intensity); - TEST_ASSERT_EQUAL_INT(3822, static_cast(backMatrix[2][0])->intensity); - TEST_ASSERT_EQUAL_INT(4095, static_cast(backMatrix[2][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[2][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[2][3])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[3][0])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[3][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[3][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[3][3])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[4][0])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[4][1])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[4][2])->intensity); - TEST_ASSERT_EQUAL_INT(0, static_cast(backMatrix[4][3])->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[0][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(273.0F / 4095.0F, static_cast(frontMatrix[0][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[0][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[0][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(546.0F / 4095.0F, static_cast(frontMatrix[1][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(819.0F / 4095.0F, static_cast(frontMatrix[1][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[1][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[1][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1092.0F / 4095.0F, static_cast(frontMatrix[2][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1365.0F / 4095.0F, static_cast(frontMatrix[2][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[2][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[2][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1638.0F / 4095.0F, static_cast(frontMatrix[3][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1911.0F / 4095.0F, static_cast(frontMatrix[3][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[3][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[3][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2184.0F / 4095.0F, static_cast(frontMatrix[4][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2457.0F / 4095.0F, static_cast(frontMatrix[4][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[4][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(frontMatrix[4][3])->intensity, 2); + + ASSERT_EQUAL_FLOAT_ROUNDED(2730.0F / 4095.0F, static_cast(backMatrix[0][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3003.0F / 4095.0F, static_cast(backMatrix[0][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[0][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[0][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3276.0F / 4095.0F, static_cast(backMatrix[1][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3549.0F / 4095.0F, static_cast(backMatrix[1][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[1][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[1][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3822.0F / 4095.0F, static_cast(backMatrix[2][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(4095.0F / 4095.0F, static_cast(backMatrix[2][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[2][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[2][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[3][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[3][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[3][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[3][3])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[4][0])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[4][1])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[4][2])->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, static_cast(backMatrix[4][3])->intensity, 2); } void test_layout_tactal(void) { - static constexpr size_t bhLayoutSize = BH_LAYOUT_TACTAL_SIZE; - static const ::SenseShift::Body::Haptics::Position bhLayout[bhLayoutSize] = BH_LAYOUT_TACTAL; + static const std::array bhLayout = { BH_LAYOUT_TACTAL }; - auto body = new HapticBody(); + auto body = new FloatBody(); TestActuator* actuator0 = new TestActuator(); TestActuator* actuator1 = new TestActuator(); @@ -181,28 +183,28 @@ void test_layout_tactal(void) TestActuator* actuator4 = new TestActuator(); TestActuator* actuator5 = new TestActuator(); - auto outputs = PlaneMapper_Margin::mapMatrixCoordinates({ + auto outputs = PlaneMapper_Margin::mapMatrixCoordinates({ { actuator0, actuator1, actuator2, actuator3, actuator4, actuator5 }, }); - auto plane = new VibroPlane(outputs); + auto plane = new FloatPlane(outputs); body->addTarget(Target::FaceFront, plane); Decoder::applyPlain(body, { 0x64, 0x00, 0x00, 0x00, 0x00, 0x00 }, bhLayout, Effect::Vibro, Target::FaceFront); - TEST_ASSERT_EQUAL_INT(4095, actuator0->intensity); - TEST_ASSERT_EQUAL_INT(0, actuator1->intensity); - TEST_ASSERT_EQUAL_INT(0, actuator2->intensity); - TEST_ASSERT_EQUAL_INT(0, actuator3->intensity); - TEST_ASSERT_EQUAL_INT(0, actuator4->intensity); - TEST_ASSERT_EQUAL_INT(0, actuator5->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(1.0F, actuator0->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator1->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator2->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator3->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator4->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuator5->intensity, 2); Decoder::applyPlain(body, { 0x10, 0x20, 0x30, 0x40, 0x50, 0x60 }, bhLayout, Effect::Vibro, Target::FaceFront); - TEST_ASSERT_EQUAL_INT(655, actuator0->intensity); - TEST_ASSERT_EQUAL_INT(1310, actuator1->intensity); - TEST_ASSERT_EQUAL_INT(1965, actuator2->intensity); - TEST_ASSERT_EQUAL_INT(2620, actuator3->intensity); - TEST_ASSERT_EQUAL_INT(3276, actuator4->intensity); - TEST_ASSERT_EQUAL_INT(3931, actuator5->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(0.16F, actuator0->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1310.0F / 4095.0F, actuator1->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1965.0F / 4095.0F, actuator2->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2620.0F / 4095.0F, actuator3->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3276.0F / 4095.0F, actuator4->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3931.0F / 4095.0F, actuator5->intensity, 2); } void test_layout_tactglove(void) @@ -214,7 +216,7 @@ void test_layout_tactglove(void) TestActuator* actuatorLittle = new TestActuator(); TestActuator* actuatorWrist = new TestActuator(); - auto body = new HapticBody(); + auto body = new FloatBody(); const auto& bhLayout = TactGloveLeftLayout; addTactGloveActuators( @@ -229,20 +231,20 @@ void test_layout_tactglove(void) ); Decoder::applyPlain(body, { 0x64, 0x00, 0x00, 0x00, 0x00, 0x00 }, bhLayout, Effect::Vibro); - TEST_ASSERT_EQUAL_INT(4095, actuatorThumb->intensity); - TEST_ASSERT_EQUAL_INT(0, actuatorIndex->intensity); - TEST_ASSERT_EQUAL_INT(0, actuatorMiddle->intensity); - TEST_ASSERT_EQUAL_INT(0, actuatorRing->intensity); - TEST_ASSERT_EQUAL_INT(0, actuatorLittle->intensity); - TEST_ASSERT_EQUAL_INT(0, actuatorWrist->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(4095.0F / 4095.0F, actuatorThumb->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuatorIndex->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuatorMiddle->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuatorRing->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuatorLittle->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(0, actuatorWrist->intensity, 2); Decoder::applyPlain(body, { 0x10, 0x20, 0x30, 0x40, 0x50, 0x60 }, bhLayout, Effect::Vibro); - TEST_ASSERT_EQUAL_INT(655, actuatorThumb->intensity); - TEST_ASSERT_EQUAL_INT(1310, actuatorIndex->intensity); - TEST_ASSERT_EQUAL_INT(1965, actuatorMiddle->intensity); - TEST_ASSERT_EQUAL_INT(2620, actuatorRing->intensity); - TEST_ASSERT_EQUAL_INT(3276, actuatorLittle->intensity); - TEST_ASSERT_EQUAL_INT(3931, actuatorWrist->intensity); + ASSERT_EQUAL_FLOAT_ROUNDED(655.0F / 4095.0F, actuatorThumb->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1310.0F / 4095.0F, actuatorIndex->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(1965.0F / 4095.0F, actuatorMiddle->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(2620.0F / 4095.0F, actuatorRing->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3276.0F / 4095.0F, actuatorLittle->intensity, 2); + ASSERT_EQUAL_FLOAT_ROUNDED(3931.0F / 4095.0F, actuatorWrist->intensity, 2); } int process(void) diff --git a/test/test_body_gestures/main.cpp b/test/test_body_gestures/main.cpp new file mode 100644 index 00000000..e63fb03f --- /dev/null +++ b/test/test_body_gestures/main.cpp @@ -0,0 +1,107 @@ +#include +#include +#include + +using namespace SenseShift::Input; +using namespace SenseShift::Body::Hands::Input; + +void test_gesture_trigger(void) +{ + float threshold = 0.5f; + auto* index = new FloatSensor(); + + auto* gesture = new TriggerGesture(index, threshold, true); + gesture->init(); + + index->publishState(0.4f); + TEST_ASSERT_FALSE(gesture->getValue()); + + index->publishState(0.6f); + TEST_ASSERT_TRUE(gesture->getValue()); +} + +void test_gesture_grab(void) +{ + float threshold = 0.5f; + + auto* index = new FloatSensor(); + auto* middle = new FloatSensor(); + auto* ring = new FloatSensor(); + auto* pinky = new FloatSensor(); + + auto* gesture = new GrabGesture( + { + .index = index, + .middle = middle, + .ring = ring, + .pinky = pinky, + }, + threshold, + true + ); + gesture->init(); + + TEST_ASSERT_FALSE(gesture->getValue()); + + index->publishState(0.6); + TEST_ASSERT_FALSE(gesture->getValue()); + + middle->publishState(0.6); + TEST_ASSERT_FALSE(gesture->getValue()); + + ring->publishState(0.6); + TEST_ASSERT_FALSE(gesture->getValue()); + + pinky->publishState(0.6); + TEST_ASSERT_TRUE(gesture->getValue()); +} + +void test_gesture_pinch(void) +{ + float threshold = 0.5f; + + auto* thumb = new FloatSensor(); + auto* index = new FloatSensor(); + + auto* gesture = new PinchGesture({ .thumb = thumb, .index = index }, threshold, true); + gesture->init(); + + TEST_ASSERT_FALSE(gesture->getValue()); + + thumb->publishState(0.6f); + TEST_ASSERT_FALSE(gesture->getValue()); + + index->publishState(0.6f); + TEST_ASSERT_TRUE(gesture->getValue()); +} + +int process(void) +{ + UNITY_BEGIN(); + + RUN_TEST(test_gesture_grab); + RUN_TEST(test_gesture_trigger); + RUN_TEST(test_gesture_pinch); + + return UNITY_END(); +} + +#ifdef ARDUINO + +#include + +void setup(void) +{ + process(); +} + +void loop(void) {} + +#else + +int main(int argc, char** argv) +{ + return process(); +} + +#endif diff --git a/test/test_core_helpers/main.cpp b/test/test_core_helpers/main.cpp new file mode 100644 index 00000000..5dbfd888 --- /dev/null +++ b/test/test_core_helpers/main.cpp @@ -0,0 +1,132 @@ +#include +#include + +#include +#include + +using namespace SenseShift; + +void test_lerp_uint8(void) +{ + TEST_ASSERT_EQUAL_UINT16(0, lerp(0.0f, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(127, lerp(0.5f, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(255, lerp(1.0f, 0, 255)); + + TEST_ASSERT_EQUAL_UINT16(0, lerp(0.0f, 0, 0)); +} + +void test_lerp_float(void) +{ + TEST_ASSERT_EQUAL_FLOAT(-1.0f, lerp(0.0f, -1.0f, 1.0f)); + TEST_ASSERT_EQUAL_FLOAT(0.0f, lerp(0.5f, -1.0f, 1.0f)); + TEST_ASSERT_EQUAL_FLOAT(1.0f, lerp(1.0f, -1.0f, 1.0f)); + + TEST_ASSERT_EQUAL_FLOAT(0.0f, lerp(0.0f, 0.0f, 0.0f)); +} + +void test_remap_uint16(void) +{ + TEST_ASSERT_EQUAL_UINT16(0, remap(0, 0, 4095, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(127, remap(2047, 0, 4095, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(255, remap(4095, 0, 4095, 0, 255)); + + TEST_ASSERT_EQUAL_UINT16(127, remap(0, 0, 4095, 127, 255)); + TEST_ASSERT_EQUAL_UINT16(190, remap(2047, 0, 4095, 127, 255)); + TEST_ASSERT_EQUAL_UINT16(255, remap(4095, 0, 4095, 127, 255)); + + TEST_ASSERT_EQUAL_UINT16(0, remap(2048, 2048, 4095, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(127, remap(3071, 2048, 4095, 0, 255)); + TEST_ASSERT_EQUAL_UINT16(255, remap(4095, 2048, 4095, 0, 255)); + + TEST_ASSERT_EQUAL_UINT16(2047, remap(343, 343, 343, 0, 4095)); +} + +void test_remap_float(void) +{ + TEST_ASSERT_EQUAL_FLOAT(0.0f, remap(0.0f, 0.0f, 1.0f, 0.0f, 255.0f)); + TEST_ASSERT_EQUAL_FLOAT(127.5f, remap(0.5f, 0.0f, 1.0f, 0.0f, 255.0f)); + TEST_ASSERT_EQUAL_FLOAT(255.0f, remap(1.0f, 0.0f, 1.0f, 0.0f, 255.0f)); + + TEST_ASSERT_EQUAL_FLOAT(0.5f, remap(0.0f, -1.0f, 1.0f, 0.0f, 1.0f)); + + TEST_ASSERT_EQUAL_FLOAT(0.125f, remap(0.2f, 0.1f, 0.9f, 0.f, 1.f)); +} + +void test_remap_simple_uint16(void) +{ + TEST_ASSERT_EQUAL_UINT16(0, remap_simple(0, 4095, 255)); + TEST_ASSERT_EQUAL_UINT16(127, remap_simple(2047, 4095, 255)); + TEST_ASSERT_EQUAL_UINT16(255, remap_simple(4095, 4095, 255)); +} + +void test_remap_simple_float(void) +{ + TEST_ASSERT_EQUAL_FLOAT(0.0f, remap_simple(0.0f, 1.0f, 255.0f)); + TEST_ASSERT_EQUAL_FLOAT(127.5f, remap_simple(0.5f, 1.0f, 255.0f)); + TEST_ASSERT_EQUAL_FLOAT(255.0f, remap_simple(1.0f, 1.0f, 255.0f)); +} + +// Wtf? https://stackoverflow.com/questions/4295890 +#define COMMAE , + +void test_lookup_table_interpolate_float(void) +{ + const std::map table = { + { 0.0f, 13.0f }, + { 0.5f, 16.0f }, + { 0.6f, 17.0f }, + { 1.0f, 18.0f }, + }; + + TEST_ASSERT_EQUAL_FLOAT(13.0f, lookup_table_interpolate(table, 0.0f)); + TEST_ASSERT_EQUAL_FLOAT(13.6f, lookup_table_interpolate(table, 0.1f)); + TEST_ASSERT_EQUAL_FLOAT(14.2f, lookup_table_interpolate(table, 0.2f)); + TEST_ASSERT_EQUAL_FLOAT(14.5f, lookup_table_interpolate(table, 0.25f)); + TEST_ASSERT_EQUAL_FLOAT(14.8f, lookup_table_interpolate(table, 0.3f)); + TEST_ASSERT_EQUAL_FLOAT(15.4f, lookup_table_interpolate(table, 0.4f)); + TEST_ASSERT_EQUAL_FLOAT(16.0f, lookup_table_interpolate(table, 0.5f)); + TEST_ASSERT_EQUAL_FLOAT(16.5f, lookup_table_interpolate(table, 0.55f)); + TEST_ASSERT_EQUAL_FLOAT(17.0f, lookup_table_interpolate(table, 0.6f)); + TEST_ASSERT_EQUAL_FLOAT(17.25f, lookup_table_interpolate(table, 0.7f)); + TEST_ASSERT_EQUAL_FLOAT(17.5f, lookup_table_interpolate(table, 0.8f)); + TEST_ASSERT_EQUAL_FLOAT(17.75f, lookup_table_interpolate(table, 0.9f)); + TEST_ASSERT_EQUAL_FLOAT(18.0f, lookup_table_interpolate(table, 1.0f)); +} + +int process(void) +{ + UNITY_BEGIN(); + + RUN_TEST(test_lerp_uint8); + RUN_TEST(test_lerp_float); + + RUN_TEST(test_remap_uint16); + RUN_TEST(test_remap_float); + + RUN_TEST(test_remap_simple_uint16); + RUN_TEST(test_remap_simple_float); + + RUN_TEST(test_lookup_table_interpolate_float); + + return UNITY_END(); +} + +#ifdef ARDUINO + +#include + +void setup(void) +{ + process(); +} + +void loop(void) {} + +#else + +int main(int argc, char** argv) +{ + return process(); +} + +#endif diff --git a/test/test_haptics_body/main.cpp b/test/test_haptics_body/main.cpp index 7f00d150..96b39736 100644 --- a/test/test_haptics_body/main.cpp +++ b/test/test_haptics_body/main.cpp @@ -4,27 +4,27 @@ using namespace SenseShift::Body::Haptics; using namespace SenseShift::Output; -class TestActuator : public IActuator { +class TestActuator : public IOutput { public: bool isSetup = false; - uint16_t intensity = 0; + float intensity = 0; - TestActuator() : IActuator() {} - void setup() override { this->isSetup = true; } - void writeOutput(uint16_t intensity) override { this->intensity = intensity; } + TestActuator() : IFloatOutput() {} + void init() override { this->isSetup = true; } + void writeState(float newIntensity) override { this->intensity = newIntensity; } }; void test_it_sets_up_planes(void) { - auto body = new HapticBody(); + auto body = new FloatBody(); - VibroPlane::ActuatorMap outputs = { + FloatPlane::ActuatorMap outputs = { { { 0, 0 }, new TestActuator() }, { { 0, 1 }, new TestActuator() }, { { 1, 0 }, new TestActuator() }, { { 1, 1 }, new TestActuator() }, }; - auto plane = new VibroPlane(outputs); + auto plane = new FloatPlane(outputs); body->addTarget(Target::ChestFront, plane); body->setup(); @@ -34,14 +34,14 @@ void test_it_sets_up_planes(void) } } -void test_it_handles_effect__vibro(void) +void test_it_handles_effect(void) { auto actuator1 = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - auto body = new HapticBody(); + auto body = new FloatBody(); - auto plane = new VibroPlane({ + auto plane = new FloatPlane({ { { 0, 0 }, actuator1 }, { { 0, 1 }, actuator2 }, { { 1, 0 }, actuator3 }, @@ -50,35 +50,17 @@ void test_it_handles_effect__vibro(void) body->addTarget(Target::ChestFront, plane); - body->effect({ - .effect = Effect::Vibro, - .target = Target::ChestFront, - .position = { 0, 0 }, - .data = (VibroEffectData) 64, - }); - body->effect({ - .effect = Effect::Vibro, - .target = Target::ChestFront, - .position = { 0, 1 }, - .data = (VibroEffectData) 128, - }); - body->effect({ - .effect = Effect::Vibro, - .target = Target::ChestFront, - .position = { 1, 0 }, - .data = (VibroEffectData) 192, - }); - body->effect({ - .effect = Effect::Vibro, - .target = Target::ChestFront, - .position = { 1, 1 }, - .data = (VibroEffectData) 255, - }); + TEST_ASSERT_TRUE(body->getTarget(Target::ChestFront).has_value()); + + body->effect(Target::ChestFront, { 0, 0 }, 0.25F); + body->effect(Target::ChestFront, { 0, 1 }, 0.5F); + body->effect(Target::ChestFront, { 1, 0 }, 0.75F); + body->effect(Target::ChestFront, { 1, 1 }, 1.0F); - TEST_ASSERT_EQUAL(64, actuator1->intensity); - TEST_ASSERT_EQUAL(128, actuator2->intensity); - TEST_ASSERT_EQUAL(192, actuator3->intensity); - TEST_ASSERT_EQUAL(255, actuator4->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.25F, actuator1->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.5F, actuator2->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.75F, actuator3->intensity); + TEST_ASSERT_EQUAL_FLOAT(1.0F, actuator4->intensity); } int process(void) @@ -86,7 +68,7 @@ int process(void) UNITY_BEGIN(); RUN_TEST(test_it_sets_up_planes); - RUN_TEST(test_it_handles_effect__vibro); + RUN_TEST(test_it_handles_effect); return UNITY_END(); } diff --git a/test/test_haptics_plane/main.cpp b/test/test_haptics_plane/main.cpp index 7c8890cc..a9c329a1 100644 --- a/test/test_haptics_plane/main.cpp +++ b/test/test_haptics_plane/main.cpp @@ -4,26 +4,26 @@ using namespace SenseShift::Body::Haptics; using namespace SenseShift::Output; -class TestActuator : public IActuator { +class TestActuator : public IOutput { public: bool isSetup = false; - uint16_t intensity = 0; + float intensity = 0; - TestActuator() : IActuator() {} - void setup() override { this->isSetup = true; } - void writeOutput(uint16_t intensity) override { this->intensity = intensity; } + TestActuator() : IFloatOutput() {} + void init() override { this->isSetup = true; } + void writeState(float newIntensity) override { this->intensity = newIntensity; } }; void test_it_sets_up_actuators(void) { - VibroPlane::ActuatorMap outputs = { + FloatPlane::ActuatorMap outputs = { { { 0, 0 }, new TestActuator() }, { { 0, 1 }, new TestActuator() }, { { 1, 0 }, new TestActuator() }, { { 1, 1 }, new TestActuator() }, }; - auto plane = new VibroPlane(outputs); + auto plane = new FloatPlane(outputs); plane->setup(); TEST_ASSERT_EQUAL(outputs.size(), plane->getAvailablePoints()->size()); @@ -39,24 +39,24 @@ void test_it_writes_to_correct_output(void) auto actuator = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - VibroPlane::ActuatorMap outputs = { + FloatPlane::ActuatorMap outputs = { { { 0, 0 }, actuator }, { { 0, 1 }, actuator2 }, { { 1, 0 }, actuator3 }, { { 1, 1 }, actuator4 }, }; - auto plane = new VibroPlane(outputs); + auto plane = new FloatPlane(outputs); - plane->effect({ 0, 0 }, 64); - plane->effect({ 0, 1 }, 128); - plane->effect({ 1, 0 }, 192); - plane->effect({ 1, 1 }, 255); + plane->effect({ 0, 0 }, 0.25F); + plane->effect({ 0, 1 }, 0.5F); + plane->effect({ 1, 0 }, 0.75F); + plane->effect({ 1, 1 }, 1.0F); - TEST_ASSERT_EQUAL_UINT8(64, actuator->intensity); - TEST_ASSERT_EQUAL_UINT8(128, actuator2->intensity); - TEST_ASSERT_EQUAL_UINT8(192, actuator3->intensity); - TEST_ASSERT_EQUAL_UINT8(255, actuator4->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.25F, actuator->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.5F, actuator2->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.75F, actuator3->intensity); + TEST_ASSERT_EQUAL_FLOAT(1.0F, actuator4->intensity); } void test_it_updates_state(void) @@ -64,31 +64,31 @@ void test_it_updates_state(void) auto actuator = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - VibroPlane::ActuatorMap outputs = { + FloatPlane::ActuatorMap outputs = { { { 0, 0 }, actuator }, { { 0, 1 }, actuator2 }, { { 1, 0 }, actuator3 }, { { 1, 1 }, actuator4 }, }; - auto plane = new VibroPlane(outputs); + auto plane = new FloatPlane(outputs); - plane->effect({ 0, 0 }, 64); - plane->effect({ 0, 1 }, 128); - plane->effect({ 1, 0 }, 192); - plane->effect({ 1, 1 }, 255); + plane->effect({ 0, 0 }, 0.25F); + plane->effect({ 0, 1 }, 0.5F); + plane->effect({ 1, 0 }, 0.75F); + plane->effect({ 1, 1 }, 1.0F); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 0, 0 }) > 0); - TEST_ASSERT_EQUAL_UINT8(64, plane->getActuatorStates()->at({ 0, 0 })); + TEST_ASSERT_EQUAL_FLOAT(0.25F, plane->getActuatorStates()->at({ 0, 0 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 0, 1 }) > 0); - TEST_ASSERT_EQUAL_UINT8(128, plane->getActuatorStates()->at({ 0, 1 })); + TEST_ASSERT_EQUAL_FLOAT(0.5F, plane->getActuatorStates()->at({ 0, 1 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 1, 0 }) > 0); - TEST_ASSERT_EQUAL_UINT8(192, plane->getActuatorStates()->at({ 1, 0 })); + TEST_ASSERT_EQUAL_FLOAT(0.75F, plane->getActuatorStates()->at({ 1, 0 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 1, 1 }) > 0); - TEST_ASSERT_EQUAL_UINT8(255, plane->getActuatorStates()->at({ 1, 1 })); + TEST_ASSERT_EQUAL_FLOAT(1.0F, plane->getActuatorStates()->at({ 1, 1 })); } void test_closest_it_writes_to_correct_if_exact(void) @@ -96,24 +96,24 @@ void test_closest_it_writes_to_correct_if_exact(void) auto actuator = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - VibroPlane_Closest::ActuatorMap outputs = { + FloatPlane_Closest::ActuatorMap outputs = { { { 0, 0 }, actuator }, { { 0, 1 }, actuator2 }, { { 1, 0 }, actuator3 }, { { 1, 1 }, actuator4 }, }; - auto plane = new VibroPlane_Closest(outputs); + auto plane = new FloatPlane_Closest(outputs); - plane->effect({ 0, 0 }, 1); - plane->effect({ 0, 1 }, 2); - plane->effect({ 1, 0 }, 3); - plane->effect({ 1, 1 }, 4); + plane->effect({ 0, 0 }, 0.25F); + plane->effect({ 0, 1 }, 0.5F); + plane->effect({ 1, 0 }, 0.75F); + plane->effect({ 1, 1 }, 1.0F); - TEST_ASSERT_EQUAL(1, actuator->intensity); - TEST_ASSERT_EQUAL(2, actuator2->intensity); - TEST_ASSERT_EQUAL(3, actuator3->intensity); - TEST_ASSERT_EQUAL(4, actuator4->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.25F, actuator->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.5F, actuator2->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.75F, actuator3->intensity); + TEST_ASSERT_EQUAL_FLOAT(1.0F, actuator4->intensity); } void test_closest_it_correctly_finds_closest(void) @@ -121,22 +121,22 @@ void test_closest_it_correctly_finds_closest(void) auto actuator = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - VibroPlane_Closest::ActuatorMap outputs = { + FloatPlane_Closest::ActuatorMap outputs = { { { 0, 0 }, actuator }, { { 0, 64 }, actuator2 }, { { 64, 0 }, actuator3 }, { { 64, 64 }, actuator4 }, }; - auto plane = new VibroPlane_Closest(outputs); + auto plane = new FloatPlane_Closest(outputs); - plane->effect({ 16, 16 }, 16); - plane->effect({ 65, 65 }, 65); + plane->effect({ 16, 16 }, 0.25F); + plane->effect({ 65, 65 }, 0.5F); - TEST_ASSERT_EQUAL(16, actuator->intensity); - TEST_ASSERT_EQUAL(0, actuator2->intensity); - TEST_ASSERT_EQUAL(0, actuator3->intensity); - TEST_ASSERT_EQUAL(65, actuator4->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.25F, actuator->intensity); + TEST_ASSERT_EQUAL_FLOAT(0, actuator2->intensity); + TEST_ASSERT_EQUAL_FLOAT(0, actuator3->intensity); + TEST_ASSERT_EQUAL_FLOAT(0.5F, actuator4->intensity); } void test_closest_it_updates_state(void) @@ -144,29 +144,29 @@ void test_closest_it_updates_state(void) auto actuator = new TestActuator(), actuator2 = new TestActuator(), actuator3 = new TestActuator(), actuator4 = new TestActuator(); - VibroPlane_Closest::ActuatorMap outputs = { + FloatPlane_Closest::ActuatorMap outputs = { { { 0, 0 }, actuator }, { { 0, 64 }, actuator2 }, { { 64, 0 }, actuator3 }, { { 64, 64 }, actuator4 }, }; - auto plane = new VibroPlane_Closest(outputs); + auto plane = new FloatPlane_Closest(outputs); - plane->effect({ 16, 16 }, 16); - plane->effect({ 65, 65 }, 65); + plane->effect({ 16, 16 }, 0.25F); + plane->effect({ 65, 65 }, 0.5F); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 0, 0 }) > 0); - TEST_ASSERT_EQUAL(16, plane->getActuatorStates()->at({ 0, 0 })); + TEST_ASSERT_EQUAL_FLOAT(0.25F, plane->getActuatorStates()->at({ 0, 0 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 0, 64 }) > 0); - TEST_ASSERT_EQUAL(0, plane->getActuatorStates()->at({ 0, 64 })); + TEST_ASSERT_EQUAL_FLOAT(0, plane->getActuatorStates()->at({ 0, 64 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 64, 0 }) > 0); - TEST_ASSERT_EQUAL(0, plane->getActuatorStates()->at({ 64, 0 })); + TEST_ASSERT_EQUAL_FLOAT(0, plane->getActuatorStates()->at({ 64, 0 })); TEST_ASSERT_TRUE(plane->getActuatorStates()->count({ 64, 64 }) > 0); - TEST_ASSERT_EQUAL(65, plane->getActuatorStates()->at({ 64, 64 })); + TEST_ASSERT_EQUAL_FLOAT(0.5F, plane->getActuatorStates()->at({ 64, 64 })); } void test_plain_mapper_margin_map_points(void) diff --git a/test/test_util_calibration/main.cpp b/test/test_io_calibration/main.cpp similarity index 60% rename from test/test_util_calibration/main.cpp rename to test/test_io_calibration/main.cpp index 1fa42a23..f554fab5 100644 --- a/test/test_util_calibration/main.cpp +++ b/test/test_io_calibration/main.cpp @@ -1,63 +1,67 @@ -#include +#include "senseshift/input/calibration.hpp" #include -using namespace SenseShift::Calibration; +using namespace SenseShift::Input::Calibration; void test_minmax_calibrator(void) { - auto calibrator = new MinMaxCalibrator(); + const auto calibrator = new MinMaxCalibrator(); // test uncalibrated neutral value - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(0)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(10)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(2048)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(4086)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(4096)); - - calibrator->update(10); - calibrator->update(4086); - - TEST_ASSERT_EQUAL_UINT16(0, calibrator->calibrate(0)); - TEST_ASSERT_EQUAL_UINT16(0, calibrator->calibrate(10)); - TEST_ASSERT_EQUAL_UINT16(118, calibrator->calibrate(128)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(2048)); - TEST_ASSERT_EQUAL_UINT16(3977, calibrator->calibrate(3968)); - TEST_ASSERT_EQUAL_UINT16(4096, calibrator->calibrate(4086)); - TEST_ASSERT_EQUAL_UINT16(4096, calibrator->calibrate(4096)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(0)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(10)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(2048)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(4086)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(4096)); + + calibrator->update(0.1F); + calibrator->update(0.9F); + + TEST_ASSERT_EQUAL_FLOAT(0.0F, calibrator->calibrate(0.0F)); + TEST_ASSERT_EQUAL_FLOAT(0.0F, calibrator->calibrate(0.1F)); + TEST_ASSERT_EQUAL_FLOAT(0.125F, calibrator->calibrate(0.2F)); + TEST_ASSERT_EQUAL_FLOAT(0.25F, calibrator->calibrate(0.3F)); + TEST_ASSERT_EQUAL_FLOAT(0.375F, calibrator->calibrate(0.4F)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(0.5F)); + TEST_ASSERT_EQUAL_FLOAT(0.625F, calibrator->calibrate(0.6F)); + TEST_ASSERT_EQUAL_FLOAT(0.75F, calibrator->calibrate(0.7F)); + TEST_ASSERT_EQUAL_FLOAT(0.875F, calibrator->calibrate(0.8F)); + TEST_ASSERT_EQUAL_FLOAT(1.0F, calibrator->calibrate(0.9F)); + TEST_ASSERT_EQUAL_FLOAT(1.0F, calibrator->calibrate(1.0F)); calibrator->reset(); // test uncalibrated neutral value (again) - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(0)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(10)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(2048)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(4086)); - TEST_ASSERT_EQUAL_UINT16(2048, calibrator->calibrate(4096)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(0)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(10)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(2048)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(4086)); + TEST_ASSERT_EQUAL_FLOAT(0.5F, calibrator->calibrate(4096)); } void test_center_point_deviation_calibrator(void) { - CenterPointDeviationCalibrator calibrator; + auto calibrator = new CenterPointDeviationCalibrator(100, 10, 0, 255); // Test reset function - calibrator.reset(); + calibrator->reset(); // Test update function - calibrator.update(50); - calibrator.update(75); - calibrator.update(25); + calibrator->update(50); + calibrator->update(75); + calibrator->update(25); // Test calibrate function - TEST_ASSERT_EQUAL_INT(255, calibrator.calibrate(100)); - TEST_ASSERT_EQUAL_INT(191, calibrator.calibrate(75)); - TEST_ASSERT_EQUAL_INT(63, calibrator.calibrate(50)); - TEST_ASSERT_EQUAL_INT(0, calibrator.calibrate(25)); - TEST_ASSERT_EQUAL_INT(0, calibrator.calibrate(0)); + TEST_ASSERT_EQUAL_INT(255, calibrator->calibrate(100)); + TEST_ASSERT_EQUAL_INT(191, calibrator->calibrate(75)); + TEST_ASSERT_EQUAL_INT(63, calibrator->calibrate(50)); + TEST_ASSERT_EQUAL_INT(0, calibrator->calibrate(25)); + TEST_ASSERT_EQUAL_INT(0, calibrator->calibrate(0)); } void test_fixed_center_point_deviation_calibrator(void) { - auto calibrator = new FixedCenterPointDeviationCalibrator(); + auto calibrator = new FixedCenterPointDeviationCalibrator(512, 64, 0, 4096); // below deviation TEST_ASSERT_EQUAL_UINT16(0, calibrator->calibrate(0)); diff --git a/test/test_io_filter/main.cpp b/test/test_io_filter/main.cpp new file mode 100644 index 00000000..f5002158 --- /dev/null +++ b/test/test_io_filter/main.cpp @@ -0,0 +1,172 @@ +#include +#include +#include + +#define ASSERT_EQUAL_FLOAT_ROUNDED(expected, actual, precision) \ + TEST_ASSERT_EQUAL_FLOAT( \ + std::round(expected* std::pow(10, precision)) / std::pow(10, precision), \ + std::round(actual * std::pow(10, precision)) / std::pow(10, precision) \ + ) + +using namespace SenseShift::Input; +using namespace SenseShift::Input::Filter; + +void test_add_filter(void) +{ + IFilter* filter = new AddFilter(5.0f); + + TEST_ASSERT_EQUAL_FLOAT(10.0f, filter->filter(nullptr, 5.0f)); + TEST_ASSERT_EQUAL_FLOAT(11.0f, filter->filter(nullptr, 6.0f)); + TEST_ASSERT_EQUAL_FLOAT(12.0f, filter->filter(nullptr, 7.0f)); +} + +void test_subtract_filter(void) +{ + IFilter* filter = new SubtractFilter(5.0f); + + TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 5.0f)); + TEST_ASSERT_EQUAL_FLOAT(-1.0f, filter->filter(nullptr, 4.0f)); + TEST_ASSERT_EQUAL_FLOAT(-2.0f, filter->filter(nullptr, 3.0f)); +} + +void test_multiply_filter(void) +{ + IFilter* filter = new MultiplyFilter(5.0f); + + TEST_ASSERT_EQUAL_FLOAT(25.0f, filter->filter(nullptr, 5.0f)); + TEST_ASSERT_EQUAL_FLOAT(30.0f, filter->filter(nullptr, 6.0f)); + TEST_ASSERT_EQUAL_FLOAT(35.0f, filter->filter(nullptr, 7.0f)); +} + +void test_voltage_divider_filter(void) +{ + IFilter* filter = new VoltageDividerFilter(27000.0F, 100000.0F); + + TEST_ASSERT_EQUAL_FLOAT(0.0F, filter->filter(nullptr, 0.0F)); + TEST_ASSERT_EQUAL_FLOAT(3.429F, filter->filter(nullptr, 2.7F)); + TEST_ASSERT_EQUAL_FLOAT(3.81F, filter->filter(nullptr, 3.0F)); + TEST_ASSERT_EQUAL_FLOAT(4.191F, filter->filter(nullptr, 3.3F)); +} + +void test_clamp_filter(void) +{ + IFilter* filter = new ClampFilter(0.0f, 1.0f); + + TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, -1.0f)); + TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 0.0f)); + TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.5f)); + TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f)); + TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 2.0f)); +} + +void test_lambda_filter(void) +{ + IFilter* filter = new LambdaFilter([](float value) { + return value * 42.F; // Cause after all, 42 is the answer to everything. + }); + + TEST_ASSERT_EQUAL_FLOAT(42.0f, filter->filter(nullptr, 1.0f)); + TEST_ASSERT_EQUAL_FLOAT(84.0f, filter->filter(nullptr, 2.0f)); + TEST_ASSERT_EQUAL_FLOAT(126.0f, filter->filter(nullptr, 3.0f)); +} + +void test_sliding_window_moving_average_filter(void) +{ + IFilter* filter = new SlidingWindowMovingAverageFilter(3); + + TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f)); // 1 / 1 = 1 + TEST_ASSERT_EQUAL_FLOAT(1.5f, filter->filter(nullptr, 2.0f)); // (1 + 2) / 2 = 1.5 + TEST_ASSERT_EQUAL_FLOAT(2.0f, filter->filter(nullptr, 3.0f)); // (1 + 2 + 3) / 3 = 2 + TEST_ASSERT_EQUAL_FLOAT(3.0f, filter->filter(nullptr, 4.0f)); // (2 + 3 + 4) / 3 = 3 + TEST_ASSERT_EQUAL_FLOAT(4.0f, filter->filter(nullptr, 5.0f)); // (3 + 4 + 5) / 3 = 4 + TEST_ASSERT_EQUAL_FLOAT(5.0f, filter->filter(nullptr, 6.0f)); // (4 + 5 + 6) / 3 = 5 + TEST_ASSERT_EQUAL_FLOAT(6.0f, filter->filter(nullptr, 7.0f)); // (5 + 6 + 7) / 3 = 6 + TEST_ASSERT_EQUAL_FLOAT(7.0f, filter->filter(nullptr, 8.0f)); // (6 + 7 + 8) / 3 = 7 + TEST_ASSERT_EQUAL_FLOAT(8.0f, filter->filter(nullptr, 9.0f)); // (7 + 8 + 9) / 3 = 8 + TEST_ASSERT_EQUAL_FLOAT(9.0f, filter->filter(nullptr, 10.0f)); // (8 + 9 + 10) / 3 = 9 + TEST_ASSERT_EQUAL_FLOAT(10.0f, filter->filter(nullptr, 11.0f)); // (9 + 10 + 11) / 3 = 10 + + ASSERT_EQUAL_FLOAT_ROUNDED(10.67f, filter->filter(nullptr, 11.0f), 2); // (10 + 11 + 11) / 3 = 10.67 + TEST_ASSERT_EQUAL_FLOAT(11.0f, filter->filter(nullptr, 11.0f)); // (11 + 11 + 11) / 3 = 11 + ASSERT_EQUAL_FLOAT_ROUNDED(11.33f, filter->filter(nullptr, 12.0f), 2); // (11 + 11 + 12) / 3 = 11.33 +} + +void test_exponential_moving_average_filter(void) +{ + IFilter* filter = new ExponentialMovingAverageFilter(0.5f); + ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0 + ASSERT_EQUAL_FLOAT_ROUNDED(1.5f, filter->filter(nullptr, 2.0f), 2); // (0.5 * 1.0) + (0.5 * 2.0) = 1.5 + ASSERT_EQUAL_FLOAT_ROUNDED(2.25f, filter->filter(nullptr, 3.0f), 2); // (0.5 * 1.5) + (0.5 * 3.0) = 2.25 + ASSERT_EQUAL_FLOAT_ROUNDED(3.125f, filter->filter(nullptr, 4.0f), 2); // (0.5 * 2.25) + (0.5 * 4.0) = 3.125 + ASSERT_EQUAL_FLOAT_ROUNDED(4.0625f, filter->filter(nullptr, 5.0f), 2); // (0.5 * 3.125) + (0.5 * 5.0) = 4.0625 + + filter = new ExponentialMovingAverageFilter(0.1f); + ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0 + ASSERT_EQUAL_FLOAT_ROUNDED(1.1f, filter->filter(nullptr, 2.0f), 2); // (0.1 * 2.0) + (0.9 * 1.0) = 1.1 + ASSERT_EQUAL_FLOAT_ROUNDED(1.29f, filter->filter(nullptr, 3.0f), 2); // (0.1 * 3.0) + (0.9 * 1.1) = 1.29 + ASSERT_EQUAL_FLOAT_ROUNDED(1.561f, filter->filter(nullptr, 4.0f), 2); // (0.1 * 4.0) + (0.9 * 1.29) = 1.561 + ASSERT_EQUAL_FLOAT_ROUNDED(1.9049f, filter->filter(nullptr, 5.0f), 2); // (0.1 * 5.0) + (0.9 * 1.561) = 1.9049 + + filter = new ExponentialMovingAverageFilter(0.9f); + ASSERT_EQUAL_FLOAT_ROUNDED(1.0f, filter->filter(nullptr, 1.0f), 2); // 1.0 + ASSERT_EQUAL_FLOAT_ROUNDED(1.9f, filter->filter(nullptr, 2.0f), 2); // (0.9 * 2.0) + (0.1 * 1.0) = 1.9 + ASSERT_EQUAL_FLOAT_ROUNDED(2.89f, filter->filter(nullptr, 3.0f), 2); // (0.9 * 3.0) + (0.1 * 1.9) = 2.89 + ASSERT_EQUAL_FLOAT_ROUNDED(3.889f, filter->filter(nullptr, 4.0f), 2); // (0.9 * 4.0) + (0.1 * 2.89) = 3.889 + ASSERT_EQUAL_FLOAT_ROUNDED(4.8889f, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 3.889) = 4.8889 + ASSERT_EQUAL_FLOAT_ROUNDED(4.98889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.8889) = 4.98889 + ASSERT_EQUAL_FLOAT_ROUNDED(4.99889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.98889) = 4.99889 + ASSERT_EQUAL_FLOAT_ROUNDED(4.999889, filter->filter(nullptr, 5.0f), 2); // (0.9 * 5.0) + (0.1 * 4.99889) = 4.999889 +} + +void test_center_deadzone_filter(void) +{ + IFilter* filter = new CenterDeadzoneFilter(0.1f); + + TEST_ASSERT_EQUAL_FLOAT(0.0f, filter->filter(nullptr, 0.0f)); + TEST_ASSERT_EQUAL_FLOAT(0.1f, filter->filter(nullptr, 0.1f)); + + // Inside the deadzone + TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.43f)); + TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.5f)); + TEST_ASSERT_EQUAL_FLOAT(0.5f, filter->filter(nullptr, 0.57f)); + + TEST_ASSERT_EQUAL_FLOAT(0.9f, filter->filter(nullptr, 0.9f)); + TEST_ASSERT_EQUAL_FLOAT(1.0f, filter->filter(nullptr, 1.0f)); +} + +int process(void) +{ + UNITY_BEGIN(); + + RUN_TEST(test_add_filter); + RUN_TEST(test_subtract_filter); + RUN_TEST(test_multiply_filter); + RUN_TEST(test_voltage_divider_filter); + RUN_TEST(test_clamp_filter); + RUN_TEST(test_lambda_filter); + RUN_TEST(test_sliding_window_moving_average_filter); + RUN_TEST(test_exponential_moving_average_filter); + RUN_TEST(test_center_deadzone_filter); + + return UNITY_END(); +} + +#ifdef ARDUINO + +#include + +void setup(void) +{ + process(); +} + +void loop(void) {} + +#else + +int main(int argc, char** argv) +{ + return process(); +} + +#endif \ No newline at end of file diff --git a/test/test_io_sensor/main.cpp b/test/test_io_sensor/main.cpp index 74b4ace3..188cbeaf 100644 --- a/test/test_io_sensor/main.cpp +++ b/test/test_io_sensor/main.cpp @@ -1,23 +1,33 @@ +#include #include #include using namespace SenseShift::Input; -using namespace SenseShift::Calibration; -class TestAnalogSensor : public ISimpleSensor { +class TestAnalogCountingSensor : public ISimpleSensor { public: int count = 0; int setupCounter = 0; void init() override { this->setupCounter++; }; - int getValue() override { return ++this->count; }; + auto getValue() -> int override { return ++this->count; }; +}; + +class TestAnalogSensor : public ISimpleSensor { + public: + int value = 0; + int setupCounter = 0; + + void init() override { this->setupCounter++; }; + + auto getValue() -> int override { return this->value; }; }; void test_memoized_sensor(void) { - auto inner = new TestAnalogSensor(); - auto sensor = new MemoizedSensor(inner); + auto inner = new TestAnalogCountingSensor(); + auto sensor = new SimpleSensorDecorator(inner); TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); sensor->init(); @@ -32,80 +42,99 @@ void test_memoized_sensor(void) TEST_ASSERT_EQUAL_INT(1, sensor->getValue()); } -class DummyCalibrator : public ICalibrator { +class DummyCalibrator : public ::SenseShift::Input::Calibration::ICalibrator { public: uint8_t resetCounter = 0; - int calibrated = 0; + float calibrated = 0.0f; void reset() override { this->resetCounter++; - this->calibrated = 0; + this->calibrated = 0.0f; }; - void update(int input) override { this->calibrated = input; }; - int calibrate(int input) const override { return calibrated; }; + void update(float input) override { this->calibrated = input; }; + float calibrate(float input) const override { return calibrated; }; }; void test_calibrated_sensor(void) { - auto inner = new TestAnalogSensor(); + auto inner = new FloatSensor(); auto calibrator = new DummyCalibrator(); - auto sensor = new CalibratedSimpleSensor(inner, calibrator); - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); + auto sensor = new SimpleSensorDecorator(inner); + sensor->setCalibrator(calibrator); - calibrator->update(-1); - TEST_ASSERT_EQUAL_INT(-1, sensor->getValue()); + calibrator->update(-1.0f); + sensor->publishState(0.0f); + TEST_ASSERT_EQUAL_FLOAT(-1.0f, sensor->getValue()); - sensor->enableCalibration(); - TEST_ASSERT_EQUAL_INT(2, sensor->getValue()); + sensor->publishState(100.0f); + TEST_ASSERT_EQUAL_FLOAT(-1.0f, sensor->getValue()); - sensor->disableCalibration(); - TEST_ASSERT_EQUAL_INT(2, sensor->getValue()); + calibrator->update(2.0f); + sensor->publishState(102.0f); + TEST_ASSERT_EQUAL_FLOAT(2.0f, sensor->getValue()); - sensor->resetCalibration(); - TEST_ASSERT_EQUAL_INT(0, sensor->getValue()); - TEST_ASSERT_EQUAL_INT(1, calibrator->resetCounter); + sensor->startCalibration(); + sensor->publishState(200.0f); + TEST_ASSERT_EQUAL_FLOAT(200.0f, sensor->getValue()); + + sensor->publishState(202.0f); + TEST_ASSERT_EQUAL_FLOAT(202.0f, sensor->getValue()); + + sensor->stopCalibration(); + sensor->publishState(300.0f); + TEST_ASSERT_EQUAL_FLOAT(202.0f, sensor->getValue()); } -void test_average_sensor(void) +void test_sensor_filter_multiply(void) { auto inner = new TestAnalogSensor(); - auto sensor = new AverageSensor(inner, 3); + auto sensor = new SimpleSensorDecorator(inner); + sensor->addFilters({ new ::SenseShift::Input::Filter::MultiplyFilter(2) }); TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); sensor->init(); TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - // TODO: mock inner sensor, to return more interesting values - TEST_ASSERT_EQUAL_INT(2, sensor->getValue()); // (1 + 2 + 3) / 3 = 2 - TEST_ASSERT_EQUAL_INT(5, sensor->getValue()); // (4 + 5 + 6) / 3 = 5 - TEST_ASSERT_EQUAL_INT(8, sensor->getValue()); // (7 + 8 + 9) / 3 = 8 - TEST_ASSERT_EQUAL_INT(11, sensor->getValue()); // (10 + 11 + 12) / 3 = 11 - - inner->count = 0; - sensor = new AverageSensor(inner, 10); + inner->value = 1; + sensor->tick(); + TEST_ASSERT_EQUAL_INT(2, sensor->getValue()); - TEST_ASSERT_EQUAL_INT(5, sensor->getValue()); // (1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10) / 10 = 5 + inner->value = 16; + sensor->tick(); + TEST_ASSERT_EQUAL_INT(32, sensor->getValue()); } -void test_static_median_sensor(void) +void test_sensor_analog_threshold(void) { auto inner = new TestAnalogSensor(); - auto sensor = new StaticMedianSensor(inner); + auto source = new SimpleSensorDecorator(inner); + auto sensor = new AnalogThresholdSensor(source, 120, 80, true); TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); sensor->init(); TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - // lmao, literally the same as average sensor - // TODO: mock inner sensor, to return more interesting values - TEST_ASSERT_EQUAL_INT(2, sensor->getValue()); // (1, 2, 3) = 2 - TEST_ASSERT_EQUAL_INT(5, sensor->getValue()); // (4, 5, 6) = 5 - TEST_ASSERT_EQUAL_INT(8, sensor->getValue()); // (7, 8, 9) = 8 - TEST_ASSERT_EQUAL_INT(11, sensor->getValue()); // (10, 11, 12) = 11 + // 100 is below the threshold, so the sensor should be off + inner->value = 100; + source->tick(); + TEST_ASSERT_FALSE(sensor->getValue()); + + // 130 is above the threshold, so the sensor should be on + inner->value = 130; + source->tick(); + TEST_ASSERT_TRUE(sensor->getValue()); + + // 90 is below the upper threshold, but above the lower threshold, so the sensor should stay on due to hysteresis + inner->value = 90; + source->tick(); + TEST_ASSERT_TRUE(sensor->getValue()); + + // 70 is below the lower threshold, so the sensor should be off + inner->value = 70; + source->tick(); + TEST_ASSERT_FALSE(sensor->getValue()); } int process(void) @@ -114,8 +143,8 @@ int process(void) RUN_TEST(test_memoized_sensor); RUN_TEST(test_calibrated_sensor); - RUN_TEST(test_average_sensor); - RUN_TEST(test_static_median_sensor); + RUN_TEST(test_sensor_filter_multiply); + RUN_TEST(test_sensor_analog_threshold); return UNITY_END(); } diff --git a/test/test_opengloves/main.cpp b/test/test_opengloves/main.cpp deleted file mode 100644 index 7f607d6f..00000000 --- a/test/test_opengloves/main.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include - -using namespace OpenGloves; - -class TestAnalogSensor : public SenseShift::Input::ISimpleSensor { - private: - uint16_t count = 0; - - public: - int setupCounter = 0; - - void init() override { this->setupCounter++; }; - - uint16_t getValue() override { return ++this->count; }; -}; - -class TestBinarySensor : public SenseShift::Input::ISimpleSensor { - public: - bool value = false; - int setupCounter = 0; - - void init() override { this->setupCounter++; }; - - bool getValue() override { return this->value; }; -}; - -class TestFingerSensor : public SenseShift::Input::ISimpleSensor { - public: - FingerValue value; - int setupCounter = 0; - - void init() override { this->setupCounter++; }; - - FingerValue getValue() override { return this->value; }; -}; - -void test_string_encoded_sensor_uint16(void) -{ - auto inner = new TestAnalogSensor(); - auto sensor = new StringEncodedMemoizedSensor(inner, IEncodedInput::Type::INDEX); - - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - - TEST_ASSERT_EQUAL_INT(0, sensor->getValue()); - TEST_ASSERT_EQUAL_INT(0, sensor->getValue()); - - sensor->updateValue(); - - TEST_ASSERT_EQUAL_INT(1, sensor->getValue()); - TEST_ASSERT_EQUAL_INT(1, sensor->getValue()); - - char buffer[6]; - TEST_ASSERT_EQUAL_INT(2, sensor->encodeString(buffer)); - TEST_ASSERT_EQUAL_STRING("B1", buffer); -} - -void test_string_encoded_sensor_bool(void) -{ - auto inner = new TestBinarySensor(); - auto sensor = new StringEncodedMemoizedSensor(inner, IEncodedInput::Type::A_BTN); - - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - - TEST_ASSERT_FALSE(sensor->getValue()); - - char buffer[1]; - TEST_ASSERT_EQUAL_INT(0, sensor->encodeString(buffer)); - TEST_ASSERT_EQUAL_STRING("", buffer); - - sensor->updateValue(); - - TEST_ASSERT_FALSE(sensor->getValue()); - - inner->value = true; - - TEST_ASSERT_FALSE(sensor->getValue()); - - sensor->updateValue(); - - TEST_ASSERT_TRUE(sensor->getValue()); - - TEST_ASSERT_EQUAL_INT(1, sensor->encodeString(buffer)); - TEST_ASSERT_EQUAL_STRING("J", buffer); -} - -void test_string_encoded_sensor_fingervalue(void) -{ - auto inner = new TestFingerSensor(); - auto sensor = new StringEncodedMemoizedSensor(inner, IEncodedInput::Type::THUMB); - - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - - // curl-only - char buffer[sensor->getEncodedLength()]; - sensor->encodeString(buffer); - TEST_ASSERT_EQUAL_STRING("A0", buffer); - - inner->value.curl = { 256 }; - sensor->updateValue(); - sensor->encodeString(buffer); - TEST_ASSERT_EQUAL_STRING("A256", buffer); - - // curl + splay - inner->value.splay = 420; - sensor->updateValue(); - sensor->encodeString(buffer); - TEST_ASSERT_EQUAL_STRING("A256(AB)420", buffer); - - // multi-curl - inner->value.curl = { 128, 256, 512 }; - inner->value.splay = std::nullopt; - sensor->updateValue(); - sensor->encodeString(buffer); - TEST_ASSERT_EQUAL_STRING("A298(AAA)128(AAB)256(AAC)512", buffer); // 298 = (128 + 256 + 512) / 3 - - // multi-curl + splay - inner->value.splay = 69; - sensor->updateValue(); - sensor->encodeString(buffer); - TEST_ASSERT_EQUAL_STRING("A298(AAA)128(AAB)256(AAC)512(AB)69", buffer); // 298 = (128 + 256 + 512) / 3 -} - -int process(void) -{ - UNITY_BEGIN(); - - RUN_TEST(test_string_encoded_sensor_uint16); - RUN_TEST(test_string_encoded_sensor_bool); - RUN_TEST(test_string_encoded_sensor_fingervalue); - - return UNITY_END(); -} - -#ifdef ARDUINO - -#include - -void setup(void) -{ - process(); -} - -void loop(void) {} - -#else - -int main(int argc, char** argv) -{ - return process(); -} - -#endif diff --git a/test/test_opengloves_alpha_encoding/main.cpp b/test/test_opengloves_alpha_encoding/main.cpp index 68be53cd..7fd164d0 100644 --- a/test/test_opengloves_alpha_encoding/main.cpp +++ b/test/test_opengloves_alpha_encoding/main.cpp @@ -1,111 +1,195 @@ -#include +#include #include -using namespace OpenGloves; -using namespace SenseShift::OpenGloves; +using namespace og; -void testSplitCommands(void) +void test_encode_input_peripherals(void) { - std::map> input_strings = { - // curl only + const IEncoder* encoder = new AlphaEncoder(); + + const auto defaultEmpty = InputPeripheralData(); + + auto halfCurl = InputPeripheralData(); + halfCurl.curl = { + .thumb = { .curl_total = 0.5 }, + .index = { .curl_total = 0.5 }, + .middle = { .curl_total = 0.5 }, + .ring = { .curl_total = 0.5 }, + .pinky = { .curl_total = 0.5 }, + }; + + auto zeroCurl = InputPeripheralData(); + zeroCurl.curl = { + .thumb = { .curl_total = 0.0 }, + .index = { .curl_total = 0.0 }, + .middle = { .curl_total = 0.0 }, + .ring = { .curl_total = 0.0 }, + .pinky = { .curl_total = 0.0 }, + }; + + auto fullCurl = InputPeripheralData(); + fullCurl.curl = { + .thumb = { .curl_total = 1.0 }, + .index = { .curl_total = 1.0 }, + .middle = { .curl_total = 1.0 }, + .ring = { .curl_total = 1.0 }, + .pinky = { .curl_total = 1.0 }, + }; + + auto halfSplay = InputPeripheralData(); + halfSplay.splay = { + .thumb = 0.5, + .index = 0.5, + .middle = 0.5, + .ring = 0.5, + .pinky = 0.5, + }; + + auto increasingJoints = InputPeripheralData(); + increasingJoints.curl = { + .thumb = { .curl = { 0.25f, 0.5f, 0.75f, 1.0f } }, + .index = { .curl = { 0.25f, 0.5f, 0.75f, 1.0f } }, + .middle = { .curl = { 0.25f, 0.5f, 0.75f, 1.0f } }, + .ring = { .curl = { 0.25f, 0.5f, 0.75f, 1.0f } }, + .pinky = { .curl = { 0.25f, 0.5f, 0.75f, 1.0f } }, + }; + + auto joystick = InputPeripheralData(); + joystick.joystick = { + .x = 0.5, + .y = 0.5, + .press = true, + }; + + auto buttons = InputPeripheralData(); + buttons.button_a = { .press = true }; + buttons.button_calibrate = { .press = true }; + + auto gesture = InputPeripheralData(); + gesture.pinch = { true }; + gesture.grab = { true }; + + auto halfCurlSplay = InputPeripheralData(halfCurl); + halfCurlSplay.curl = { + .thumb = { .curl_total = 0.5 }, + .index = { .curl_total = 0.5 }, + .middle = { .curl_total = 0.5 }, + .ring = { .curl_total = 0.5 }, + .pinky = { .curl_total = 0.5 }, + }; + halfCurlSplay.splay = { + .thumb = 0.5, + .index = 0.5, + .middle = 0.5, + .ring = 0.5, + .pinky = 0.5, + }; + + const std::vector> cases = { { - "A2048\n\0", - { - { Command::ThumbCurl, 2048 }, - }, + defaultEmpty, + "A0B0C0D0E0\n", }, - // curl only { - "A100B200C300D400E500", - { - { Command::ThumbCurl, 100 }, - { Command::IndexCurl, 200 }, - { Command::MiddleCurl, 300 }, - { Command::RingCurl, 400 }, - { Command::PinkyCurl, 500 }, - }, + halfCurl, + "A2047B2047C2047D2047E2047\n", }, - // curl (unordered) { - "E500A100B200D400C300", - { - { Command::ThumbCurl, 100 }, - { Command::IndexCurl, 200 }, - { Command::MiddleCurl, 300 }, - { Command::RingCurl, 400 }, - { Command::PinkyCurl, 500 }, - }, + zeroCurl, + "A0B0C0D0E0\n", + }, + { + fullCurl, + "A4095B4095C4095D4095E4095\n", + }, + { + halfSplay, + "A0(AB)2047B0(BB)2047C0(CB)2047D0(DB)2047E0(EB)2047\n", }, - // curl (with invalid data) { - "A100B200C300D400E", - { - { Command::ThumbCurl, 100 }, - { Command::IndexCurl, 200 }, - { Command::MiddleCurl, 300 }, - { Command::RingCurl, 400 }, + increasingJoints, + "A1023(AAB)2047(AAC)3071(AAD)4095B1023(BAB)2047(BAC)3071(BAD)4095C1023(CAB)2047(CAC)3071(CAD)4095D1023(DAB)2047(DAC)3071(DAD)4095E1023(EAB)2047(EAC)3071(EAD)4095\n", + }, + { + joystick, + "A0B0C0D0E0F2047G2047H\n", + }, + { + buttons, + "A0B0C0D0E0JO\n", + }, + { + gesture, + "A0B0C0D0E0ML\n", + }, + { + halfCurlSplay, + "A2047(AB)2047B2047(BB)2047C2047(CB)2047D2047(DB)2047E2047(EB)2047\n", + } + }; + + for (auto i = 0; i < cases.size(); i++) { + const auto [data, expected] = cases[i]; + const auto encoded = encoder->encode_input(data); + TEST_ASSERT_EQUAL_STRING_MESSAGE( + expected.c_str(), + encoded.c_str(), + ("Failed case " + std::to_string(i)).c_str() + ); + } +} + +void test_decode_output_ffb(void) +{ + const IEncoder* encoder = new AlphaEncoder(); + + std::map cases = { + { + "A0B0C0D0E0\n", + OutputForceFeedbackData{ + .thumb = 0.0f, + .index = 0.0f, + .middle = 0.0f, + .ring = 0.0f, + .pinky = 0.0f, }, }, - // curl (with unknown prefix) { - "X100A100B200C300D400E500", - { - { Command::ThumbCurl, 100 }, - { Command::IndexCurl, 200 }, - { Command::MiddleCurl, 300 }, - { Command::RingCurl, 400 }, - { Command::PinkyCurl, 500 }, + "A0\n", + OutputForceFeedbackData{ + .thumb = 0.0f, + .index = 0.0f, + .middle = 0.0f, + .ring = 0.0f, + .pinky = 0.0f, }, }, - // curl + splay { - "A1(AB)2B3(BB)4C5(CB)6D7(DB)8E9(EB)10", - { - { Command::ThumbCurl, 1 }, - { Command::ThumbSplay, 2 }, - { Command::IndexCurl, 3 }, - { Command::IndexSplay, 4 }, - { Command::MiddleCurl, 5 }, - { Command::MiddleSplay, 6 }, - { Command::RingCurl, 7 }, - { Command::RingSplay, 8 }, - { Command::PinkyCurl, 9 }, - { Command::PinkySplay, 10 }, + "A819B1638C2457D3276E4095\n", + OutputForceFeedbackData{ + .thumb = 0.2f, + .index = 0.4f, + .middle = 0.6f, + .ring = 0.8f, + .pinky = 1.0f, }, }, - // curl + splay (unordered) { - "E9A1B3D7C5(BB)4(AB)2(EB)10(CB)6(DB)8", - { - { Command::ThumbCurl, 1 }, - { Command::ThumbSplay, 2 }, - { Command::IndexCurl, 3 }, - { Command::IndexSplay, 4 }, - { Command::MiddleCurl, 5 }, - { Command::MiddleSplay, 6 }, - { Command::RingCurl, 7 }, - { Command::RingSplay, 8 }, - { Command::PinkyCurl, 9 }, - { Command::PinkySplay, 10 }, + "A4095B4095C4095D4095E4095\n", + OutputForceFeedbackData{ + .thumb = 1.0f, + .index = 1.0f, + .middle = 1.0f, + .ring = 1.0f, + .pinky = 1.0f, }, }, }; - auto encoding_service = AlphaEncodingService(); - - for (auto& [input_string, expected_commands] : input_strings) { - std::map commands = {}; - encoding_service.deserialize(input_string.c_str(), input_string.length(), commands); - - TEST_ASSERT_EQUAL_size_t_MESSAGE( - expected_commands.size(), - commands.size(), - "Expected commands size does not match actual commands size" - ); - - for (auto& [command, value] : expected_commands) { - TEST_ASSERT_EQUAL_INT(value, commands[command]); - } + for (const auto& [data, expected] : cases) { + const auto decoded = encoder->decode_output(data.c_str(), data.size()); + TEST_ASSERT_TRUE(std::holds_alternative(decoded)); + TEST_ASSERT_TRUE(std::get(decoded) == expected); } } @@ -113,7 +197,8 @@ int process(void) { UNITY_BEGIN(); - RUN_TEST(testSplitCommands); + RUN_TEST(test_encode_input_peripherals); + RUN_TEST(test_decode_output_ffb); return UNITY_END(); } diff --git a/test/test_opengloves_finger/main.cpp b/test/test_opengloves_finger/main.cpp deleted file mode 100644 index 569bcda8..00000000 --- a/test/test_opengloves_finger/main.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#include -#include - -using namespace OpenGloves; -using namespace SenseShift::Calibration; - -class TestAnalogSensor : public SenseShift::Input::ISimpleSensor { - private: - uint16_t count = 0; - - public: - int setupCounter = 0; - - void init() override { this->setupCounter++; }; - - uint16_t getValue() override { return this->count++; }; -}; - -class DummyCalibrator : public ICalibrator { - public: - uint8_t resetCounter = 0; - std::optional calibrated = std::nullopt; - - void reset() override - { - this->resetCounter++; - this->calibrated = std::nullopt; - }; - void update(uint16_t input) override { this->calibrated = input; }; - uint16_t calibrate(uint16_t input) const override { return this->calibrated.value_or(input); }; -}; - -void test_simple_finger_sensor_curl(void) -{ - auto* inner = new TestAnalogSensor(); - auto* calibrator = new DummyCalibrator(); - auto* calibrated = new SenseShift::Input::CalibratedSimpleSensor(inner, calibrator); - auto* sensor = new SimpleFingerSensor(calibrated); - - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - - // since the sensor is not memoized, the value is updated on every call - TEST_ASSERT_EQUAL_INT(0, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(1, sensor->getCurl()); - - calibrator->calibrated = 100; - - TEST_ASSERT_EQUAL_INT(100, sensor->getValue().curl[0]); // 2 is inside - TEST_ASSERT_EQUAL_INT(100, sensor->getCurl()); // 3 is inside - - calibrator->calibrated = std::nullopt; - - TEST_ASSERT_EQUAL_INT(4, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(5, sensor->getCurl()); -} - -void test_simple_finger_sensor_curl_flex(void) -{ - auto* inner_curl = new TestAnalogSensor(); - auto* calibrator_curl = new DummyCalibrator(); - auto* calibrated_curl = new SenseShift::Input::CalibratedSimpleSensor(inner_curl, calibrator_curl); - - auto* inner_flex = new TestAnalogSensor(); - auto* calibrator_flex = new DummyCalibrator(); - auto* calibrated_flex = new SenseShift::Input::CalibratedSimpleSensor(inner_flex, calibrator_flex); - - auto* sensor = new SimpleFingerSensor(calibrated_curl, calibrated_flex); - - TEST_ASSERT_EQUAL_INT(0, inner_curl->setupCounter); - TEST_ASSERT_EQUAL_INT(0, inner_flex->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner_curl->setupCounter); - TEST_ASSERT_EQUAL_INT(1, inner_flex->setupCounter); - - // since the sensor is not memoized, the value is updated on every call - TEST_ASSERT_EQUAL_INT(0, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(1, sensor->getValue().splay.value()); - TEST_ASSERT_EQUAL_INT(2, sensor->getCurl()); - - calibrator_curl->calibrated = 100; - calibrator_flex->calibrated = 200; - - TEST_ASSERT_EQUAL_INT(100, sensor->getValue().curl[0]); // 3 is inside - TEST_ASSERT_EQUAL_INT(200, sensor->getValue().splay.value()); // 4 is inside - TEST_ASSERT_EQUAL_INT(100, sensor->getCurl()); // 5 is inside - - calibrator_curl->calibrated = std::nullopt; - calibrator_flex->calibrated = std::nullopt; - - TEST_ASSERT_EQUAL_INT(6, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(7, sensor->getValue().splay.value()); - TEST_ASSERT_EQUAL_INT(8, sensor->getCurl()); -} - -// todo: tests for multi-curl - -void test_finger_sensor_curl(void) -{ - auto* inner = new TestAnalogSensor(); - auto* calibrator = new DummyCalibrator(); - auto* calibrated = new SenseShift::Input::CalibratedSimpleSensor(inner, calibrator); - auto* sensor = new FingerSensor(calibrated, IEncodedInput::Type::INDEX); - - TEST_ASSERT_EQUAL_INT(0, inner->setupCounter); - sensor->init(); - TEST_ASSERT_EQUAL_INT(1, inner->setupCounter); - - TEST_ASSERT_EQUAL_INT(0, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(0, sensor->getCurl()); - - sensor->updateValue(); - - TEST_ASSERT_EQUAL_INT(0, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(0, sensor->getCurl()); - - calibrator->calibrated = 100; - - TEST_ASSERT_EQUAL_INT(0, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(0, sensor->getCurl()); - - sensor->updateValue(); // +1 is inside - - TEST_ASSERT_EQUAL_INT(100, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(100, sensor->getCurl()); - - TEST_ASSERT_EQUAL_INT(0, calibrator->resetCounter); - sensor->resetCalibration(); - TEST_ASSERT_EQUAL_INT(1, calibrator->resetCounter); - - sensor->enableCalibration(); - sensor->updateValue(); // +1 is inside - TEST_ASSERT_EQUAL_INT(2, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(2, sensor->getCurl()); - - sensor->updateValue(); // +1 is inside - TEST_ASSERT_EQUAL_INT(3, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(3, sensor->getCurl()); - - sensor->disableCalibration(); - sensor->updateValue(); // +1 is inside, but calibrated to 3 - TEST_ASSERT_EQUAL_INT(3, sensor->getValue().curl[0]); - TEST_ASSERT_EQUAL_INT(3, sensor->getCurl()); -} - -int process(void) -{ - UNITY_BEGIN(); - - RUN_TEST(test_simple_finger_sensor_curl); - RUN_TEST(test_simple_finger_sensor_curl_flex); - - RUN_TEST(test_finger_sensor_curl); - - return UNITY_END(); -} - -#ifdef ARDUINO - -#include - -void setup(void) -{ - process(); -} - -void loop(void) {} - -#else - -int main(int argc, char** argv) -{ - return process(); -} - -#endif diff --git a/test/test_opengloves_gesture/main.cpp b/test/test_opengloves_gesture/main.cpp deleted file mode 100644 index 0d89a11a..00000000 --- a/test/test_opengloves_gesture/main.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include - -using namespace OpenGloves; - -class TestCurlFinger : public ICurl { - public: - uint16_t value; - - TestCurlFinger(uint16_t initialValue = 0) : value(initialValue){}; - - uint16_t getCurl() override { return this->value; } -}; - -void test_gesture_grab(void) -{ - uint16_t threshold = 2047; - - auto index = TestCurlFinger(threshold - 1); - auto middle = TestCurlFinger(threshold - 1); - auto ring = TestCurlFinger(threshold - 1); - auto pinky = TestCurlFinger(threshold - 1); - - auto gesture = GrabGesture(index, middle, ring, pinky, threshold); - - TEST_ASSERT_FALSE(gesture.getValue()); - - index.value = threshold + 1; - TEST_ASSERT_FALSE(gesture.getValue()); - - middle.value = threshold + 1; - TEST_ASSERT_FALSE(gesture.getValue()); - - ring.value = threshold + 1; - TEST_ASSERT_FALSE(gesture.getValue()); - - pinky.value = threshold + 1; - TEST_ASSERT_TRUE(gesture.getValue()); -} - -void test_gesture_trigger(void) -{ - uint16_t threshold = 2047; - - auto index = TestCurlFinger(threshold - 1); - - auto gesture = TriggerGesture(index, threshold); - - TEST_ASSERT_FALSE(gesture.getValue()); - - index.value = threshold + 1; - TEST_ASSERT_TRUE(gesture.getValue()); -} - -void test_gesture_pinch(void) -{ - uint16_t threshold = 2047; - - auto index = TestCurlFinger(threshold - 1); - auto middle = TestCurlFinger(threshold - 1); - - auto gesture = PinchGesture(index, middle, threshold); - - TEST_ASSERT_FALSE(gesture.getValue()); - - index.value = threshold + 1; - TEST_ASSERT_FALSE(gesture.getValue()); - - middle.value = threshold + 1; - TEST_ASSERT_TRUE(gesture.getValue()); -} - -int process(void) -{ - UNITY_BEGIN(); - - RUN_TEST(test_gesture_grab); - RUN_TEST(test_gesture_trigger); - RUN_TEST(test_gesture_pinch); - - return UNITY_END(); -} - -#ifdef ARDUINO - -#include - -void setup(void) -{ - process(); -} - -void loop(void) {} - -#else - -int main(int argc, char** argv) -{ - return process(); -} - -#endif diff --git a/test/test_util_buffer/main.cpp b/test/test_util_buffer/main.cpp index 40f1221c..14eec519 100644 --- a/test/test_util_buffer/main.cpp +++ b/test/test_util_buffer/main.cpp @@ -1,4 +1,3 @@ -#include #include #include diff --git a/test/test_util/main.cpp b/test/test_util_container/main.cpp similarity index 63% rename from test/test_util/main.cpp rename to test/test_util_container/main.cpp index 4be719ba..ed0279ca 100644 --- a/test/test_util/main.cpp +++ b/test/test_util_container/main.cpp @@ -45,30 +45,6 @@ void test_contains_string(void) TEST_ASSERT_FALSE(contains(s, 'z')); } -void test_accurate_map(void) -{ - TEST_ASSERT_EQUAL_UINT16(0, accurateMap(0, 0, 4095, 0, 255)); - TEST_ASSERT_EQUAL_UINT16(127, accurateMap(2047, 0, 4095, 0, 255)); - TEST_ASSERT_EQUAL_UINT16(255, accurateMap(4095, 0, 4095, 0, 255)); - - TEST_ASSERT_EQUAL_UINT16(127, accurateMap(0, 0, 4095, 127, 255)); - TEST_ASSERT_EQUAL_UINT16(190, accurateMap(2047, 0, 4095, 127, 255)); - TEST_ASSERT_EQUAL_UINT16(255, accurateMap(4095, 0, 4095, 127, 255)); - - TEST_ASSERT_EQUAL_UINT16(0, accurateMap(2048, 2048, 4095, 0, 255)); - TEST_ASSERT_EQUAL_UINT16(127, accurateMap(3071, 2048, 4095, 0, 255)); - TEST_ASSERT_EQUAL_UINT16(255, accurateMap(4095, 2048, 4095, 0, 255)); - - TEST_ASSERT_EQUAL_UINT16(2047, accurateMap(343, 343, 343, 0, 4095)); -} - -void test_simple_map(void) -{ - TEST_ASSERT_EQUAL_UINT16(0, simpleMap(0, 4095, 255)); - TEST_ASSERT_EQUAL_UINT16(127, simpleMap(2047, 4095, 255)); - TEST_ASSERT_EQUAL_UINT16(255, simpleMap(4095, 4095, 255)); -} - int process(void) { UNITY_BEGIN(); @@ -77,9 +53,6 @@ int process(void) RUN_TEST(test_contains_iterator); RUN_TEST(test_contains_string); - RUN_TEST(test_accurate_map); - RUN_TEST(test_simple_map); - return UNITY_END(); } @@ -101,4 +74,4 @@ int main(int argc, char** argv) return process(); } -#endif +#endif \ No newline at end of file