diff --git a/# backup/01 debug_panto_heartbeats.py b/# backup/01 debug_panto_heartbeats.py deleted file mode 100644 index a4ab0d1..0000000 --- a/# backup/01 debug_panto_heartbeats.py +++ /dev/null @@ -1,27 +0,0 @@ -import serial -import binascii - -com = "/dev/cu.usbserial-0001" - -con = serial.Serial(com, baudrate=115200, timeout=5) -while True: - header = con.read(size=2) - print(binascii.hexlify(header)) - - msg_type = con.read(size=1) - print(binascii.hexlify(msg_type)) - - package_id = con.read(size=1) - print(binascii.hexlify(package_id)) - - message_size = con.read(size=2) - print(binascii.hexlify(message_size)) - - body = con.read(size=int.from_bytes(message_size, byteorder="big")) - print(binascii.hexlify(body)) - - if msg_type == bytes.fromhex("20"): - print("received debug message:") - print(body) - -con.close() \ No newline at end of file diff --git a/# backup/02 debug_panto.py b/# backup/02 debug_panto.py deleted file mode 100644 index 06b4c52..0000000 --- a/# backup/02 debug_panto.py +++ /dev/null @@ -1,75 +0,0 @@ -import serial -import binascii -import logging - -com = "/dev/cu.usbserial-0001" - -logging.basicConfig(level=logging.INFO) - -con = serial.Serial(com, baudrate=115200, timeout=5) - -while True: - header = con.read(size=2) - - msg_type = con.read(size=1) - - package_id = con.read(size=1) - - - message_size = con.read(size=2) - - - body = con.read(size=int.from_bytes(message_size, byteorder="big")) - - - if msg_type == bytes.fromhex("20"): - logging.info("received debug message:") - logging.info(body) - - elif msg_type == bytes.fromhex("00"): - logging.info("receive sync") - con.write(bytes.fromhex("445080000000")) - elif msg_type == bytes.fromhex("01"): - logging.info("receive heartbeat") - con.write(bytes.fromhex("445081000000")) - elif msg_type == bytes.fromhex("10"): - logging.info("receive position") - """ - 4450 // magic number - 10 // message type: position - 00 // packet ID: not utilized - 0028 // payload length: 2 handles, 5 values each, 4 bytes each - 2*5*4 = 40 = 0x28 - FFFFFFFF // x position of first handle - FFFFFFFF // y position of first handle - FFFFFFFF // rotation of first handle - FFFFFFFF // x position of first handle's god object - FFFFFFFF // y position of first handle's god object - FFFFFFFF // x position of second handle - FFFFFFFF // y position of second handle - FFFFFFFF // rotation of second handle - FFFFFFFF // x position of second handle's god object - FFFFFFFF // y position of second handle's god object - """ - x_1 = int.from_bytes(body[:4], byteorder='big', signed=False) - y_1 = int.from_bytes(body[4:8], byteorder='big', signed=False) - r_1 = int.from_bytes(body[8:12], byteorder='big', signed=False) - g_x1 = int.from_bytes(body[12:16], byteorder='big', signed=False) - g_y1 = int.from_bytes(body[16:20], byteorder='big', signed=False) - x_2 = int.from_bytes(body[20:24], byteorder='big', signed=False) - y_2 = int.from_bytes(body[24:28], byteorder='big', signed=False) - r_2 = int.from_bytes(body[28:32], byteorder='big', signed=False) - g_x2 = int.from_bytes(body[32:36], byteorder='big', signed=False) - g_y2 = int.from_bytes(body[36:40], byteorder='big', signed=False) - - - logging.info(f"{x_1} {y_1} {r_1} {g_x1} {g_y1} {x_2} {y_2} {r_2} {g_x2} {g_y2}") - else: - logging.warning(binascii.hexlify(header)) - - logging.warning((binascii.hexlify(msg_type))) - - logging.warning(binascii.hexlify(package_id)) - logging.warning(binascii.hexlify(message_size)) - logging.warning(binascii.hexlify(body)) - -con.close() \ No newline at end of file diff --git a/# backup/todo.md b/# backup/todo.md deleted file mode 100644 index c44f66f..0000000 --- a/# backup/todo.md +++ /dev/null @@ -1,23 +0,0 @@ -# UX -- download pycharm - - platformio is hard to use - -- finding comport - - mac: all time same , /dev/cu.SLAB_USBtoUART - - windows: a user need to chekc COM number on device manager - - Linux: "/dev/ttyUSB0" maybe - - first of all, they need to install the driver - -- encoder test - - - we don't know which number is which - - we don't know when we can say it works properly - - a user move linkage to the end and check the value is correct or not - - we need some documentation or video - -- add mecnanical linkage test - -- motor (sync) test - - - - diff --git a/.gitmodules b/.gitmodules index caf0633..e69de29 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "firmware/10 panto firmware"] - path = firmware/10 panto firmware - url = git@github.com:HassoPlattnerInstituteHCI/dualpantoframework.git diff --git a/BIS/BIS.md b/BIS/BIS.md new file mode 100644 index 0000000..27233f1 --- /dev/null +++ b/BIS/BIS.md @@ -0,0 +1,29 @@ +# BIS + +We use this dualpanto-testing repo for assignment, and uploading dualpanto firmware too. + +**Please check and install all requirements at [Readme.md](../Readme.md)** + + +## Assignment Week7: rendering wall +Assignment for Week7 is to write code of god objeect partially, and get familiar with uploading firmware and this testing work flow. + +1. make sure install ESP32 driver, IDE, and all python library on requirements.txt +2. go to `dualpant-testing/firmware/haptics/BIS week7/src`,find TODO and write code +3. `python -m unittest test_firmware.Haptics.test_BIS_week7` +4. if you feel the device has issue, follow [test flow (Mechanics and Hardware)](../Readme.md) +5. take the video that you interacts with rendered wall + +## Assignment Week8: +**WIP** + +## Uploading DualpantoFramework +From Week9, we will develop dualpanto app. sometimes you want to upload dualpanto famework again and again when... + +1. you update dualpanto framework (e.g. you tune PID) +2. you plug dualpanto device into different PC + - we observe dualpanto has some wall rendering issue when you connect dualpanto to other PC. e.g, you uplaod firmware from mac and connect to windows pc. + - this is not always happend. but You need to keep this in mind. + - We haven't known why so. Welcome you to contribute here! + +`python -m unittest test_firmware.UploadDualpantoFrameowrk.test_upload_dp_firmware` diff --git a/BIS/before_ask_ta.md b/BIS/before_ask_ta.md new file mode 100644 index 0000000..e69de29 diff --git a/CheckLists/mechanical.md b/CheckLists/mechanical.md deleted file mode 100644 index ea9b1f7..0000000 --- a/CheckLists/mechanical.md +++ /dev/null @@ -1,9 +0,0 @@ -# Checking Mechanical and some random Issue -First things you needs to check is mechanical issue -(or assembly issue) of dualpanto device. - -#### #01 Home the linkages by moving them to the initial position and reset the microcontroller -#### #02 Reconnect the USB cable (power cycling) -#### #03 Check your battery level -#### #04 check motor solder connection -#### #05 check connection between motor and linkages: one set screw should be against the “d-cut” of the shaft \ No newline at end of file diff --git a/Readme.md b/Readme.md index cc9d4c5..957d0c2 100644 --- a/Readme.md +++ b/Readme.md @@ -1,6 +1,11 @@ # Dualpanto Testing -Goal of this repository is to provide some (semi) automatic testing for the dualpanto platform. This involves basic functionality checks of the hardware, the firmware and potentially the unity integration +The goal of this repository is to provide (semi) automatic testing for the dualpanto platform. This involves basic functionality checks of the hardware, the haptic rendering and the unity integration. + +This project is work-in-progress. Welcome you to contribute. +### For BIS participants +Please check [BIS.md](BIS/BIS.md) first. + ## Requirements ### Install the ESP32 driver @@ -18,8 +23,6 @@ We highly recommend to use PyCharm to run this test flow. you can run unittest from Markdown preview (or [execute unittest from scripts](https://www.jetbrains.com/help/pycharm/testing-your-first-python-application.html#create-test)) -[//]: # (![pycharm_unittest](resources/images/pycharm_unittest_1.jpg)) - #### VScode @@ -32,22 +35,18 @@ you can run unittest from Markdown preview (or [execute unittest from scripts]( you can also run all unittest from command line interface if you want. # Test Flow -If you don't have any intuition about where the issue that you have comes from, -you want to test from bottom up: mechanical -> hardware -> framework -> communication ->unity. - -Some of the test is not software automatable since they are related to haptics and mechanics. check `How to test?`. +Most of the test is not semi-automated since they are related to haptics and mechanical issue. Check `How to test?`. ## Configurate test flow -1. open `config.py` and enter your ESP port. -2. when you upload a firmware, you need to push button back of dualpanto. please check [this](https://github.com/HassoPlattnerInstituteHCI/dualpanto/blob/main/BIS.md#upload-firmware) -. +1. open `config.py` and enter your ESP port. +2. when you upload a firmware, you probably need to push button back of dualpanto (it depends on OS). +![swith](./resources/dualpanto_switch.jpg) ## 1. Mechanical and Hardware -test with IDE or from command line -0. [check mechanical configuration](CheckLists/mechanical.md) +0. [check mechanical configuration](physical_test%2Fmechanical_issue_check_list.md) 1. `python -m unittest test_firmware.Basic` 2. `python -m unittest test_hardware.Linkage.test_encoder`[How to test?]() 3. `python -m unittest test_hardware.Linkage.test_sync`[How to test?]() @@ -62,7 +61,8 @@ flowchart TD; subgraph test_firmware FB[1.Basic]; - FB -- not work --> TA0((Ask TA)) + FB -- not work --> CH(check USB connection,
COM port and
platformIO properly installed) + CH -- no idea --> TA0((Ask TA)) end FB -- work --> LTE[2.test_Encoder] @@ -93,12 +93,9 @@ flowchart TD; click MC "https://discord.com" ``` -## 2. Firmware -**WIP** - -test with IDE or from command line +## 2. Rendering Haptics = DualPanto Firmware -0. [check mechanical configuration](CheckLists/mechanical.md) +0. [check mechanical configuration](physical_test/mechanical.md) 1. `python -m unittest test_firmware.Haptics.test_line_wall` [How to test?]() 2. `python -m unittest test_firmware.Haptics.test_force_field` **WIP** 3. `python -m unittest test_firmware.Haptics.test_rail` **WIP** @@ -107,65 +104,16 @@ test with IDE or from command line 6. `python -m unittest test_firmware.Haptics.test_moving_obstacle` **WIP** 7. `python -m unittest test_firmware.Kinematics.test_forward` **WIP** 8. `python -m unittest test_firmware.Kinematics.test_inverse` **WIP** +9. (WIP: mathing it and me's rocation for each dualpanto) -```mermaid -%%{init: {'theme': 'neutral' } }%% -flowchart TD; - ST((start)) --> MC[mechanical]; - MC -- checked --> TLW; - - subgraph test_firmware.Haptics - TLW[1.test_line_wall] --> TFF[2.test_force_field] - TLW --not work--> TA0((Ask TA)) - TFF --> TR[3.test_rail] - TR --> DONE((done)) - -%% TR[test_rail] -%% -%% TRW[test_rectangle_obstacle] -%% -%% TMW[test_moving_obstacle] - - - end - - - - style TA0 fill:#FF9B00 - style DONE fill:#99CC00 - -``` ## 3. Communication between DualPanto and PC **WIP** -test with IDE or from command line - -1. `python -m unittest test_hardware.HardwareTest.test_compile_firmware` [How to test?]() - -```mermaid -%%{init: {'theme': 'neutral' } }%% -flowchart TD; - ST((start)) --> MC[mechanical]; - - subgraph test_protocol - MC[test_message_count]-- work -->SGO[test_set_god_object]; - end -``` - -Please clone the current panto firmware into `firmware/10 panto firmware` and run `npm run script config` - -On a firmware level we need to test that the dualpanto can -- perform the handshake -- keep the connection alive -- move the handles if instructed -- report the handle position -- accept obstacles -- render obstacles - ## 4. Unity (Communication between DualPanto and Unity) -TODO + +**WIP** ## Development This project is currenty under developement. diff --git a/firmware/10 panto firmware b/firmware/10 panto firmware deleted file mode 160000 index d1fe262..0000000 --- a/firmware/10 panto firmware +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d1fe262166f9b4056af7a65c5f623c22d5555f52 diff --git a/firmware/09 god object/firmware/.gitignore b/firmware/haptics/BIS week7/firmware/.gitignore similarity index 100% rename from firmware/09 god object/firmware/.gitignore rename to firmware/haptics/BIS week7/firmware/.gitignore diff --git a/firmware/09 god object/firmware/.idea/.gitignore b/firmware/haptics/BIS week7/firmware/.idea/.gitignore similarity index 100% rename from firmware/09 god object/firmware/.idea/.gitignore rename to firmware/haptics/BIS week7/firmware/.idea/.gitignore diff --git a/firmware/09 god object/firmware/.idea/misc.xml b/firmware/haptics/BIS week7/firmware/.idea/misc.xml similarity index 100% rename from firmware/09 god object/firmware/.idea/misc.xml rename to firmware/haptics/BIS week7/firmware/.idea/misc.xml diff --git a/firmware/09 god object/firmware/.idea/vcs.xml b/firmware/haptics/BIS week7/firmware/.idea/vcs.xml similarity index 100% rename from firmware/09 god object/firmware/.idea/vcs.xml rename to firmware/haptics/BIS week7/firmware/.idea/vcs.xml diff --git a/firmware/09 god object/firmware/include/config/config.hpp b/firmware/haptics/BIS week7/firmware/include/config/config.hpp similarity index 100% rename from firmware/09 god object/firmware/include/config/config.hpp rename to firmware/haptics/BIS week7/firmware/include/config/config.hpp diff --git a/firmware/09 god object/firmware/include/hardware/angleAccessor.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/angleAccessor.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/angleAccessor.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/angleAccessor.hpp diff --git a/firmware/09 god object/firmware/include/hardware/panto.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/panto.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/panto.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/panto.hpp diff --git a/firmware/09 god object/firmware/include/hardware/spiCommands.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/spiCommands.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/spiCommands.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/spiCommands.hpp diff --git a/firmware/09 god object/firmware/include/hardware/spiEncoder.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/spiEncoder.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/spiEncoder.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/spiEncoder.hpp diff --git a/firmware/09 god object/firmware/include/hardware/spiEncoderChain.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/spiEncoderChain.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/spiEncoderChain.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/spiEncoderChain.hpp diff --git a/firmware/09 god object/firmware/include/hardware/spiPacket.hpp b/firmware/haptics/BIS week7/firmware/include/hardware/spiPacket.hpp similarity index 100% rename from firmware/09 god object/firmware/include/hardware/spiPacket.hpp rename to firmware/haptics/BIS week7/firmware/include/hardware/spiPacket.hpp diff --git a/firmware/09 god object/firmware/include/ioMain.hpp b/firmware/haptics/BIS week7/firmware/include/ioMain.hpp similarity index 100% rename from firmware/09 god object/firmware/include/ioMain.hpp rename to firmware/haptics/BIS week7/firmware/include/ioMain.hpp diff --git a/firmware/09 god object/firmware/include/physics/annotatedEdge.hpp b/firmware/haptics/BIS week7/firmware/include/physics/annotatedEdge.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/annotatedEdge.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/annotatedEdge.hpp diff --git a/firmware/09 god object/firmware/include/physics/collider.hpp b/firmware/haptics/BIS week7/firmware/include/physics/collider.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/collider.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/collider.hpp diff --git a/firmware/09 god object/firmware/include/physics/edge.hpp b/firmware/haptics/BIS week7/firmware/include/physics/edge.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/edge.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/edge.hpp diff --git a/firmware/09 god object/firmware/include/physics/godObject.hpp b/firmware/haptics/BIS week7/firmware/include/physics/godObject.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/godObject.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/godObject.hpp diff --git a/firmware/09 god object/firmware/include/physics/godObjectAction.hpp b/firmware/haptics/BIS week7/firmware/include/physics/godObjectAction.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/godObjectAction.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/godObjectAction.hpp diff --git a/firmware/09 god object/firmware/include/physics/godObjectActionData.hpp b/firmware/haptics/BIS week7/firmware/include/physics/godObjectActionData.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/godObjectActionData.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/godObjectActionData.hpp diff --git a/firmware/09 god object/firmware/include/physics/godObjectActionType.hpp b/firmware/haptics/BIS week7/firmware/include/physics/godObjectActionType.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/godObjectActionType.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/godObjectActionType.hpp diff --git a/firmware/09 god object/firmware/include/physics/hashtable.hpp b/firmware/haptics/BIS week7/firmware/include/physics/hashtable.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/hashtable.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/hashtable.hpp diff --git a/firmware/09 god object/firmware/include/physics/indexedEdge.hpp b/firmware/haptics/BIS week7/firmware/include/physics/indexedEdge.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/indexedEdge.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/indexedEdge.hpp diff --git a/firmware/09 god object/firmware/include/physics/obstacle.hpp b/firmware/haptics/BIS week7/firmware/include/physics/obstacle.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/obstacle.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/obstacle.hpp diff --git a/firmware/09 god object/firmware/include/physics/pantoPhysics.hpp b/firmware/haptics/BIS week7/firmware/include/physics/pantoPhysics.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/pantoPhysics.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/pantoPhysics.hpp diff --git a/firmware/09 god object/firmware/include/physics/rail.hpp b/firmware/haptics/BIS week7/firmware/include/physics/rail.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physics/rail.hpp rename to firmware/haptics/BIS week7/firmware/include/physics/rail.hpp diff --git a/firmware/09 god object/firmware/include/physicsMain.hpp b/firmware/haptics/BIS week7/firmware/include/physicsMain.hpp similarity index 100% rename from firmware/09 god object/firmware/include/physicsMain.hpp rename to firmware/haptics/BIS week7/firmware/include/physicsMain.hpp diff --git a/firmware/09 god object/firmware/include/tasks/task.hpp b/firmware/haptics/BIS week7/firmware/include/tasks/task.hpp similarity index 100% rename from firmware/09 god object/firmware/include/tasks/task.hpp rename to firmware/haptics/BIS week7/firmware/include/tasks/task.hpp diff --git a/firmware/09 god object/firmware/include/tasks/taskFunction.hpp b/firmware/haptics/BIS week7/firmware/include/tasks/taskFunction.hpp similarity index 100% rename from firmware/09 god object/firmware/include/tasks/taskFunction.hpp rename to firmware/haptics/BIS week7/firmware/include/tasks/taskFunction.hpp diff --git a/firmware/09 god object/firmware/include/tasks/taskRegistry.hpp b/firmware/haptics/BIS week7/firmware/include/tasks/taskRegistry.hpp similarity index 100% rename from firmware/09 god object/firmware/include/tasks/taskRegistry.hpp rename to firmware/haptics/BIS week7/firmware/include/tasks/taskRegistry.hpp diff --git a/firmware/09 god object/firmware/include/utils/assert.hpp b/firmware/haptics/BIS week7/firmware/include/utils/assert.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/assert.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/assert.hpp diff --git a/firmware/09 god object/firmware/include/utils/crashDump.hpp b/firmware/haptics/BIS week7/firmware/include/utils/crashDump.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/crashDump.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/crashDump.hpp diff --git a/firmware/09 god object/firmware/include/utils/framerateLimiter.hpp b/firmware/haptics/BIS week7/firmware/include/utils/framerateLimiter.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/framerateLimiter.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/framerateLimiter.hpp diff --git a/firmware/09 god object/firmware/include/utils/performanceMonitor.hpp b/firmware/haptics/BIS week7/firmware/include/utils/performanceMonitor.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/performanceMonitor.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/performanceMonitor.hpp diff --git a/firmware/09 god object/firmware/include/utils/receiveHandler.hpp b/firmware/haptics/BIS week7/firmware/include/utils/receiveHandler.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/receiveHandler.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/receiveHandler.hpp diff --git a/firmware/09 god object/firmware/include/utils/receiveState.hpp b/firmware/haptics/BIS week7/firmware/include/utils/receiveState.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/receiveState.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/receiveState.hpp diff --git a/firmware/09 god object/firmware/include/utils/serial.hpp b/firmware/haptics/BIS week7/firmware/include/utils/serial.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/serial.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/serial.hpp diff --git a/firmware/09 god object/firmware/include/utils/utils.hpp b/firmware/haptics/BIS week7/firmware/include/utils/utils.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/utils.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/utils.hpp diff --git a/firmware/09 god object/firmware/include/utils/vector.hpp b/firmware/haptics/BIS week7/firmware/include/utils/vector.hpp similarity index 100% rename from firmware/09 god object/firmware/include/utils/vector.hpp rename to firmware/haptics/BIS week7/firmware/include/utils/vector.hpp diff --git a/firmware/09 god object/firmware/platformio.ini b/firmware/haptics/BIS week7/firmware/platformio.ini similarity index 100% rename from firmware/09 god object/firmware/platformio.ini rename to firmware/haptics/BIS week7/firmware/platformio.ini diff --git a/firmware/09 god object/firmware/src/config/config.cpp b/firmware/haptics/BIS week7/firmware/src/config/config.cpp similarity index 100% rename from firmware/09 god object/firmware/src/config/config.cpp rename to firmware/haptics/BIS week7/firmware/src/config/config.cpp diff --git a/firmware/09 god object/firmware/src/hardware/panto.cpp b/firmware/haptics/BIS week7/firmware/src/hardware/panto.cpp similarity index 100% rename from firmware/09 god object/firmware/src/hardware/panto.cpp rename to firmware/haptics/BIS week7/firmware/src/hardware/panto.cpp diff --git a/firmware/09 god object/firmware/src/hardware/spiCommands.cpp b/firmware/haptics/BIS week7/firmware/src/hardware/spiCommands.cpp similarity index 100% rename from firmware/09 god object/firmware/src/hardware/spiCommands.cpp rename to firmware/haptics/BIS week7/firmware/src/hardware/spiCommands.cpp diff --git a/firmware/09 god object/firmware/src/hardware/spiEncoder.cpp b/firmware/haptics/BIS week7/firmware/src/hardware/spiEncoder.cpp similarity index 100% rename from firmware/09 god object/firmware/src/hardware/spiEncoder.cpp rename to firmware/haptics/BIS week7/firmware/src/hardware/spiEncoder.cpp diff --git a/firmware/09 god object/firmware/src/hardware/spiEncoderChain.cpp b/firmware/haptics/BIS week7/firmware/src/hardware/spiEncoderChain.cpp similarity index 100% rename from firmware/09 god object/firmware/src/hardware/spiEncoderChain.cpp rename to firmware/haptics/BIS week7/firmware/src/hardware/spiEncoderChain.cpp diff --git a/firmware/09 god object/firmware/src/hardware/spiPacket.cpp b/firmware/haptics/BIS week7/firmware/src/hardware/spiPacket.cpp similarity index 100% rename from firmware/09 god object/firmware/src/hardware/spiPacket.cpp rename to firmware/haptics/BIS week7/firmware/src/hardware/spiPacket.cpp diff --git a/firmware/09 god object/firmware/src/ioMain.cpp b/firmware/haptics/BIS week7/firmware/src/ioMain.cpp similarity index 100% rename from firmware/09 god object/firmware/src/ioMain.cpp rename to firmware/haptics/BIS week7/firmware/src/ioMain.cpp diff --git a/firmware/09 god object/firmware/src/main.cpp b/firmware/haptics/BIS week7/firmware/src/main.cpp similarity index 100% rename from firmware/09 god object/firmware/src/main.cpp rename to firmware/haptics/BIS week7/firmware/src/main.cpp diff --git a/firmware/09 god object/firmware/src/physics/annotatedEdge.cpp b/firmware/haptics/BIS week7/firmware/src/physics/annotatedEdge.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/annotatedEdge.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/annotatedEdge.cpp diff --git a/firmware/09 god object/firmware/src/physics/collider.cpp b/firmware/haptics/BIS week7/firmware/src/physics/collider.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/collider.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/collider.cpp diff --git a/firmware/haptics/BIS week7/firmware/src/physics/godObject.cpp b/firmware/haptics/BIS week7/firmware/src/physics/godObject.cpp new file mode 100644 index 0000000..9d62211 --- /dev/null +++ b/firmware/haptics/BIS week7/firmware/src/physics/godObject.cpp @@ -0,0 +1,486 @@ +#include "physics/godObject.hpp" + +#include "config/config.hpp" +#include "utils/serial.hpp" + +GodObject::GodObject(Vector2D position) + : m_position(position), m_tetherPosition(position), m_obstacleMutex(portMUX_INITIALIZER_UNLOCKED), m_possibleCollisions(new std::set()) +{ +} + +GodObject::~GodObject() +{ + delete m_possibleCollisions; +} + +void GodObject::setMovementDirection(Vector2D movementDirection) +{ + m_movementDirection = movementDirection; +} + +Hashtable& GodObject::hashtable() +{ + if (!m_hashtable) + { + m_hashtable = new Hashtable(); + } + return *m_hashtable; +} + +void GodObject::update() +{ + if (m_actionQueue.empty()) + { + return; + } + + portENTER_CRITICAL(&m_obstacleMutex); + for (auto i = 0; i < obstacleChangesPerFrame && !m_actionQueue.empty(); ++i) + { + auto action = m_actionQueue.front(); + m_actionQueue.pop_front(); + switch (action->m_type) + { + case HT_ENABLE_EDGE: + { + hashtable().add(action->m_data.m_annotatedEdge); + break; + } + case HT_DISABLE_EDGE: + { + hashtable().remove(action->m_data.m_annotatedEdge); + break; + } + case GO_REMOVE_OBSTACLE: + { + try{ + + delete m_obstacles.at(action->m_data.m_obstacleId); + m_obstacles.erase(action->m_data.m_obstacleId); + } catch (const std::out_of_range &oor){ + DPSerial::sendInstantDebugLog("Could not remove obstacle"); + } + break; + } + default: + { + break; + } + } + delete action; + } + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::dumpHashtable() +{ + portENTER_CRITICAL(&m_obstacleMutex); + hashtable().print(); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +// returns if force is applied or not +bool GodObject::move(bool isTweening, bool isFrozen) +{ + auto lastState = m_processingObstacleCollision; + // if the number of collisions increased since the last frame then we ran into a corner + auto lastNumCollisions = m_numCollisions; + + m_processingObstacleCollision = false; + + + Vector2D nextGoPosition; + Vector2D handlePosition = m_position + m_movementDirection; + if (isFrozen){ + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + return true; + } + if (isTweening) { + m_position = handlePosition; + if (m_tethered) { + m_tetherPosition = handlePosition; + } + return false; + } + float movementStepLength = m_movementDirection.length(); // only used for tethering + if (m_tethered && !isTweening) { + double distHandleToGo = m_movementDirection.length(); + if ((distHandleToGo < m_tetherInnerRadius) || (m_tetherState!=Outer && (distHandleToGo > 10))) { + // the latter condition can happen at startup of the device. + // in this case we don't want to apply forces. + m_tetherState = Inner; + return false; + } + // find out the current tether state + if (distHandleToGo > m_tetherInnerRadius && distHandleToGo < m_tetherOuterRadius){ + m_tetherState = Active; + } else { + m_tetherState = Outer; + } + // the speed of the god object increases proportionally with the distance bw handle and go. The max speed of the go is dependent on the outer tether radius. + movementStepLength = min(m_tetherOuterRadius, distHandleToGo); + + // this is the movement of the god object that follows the tether + if (distHandleToGo != 0) + { + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * m_tetherFactor); + } else { + nextGoPosition = handlePosition; + } + } else { + nextGoPosition = handlePosition; + } + + // no matter what the tether state is we need to check if the god object is colliding with an obstacle + Vector2D godObjectPos; + portENTER_CRITICAL(&m_obstacleMutex); + if (m_tethered && m_tetherState == Outer && m_tetherStrategy == Exploration){ + // pausing the game means that the god object position doesn't update. + godObjectPos = m_position; + } else { + // this is the default case + godObjectPos = checkCollisions(nextGoPosition, m_position); + } + if (m_tethered && m_processingObstacleCollision && m_tetherState == Outer && m_tetherStrategy != MaxSpeed) { + // for the Exploration and Leash mode the tether state can't be outer once the god object collides + // (otherwise the wall force will be weaker when the handle moves further into the wall) + m_tetherState = Active; + } + + // check if collision with a passable obstacle or a haptic guide is present + if (m_tethered && m_processingObstacleCollision && m_tetherState == Active) { + m_processingObstacleCollision = false; + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * c_railsTetherFactor); + auto pos = checkCollisions(nextGoPosition, m_position); + // if the handle is already past the obstacle (no more collision present) then the god object jumps to its position + if (!m_processingObstacleCollision) { + m_position = pos; + } else { + m_position = godObjectPos; + } + } else { + m_position = godObjectPos; + } + + if (m_tethered && m_tetherState == Outer && !isTweening && m_tetherStrategy != MaxSpeed) + { + m_tetherPosition = checkCollisions(handlePosition, m_tetherPosition); + } else { + m_tetherPosition = handlePosition; + } + portEXIT_CRITICAL(&m_obstacleMutex); + + m_doneColliding = lastState && !m_processingObstacleCollision; + + if (!m_tethered) { + if (m_processingObstacleCollision) + { + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + } + return m_processingObstacleCollision; + } else { + // newCollision is only important for the pock + bool newCollision = lastNumCollisions < m_numCollisions; + return processTetheringForce(handlePosition, newCollision); + } +} + +Vector2D GodObject::getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition){ + + // TODO: + // Given a position of god-object and the position of the handle, + // calculate rendering force. + + // Variables given: + // Vector2D godObjectPosition, handlePosition : position of god-object and handle. + // float K : proportional gain for force used in the god-object rendering. + + // Returns: + // Vector2D force, rendering force that you will apply to the device. + + const float K = forcePidFactor[0][0]; // check config.hpp for PID parameter + Vector2D force = Vector2D(0,0); + + // YOUR CODE STARTS + + + // YOUR CODE ENDS + return force; +} + +Vector2D GodObject::getTetherForce(Vector2D error){ + auto force = 0 + return force; +} + +void GodObject::renderForce(Vector2D collisionForce, Vector2D tetherForce) { + m_activeForce = collisionForce + tetherForce; +} + +bool GodObject::processTetheringForce(Vector2D handlePosition, bool newCollision) { + // returns if force is active or handle is freely moving + if (m_tetherState==Outer && m_tetherStrategy!=MaxSpeed) { + // for the 2 tether strategies where the god object is pulled on a virtual leash towards the tether position + auto error = m_movementDirection.normalize() * c_tetherForcePullingBack; + auto tetherForce = getTetherForce(error); //TODO: if we had a collision previously in Active state and moved from there into Outer state then the rendered force should be the sum of the previous force and the collision force + if (m_processingObstacleCollision) { + // think about adding a second pock here as well + renderForce(getCollisionForce(m_tetherPosition, handlePosition), tetherForce); + } else { + // weak constant force pulling the tether back to the god object + renderForce(Vector2D(0,0), tetherForce); + } + return true; + } else { + auto movementLength = (m_tetherPosition - m_position).length(); + auto error = m_movementDirection.normalize() * (m_tetherInnerRadius - movementLength); + if (newCollision && m_tetherPockEnabled) { + // weak constant force pushing the handle into the the wall so that the user gets force feedback at their fingertip + error = m_movementDirection.normalize() * c_tetherPockDistance; // this can't work because we include the last tether error + } + auto tetherForce = getTetherForce(error); + + if (m_processingObstacleCollision) { + // god object collision + renderForce(getCollisionForce(m_position, handlePosition), tetherForce); + return true; + } else { + if (!m_doneColliding) { + // regular tether force active that pushes the handle back to the inner tether radius + + renderForce(Vector2D(0,0), tetherForce); + } + return !m_doneColliding; + } + } + return false; +} + +Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPosition) +{ + /* + Collision detection works in 3 stages: + 1. Select collision candidates using a 2D lookup table. Every cell in that table contains the edges that go through it. That's why only particular edges have to be checked for collisions. + 2. The actual collision detection. + 3. If a collision is detected calculate the new god object position. + + If a collision is detected the collision detection is repeated with the new target position. This way we can check if the new position is accessible at all or not. + + Added by Julius on 30.09.20 + For more information check Lukas Wagners MT (section 4.3.1): https://www.dropbox.com/home/2018%20CHI%20Dueling%20Pantographs/Layer%202%20Firmware%20(Lukas%20Wagner)?preview=2019_09_07+ESP+Firmware+for+God+Haptic+Objects+%3D+Masterarbeit+(Lukas+Wagner).pdf + */ + + if (currentPosition == targetPoint || !m_hashtable) + { + return targetPoint; + } + // 1. select collision candidates + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + if (m_possibleCollisions->empty()) + { + return targetPoint; + } + + bool foundCollision; + u_short numCollisions = 0; + // 2. check if collisions are present between 2 vectors + // first vector goes from god object to handle and the + // second vector is the potential collision edge + do + { + // result vars + foundCollision = false; + double shortestMovementRatio = 0; + Vector2D closestEdgeFirst; + Vector2D closestEdgeFirstMinusSecond; + + // direction of movement: value is constant for loop + const auto posMinusTarget = currentPosition - targetPoint; + + if (posMinusTarget == Vector2D(0, 0)) + { + return targetPoint; + } + + for (auto&& indexedEdge : *m_possibleCollisions) + { + auto edge = indexedEdge.m_obstacle->getEdge(indexedEdge.m_index); + auto edgeFirst = edge.m_first; + auto firstMinusPos = edgeFirst - currentPosition; + auto firstMinusSecond = edgeFirst - edge.m_second; + auto divisor = + Vector2D::determinant(firstMinusSecond, posMinusTarget); + + auto movementRatio = + -Vector2D::determinant(firstMinusSecond, firstMinusPos) / + divisor; + if (movementRatio < 0 || movementRatio > 1) + { + continue; + } + + auto edgeRatio = + Vector2D::determinant(firstMinusPos, posMinusTarget) / divisor; + if (edgeRatio < 0 || edgeRatio > 1) + { + continue; + } + + // we have a collision! + if (!foundCollision || movementRatio < shortestMovementRatio) // I think the second condition never gets called because the movementRatio loop + // would already continue if the movementRatio was below 0 (which is the shortestMovementRatio) + { + // if a collision with a passable object is detected (e.g. a haptic rail) and the handle is not within the bounds of the colliding object, + // discard the collision and continue + auto ob = indexedEdge.m_obstacle; + if (ob->passable && !ob->contains(targetPoint)) + { + continue; + } + foundCollision = true; + if (!ob->passable){ + numCollisions++; + } + shortestMovementRatio = movementRatio; + closestEdgeFirst = edgeFirst; + closestEdgeFirstMinusSecond = firstMinusSecond; + } + } + + // calculate new god object position + if (foundCollision) + { + m_processingObstacleCollision = true; + + // god object slides along the colliding edge according to the handle movement but with tethered speed + + if (m_tethered) { + // if the movement is tethered the targetPoint can not be further away from the current position than the outer tether radius + const Vector2D movementVector = targetPoint - currentPosition; + double movementLength = min(m_tetherOuterRadius, movementVector.length()); + targetPoint = currentPosition + (movementVector.normalize() * movementLength); + } + + auto perpendicular = Vector2D( + -closestEdgeFirstMinusSecond.y, + closestEdgeFirstMinusSecond.x); + auto resolveRatio = + -Vector2D::determinant( + closestEdgeFirstMinusSecond, + closestEdgeFirst - targetPoint) / + Vector2D::determinant( + closestEdgeFirstMinusSecond, + perpendicular); + auto resolveVec = perpendicular * resolveRatio; + auto resolveLength = resolveVec.length(); + // c_resolveDistance is super small --> we need to add a tiny padding between the godobject and the edge so that it's not getting stuck in the edge + targetPoint = targetPoint - (resolveVec * ((resolveLength + c_resolveDistance) / resolveLength)); + + + // check for the new point if there is another collision with any other edge + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + } + // there can be multiple collisions, that's why we have to loop as well over the other possible collisions + } while (foundCollision); + m_numCollisions = numCollisions; + return targetPoint; +} + +void GodObject::createObstacle(uint16_t id, std::vector points, bool passable) +{ + // create obstacle or passable obstacle + auto ob = new Obstacle(points, passable); + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.emplace(id, ob); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::createRail(uint16_t id, std::vector points, double displacement) +{ + portENTER_CRITICAL(&m_obstacleMutex); + Rail* rail = new Rail(points, displacement); + m_obstacles.emplace(id, rail); + portEXIT_CRITICAL(&m_obstacleMutex); + return; + +} + +void GodObject::addToObstacle(uint16_t id, std::vector points) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.at(id)->add(points); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +void GodObject::removeObstacle(uint16_t id) +{ + enableObstacle(id, false); + m_actionQueue.push_back(new GodObjectAction(GO_REMOVE_OBSTACLE, id)); +} + +void GodObject::enableObstacle(uint16_t id, bool enable) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + if (enable != it->second->enabled()) + { + const auto edges = it->second->getIndexedEdges(); + const auto action = enable ? HT_ENABLE_EDGE : HT_DISABLE_EDGE; + for (const auto& edge : edges) + { + m_actionQueue.push_back(new GodObjectAction( + action, + new AnnotatedEdge( + new IndexedEdge(edge.m_obstacle, edge.m_index), + new Edge(edge.getEdge())))); + } + } + it->second->enable(enable); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +Vector2D GodObject::getPosition() +{ + return m_position; +} + +Vector2D GodObject::getActiveForce() +{ + return m_activeForce; +} + +bool GodObject::getProcessingObstacleCollision() +{ + return m_processingObstacleCollision; +} + +bool GodObject::getDoneColliding() +{ + return m_doneColliding; +} + +bool GodObject::tethered() +{ + return m_tethered; +} + +void GodObject::setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled) +{ + m_tethered = active; + m_tetherFactor = tetherFactor; + m_tetherInnerRadius = innerTetherRadius; + m_tetherOuterRadius = outerTetherRadius; + m_tetherStrategy = strategy; + m_tetherPockEnabled = pockEnabled; +} diff --git a/firmware/09 god object/firmware/src/physics/hashtable.cpp b/firmware/haptics/BIS week7/firmware/src/physics/hashtable.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/hashtable.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/hashtable.cpp diff --git a/firmware/09 god object/firmware/src/physics/indexedEdge.cpp b/firmware/haptics/BIS week7/firmware/src/physics/indexedEdge.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/indexedEdge.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/indexedEdge.cpp diff --git a/firmware/09 god object/firmware/src/physics/obstacle.cpp b/firmware/haptics/BIS week7/firmware/src/physics/obstacle.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/obstacle.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/obstacle.cpp diff --git a/firmware/09 god object/firmware/src/physics/pantoPhysics.cpp b/firmware/haptics/BIS week7/firmware/src/physics/pantoPhysics.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/pantoPhysics.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/pantoPhysics.cpp diff --git a/firmware/09 god object/firmware/src/physics/rail.cpp b/firmware/haptics/BIS week7/firmware/src/physics/rail.cpp similarity index 100% rename from firmware/09 god object/firmware/src/physics/rail.cpp rename to firmware/haptics/BIS week7/firmware/src/physics/rail.cpp diff --git a/firmware/haptics/BIS week7/firmware/src/physicsMain.cpp b/firmware/haptics/BIS week7/firmware/src/physicsMain.cpp new file mode 100644 index 0000000..2eedae2 --- /dev/null +++ b/firmware/haptics/BIS week7/firmware/src/physicsMain.cpp @@ -0,0 +1,153 @@ +#include "physicsMain.hpp" + +#include "hardware/panto.hpp" +#include "hardware/spiEncoderChain.hpp" +#include "physics/pantoPhysics.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/serial.hpp" + +FramerateLimiter spiErrorLimiter = FramerateLimiter::fromSeconds(1); + +#ifdef LINKAGE_ENCODER_USE_SPI +SPIEncoderChain* spi; +#endif + +void physicsSetup() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + spi = new SPIEncoderChain(numberOfSpiEncoders); + #endif + + for (auto i = 0; i < pantoCount; ++i) + { + pantos.emplace_back(i); + } + delay(1000); + + xTaskNotifyGive(Tasks.at("I/O").getHandle()); + + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + + EEPROM.begin(sizeof(uint32_t)*numberOfSpiEncoders); + + //calibrateEncoders; Comment if not needed + // for (auto i = 0; i < pantoCount; ++i) + // { pantos[i].calibrateEncoders(i);} + + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); //calibrating only handle pulse encoder + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + + pantos[i].setAngleAccessor(j, spi->getAngleAccessor(index)); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + + for (unsigned char i = 0; i < pantoCount; ++i) + { + pantoPhysics.emplace_back(&pantos[i]); + } +} + +void physicsLoop() +{ + PERFMON_START("[a] Read encoders"); + // PERFMON_START("[aa] Query SPI"); + #ifdef LINKAGE_ENCODER_USE_SPI + spi->update(); + #endif + // PERFMON_STOP("[aa] Query SPI"); + + // PERFMON_START("[ab] Calculation loop"); + for (auto i = 0; i < pantoCount; ++i) + { + // PERFMON_START("[aba] Actually read"); + pantos[i].readEncoders(); + // PERFMON_STOP("[aba] Actually read"); + PERFMON_START("[abb] Calc fwd kinematics"); + pantos[i].forwardKinematics(); + PERFMON_STOP("[abb] Calc fwd kinematics"); + } + // PERFMON_STOP("[ab] Calculation loop"); + PERFMON_STOP("[a] Read encoders"); + + PERFMON_START("[b] Calculate physics"); + for (auto i = 0; i < pantoCount; ++i) + { + pantoPhysics[i].step(); + } + PERFMON_STOP("[b] Calculate physics"); + + PERFMON_START("[c] Actuate motors"); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].actuateMotors(); + } + PERFMON_STOP("[c] Actuate motors"); + + if(spiErrorLimiter.step()) { + // DPSerial::sendQueuedDebugLog("SPI Errors: %i out of %i requests", spi->getErrors(), spi->getRequests()); + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[0][%i]: %i out of %i requests",i, + // pantos[0].getEncoderErrorCounts(i), pantos[0].getEncoderRequestsCounts(i)); + // } + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[1][%i]: %i out of %i requests",i, + // pantos[1].getEncoderErrorCounts(i), pantos[1].getEncoderRequestsCounts(i)); + // } + // spi->resetErrors(); + } + + PERFMON_START("[d] Calibrate Pantos"); + bool flag = false; + for(auto i = 0; i < pantoCount; ++i){ + if(pantos[i].getCalibrationState()){ + flag = true; + break; + } + } + if(flag){ + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + } + PERFMON_STOP("[d] Calibrate Pantos"); +} diff --git a/firmware/09 god object/firmware/src/tasks/task.cpp b/firmware/haptics/BIS week7/firmware/src/tasks/task.cpp similarity index 100% rename from firmware/09 god object/firmware/src/tasks/task.cpp rename to firmware/haptics/BIS week7/firmware/src/tasks/task.cpp diff --git a/firmware/09 god object/firmware/src/tasks/taskRegistry.cpp b/firmware/haptics/BIS week7/firmware/src/tasks/taskRegistry.cpp similarity index 100% rename from firmware/09 god object/firmware/src/tasks/taskRegistry.cpp rename to firmware/haptics/BIS week7/firmware/src/tasks/taskRegistry.cpp diff --git a/firmware/09 god object/firmware/src/utils/crashDump.cpp b/firmware/haptics/BIS week7/firmware/src/utils/crashDump.cpp similarity index 100% rename from firmware/09 god object/firmware/src/utils/crashDump.cpp rename to firmware/haptics/BIS week7/firmware/src/utils/crashDump.cpp diff --git a/firmware/09 god object/firmware/src/utils/framerateLimiter.cpp b/firmware/haptics/BIS week7/firmware/src/utils/framerateLimiter.cpp similarity index 100% rename from firmware/09 god object/firmware/src/utils/framerateLimiter.cpp rename to firmware/haptics/BIS week7/firmware/src/utils/framerateLimiter.cpp diff --git a/firmware/09 god object/firmware/src/utils/performanceMonitor.cpp b/firmware/haptics/BIS week7/firmware/src/utils/performanceMonitor.cpp similarity index 100% rename from firmware/09 god object/firmware/src/utils/performanceMonitor.cpp rename to firmware/haptics/BIS week7/firmware/src/utils/performanceMonitor.cpp diff --git a/firmware/09 god object/firmware/src/utils/serial.cpp b/firmware/haptics/BIS week7/firmware/src/utils/serial.cpp similarity index 100% rename from firmware/09 god object/firmware/src/utils/serial.cpp rename to firmware/haptics/BIS week7/firmware/src/utils/serial.cpp diff --git a/firmware/09 god object/firmware/src/utils/vector.cpp b/firmware/haptics/BIS week7/firmware/src/utils/vector.cpp similarity index 100% rename from firmware/09 god object/firmware/src/utils/vector.cpp rename to firmware/haptics/BIS week7/firmware/src/utils/vector.cpp diff --git a/firmware/09 god object/utils/backtrace/backtrace.sh b/firmware/haptics/BIS week7/utils/backtrace/backtrace.sh similarity index 100% rename from firmware/09 god object/utils/backtrace/backtrace.sh rename to firmware/haptics/BIS week7/utils/backtrace/backtrace.sh diff --git a/firmware/09 god object/utils/protocol/include/protocol/header.hpp b/firmware/haptics/BIS week7/utils/protocol/include/protocol/header.hpp similarity index 100% rename from firmware/09 god object/utils/protocol/include/protocol/header.hpp rename to firmware/haptics/BIS week7/utils/protocol/include/protocol/header.hpp diff --git a/firmware/09 god object/utils/protocol/include/protocol/messageType.hpp b/firmware/haptics/BIS week7/utils/protocol/include/protocol/messageType.hpp similarity index 100% rename from firmware/09 god object/utils/protocol/include/protocol/messageType.hpp rename to firmware/haptics/BIS week7/utils/protocol/include/protocol/messageType.hpp diff --git a/firmware/09 god object/utils/protocol/include/protocol/protocol.hpp b/firmware/haptics/BIS week7/utils/protocol/include/protocol/protocol.hpp similarity index 100% rename from firmware/09 god object/utils/protocol/include/protocol/protocol.hpp rename to firmware/haptics/BIS week7/utils/protocol/include/protocol/protocol.hpp diff --git a/firmware/09 god object/utils/protocol/src/protocol/messageType.cpp b/firmware/haptics/BIS week7/utils/protocol/src/protocol/messageType.cpp similarity index 100% rename from firmware/09 god object/utils/protocol/src/protocol/messageType.cpp rename to firmware/haptics/BIS week7/utils/protocol/src/protocol/messageType.cpp diff --git a/firmware/09 god object/utils/protocol/src/protocol/protocol.cpp b/firmware/haptics/BIS week7/utils/protocol/src/protocol/protocol.cpp similarity index 100% rename from firmware/09 god object/utils/protocol/src/protocol/protocol.cpp rename to firmware/haptics/BIS week7/utils/protocol/src/protocol/protocol.cpp diff --git a/firmware/09 god object/utils/scripts/generateHardwareConfig.js b/firmware/haptics/BIS week7/utils/scripts/generateHardwareConfig.js similarity index 100% rename from firmware/09 god object/utils/scripts/generateHardwareConfig.js rename to firmware/haptics/BIS week7/utils/scripts/generateHardwareConfig.js diff --git a/firmware/09 god object/utils/scripts/run.js b/firmware/haptics/BIS week7/utils/scripts/run.js similarity index 100% rename from firmware/09 god object/utils/scripts/run.js rename to firmware/haptics/BIS week7/utils/scripts/run.js diff --git a/firmware/09 god object/utils/scripts/tools.js b/firmware/haptics/BIS week7/utils/scripts/tools.js similarity index 100% rename from firmware/09 god object/utils/scripts/tools.js rename to firmware/haptics/BIS week7/utils/scripts/tools.js diff --git a/firmware/09 god object/utils/serial/CMakeLists.txt b/firmware/haptics/BIS week7/utils/serial/CMakeLists.txt similarity index 100% rename from firmware/09 god object/utils/serial/CMakeLists.txt rename to firmware/haptics/BIS week7/utils/serial/CMakeLists.txt diff --git a/firmware/09 god object/utils/serial/binding.gyp b/firmware/haptics/BIS week7/utils/serial/binding.gyp similarity index 100% rename from firmware/09 god object/utils/serial/binding.gyp rename to firmware/haptics/BIS week7/utils/serial/binding.gyp diff --git a/firmware/09 god object/utils/serial/include/crashAnalyzer.hpp b/firmware/haptics/BIS week7/utils/serial/include/crashAnalyzer.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/crashAnalyzer.hpp rename to firmware/haptics/BIS week7/utils/serial/include/crashAnalyzer.hpp diff --git a/firmware/09 god object/utils/serial/include/libInterface.hpp b/firmware/haptics/BIS week7/utils/serial/include/libInterface.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/libInterface.hpp rename to firmware/haptics/BIS week7/utils/serial/include/libInterface.hpp diff --git a/firmware/09 god object/utils/serial/include/node.hpp b/firmware/haptics/BIS week7/utils/serial/include/node.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/node.hpp rename to firmware/haptics/BIS week7/utils/serial/include/node.hpp diff --git a/firmware/09 god object/utils/serial/include/packet.hpp b/firmware/haptics/BIS week7/utils/serial/include/packet.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/packet.hpp rename to firmware/haptics/BIS week7/utils/serial/include/packet.hpp diff --git a/firmware/09 god object/utils/serial/include/serial.hpp b/firmware/haptics/BIS week7/utils/serial/include/serial.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/serial.hpp rename to firmware/haptics/BIS week7/utils/serial/include/serial.hpp diff --git a/firmware/09 god object/utils/serial/include/serial_export.hpp b/firmware/haptics/BIS week7/utils/serial/include/serial_export.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/serial_export.hpp rename to firmware/haptics/BIS week7/utils/serial/include/serial_export.hpp diff --git a/firmware/09 god object/utils/serial/include/standalone.hpp b/firmware/haptics/BIS week7/utils/serial/include/standalone.hpp similarity index 100% rename from firmware/09 god object/utils/serial/include/standalone.hpp rename to firmware/haptics/BIS week7/utils/serial/include/standalone.hpp diff --git a/firmware/09 god object/utils/serial/src/cppLib/lib.cpp b/firmware/haptics/BIS week7/utils/serial/src/cppLib/lib.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/cppLib/lib.cpp rename to firmware/haptics/BIS week7/utils/serial/src/cppLib/lib.cpp diff --git a/firmware/09 god object/utils/serial/src/crashAnalyzer/analyze.cpp b/firmware/haptics/BIS week7/utils/serial/src/crashAnalyzer/analyze.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/crashAnalyzer/analyze.cpp rename to firmware/haptics/BIS week7/utils/serial/src/crashAnalyzer/analyze.cpp diff --git a/firmware/09 god object/utils/serial/src/crashAnalyzer/buffer.cpp b/firmware/haptics/BIS week7/utils/serial/src/crashAnalyzer/buffer.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/crashAnalyzer/buffer.cpp rename to firmware/haptics/BIS week7/utils/serial/src/crashAnalyzer/buffer.cpp diff --git a/firmware/09 god object/utils/serial/src/node/main.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/main.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/main.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/main.cpp diff --git a/firmware/09 god object/utils/serial/src/node/poll.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/poll.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/poll.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/poll.cpp diff --git a/firmware/09 god object/utils/serial/src/node/receiveHelpers.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/receiveHelpers.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/receiveHelpers.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/receiveHelpers.cpp diff --git a/firmware/09 god object/utils/serial/src/node/send.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/send.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/send.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/send.cpp diff --git a/firmware/09 god object/utils/serial/src/node/sendHelpers.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/sendHelpers.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/sendHelpers.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/sendHelpers.cpp diff --git a/firmware/09 god object/utils/serial/src/node/setup.cpp b/firmware/haptics/BIS week7/utils/serial/src/node/setup.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/node/setup.cpp rename to firmware/haptics/BIS week7/utils/serial/src/node/setup.cpp diff --git a/firmware/09 god object/utils/serial/src/serial/packet.cpp b/firmware/haptics/BIS week7/utils/serial/src/serial/packet.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/serial/packet.cpp rename to firmware/haptics/BIS week7/utils/serial/src/serial/packet.cpp diff --git a/firmware/09 god object/utils/serial/src/serial/shared.cpp b/firmware/haptics/BIS week7/utils/serial/src/serial/shared.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/serial/shared.cpp rename to firmware/haptics/BIS week7/utils/serial/src/serial/shared.cpp diff --git a/firmware/09 god object/utils/serial/src/serial/unix.cpp b/firmware/haptics/BIS week7/utils/serial/src/serial/unix.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/serial/unix.cpp rename to firmware/haptics/BIS week7/utils/serial/src/serial/unix.cpp diff --git a/firmware/09 god object/utils/serial/src/serial/win.cpp b/firmware/haptics/BIS week7/utils/serial/src/serial/win.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/serial/win.cpp rename to firmware/haptics/BIS week7/utils/serial/src/serial/win.cpp diff --git a/firmware/09 god object/utils/serial/src/standalone/main.cpp b/firmware/haptics/BIS week7/utils/serial/src/standalone/main.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/standalone/main.cpp rename to firmware/haptics/BIS week7/utils/serial/src/standalone/main.cpp diff --git a/firmware/09 god object/utils/serial/src/standalone/standalone.cpp b/firmware/haptics/BIS week7/utils/serial/src/standalone/standalone.cpp similarity index 100% rename from firmware/09 god object/utils/serial/src/standalone/standalone.cpp rename to firmware/haptics/BIS week7/utils/serial/src/standalone/standalone.cpp diff --git a/firmware/09 god object/utils/serial/unity/Serial.cs b/firmware/haptics/BIS week7/utils/serial/unity/Serial.cs similarity index 100% rename from firmware/09 god object/utils/serial/unity/Serial.cs rename to firmware/haptics/BIS week7/utils/serial/unity/Serial.cs diff --git a/firmware/09 god object/utils/serial/unity/linux.sh b/firmware/haptics/BIS week7/utils/serial/unity/linux.sh similarity index 100% rename from firmware/09 god object/utils/serial/unity/linux.sh rename to firmware/haptics/BIS week7/utils/serial/unity/linux.sh diff --git a/firmware/09 god object/utils/serial/unity/mac.sh b/firmware/haptics/BIS week7/utils/serial/unity/mac.sh similarity index 100% rename from firmware/09 god object/utils/serial/unity/mac.sh rename to firmware/haptics/BIS week7/utils/serial/unity/mac.sh diff --git a/firmware/09 god object/utils/serial/unity/win.bat b/firmware/haptics/BIS week7/utils/serial/unity/win.bat similarity index 100% rename from firmware/09 god object/utils/serial/unity/win.bat rename to firmware/haptics/BIS week7/utils/serial/unity/win.bat diff --git a/firmware/haptics/force field/firmware/.gitignore b/firmware/haptics/force field/firmware/.gitignore new file mode 100644 index 0000000..6106fab --- /dev/null +++ b/firmware/haptics/force field/firmware/.gitignore @@ -0,0 +1,4 @@ +.vscode +.pio +.pioenvs +.piolibdeps diff --git a/firmware/haptics/force field/firmware/.idea/.gitignore b/firmware/haptics/force field/firmware/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/firmware/haptics/force field/firmware/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/firmware/haptics/force field/firmware/.idea/misc.xml b/firmware/haptics/force field/firmware/.idea/misc.xml new file mode 100644 index 0000000..d858eb1 --- /dev/null +++ b/firmware/haptics/force field/firmware/.idea/misc.xml @@ -0,0 +1,17 @@ + + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/.idea/vcs.xml b/firmware/haptics/force field/firmware/.idea/vcs.xml new file mode 100644 index 0000000..8aefe6c --- /dev/null +++ b/firmware/haptics/force field/firmware/.idea/vcs.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/include/config/config.hpp b/firmware/haptics/force field/firmware/include/config/config.hpp new file mode 100644 index 0000000..ab0218a --- /dev/null +++ b/firmware/haptics/force field/firmware/include/config/config.hpp @@ -0,0 +1,103 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is 40. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {0x8F, 0x37, 0xA2, 0x7A, 0x29, 0xDA, 0x7D, 0xB4, 0xBB, 0x54, 0x0F, 0x13, 0x43, 0x71, 0xBC, 0xE9}; +const float opMinDist = 65, + opMaxDist = 210, + opAngle = 2.2; +extern float forceFactor; +const uint8_t pantoCount = 2; +const uint8_t dummyPin = 40; +#define LINKAGE_ENCODER_USE_SPI +const uint32_t numberOfSpiEncoders = 4; +const float linkageBaseX[] = { + -23.7, 61.1, 0, -61.1, 23.7, 0 +}; +const float linkageBaseY[] = { + 0, 0, 0, 0, 0, 0 +}; +const float linkageInnerLength[] = { + 95.07, 57.71, 0, 57.71, 95.07, 0 +}; +const float linkageOuterLength[] = { + 119.79, 119.79, 0, 119.79, 119.79, 0 +}; +const uint8_t linkageHandleMount[] = { + 0, 0, 1, 0, 0, 0 +}; +const float motorPowerLimit[] = { + 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 +}; +const float motor_powerLimitForce[] = { + 0.6, 0.6, 0.2, 0.6, 0.6, 0.2 +}; +extern float pidFactor[6][3]; +const float forceP = 0.375; +const float forceI = 0; +const float forceD = 0; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + 40, 40, 25, 40, 40, 23 +}; +const uint8_t motorPwmPinForwards[] = { + 16, 2, 40, 22, 19, 40 +}; +const uint8_t motorPwmPinBackwards[] = { + 4, 17, 40, 21, 18, 40 +}; +const uint8_t motorDirAPin[] = { + 40, 40, 32, 40, 40, 26 +}; +const uint8_t motorDirBPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const bool motorFlipped[] = { + false, false, true, false, false, true +}; +const uint8_t encoderAPin[] = { + 40, 40, 35, 40, 40, 36 +}; +const uint8_t encoderBPin[] = { + 40, 40, 34, 40, 40, 39 +}; +const uint8_t encoderIndexPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const uint32_t encoderSteps[] = { + 16384, 16384, 260, 16384, 16384, 260 +}; +const uint32_t encoderSpiIndex[] = { + 3, 2, 4294967295, 0, 1, 4294967295 +}; +const float encoderFlipped[] = { + 1, 1, 1, -1, -1, -1 +}; +const float setupAngle[] = { + -0.5, 0.001, 0, -0.5, 0.001, 0 +}; +constexpr float rangeMinX = -180; +constexpr float rangeMinY = -205; +constexpr float rangeMaxX = 180; +constexpr float rangeMaxY = 5; +constexpr uint32_t hashtableMaxMemory = 100000; +constexpr uint32_t hashtableUsedMemory = 98532; +constexpr uint32_t hashtableMaxCells = 8333; +constexpr uint32_t hashtableNumCells = 8211; +constexpr uint32_t hashtableStepsX = 119; +constexpr uint32_t hashtableStepsY = 69; +constexpr double hashtableStepSizeX = 3.0252100840336134; +constexpr double hashtableStepSizeY = 3.0434782608695654; +const uint32_t obstacleChangesPerFrame = 1; \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/include/hardware/angleAccessor.hpp b/firmware/haptics/force field/firmware/include/hardware/angleAccessor.hpp new file mode 100644 index 0000000..fc7e636 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/angleAccessor.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function AngleAccessor; diff --git a/firmware/haptics/force field/firmware/include/hardware/panto.hpp b/firmware/haptics/force field/firmware/include/hardware/panto.hpp new file mode 100644 index 0000000..143d308 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/panto.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include + +#include "config/config.hpp" +#include "hardware/angleAccessor.hpp" +#include "utils/vector.hpp" +#include + +// make sure results are in range -270° ~ 0° ~ +90° +#define ensureAngleRange(angle) \ + angle > HALF_PI ? \ + angle - TWO_PI : \ + angle < -(PI + HALF_PI) ? \ + angle + TWO_PI : \ + angle + +class Panto +{ +private: + static const uint16_t c_ledcFrequency = 20000; + static const uint16_t c_ledcResolution = 12; + static const uint16_t PWM_MAX = 4095; // (2^12)-1 + static const uint8_t c_dofCount = 3; + const uint8_t c_localLeftIndex = 0; + const uint8_t c_localRightIndex = 1; + const uint8_t c_localHandleIndex = 2; + + const uint8_t c_pantoIndex; + const uint8_t c_globalIndexOffset; + const uint8_t c_globalLeftIndex; + const uint8_t c_globalRightIndex; + const uint8_t c_globalHandleIndex; + const float c_leftInnerLength; + const float c_rightInnerLength; + const float c_leftOuterLength; + const float c_rightOuterLength; + const float c_leftInnerLengthDoubled; + const float c_rightInnerLengthDoubled; + const float c_leftInnerLengthSquared; + const float c_rightInnerLengthSquared; + const float c_leftOuterLengthSquared; + const float c_rightOuterLengthSquared; + const float c_leftInnerLengthSquaredMinusLeftOuterLengthSquared; + const float c_rightInnerLengthSquaredMinusRightOuterLengthSquared; + const float c_leftOuterLengthSquaredMinusRightOuterLengthSquared; + const bool c_handleMountedOnRightArm; + const float c_leftBaseX; + const float c_leftBaseY; + const float c_rightBaseX; + const float c_rightBaseY; + + #ifdef LINKAGE_ENCODER_USE_SPI + AngleAccessor m_angleAccessors[2]; + #endif + Encoder* m_encoder[c_dofCount]; + float m_actuationAngle[c_dofCount]; + float m_previousAngle[c_dofCount]; + float m_previousAngles[c_dofCount][5]; + float m_targetAngle[c_dofCount]; + float m_previousDiff[c_dofCount]; + float m_integral[c_dofCount]; + uint32_t m_prevTime = 0; + + int m_previousAnglesCount = 0; + int m_encoderErrorCount = 0; + int m_encoderErrorCounts[4] = {0,0,0,0}; + int m_encoderRequestCount = 0; + int m_encoderRequestCounts[4] = {0,0,0,0}; + float m_leftInnerAngle = 0; + float m_rightInnerAngle = 0; + float m_pointingAngle = 0; + float m_handleX = 0; + float m_handleY = 0; + float m_targetX = 0; + float m_targetY = 0; + float m_startX = 0; + float m_startY = 0; + float m_filteredX = 0; + float m_filteredY = 0; + float m_tweeningValue = 0.0f; + float m_tweeningStep = 0.00001f; + float m_tweeningSpeed = 1.0f; + uint32_t m_tweeningPrevtime = 0; + float m_dt = 0.0001f; + float delta = 0.01; + float velocity = 1.0f; + bool m_isforceRendering = false; + bool m_inTransition = false; + float m_jacobian[2][2] = {{0.0, 0.0}, {0.0, 0.0}}; + bool m_isFrozen = false; + bool m_isCalibrating = false; + + void inverseKinematics(); + void setMotor( + const uint8_t& localIndex, const bool& dir, const float& power); + void disengageMotors(); +public: + Panto(uint8_t pantoIndex); + float getActuationAngle(const uint8_t index) const; + Vector2D getPosition() const; + float getRotation() const; + void setAngleAccessor(const uint8_t index, const AngleAccessor accessor); + void setTarget(const Vector2D target, const bool isForceRendering); + void setSpeed(const float speed); + void setRotation(const float rotation); + bool getCalibrationState(); + void calibrateEncoders(); + void calibratePanto(); + void calibrationEnd(); + void resetActuationAngle(); + void readEncoders(); + void forwardKinematics(); + void actuateMotors(); + int getEncoderErrorCount(); + int getEncoderRequests(); + int getEncoderErrorCounts(int i); + int getEncoderRequestsCounts(int i); + uint8_t getPantoIndex(); + bool getInTransition(); + void setInTransition(bool inTransition); + bool getIsFrozen(); + void setIsFrozen(bool isFrozen); +}; + +extern std::vector pantos; \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/include/hardware/spiCommands.hpp b/firmware/haptics/force field/firmware/include/hardware/spiCommands.hpp new file mode 100644 index 0000000..4f80dbe --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/spiCommands.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace SPICommands +{ + extern const uint16_t c_readAngle; + extern const uint16_t c_clearError; + extern const uint16_t c_nop; + extern const uint16_t c_highZeroRead; + extern const uint16_t c_lowZeroRead; + extern const uint16_t c_highZeroWrite; + extern const uint16_t c_lowZeroWrite; +}; diff --git a/firmware/haptics/force field/firmware/include/hardware/spiEncoder.hpp b/firmware/haptics/force field/firmware/include/hardware/spiEncoder.hpp new file mode 100644 index 0000000..21e9575 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/spiEncoder.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include "hardware/spiPacket.hpp" + +class SPIEncoder +{ + friend class SPIEncoderChain; +private: + SPIPacket m_lastPacket; + uint16_t m_lastValidAngle; + SPIClass* m_spi; + SPIEncoder() = default; + SPIEncoder(SPIClass* spi); + void transfer(uint16_t transmisson); +public: + uint32_t getAngle(); +}; diff --git a/firmware/haptics/force field/firmware/include/hardware/spiEncoderChain.hpp b/firmware/haptics/force field/firmware/include/hardware/spiEncoderChain.hpp new file mode 100644 index 0000000..0b5bb86 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/spiEncoderChain.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include "hardware/angleAccessor.hpp" +#include "hardware/spiEncoder.hpp" + +class SPIEncoderChain +{ +private: + SPISettings m_settings; + SPIClass m_spi; + uint32_t m_numberOfEncoders; + std::vector m_encoders; + std::vector m_values; + std::vector m_zeros; + uint32_t errors = 0; + uint32_t requests = 0; + uint8_t m_maxTries = 4; + uint8_t m_currentTry = 0; + // static const uint32_t c_hspiSsPin = 15; + + static const uint32_t c_hspiSsPin1 = 15; + static const uint32_t c_hspiSsPin2 = 5; + static const uint16_t c_nop = 0x0; + static const uint16_t c_clearError = 0x4001; + static const uint16_t c_readAngle = 0xFFFF; + static const uint16_t c_dataMask = 0x3FFF; + + void begin(); + void end(); + std::vector getZero(); + void transfer(uint16_t transmission); + void setZero(std::vector newZero); +public: + SPIEncoderChain(uint32_t numberOfEncoders); + void update(int channel); + void update(); + void clearError(); + void setZero(); + bool needsZero(); + void wakeUp(); + void __setZeros(); + void setPosition(std::vector positions); + uint32_t getErrors(); + uint32_t getRequests(); + void resetErrors(); + AngleAccessor getAngleAccessor(uint32_t index); +}; \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/include/hardware/spiPacket.hpp b/firmware/haptics/force field/firmware/include/hardware/spiPacket.hpp new file mode 100644 index 0000000..73750d6 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/hardware/spiPacket.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +struct SPIPacket +{ + bool m_parity; + bool m_flag; + uint16_t m_data; + bool m_valid; + uint16_t m_transmission; + static const uint16_t c_parityMask = 0x8000; + static const uint16_t c_flagMask = 0x4000; + static const uint16_t c_dataMask = 0x3FFF; + SPIPacket(bool flag, uint16_t data); + SPIPacket(uint16_t transmisson); + SPIPacket() = default; + bool calcParity(); + bool parityError(); + bool commandInvalidError(); + bool framingError(); +}; diff --git a/firmware/haptics/force field/firmware/include/ioMain.hpp b/firmware/haptics/force field/firmware/include/ioMain.hpp new file mode 100644 index 0000000..1641622 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/ioMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void ioSetup(); +void ioLoop(); diff --git a/firmware/haptics/force field/firmware/include/physics/annotatedEdge.hpp b/firmware/haptics/force field/firmware/include/physics/annotatedEdge.hpp new file mode 100644 index 0000000..d2c8370 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/annotatedEdge.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +struct Edge; +struct IndexedEdge; +class Obstacle; + +struct AnnotatedEdge +{ + IndexedEdge* m_indexedEdge; + Edge* m_edge; + AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge); + AnnotatedEdge(const AnnotatedEdge& other); + void destroy(); +}; diff --git a/firmware/haptics/force field/firmware/include/physics/collider.hpp b/firmware/haptics/force field/firmware/include/physics/collider.hpp new file mode 100644 index 0000000..5d78845 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/collider.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" +#include "utils/vector.hpp" + +class Collider +{ +protected: + std::vector m_points; +public: + Collider(std::vector points); + void add(std::vector points); + virtual bool contains(Vector2D point); + Edge getEdge(uint32_t index); +}; diff --git a/firmware/haptics/force field/firmware/include/physics/edge.hpp b/firmware/haptics/force field/firmware/include/physics/edge.hpp new file mode 100644 index 0000000..35b00ff --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/edge.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "utils/vector.hpp" + +struct Edge +{ + Vector2D m_first; + Vector2D m_second; + Edge(Vector2D first, Vector2D second) : m_first(first), m_second(second) {}; + Edge() : m_first(Vector2D()), m_second(Vector2D()) {}; +}; diff --git a/firmware/haptics/force field/firmware/include/physics/godObject.hpp b/firmware/haptics/force field/firmware/include/physics/godObject.hpp new file mode 100644 index 0000000..919353c --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/godObject.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include + +#include "hardware/panto.hpp" +#include "physics/godObjectAction.hpp" +#include "physics/indexedEdge.hpp" +#include "physics/obstacle.hpp" +#include "physics/rail.hpp" +#include "physics/hashtable.hpp" +#include "utils/vector.hpp" + +/* +The speed control is in detail described in our Dropbox (Interaction Techniques Haptic in Unity): +https://www.dropbox.com/scl/fi/uljoe140fet2b53bjhr4y/DualPanto-Speed-Control.pptx?dl=0&rlkey=6k77wrfnb3oaxg186489tpinj +*/ + +enum TetherState {Inner, Active, Outer}; +/* the handle can be in 3 states: + 1. Inner: the handle can freely move within a close radius around the god object without triggering speed control (otherwise there would always be force rendered, even if the player was just standing around). + 2. Active: the god object is under speed control. Its acceleration is proportional to the distance between god object and handle. + 3. Outer: out of outer tether radius. Multiple strategies to treat this scenario are described below. +*/ + +enum OutOfTetherStrategy {MaxSpeed, Exploration, Leash}; +/* possible strategies when the handle is pushed out of the outer tether radius. + MaxSpeed: the god object continues to move at max speed and the handle gets pushed back to the new god object position player. On the Unity side there could be an additional penalization (with auditory cues), e.g. that the player loses health points when moving out of the tether (in FPS). + Exploration: the god object position doesn't update anymore and the game is paused (speed control is disabled). The handle can still collide with obstacles and is in exploration mode. + Leash: the handle is "on leash" and can collide with obstacles. The god object is moving at max speed to the position of the handle (along the direct vector between both points). A weak constant pulling force indicates where the god object is. +*/ + +class GodObject +{ +private: + static constexpr double c_resolveDistance = 0.00001; + static constexpr double c_tetherForcePullingBack = -0.5; + static constexpr double c_tetherPockDistance = 500; // when tethered and collision with wall happens push the handle this far into the wall (along the direction of movement) + static constexpr double c_railsTetherFactor = 0.75; // if speed control is enabled and the god object collides with a wall: move the god object by this factor along the vector between handle and god object. This enables jumping rails under speed control. + + Vector2D m_position; + Vector2D m_tetherPosition; + Vector2D m_movementDirection; + Vector2D m_activeForce; + std::map m_obstacles; + Hashtable *m_hashtable = nullptr; + portMUX_TYPE m_obstacleMutex; + bool m_processingObstacleCollision; + u_short m_numCollisions = 0; // for speed control; when the speed is controlled and a collision with multiple walls occurs (in a corner) then the 2nd collision must also be feelable + bool m_doneColliding; + Vector2D m_lastError; + std::set* m_possibleCollisions; + std::deque m_actionQueue; + + // tether related properties + bool m_tethered = false; + double m_tetherFactor = 0; + Vector2D m_lastErrorTether; + double m_tetherInnerRadius = 0; + double m_tetherOuterRadius = 0; + TetherState m_tetherState = Inner; + double m_tetherSafeZonePadding = 0; // padding on the inner border to avoid that the tether gets pushed into the free moving zone immediately once the inner radius is passed + OutOfTetherStrategy m_tetherStrategy = Leash; + bool m_tetherPockEnabled = false; + + Hashtable& hashtable(); + +public: + GodObject(Vector2D position = Vector2D()); + ~GodObject(); + void setMovementDirection(Vector2D movementDirection); + void update(); + void dumpHashtable(); + bool move(bool isTweening, bool isFrozen); + Vector2D checkCollisions(Vector2D targetPoint, Vector2D currentPosition); + void createObstacle(uint16_t id, std::vector points, bool passable); + void createRail(uint16_t id, std::vector points, double displacement); + void addToObstacle(uint16_t id, std::vector points); + void removeObstacle(uint16_t id); + void enableObstacle(uint16_t id, bool enable = true); + Vector2D getPosition(); + Vector2D getActiveForce(); + Vector2D getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition); + Vector2D getTetherForce(Vector2D error); + void renderForce(Vector2D collisionForce, Vector2D tetherForce); + bool processTetheringForce(Vector2D handlePosition, bool lastCollisionState); + bool getProcessingObstacleCollision(); + bool getDoneColliding(); + bool tethered(); + void setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled); +}; diff --git a/firmware/haptics/force field/firmware/include/physics/godObjectAction.hpp b/firmware/haptics/force field/firmware/include/physics/godObjectAction.hpp new file mode 100644 index 0000000..97c9c5d --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/godObjectAction.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "physics/annotatedEdge.hpp" +#include "physics/godObjectActionData.hpp" +#include "physics/godObjectActionType.hpp" + +struct GodObjectAction +{ + GodObjectActionType m_type; + GodObjectActionData m_data; + GodObjectAction( + GodObjectActionType type, + AnnotatedEdge* data) + : m_type(type) + , m_data{.m_annotatedEdge = data} { }; + GodObjectAction( + GodObjectActionType type, + uint16_t data) + : m_type(type) + // Intellisense might show an error here, but that's a bug. + // https://github.com/Microsoft/vscode-cpptools/issues/3491 + , m_data{.m_obstacleId = data} { }; +}; diff --git a/firmware/haptics/force field/firmware/include/physics/godObjectActionData.hpp b/firmware/haptics/force field/firmware/include/physics/godObjectActionData.hpp new file mode 100644 index 0000000..1a23cac --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/godObjectActionData.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include + +#include "physics/annotatedEdge.hpp" +#include "physics/obstacle.hpp" + +union GodObjectActionData +{ + AnnotatedEdge* m_annotatedEdge; + uint16_t m_obstacleId; +}; diff --git a/firmware/haptics/force field/firmware/include/physics/godObjectActionType.hpp b/firmware/haptics/force field/firmware/include/physics/godObjectActionType.hpp new file mode 100644 index 0000000..9b31a78 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/godObjectActionType.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum GodObjectActionType +{ + HT_ENABLE_EDGE, + HT_DISABLE_EDGE, + GO_REMOVE_OBSTACLE +}; diff --git a/firmware/haptics/force field/firmware/include/physics/hashtable.hpp b/firmware/haptics/force field/firmware/include/physics/hashtable.hpp new file mode 100644 index 0000000..2a272d0 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/hashtable.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include "config/config.hpp" +#include "hardware/panto.hpp" +#include "physics/annotatedEdge.hpp" +#include "physics/indexedEdge.hpp" + +class Hashtable +{ +private: + static int32_t get1dIndex(double value, double min, double step); + + std::vector m_cells[hashtableNumCells]; + std::vector getCellIndices(Edge edge); + std::set expand(const std::vector& edges); +public: + Hashtable(); + void add(AnnotatedEdge* edge); + void remove(AnnotatedEdge* edge); + void getPossibleCollisions(Edge movement, std::set* result); + void print(); +}; diff --git a/firmware/haptics/force field/firmware/include/physics/indexedEdge.hpp b/firmware/haptics/force field/firmware/include/physics/indexedEdge.hpp new file mode 100644 index 0000000..4b20339 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/indexedEdge.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" + +class Obstacle; + +struct IndexedEdge +{ + Obstacle* m_obstacle; + uint32_t m_index; + IndexedEdge(Obstacle* obstacle, uint32_t index); + bool operator==(const IndexedEdge& other) const; + bool operator<(const IndexedEdge& other) const; + Edge getEdge() const; +}; diff --git a/firmware/haptics/force field/firmware/include/physics/obstacle.hpp b/firmware/haptics/force field/firmware/include/physics/obstacle.hpp new file mode 100644 index 0000000..1f67687 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/obstacle.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +#include "physics/indexedEdge.hpp" +#include "physics/collider.hpp" + +class Obstacle : public Collider +{ +private: + bool m_enabled = false; + +public: + bool passable = false; + Obstacle(std::vector points); + Obstacle(std::vector points, bool passable); + bool enabled(); + void enable(bool enable = true); + std::vector getIndexedEdges( + uint32_t first = 0, uint32_t last = -1); +}; diff --git a/firmware/haptics/force field/firmware/include/physics/pantoPhysics.hpp b/firmware/haptics/force field/firmware/include/physics/pantoPhysics.hpp new file mode 100644 index 0000000..8b5bded --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/pantoPhysics.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include "hardware/panto.hpp" +#include "physics/godObject.hpp" +#include "utils/vector.hpp" + +class PantoPhysics +{ +private: + Panto* m_panto; + Vector2D m_currentPosition; + GodObject* m_godObject; +public: + PantoPhysics(Panto* panto); + GodObject* godObject(); + void step(); +}; + +extern std::vector pantoPhysics; diff --git a/firmware/haptics/force field/firmware/include/physics/rail.hpp b/firmware/haptics/force field/firmware/include/physics/rail.hpp new file mode 100644 index 0000000..00f2631 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physics/rail.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "physics/obstacle.hpp" + +class Rail : public Obstacle +{ + +private: + uint32_t displacement; // orthogonal distance to the rail until where the user still feels the applied force of the rail + +public: + Rail(std::vector points, double displacement); + virtual bool contains(Vector2D point); +}; diff --git a/firmware/haptics/force field/firmware/include/physicsMain.hpp b/firmware/haptics/force field/firmware/include/physicsMain.hpp new file mode 100644 index 0000000..e98e807 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/physicsMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void physicsSetup(); +void physicsLoop(); diff --git a/firmware/haptics/force field/firmware/include/tasks/task.hpp b/firmware/haptics/force field/firmware/include/tasks/task.hpp new file mode 100644 index 0000000..ec2acf9 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/tasks/task.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "tasks/taskFunction.hpp" + +class Task +{ +private: + // default values + static const uint32_t c_defaultStackDepth = 8192; + static const uint32_t c_defaultPriority = 1; + static const uint32_t c_defaultFpsInterval = 1000; + + // task data + TaskFunction m_setupFunc; + TaskFunction m_loopFunc; + const char* m_name; + uint32_t m_stackDepth; + uint32_t m_priority; + TaskHandle_t m_handle; + int m_core; + + // task function + static void taskLoop(void* parameters); + + // fps counter data + uint32_t m_fpsInterval; + FramerateLimiter m_fpsCalcLimiter; + uint32_t m_loopCount; + static std::map s_fpsMap; + bool m_logFps; + + // fps counter funcs + inline void initFps(); + inline void checkFps(); +public: + Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core); + void run(); + void setLogFps(bool logFps = true); + TaskHandle_t getHandle(); +}; diff --git a/firmware/haptics/force field/firmware/include/tasks/taskFunction.hpp b/firmware/haptics/force field/firmware/include/tasks/taskFunction.hpp new file mode 100644 index 0000000..e3fb3dc --- /dev/null +++ b/firmware/haptics/force field/firmware/include/tasks/taskFunction.hpp @@ -0,0 +1,3 @@ +#pragma once + +typedef void (*TaskFunction)(); diff --git a/firmware/haptics/force field/firmware/include/tasks/taskRegistry.hpp b/firmware/haptics/force field/firmware/include/tasks/taskRegistry.hpp new file mode 100644 index 0000000..8bcf229 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/tasks/taskRegistry.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include +#include + +#include "tasks/task.hpp" + +typedef std::map TaskRegistry; +extern TaskRegistry Tasks; diff --git a/firmware/haptics/force field/firmware/include/utils/assert.hpp b/firmware/haptics/force field/firmware/include/utils/assert.hpp new file mode 100644 index 0000000..70f3d82 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/assert.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "utils/serial.hpp" + +void __fail( + const char* check, std::string val1, std::string val2, + const char* file, int line, const char* func) +{ + DPSerial::sendInstantDebugLog( + "Assertion %s failed with values %s and %s: file \"%s\", line %i, %s", + check, val1.c_str(), val2.c_str(), file, line, func); + abort(); +} + +#define __CHECK(operator, val1, val2) (val1 operator val2) ?\ + (void)0 :\ + __fail(\ + #val1 " " #operator " " #val2,\ + std::to_string(val1), std::to_string(val2),\ + __FILE__, __LINE__, __PRETTY_FUNCTION__); + +#define ASSERT_EQ(val1, val2) __CHECK(==, val1, val2) +#define ASSERT_LT(val1, val2) __CHECK(<, val1, val2) +#define ASSERT_LE(val1, val2) __CHECK(<=, val1, val2) +#define ASSERT_GT(val1, val2) __CHECK(>, val1, val2) +#define ASSERT_GE(val1, val2) __CHECK(>=, val1, val2) +#define ASSERT_NE(val1, val2) __CHECK(!=, val1, val2) diff --git a/firmware/haptics/force field/firmware/include/utils/crashDump.hpp b/firmware/haptics/force field/firmware/include/utils/crashDump.hpp new file mode 100644 index 0000000..d526096 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/crashDump.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +class CrashDump +{ +private: + std::ostringstream m_stream; +public: + CrashDump(); + void clear(); + void dump(); + template CrashDump& operator<<(T value) + { + m_stream << value; + return *this; + } +}; + +#ifdef ENABLE_CRASHDUMP +#define CRASHDUMP(dump, call) dump call +#else +#define CRASHDUMP(dump, call) +#endif + +extern CrashDump physicsDump; diff --git a/firmware/haptics/force field/firmware/include/utils/framerateLimiter.hpp b/firmware/haptics/force field/firmware/include/utils/framerateLimiter.hpp new file mode 100644 index 0000000..94d43b2 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/framerateLimiter.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +class FramerateLimiter +{ +private: + static const uint64_t c_periodLength = ULONG_MAX; + static uint64_t s_period; + static uint64_t s_lastNow; + static uint64_t now(); + uint64_t m_delta; + uint64_t m_last; + FramerateLimiter(uint64_t microseconds); +public: + static FramerateLimiter fromFPS(double fps); + static FramerateLimiter fromSeconds(uint64_t seconds); + static FramerateLimiter fromMilliseconds(uint64_t milliseconds); + static FramerateLimiter fromMicroseconds(uint64_t microseconds); + bool step(); + void reset(); +}; diff --git a/firmware/haptics/force field/firmware/include/utils/performanceMonitor.hpp b/firmware/haptics/force field/firmware/include/utils/performanceMonitor.hpp new file mode 100644 index 0000000..eff8a4f --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/performanceMonitor.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include + +class PerformanceMonitor +{ +private: + static const uint32_t c_measurementCount = 1000; + struct PerformanceEntry + { + std::vector m_values; + uint32_t m_index = 0; + uint64_t m_sum = 0; + bool m_running = false; + uint64_t m_start = 0; + PerformanceEntry(); + }; + std::map> m_entries; +public: + void start(std::string label); + void stop(std::string label); + std::vector getResults(); +}; + +#ifdef ENABLE_PERFMON +#define PERFMON_START(label) PerfMon.start(label); +#define PERFMON_STOP(label) PerfMon.stop(label); +#else +#define PERFMON_START(label) +#define PERFMON_STOP(label) +#endif + +extern PerformanceMonitor PerfMon; diff --git a/firmware/haptics/force field/firmware/include/utils/receiveHandler.hpp b/firmware/haptics/force field/firmware/include/utils/receiveHandler.hpp new file mode 100644 index 0000000..3b33431 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/receiveHandler.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function ReceiveHandler; diff --git a/firmware/haptics/force field/firmware/include/utils/receiveState.hpp b/firmware/haptics/force field/firmware/include/utils/receiveState.hpp new file mode 100644 index 0000000..abb521f --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/receiveState.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; diff --git a/firmware/haptics/force field/firmware/include/utils/serial.hpp b/firmware/haptics/force field/firmware/include/utils/serial.hpp new file mode 100644 index 0000000..accc846 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/serial.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "utils/receiveHandler.hpp" +#include "utils/receiveState.hpp" + +class DPSerial : DPProtocol +{ +private: + // receive buffer limits + static const uint16_t c_rxBufferSize = 10 * c_maxPacketSize; + static const uint16_t c_rxBufferCriticalThreshold = 5 * c_maxPacketSize; + static const uint16_t c_rxBufferReadyThreshold = 2 * c_maxPacketSize; + static bool s_rxBufferCritical; + + // data storage + static Header s_header; + static const uint16_t c_debugLogBufferSize = 256; + static uint8_t s_debugLogBuffer[c_debugLogBufferSize]; + static std::queue s_debugLogQueue; + static const uint16_t c_processedQueuedMessagesPerFrame = 4; + + // multithreading safety + static portMUX_TYPE s_serialMutex; + + static ReceiveState s_receiveState; + static uint8_t s_expectedPacketId; + + // connection + static bool s_connected; + static const uint16_t c_heartbeatIntervalMs = 1000; + static unsigned long s_lastHeartbeatTime; + static const uint16_t c_maxUnacklowledgedHeartbeats = 5; + static uint16_t s_unacknowledgedHeartbeats; + + // send helper + static void sendUInt8(uint8_t data); + static void sendInt16(int16_t data); + static void sendUInt16(uint16_t data); + static void sendInt32(int32_t data); + static void sendUInt32(uint32_t data); + static void sendFloat(float data); + static void sendMessageType(MessageType data); + static void sendMagicNumber(); + static void sendHeader(MessageType messageType, uint16_t payloadSize); + + // send + static void sendSync(); + static void sendHeartbeat(); + static void sendBufferCritical(); + static void sendBufferReady(); + static void sendPacketAck(uint8_t id); + static void sendInvalidPacketId(uint8_t expected, uint8_t received); + static void sendInvalidData(); + + // receive helper + static uint8_t receiveUInt8(); + static int16_t receiveInt16(); + static uint16_t receiveUInt16(); + static int32_t receiveInt32(); + static uint32_t receiveUInt32(); + static float receiveFloat(); + static MessageType receiveMessageType(); + static void checkBuffer(); + static bool receiveMagicNumber(); + static bool receiveHeader(); + static bool payloadReady(); + + // receive + static void receiveSyncAck(); + static void receiveHearbeatAck(); + static void receiveMotor(); + static void receiveSpeed(); + static void receivePID(); + static void receiveCreateObstacle(); + static void receiveCreatePassableObstacle(); + static void receiveCreateRail(); + static void receiveAddToObstacle(); + static void receiveRemoveObstacle(); + static void receiveEnableObstacle(); + static void receiveDisableObstacle(); + static void receiveCalibrationRequest(); + static void receiveDumpHashtable(); + static void receiveInvalid(); + static void receiveFreeze(); + static void receiveFree(); + static void receiveSpeedControl(); + + // map of receive handlers + static std::map s_receiveHandlers; + +public: + // delete contructor - this class only contains static members + DPSerial() = delete; + + // setup + static void init(); + static bool ensureConnection(); + + // send + static void sendPosition(); + static void sendTransitionEnded(uint8_t panto); + static void sendInstantDebugLog(const char *message, ...); + static void sendQueuedDebugLog(const char *message, ...); + static void processDebugLogQueue(); + static void sendDebugData(); + + // receive + static void receive(); +}; diff --git a/firmware/haptics/force field/firmware/include/utils/utils.hpp b/firmware/haptics/force field/firmware/include/utils/utils.hpp new file mode 100644 index 0000000..1eaca9e --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/utils.hpp @@ -0,0 +1,7 @@ +#pragma once + +// template func must be in header to compile all necessary variants +template int sgn(T val) +{ + return (T(0) < val) - (val < T(0)); +}; diff --git a/firmware/haptics/force field/firmware/include/utils/vector.hpp b/firmware/haptics/force field/firmware/include/utils/vector.hpp new file mode 100644 index 0000000..02bf456 --- /dev/null +++ b/firmware/haptics/force field/firmware/include/utils/vector.hpp @@ -0,0 +1,22 @@ +#pragma once + +struct Vector2D +{ + double x, y; + + Vector2D() {} + Vector2D(double _x, double _y) : x(_x), y(_y) {} + + static Vector2D fromPolar(double angle, double length); + static double determinant(const Vector2D& first, const Vector2D& second); + double length() const; + double angle() const; + Vector2D normalize() const; + Vector2D operator+(const Vector2D& other) const; + Vector2D operator-(const Vector2D& other) const; + double operator*(const Vector2D& other) const; + Vector2D operator*(const double scale) const; + bool operator==(const Vector2D& other) const; + bool operator!=(const Vector2D &other) const; + double distancePointToLineSegment(Vector2D a, Vector2D b); +}; diff --git a/firmware/haptics/force field/firmware/platformio.ini b/firmware/haptics/force field/firmware/platformio.ini new file mode 100644 index 0000000..3cb404a --- /dev/null +++ b/firmware/haptics/force field/firmware/platformio.ini @@ -0,0 +1,32 @@ +[platformio] +description = Dualpanto firmware + +[protocol] +base_dir = ../utils/protocol +include_dir = ${protocol.base_dir}/include +src_dir = ${protocol.base_dir}/src +src_filter = +<../${protocol.src_dir}> + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +build_flags = + -I${protocol.include_dir} + -D_GLIBCXX_USE_C99 + -DENABLE_FPS + -g + -frtti + -mtext-section-literals +build_unflags = + # use this while debugging + # -Os +src_filter = + +<*> + ${protocol.src_filter} +monitor_speed = 115200 +lib_deps = https://github.com/PaulStoffregen/Encoder.git + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +upload_port = /dev/cu.SLAB_USBtoUART diff --git a/firmware/haptics/force field/firmware/src/config/config.cpp b/firmware/haptics/force field/firmware/src/config/config.cpp new file mode 100644 index 0000000..202b2c5 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/config/config.cpp @@ -0,0 +1,12 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = 0.0085; +float pidFactor[6][3] = { + {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30}, {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30} +}; \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/src/hardware/panto.cpp b/firmware/haptics/force field/firmware/src/hardware/panto.cpp new file mode 100644 index 0000000..063b680 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/hardware/panto.cpp @@ -0,0 +1,618 @@ +#include "hardware/panto.hpp" + +#include + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantos; + +void Panto::forwardKinematics() +{ + // base angles + // PERFMON_START("[abba] base angles"); + const auto leftBaseAngle = m_actuationAngle[c_localLeftIndex]; + const auto rightBaseAngle = m_actuationAngle[c_localRightIndex]; + const auto handleAngle = m_actuationAngle[c_localHandleIndex]; + // PERFMON_STOP("[abba] base angles"); + + // base angle sin / cos + // PERFMON_START("[abbb] base angle sin / cos"); + const auto leftBaseAngleSin = std::sin(leftBaseAngle); + const auto leftBaseAngleCos = std::cos(leftBaseAngle); + // PERFMON_STOP("[abbb] base angle sin / cos"); + + // calculate inner positions + // PERFMON_START("[abbc] calculate inner positions"); + const auto leftInnerX = + fmaf(leftBaseAngleCos, c_leftInnerLength, c_leftBaseX); + const auto leftInnerY = + fmaf(leftBaseAngleSin, c_leftInnerLength, c_leftBaseY); + const auto rightInnerX = + fmaf(std::cos(rightBaseAngle), c_rightInnerLength, c_rightBaseX); + const auto rightInnerY = + fmaf(std::sin(rightBaseAngle), c_rightInnerLength, c_rightBaseY); + // PERFMON_STOP("[abbc] calculate inner positions"); + + // diagonal between inner positions + // PERFMON_START("[abbd] diagonal between inner positions"); + const auto diagonalX = rightInnerX - leftInnerX; + const auto diagonalY = rightInnerY - leftInnerY; + const auto diagonalSquared = diagonalX * diagonalX + diagonalY * diagonalY; + const auto diagonalLength = std::sqrt(diagonalSquared); + // PERFMON_STOP("[abbd] diagonal between inner positions"); + + // left elbow angles + // - inside is between diagonal and linkage + // - offset is between zero and diagonal + // - total is between zero and linkage + // PERFMON_START("[abbe] left elbow angles"); + const auto leftElbowInsideAngleCos = + (diagonalSquared + + c_leftOuterLengthSquaredMinusRightOuterLengthSquared) / + (2 * diagonalLength * c_leftOuterLength); + const auto leftElbowInsideAngle = -std::acos(leftElbowInsideAngleCos); + const auto leftElbowOffsetAngle = std::atan2(diagonalY, diagonalX); + const auto leftElbowTotalAngle = + leftElbowInsideAngle + leftElbowOffsetAngle; + // PERFMON_STOP("[abbe] left elbow angles"); + + // left elbow angle sin / cos + // PERFMON_START("[abbf] left elbow angle sin / cos"); + const auto leftElbowTotalAngleSin = + std::sin(leftElbowTotalAngle); + const auto leftElbowTotalAngleCos = + std::cos(leftElbowTotalAngle); + // PERFMON_STOP("[abbf] left elbow angle sin / cos"); + + // handle position + // PERFMON_START("[abbg] handle position"); + m_handleX = + fmaf(leftElbowTotalAngleCos, c_leftOuterLength, leftInnerX); + m_handleY = + fmaf(leftElbowTotalAngleSin, c_leftOuterLength, leftInnerY); + // PERFMON_STOP("[abbg] handle position"); + + // right elbow angles + // PERFMON_START("[abbh] right elbow angles"); + const auto rightDiffX = m_handleX - rightInnerX; + const auto rightDiffY = m_handleY - rightInnerY; + const auto rightElbowTotalAngle = std::atan2(rightDiffY, rightDiffX); + // PERFMON_STOP("[abbh] right elbow angles"); + + // store angles + // PERFMON_START("[abbi] store angles"); + m_leftInnerAngle = leftElbowTotalAngle; + m_rightInnerAngle = rightElbowTotalAngle; + m_pointingAngle = + handleAngle + + (encoderFlipped[c_globalHandleIndex]==1? -1 : 1)* //sign changes when encoder is flipped + (c_handleMountedOnRightArm==1 ? + (-rightElbowTotalAngle) : + (leftElbowTotalAngle)); + + // PERFMON_STOP("[abbi] store angles"); + + // some weird diffs and their sinuses + // PERFMON_START("[abbj] some weird diffs and their sinuses"); + const auto rightElbowTotalAngleMinusLeftBaseAngle = + rightElbowTotalAngle - leftBaseAngle; + const auto rightElbowTotalAngleMinusLeftBaseAngleSin = + std::sin(rightElbowTotalAngleMinusLeftBaseAngle); + const auto rightElbowTotalAngleMinusRightBaseAngle = + rightElbowTotalAngle - rightBaseAngle; + const auto rightElbowTotalAngleMinusRightBaseAngleSin = + std::sin(rightElbowTotalAngleMinusRightBaseAngle); + const auto leftElbowTotalAngleMinusRightElbowTotalAngle = + leftElbowTotalAngle - rightElbowTotalAngle; + const auto leftElbowTotalAngleMinusRightElbowTotalAngleSin = + std::sin(leftElbowTotalAngleMinusRightElbowTotalAngle); + // PERFMON_STOP("[abbj] some weird diffs and their sinuses"); + + // shared factors for rows/columns + // PERFMON_START("[abbk] shared factors for rows/columns"); + const auto upperRow = + c_leftInnerLength * rightElbowTotalAngleMinusLeftBaseAngleSin; + const auto lowerRow = + c_rightInnerLength * rightElbowTotalAngleMinusRightBaseAngleSin; + const auto leftColumn = + leftElbowTotalAngleSin / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + const auto rightColumn = + leftElbowTotalAngleCos / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + // PERFMON_STOP("[abbk] shared factors for rows/columns"); + + // set jacobian matrix + // PERFMON_START("[abbl] set jacobian matrix"); + m_jacobian[0][0] = + (-c_leftInnerLength * leftBaseAngleSin) - (upperRow * leftColumn); + m_jacobian[0][1] = + (c_leftInnerLength * leftBaseAngleCos) + (upperRow * rightColumn); + m_jacobian[1][0] = + lowerRow * leftColumn; + m_jacobian[1][1] = + -lowerRow * rightColumn; + // PERFMON_STOP("[abbl] set jacobian matrix"); + inverseKinematics(); +} + +void Panto::inverseKinematics() +{ + + //update tweening delta micro here + unsigned long now = micros(); + float tweening_dt = now - m_tweeningPrevtime; + m_tweeningPrevtime = now; + + if (isnan(m_targetX) || isnan(m_targetY)) + { + m_targetAngle[c_localLeftIndex] = NAN; + m_targetAngle[c_localRightIndex] = NAN; + } + else if (m_isforceRendering) + { + m_targetAngle[c_localLeftIndex] = + m_jacobian[0][0] * m_targetX + + m_jacobian[0][1] * m_targetY; + m_targetAngle[c_localRightIndex] = + m_jacobian[1][0] * m_targetX + + m_jacobian[1][1] * m_targetY; + } + else + { + // tweening + const auto leftBaseToTargetX = m_filteredX - c_leftBaseX; + const auto leftBaseToTargetY = m_filteredY - c_leftBaseY; + const auto rightBaseToTargetX = m_filteredX - c_rightBaseX; + const auto rightBaseToTargetY = m_filteredY - c_rightBaseY; + const auto leftBaseToTargetSquared = + leftBaseToTargetX * leftBaseToTargetX + + leftBaseToTargetY * leftBaseToTargetY; + const auto rightBaseToTargetSquared = + rightBaseToTargetX * rightBaseToTargetX + + rightBaseToTargetY * rightBaseToTargetY; + const auto leftBaseToTargetLength = + std::sqrt(leftBaseToTargetSquared); + const auto rightBaseToTargetLength = + std::sqrt(rightBaseToTargetSquared); + + const auto leftInnerAngleCos = + (leftBaseToTargetSquared + + c_leftInnerLengthSquaredMinusLeftOuterLengthSquared) / + (c_leftInnerLengthDoubled * leftBaseToTargetLength); + const auto rightInnerAngleCos = + (rightBaseToTargetSquared + + c_rightInnerLengthSquaredMinusRightOuterLengthSquared) / + (c_rightInnerLengthDoubled * rightBaseToTargetLength); + const auto leftInnerAngle = std::acos(leftInnerAngleCos); + const auto rightInnerAngle = std::acos(rightInnerAngleCos); + const auto leftOffsetAngle = + std::atan2(leftBaseToTargetY, leftBaseToTargetX); + const auto rightOffsetAngle = + std::atan2(rightBaseToTargetY, rightBaseToTargetX); + + const auto leftAngle = leftOffsetAngle - leftInnerAngle; + const auto rightAngle = rightOffsetAngle + rightInnerAngle; + + m_targetAngle[c_localLeftIndex] = ensureAngleRange(leftAngle); + m_targetAngle[c_localRightIndex] = ensureAngleRange(rightAngle); + + if(abs(m_filteredX - m_targetX) + abs(m_filteredY - m_targetY) < 0.01f && m_inTransition){ + m_inTransition = false; + DPSerial::sendTransitionEnded(getPantoIndex()); + } + + m_filteredX = (m_targetX-m_startX)*m_tweeningValue+m_startX; + m_filteredY = (m_targetY-m_startY)*m_tweeningValue+m_startY; + float stepValue = 0.000001 * tweening_dt * m_tweeningSpeed; + m_tweeningValue=min(m_tweeningValue+stepValue, 1.0f); + + } +}; + +void Panto::setMotor( + const uint8_t& localIndex, const bool& dir, const float& power) +{ + const auto globalIndex = c_globalIndexOffset + localIndex; + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] == dummyPin) + { + return; + } + + const auto flippedDir = dir ^ motorFlipped[globalIndex]; + + if(motorPwmPinForwards[globalIndex] != dummyPin) + { + if(!flippedDir) { + ledcWrite(globalIndex+6, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + else { + ledcWrite(globalIndex, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex+6, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + return; + } + + + digitalWrite(motorDirAPin[globalIndex], flippedDir); + digitalWrite(motorDirBPin[globalIndex], !flippedDir); + ledcWrite(globalIndex, min(power, motorPowerLimit[globalIndex]) * PWM_MAX); +}; + +void Panto::readEncoders() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_previousAngle[localIndex] = + ensureAngleRange( + encoderFlipped[globalIndex] * + TWO_PI * m_angleAccessors[localIndex]() / + encoderSteps[globalIndex]); + m_encoderRequestCount++; + m_encoderRequestCounts[localIndex]++; + } + m_actuationAngle[c_localHandleIndex] = + (m_encoder[c_localHandleIndex]) ? + (encoderFlipped[c_globalHandleIndex] * + TWO_PI * m_encoder[c_localHandleIndex]->read() / + encoderSteps[c_globalHandleIndex]) : + NAN; + #else + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = + ensureAngleRange( + (m_encoder[localIndex]) ? + (encoderFlipped[globalIndex] * + TWO_PI * m_encoder[localIndex]->read() / + encoderSteps[globalIndex]) : + NAN); + } + #endif + + m_previousAngle[c_localHandleIndex] = m_actuationAngle[c_localHandleIndex]; + m_actuationAngle[c_localHandleIndex] = fmod(m_actuationAngle[c_localHandleIndex], TWO_PI); + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + if(m_previousAngle[localIndex]==0)return; + } + if(m_previousAnglesCount>4){ + m_previousAnglesCount=0; + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + float std = 0.0f; + float mean = 0.0f; + for(int i = 0; i < 5; i++){ + mean+=m_previousAngles[localIndex][i]; + }mean/=5.0f; + for(int i = 0; i < 5; i++){ + std+=(m_previousAngles[localIndex][i]-mean)*(m_previousAngles[localIndex][i]-mean); + }std /=5.0f; + if(std < 1.0f){ + m_actuationAngle[localIndex] = m_previousAngles[localIndex][4]; + } + else{ + m_encoderErrorCounts[localIndex]++; + // DPSerial::sendQueuedDebugLog("jumps at [panto %d][motor %d] (std>1.0f) mean = %f",c_pantoIndex, localIndex, mean); + // for(int i = 0; i < 5; i++){ + // DPSerial::sendQueuedDebugLog("previousAngles[%d][%d]=%f",localIndex, i, m_previousAngles[localIndex][i]); + // } + // m_actuationAngle[localIndex] = m_previousAngle[localIndex]; + } + } + } + else{ + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + m_previousAngles[localIndex][m_previousAnglesCount] = m_previousAngle[localIndex]; + } + } + m_previousAnglesCount++; +}; + +void Panto::actuateMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + if (isnan(m_targetAngle[localIndex])) + { + // free motor + setMotor(localIndex, false, 0); + } else if (m_isforceRendering) + { + setMotor( + localIndex, + m_targetAngle[localIndex] < 0, + fabs(m_targetAngle[localIndex]) * forceFactor); + } else + { + auto error = + m_targetAngle[localIndex] - m_actuationAngle[localIndex]; + if (localIndex == c_localHandleIndex) + { + // Linkage offsets handle + error -= + c_handleMountedOnRightArm ? + m_rightInnerAngle : + m_leftInnerAngle; + if (error > PI) + { + error -= TWO_PI; + } + else if (error < -PI) + { + error += TWO_PI; + } + if(encoderFlipped[c_globalHandleIndex]==1) error*=-1; + } + unsigned char dir = error < 0; + unsigned long now = micros(); + float dt = now - m_prevTime; + m_prevTime = now; + error = fabs(error); + // Power: PID + m_integral[localIndex] = min(0.5f, m_integral[localIndex] + error * dt); + float derivative = (error - m_previousDiff[localIndex]) / dt; + m_previousDiff[localIndex] = error; + const auto globalIndex = c_globalIndexOffset + localIndex; + const auto& pid = pidFactor[globalIndex]; + float pVal = pid[0] * error; + float dVal = pid[2] * derivative; + dVal = pVal + dVal > 0 ? dVal : 0; + setMotor( + localIndex, + dir, + pVal + + pid[1] * m_integral[localIndex] + + dVal); + } + } +}; + +void Panto::disengageMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + m_targetAngle[localIndex] = NAN; + setMotor(localIndex, false, 0); + } + m_targetX = NAN; + m_targetY = NAN; +}; + +Panto::Panto(uint8_t pantoIndex) +: c_pantoIndex(pantoIndex) +, c_globalIndexOffset(c_pantoIndex * c_dofCount) +, c_globalLeftIndex(c_globalIndexOffset + c_localLeftIndex) +, c_globalRightIndex(c_globalIndexOffset + c_localRightIndex) +, c_globalHandleIndex(c_globalIndexOffset + c_localHandleIndex) +, c_leftInnerLength(linkageInnerLength[c_globalLeftIndex]) +, c_rightInnerLength(linkageInnerLength[c_globalRightIndex]) +, c_leftOuterLength(linkageOuterLength[c_globalLeftIndex]) +, c_rightOuterLength(linkageOuterLength[c_globalRightIndex]) +, c_leftInnerLengthDoubled(2 * c_leftInnerLength) +, c_rightInnerLengthDoubled(2 * c_rightInnerLength) +, c_leftInnerLengthSquared(c_leftInnerLength * c_leftInnerLength) +, c_rightInnerLengthSquared(c_rightInnerLength * c_rightInnerLength) +, c_leftOuterLengthSquared(c_leftOuterLength * c_leftOuterLength) +, c_rightOuterLengthSquared(c_rightOuterLength * c_rightOuterLength) +, c_leftInnerLengthSquaredMinusLeftOuterLengthSquared( + c_leftInnerLengthSquared - c_leftOuterLengthSquared) +, c_rightInnerLengthSquaredMinusRightOuterLengthSquared( + c_rightInnerLengthSquared - c_rightOuterLengthSquared) +, c_leftOuterLengthSquaredMinusRightOuterLengthSquared( + c_leftOuterLengthSquared - c_rightOuterLengthSquared) +, c_handleMountedOnRightArm(linkageHandleMount[c_globalHandleIndex] == 1) +, c_leftBaseX(linkageBaseX[c_globalLeftIndex]) +, c_leftBaseY(linkageBaseY[c_globalLeftIndex]) +, c_rightBaseX(linkageBaseX[c_globalRightIndex]) +, c_rightBaseY(linkageBaseY[c_globalRightIndex]) +{ + m_targetX = NAN; + m_targetY = NAN; + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + m_targetAngle[localIndex] = NAN; + m_previousDiff[localIndex] = 0.0; + m_integral[localIndex] = 0.0; + if (encoderAPin[globalIndex] != dummyPin && + encoderBPin[globalIndex] != dummyPin) + { + m_encoder[localIndex] = new Encoder( + encoderAPin[globalIndex], encoderBPin[globalIndex]); + } + else + { + m_encoder[localIndex] = NULL; + } + + // we don't need additional checks aroud these - if the dummyPin is set properly, the ESP lib will check this anyway + pinMode(encoderIndexPin[globalIndex], INPUT); + pinMode(motorDirAPin[globalIndex], OUTPUT); + pinMode(motorDirBPin[globalIndex], OUTPUT); + + if(motorPwmPinForwards[globalIndex] == dummyPin) { + pinMode(motorPwmPin[globalIndex], OUTPUT); + + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcAttachPin(motorPwmPin[globalIndex], globalIndex); + } + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] != dummyPin) { + pinMode(motorPwmPinForwards[globalIndex], OUTPUT); + pinMode(motorPwmPinBackwards[globalIndex], OUTPUT); + + // TODO: initiate the PWM channels independent from globalIndex + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcSetup(globalIndex+6, c_ledcFrequency, c_ledcResolution); + + //DPSerial::sendInstantDebugLog("attaching gi %i to pwm %i and pwm %i\n", globalIndex, motorPwmPinForwards[globalIndex], motorPwmPinBackwards[globalIndex]); + + ledcAttachPin(motorPwmPinForwards[globalIndex], globalIndex); + ledcAttachPin(motorPwmPinBackwards[globalIndex], globalIndex+6); + + ledcWrite(globalIndex, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex, 0); + delay(10); + ledcWrite(globalIndex+6, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex+6, 0); + /* + ledcWrite(globalIndex, 0.2*PWM_MAX); + delay(2000); + ledcWrite(globalIndex, 0); + */ + + } + // TODO: Calibration + // Use encoder index pin and actuate the motors to reach it + setMotor(localIndex, false, 0); + } +}; + +void Panto::calibrateEncoders(){ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + //Write encoder values to EEPROM + EEPROM.writeInt((3*c_pantoIndex*sizeof(uint32_t)+localIndex*sizeof(uint32_t)),m_angleAccessors[localIndex]()); + } + #endif +} + +void Panto::resetActuationAngle(){ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex){ + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + } +} + +bool Panto::getCalibrationState(){ + return m_isCalibrating; +} + +void Panto::calibratePanto(){ + m_isCalibrating = true; +} + +void Panto::calibrationEnd() +{ + for (auto localIndex = 0; localIndex < 3; ++localIndex) + { + if (m_encoder[localIndex]) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_encoder[localIndex]->write( + m_actuationAngle[localIndex] / + (TWO_PI) * + encoderSteps[globalIndex]); + } + } + resetActuationAngle(); + m_isCalibrating = false; +}; + +float Panto::getActuationAngle(const uint8_t localIndex) const +{ + return m_actuationAngle[localIndex]; +}; + +Vector2D Panto::getPosition() const +{ + return Vector2D(m_handleX, m_handleY); +}; + +float Panto::getRotation() const +{ + return m_pointingAngle; +}; + +void Panto::setAngleAccessor( + const uint8_t localIndex, + const AngleAccessor accessor) +{ + m_angleAccessors[localIndex] = accessor; +}; + +void Panto::setTarget(const Vector2D target, const bool isForceRendering) +{ + m_isforceRendering = isForceRendering; + m_targetX = target.x; + m_targetY = target.y; + m_startX = m_handleX; + m_startY = m_handleY; + m_filteredX = m_startX; + m_filteredY = m_startY; + m_tweeningValue = 0.0f; + + float dx = (m_targetX - m_startX); + float dy = (m_targetY - m_startY); + float d = max((float)sqrt(dx*dx + dy*dy), 1.0f); //distance to target: avoiding 0 division + + const float velocity = 0.001 * m_tweeningSpeed; //[mm / s] maybe? + + m_tweeningStep = velocity / d; + inverseKinematics(); +}; + +void Panto::setSpeed(const float _speed){ + m_tweeningSpeed = _speed; +} + +void Panto::setRotation(const float rotation) +{ + m_targetAngle[c_localHandleIndex] = rotation; +}; + +int Panto::getEncoderErrorCount(){ + int res= m_encoderErrorCount; + m_encoderErrorCount =0; + return res; +} + +int Panto::getEncoderErrorCounts(int i){ + int res= m_encoderErrorCounts[i]; + m_encoderErrorCounts[i] =0; + return res; +} +int Panto::getEncoderRequests(){ + int res= m_encoderRequestCount; + m_encoderRequestCount =0; + return res; +} + +int Panto::getEncoderRequestsCounts(int i){ + int res= m_encoderRequestCounts[i]; + m_encoderRequestCounts[i] =0; + return res; +} + +uint8_t Panto::getPantoIndex(){ + return c_pantoIndex; +} + +void Panto::setInTransition(bool inTransition){ + m_inTransition = inTransition; +} + +bool Panto::getInTransition(){ + return m_inTransition; +} + +bool Panto::getIsFrozen(){ + return m_isFrozen; +} +void Panto::setIsFrozen(bool isFrozen){ + m_isFrozen = isFrozen; +} diff --git a/firmware/haptics/force field/firmware/src/hardware/spiCommands.cpp b/firmware/haptics/force field/firmware/src/hardware/spiCommands.cpp new file mode 100644 index 0000000..b2093cd --- /dev/null +++ b/firmware/haptics/force field/firmware/src/hardware/spiCommands.cpp @@ -0,0 +1,11 @@ +#include "hardware/spiCommands.hpp" + +#include "hardware/spiPacket.hpp" + +const uint16_t SPICommands::c_readAngle = SPIPacket(1, 0x3FFF).m_transmission; +const uint16_t SPICommands::c_clearError = SPIPacket(1, 0x1).m_transmission; +const uint16_t SPICommands::c_nop = SPIPacket(0, 0x0).m_transmission; +const uint16_t SPICommands::c_highZeroRead = SPIPacket(1, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroRead = SPIPacket(1, 0x17).m_transmission; +const uint16_t SPICommands::c_highZeroWrite = SPIPacket(0, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroWrite = SPIPacket(0, 0x17).m_transmission; diff --git a/firmware/haptics/force field/firmware/src/hardware/spiEncoder.cpp b/firmware/haptics/force field/firmware/src/hardware/spiEncoder.cpp new file mode 100644 index 0000000..6a73ea7 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/hardware/spiEncoder.cpp @@ -0,0 +1,18 @@ +#include + +SPIEncoder::SPIEncoder(SPIClass* spi) : m_spi(spi) { }; + +void SPIEncoder::transfer(uint16_t transmission) +{ + m_lastPacket = SPIPacket(m_spi->transfer16(transmission)); +} + +uint32_t SPIEncoder::getAngle() +{ + return m_lastValidAngle; + +} +// uint32_t SPIEncoder::getAngle(uint32_t index) +// { +// return m_values[index];// & c_dataMask; +// } diff --git a/firmware/haptics/force field/firmware/src/hardware/spiEncoderChain.cpp b/firmware/haptics/force field/firmware/src/hardware/spiEncoderChain.cpp new file mode 100644 index 0000000..5c53e57 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/hardware/spiEncoderChain.cpp @@ -0,0 +1,329 @@ +#include "hardware/spiEncoderChain.hpp" + +#include "hardware/spiCommands.hpp" +#include "utils/serial.hpp" +#include + +void SPIEncoderChain::begin() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); +} + +void SPIEncoderChain::end() +{ + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + + delay(1); +} + +// void SPIEncoderChain::transfer(uint16_t transmission) +// { +// begin(); +// for(auto i = 0; i < m_numberOfEncoders; ++i) +// { +// m_encoders[i].transfer(transmission); +// } +// end(); +// } + +void SPIEncoderChain::transfer(uint16_t transmission) +{ + + //upperhandle + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); +} + +void SPIEncoderChain::setZero(std::vector newZero) +{ + transfer(SPICommands::c_highZeroWrite); + + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + // } + // end(); + //upperhandle + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + transfer(SPICommands::c_lowZeroWrite); + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + // } + // end(); + + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // EEPROM.writeInt((i*sizeof(int32_t)),newZero[i] & 0x3fff); + // } + // EEPROM.commit(); + transfer(SPICommands::c_readAngle); +} + +SPIEncoderChain::SPIEncoderChain(uint32_t numberOfEncoders) +: m_settings(10000000, SPI_MSBFIRST, SPI_MODE1) //was 100000 +, m_spi(HSPI) +, m_numberOfEncoders(numberOfEncoders) +, m_encoders(numberOfEncoders, &m_spi) +, m_values(2 * 2) +, m_zeros(m_numberOfEncoders) +{ + m_spi.begin(); + pinMode(c_hspiSsPin1, OUTPUT); + pinMode(c_hspiSsPin2, OUTPUT); + pinMode(13, OUTPUT); + clearError(); + __setZeros(); + update(); +} + +void SPIEncoderChain::__setZeros(){ + for(int i=0 ; i < 4; i++)m_zeros[i]=0; + update(); + for(int i=0 ; i < 4; i++){ + m_zeros[i] = m_encoders[i].m_lastValidAngle; + } + // Serial.println("Zeros"); + + DPSerial::sendQueuedDebugLog("Zeros"); + + for(int i=0 ; i < 4; i++){ + // Serial.println(m_zeros[i]); + DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::update() +{ + update(0); + update(1); +} + +void SPIEncoderChain::update(int channel) +{ + uint16_t buf = 0; + m_spi.beginTransaction(m_settings); + //pinMode(13, OUTPUT); + digitalWrite(13, HIGH); + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + m_spi.transfer16(c_readAngle); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + delayMicroseconds(1); + + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + buf = m_spi.transfer16(c_nop); + m_values[i + channel*2] = buf & c_dataMask; + // Serial.printf("m_values%d\t%d\n", i, m_values[i + channel*2]); + // Serial.println(); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + m_spi.endTransaction(); + + for(int i=0; i < 4; i++){ + m_encoders[i].m_lastValidAngle = m_values[i]; + // DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::clearError() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_clearError); + } + digitalWrite(c_hspiSsPin1, HIGH); + + + //Serial.println("Error registers:"); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_nop); + m_spi.transfer16(c_nop); + //Serial.println(m_spi.transfer16(c_nop)); + //Serial.println(m_spi.transfer16(c_nop)); + } + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + +} + +std::vector SPIEncoderChain::getZero() //getZero returns 0 everytime power == off. +{ + // first pass - request high part of zero, don't care about current return value + transfer(SPICommands::c_highZeroRead); + + // second pass - request low part of zero, the return value contains the high part + transfer(SPICommands::c_lowZeroRead); + + std::vector result(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = m_encoders[i].m_lastPacket.m_data << 6; + } + + // third pass - jsut send nop, the return value contains the low part + transfer(SPICommands::c_nop); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] |= m_encoders[i].m_lastPacket.m_data & 0b111111; + } + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + // } + + return result; +} + +void SPIEncoderChain::setZero() +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + } + + setZero(newZero); +} + +bool SPIEncoderChain::needsZero() +{ + auto currentZero = getZero(); + + bool allZero = true; + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + allZero &= currentZero[i] == 0; + } + + transfer(SPICommands::c_readAngle); + + return allZero; +} + +void SPIEncoderChain::setPosition(std::vector positions) +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + newZero[i] -= positions[i]; + } + + setZero(newZero); +} + +void SPIEncoderChain::wakeUp(){ //call setZero(EEPROM_VALUE) + std::vector result(m_numberOfEncoders); + update(); + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + } + setZero(result); +} + +AngleAccessor SPIEncoderChain::getAngleAccessor(uint32_t index) +{ + return std::bind(&SPIEncoder::getAngle, &m_encoders[index]); +} + +uint32_t SPIEncoderChain::getErrors() { + return errors; +} + +uint32_t SPIEncoderChain::getRequests() { + return requests; +} + +void SPIEncoderChain::resetErrors() { + errors = 0; + requests = 0; +} \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/src/hardware/spiPacket.cpp b/firmware/haptics/force field/firmware/src/hardware/spiPacket.cpp new file mode 100644 index 0000000..c4a5080 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/hardware/spiPacket.cpp @@ -0,0 +1,44 @@ +#include "hardware/spiPacket.hpp" + +SPIPacket::SPIPacket(bool flag, uint16_t data) +: m_flag(flag) +, m_data(data & c_dataMask) +, m_valid(true) +{ + m_parity = calcParity(); + m_transmission = m_data | (m_flag << 14) | (m_parity << 15); +} + +SPIPacket::SPIPacket(uint16_t transmission) +: m_transmission(transmission) +{ + m_parity = m_transmission & c_parityMask; + m_flag = m_transmission & c_flagMask; + m_data = m_transmission & c_dataMask; + m_valid = m_parity == calcParity(); +} + +bool SPIPacket::calcParity() +{ + uint16_t temp = (m_data & c_dataMask) | (m_flag << 14); + // calculate https://en.wikipedia.org/wiki/Hamming_weight + temp -= (temp >> 1) & 0b0101010101010101; + temp = (temp & 0b0011001100110011) + ((temp >> 2) & 0b0011001100110011); + temp = (temp + (temp >> 4)) & 0b0000111100001111; + return ((temp * 0b0000000100000001) >> 8) % 2; +} + +bool SPIPacket::parityError() +{ + return m_data & 0b100; +} + +bool SPIPacket::commandInvalidError() +{ + return m_data & 0b10; +} + +bool SPIPacket::framingError() +{ + return m_data & 0b1; +} diff --git a/firmware/haptics/force field/firmware/src/ioMain.cpp b/firmware/haptics/force field/firmware/src/ioMain.cpp new file mode 100644 index 0000000..14917df --- /dev/null +++ b/firmware/haptics/force field/firmware/src/ioMain.cpp @@ -0,0 +1,39 @@ +#include "ioMain.hpp" + +#include "hardware/panto.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +FramerateLimiter sendLimiter = FramerateLimiter::fromFPS(60); +//todo copy limiter to physics physicsMain.cpp for SPI error logging + +void ioSetup() +{ + ulTaskNotifyTake(true, portMAX_DELAY); + delay(100); // TODO: patch: wait until pantoPhysics instantiates godobject (physicsMain); +} + +void ioLoop() +{ + PERFMON_START("[a] Receive serial"); +// DPSerial::receive(); + auto connected = DPSerial::ensureConnection(); + PERFMON_STOP("[a] Receive serial"); + + PERFMON_START("[b] Send positions"); + //DPSerial::sendDebugData(); + //DPSerial::sendInstantDebugLog("\n"); + if (connected && sendLimiter.step()) + { + DPSerial::sendPosition(); + } + PERFMON_STOP("[b] Send positions"); + + PERFMON_START("[c] Send debug logs"); + if (connected) + { + DPSerial::processDebugLogQueue(); + } + PERFMON_STOP("[c] Send debug logs"); +} diff --git a/firmware/haptics/force field/firmware/src/main.cpp b/firmware/haptics/force field/firmware/src/main.cpp new file mode 100644 index 0000000..3393809 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/main.cpp @@ -0,0 +1,37 @@ +#include + +#include "ioMain.hpp" +#include "physicsMain.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/serial.hpp" + +void setup() +{ + DPSerial::init(); + + DPSerial::sendInstantDebugLog("========== START =========="); + + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("I/O"), + std::forward_as_tuple(&ioSetup, &ioLoop, "I/O", 0)); + Tasks.at("I/O").run(); + Tasks.at("I/O").setLogFps(); + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("Physics"), + std::forward_as_tuple(&physicsSetup, &physicsLoop, "Physics", 1)); + Tasks.at("Physics").run(); + + TaskHandle_t defaultTask = xTaskGetCurrentTaskHandle(); + DPSerial::sendInstantDebugLog("default task handle is %i", defaultTask); + vTaskSuspend(NULL); + taskYIELD(); + DPSerial::sendInstantDebugLog("setup - this should not be printed"); +} + +void loop() +{ + DPSerial::sendInstantDebugLog("loop - this should not be printed"); + delay(1000); +} diff --git a/firmware/haptics/force field/firmware/src/physics/annotatedEdge.cpp b/firmware/haptics/force field/firmware/src/physics/annotatedEdge.cpp new file mode 100644 index 0000000..8072350 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/annotatedEdge.cpp @@ -0,0 +1,25 @@ +#include "physics/annotatedEdge.hpp" + +#include "utils/serial.hpp" + +#include "physics/edge.hpp" +#include "physics/indexedEdge.hpp" + +AnnotatedEdge::AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge) +: m_indexedEdge{indexedEdge} +, m_edge{edge} +{ +} + +AnnotatedEdge::AnnotatedEdge(const AnnotatedEdge& other) +: m_indexedEdge{other.m_indexedEdge} +, m_edge{other.m_edge} +{ +} + +void AnnotatedEdge::destroy() +{ + delete m_indexedEdge; + delete m_edge; + delete this; +} diff --git a/firmware/haptics/force field/firmware/src/physics/collider.cpp b/firmware/haptics/force field/firmware/src/physics/collider.cpp new file mode 100644 index 0000000..ae51266 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/collider.cpp @@ -0,0 +1,46 @@ +#include "physics/collider.hpp" + +#include "utils/serial.hpp" + +Collider::Collider(std::vector points) : m_points(points) { } + +void Collider::add(std::vector points) +{ + m_points.insert(m_points.end(), points.begin(), points.end()); +} + +bool Collider::contains(Vector2D point) +{ + // will contain result + auto inside = false; + // loop vars + auto edgeCount = m_points.size(); + auto j = edgeCount - 1; + // pre-allocate + Vector2D first, second; + + for(auto i = 0; i < edgeCount; ++i) + { + first = m_points[i]; + second = m_points[j]; + if ((first.y > point.y) != (second.y > point.y) && + (point.x < + first.x + + (second.x - first.x) * + (point.y - first.y) / + (second.y - first.y))) + { + inside = !inside; + } + j = i; + } + + return inside; +} + +Edge Collider::getEdge(uint32_t index) +{ + return Edge( + m_points[index % m_points.size()], + m_points[(index + 1) % m_points.size()]); +} diff --git a/firmware/09 god object/firmware/src/physics/godObject.cpp b/firmware/haptics/force field/firmware/src/physics/godObject.cpp similarity index 98% rename from firmware/09 god object/firmware/src/physics/godObject.cpp rename to firmware/haptics/force field/firmware/src/physics/godObject.cpp index a7aa47b..3b2b8c9 100644 --- a/firmware/09 god object/firmware/src/physics/godObject.cpp +++ b/firmware/haptics/force field/firmware/src/physics/godObject.cpp @@ -85,7 +85,7 @@ bool GodObject::move(bool isTweening, bool isFrozen) auto lastState = m_processingObstacleCollision; // if the number of collisions increased since the last frame then we ran into a corner auto lastNumCollisions = m_numCollisions; - + m_processingObstacleCollision = false; @@ -119,7 +119,7 @@ bool GodObject::move(bool isTweening, bool isFrozen) } // the speed of the god object increases proportionally with the distance bw handle and go. The max speed of the go is dependent on the outer tether radius. movementStepLength = min(m_tetherOuterRadius, distHandleToGo); - + // this is the movement of the god object that follows the tether if (distHandleToGo != 0) { @@ -130,7 +130,7 @@ bool GodObject::move(bool isTweening, bool isFrozen) } else { nextGoPosition = handlePosition; } - + // no matter what the tether state is we need to check if the god object is colliding with an obstacle Vector2D godObjectPos; portENTER_CRITICAL(&m_obstacleMutex); @@ -142,7 +142,7 @@ bool GodObject::move(bool isTweening, bool isFrozen) godObjectPos = checkCollisions(nextGoPosition, m_position); } if (m_tethered && m_processingObstacleCollision && m_tetherState == Outer && m_tetherStrategy != MaxSpeed) { - // for the Exploration and Leash mode the tether state can't be outer once the god object collides + // for the Exploration and Leash mode the tether state can't be outer once the god object collides // (otherwise the wall force will be weaker when the handle moves further into the wall) m_tetherState = Active; } @@ -162,15 +162,14 @@ bool GodObject::move(bool isTweening, bool isFrozen) m_position = godObjectPos; } - //m_position = godObjectPos; - if (m_tethered && m_tetherState == Outer && !isTweening && m_tetherStrategy != MaxSpeed) + if (m_tethered && m_tetherState == Outer && !isTweening && m_tetherStrategy != MaxSpeed) { m_tetherPosition = checkCollisions(handlePosition, m_tetherPosition); } else { m_tetherPosition = handlePosition; } portEXIT_CRITICAL(&m_obstacleMutex); - + m_doneColliding = lastState && !m_processingObstacleCollision; if (!m_tethered) { @@ -235,7 +234,7 @@ bool GodObject::processTetheringForce(Vector2D handlePosition, bool newCollision } else { if (!m_doneColliding) { // regular tether force active that pushes the handle back to the inner tether radius - + renderForce(Vector2D(0,0), tetherForce); } return !m_doneColliding; @@ -252,7 +251,7 @@ Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPositi 2. The actual collision detection. 3. If a collision is detected calculate the new god object position. - If a collision is detected the collision detection is repeated with the new target position. This way we can check if the new position is accessible at all or not. + If a collision is detected the collision detection is repeated with the new target position. This way we can check if the new position is accessible at all or not. Added by Julius on 30.09.20 For more information check Lukas Wagners MT (section 4.3.1): https://www.dropbox.com/home/2018%20CHI%20Dueling%20Pantographs/Layer%202%20Firmware%20(Lukas%20Wagner)?preview=2019_09_07+ESP+Firmware+for+God+Haptic+Objects+%3D+Masterarbeit+(Lukas+Wagner).pdf @@ -317,7 +316,7 @@ Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPositi } // we have a collision! - if (!foundCollision || movementRatio < shortestMovementRatio) // I think the second condition never gets called because the movementRatio loop + if (!foundCollision || movementRatio < shortestMovementRatio) // I think the second condition never gets called because the movementRatio loop // would already continue if the movementRatio was below 0 (which is the shortestMovementRatio) { // if a collision with a passable object is detected (e.g. a haptic rail) and the handle is not within the bounds of the colliding object, @@ -341,7 +340,7 @@ Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPositi if (foundCollision) { m_processingObstacleCollision = true; - + // god object slides along the colliding edge according to the handle movement but with tethered speed if (m_tethered) { @@ -366,7 +365,7 @@ Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPositi // c_resolveDistance is super small --> we need to add a tiny padding between the godobject and the edge so that it's not getting stuck in the edge targetPoint = targetPoint - (resolveVec * ((resolveLength + c_resolveDistance) / resolveLength)); - + // check for the new point if there is another collision with any other edge m_possibleCollisions->clear(); hashtable().getPossibleCollisions( @@ -394,7 +393,7 @@ void GodObject::createRail(uint16_t id, std::vector points, double dis m_obstacles.emplace(id, rail); portEXIT_CRITICAL(&m_obstacleMutex); return; - + } void GodObject::addToObstacle(uint16_t id, std::vector points) @@ -435,7 +434,7 @@ void GodObject::enableObstacle(uint16_t id, bool enable) } it->second->enable(enable); portEXIT_CRITICAL(&m_obstacleMutex); - } + } } Vector2D GodObject::getPosition() @@ -471,4 +470,4 @@ void GodObject::setSpeedControl(bool active, double tetherFactor, double innerTe m_tetherOuterRadius = outerTetherRadius; m_tetherStrategy = strategy; m_tetherPockEnabled = pockEnabled; -} \ No newline at end of file +} diff --git a/firmware/haptics/force field/firmware/src/physics/hashtable.cpp b/firmware/haptics/force field/firmware/src/physics/hashtable.cpp new file mode 100644 index 0000000..0a2e4bc --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/hashtable.cpp @@ -0,0 +1,224 @@ +#include "physics/hashtable.hpp" + +#include +#include + +#include "physics/edge.hpp" +#include "utils/assert.hpp" +#include "utils/serial.hpp" +#include "utils/utils.hpp" + + +int32_t Hashtable::get1dIndex(double value, double min, double step) +{ + return (int32_t)std::floor((value - min) / step); +} + +std::vector Hashtable::getCellIndices(Edge edge) +{ + std::vector result; + + // http://www.cse.yorku.ca/~amana/research/grid.pdf + const auto startX = edge.m_first.x; + const auto startY = edge.m_first.y; + const auto endX = edge.m_second.x; + const auto endY = edge.m_second.y; + auto x = get1dIndex(startX, rangeMinX, hashtableStepSizeX); + auto y = get1dIndex(startY, rangeMinY, hashtableStepSizeY); + const auto lastX = get1dIndex(endX, rangeMinX, hashtableStepSizeX); + const auto lastY = get1dIndex(endY, rangeMinY, hashtableStepSizeY); + const auto diffX = endX - startX; + const auto diffY = endY - startY; + const auto stepX = sgn(diffX); + const auto stepY = sgn(diffY); + const auto slopeX = diffY / diffX; + const auto slopeY = diffX / diffY; + auto nextX = stepX < 0 + ? (edge.m_first.x - (rangeMinX + x * hashtableStepSizeX)) + : ((rangeMinX + (x + 1) * hashtableStepSizeX) - edge.m_first.x); + nextX = hypot(nextX, nextX * slopeX); + auto nextY = stepY < 0 + ? (edge.m_first.y - (rangeMinY + y * hashtableStepSizeY)) + : ((rangeMinY + (y + 1) * hashtableStepSizeY) - edge.m_first.y); + nextY = hypot(nextY, nextY * slopeY); + const auto deltaX = + hypot(hashtableStepSizeX, hashtableStepSizeX * slopeX); + const auto deltaY = + hypot(hashtableStepSizeY * slopeY, hashtableStepSizeY); + + while(x != lastX || y != lastY) + { + // ignore cells outside the physical range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + if(nextX < nextY) + { + nextX += deltaX; + x += stepX; + } + else + { + nextY += deltaY; + y += stepY; + } + } + + // add end cell if inside range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + return result; +} + +std::set Hashtable::expand(const std::vector& edges) +{ + std::set result; + uint32_t x, y; + for (const auto& edge : edges) + { + x = edge % hashtableStepsY; + y = edge / hashtableStepsY; + + for(int32_t i = -1; i < 2; ++i) + { + if((x == 0 && i == -1) || (x == hashtableStepsX - 1 && i == 1)) + { + continue; + } + + for(int32_t j = -1; j < 2; ++j) + { + if((y == 0 && j == -1) || (y == hashtableStepsY - 1 && j == 1)) + { + continue; + } + + result.insert((y + j) * hashtableStepsY + (x + i)); + } + } + } + return result; +} + +Hashtable::Hashtable() +{ + DPSerial::sendQueuedDebugLog( + "Hashtable settings:"); + DPSerial::sendQueuedDebugLog( + "Available memory of %i bytes can hold %i cells", + hashtableMaxMemory, + hashtableMaxCells); + DPSerial::sendQueuedDebugLog( + "Possible range of values is %3f by %3f mm", + rangeMaxX - rangeMinX, + rangeMaxY - rangeMinY); + DPSerial::sendQueuedDebugLog( + "Horizontal step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeX, + hashtableStepsX); + DPSerial::sendQueuedDebugLog( + "Vertical step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeY, + hashtableStepsY); + DPSerial::sendQueuedDebugLog( + "Resulting step count is %i, using %i bytes", + hashtableNumCells, + hashtableUsedMemory); +} + +void Hashtable::add(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + m_cells[cellIndex].emplace_back( + edge->m_indexedEdge->m_obstacle, edge->m_indexedEdge->m_index); + } + edge->destroy(); +} + +void Hashtable::remove(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + auto& cell = m_cells[cellIndex]; + auto it = std::find( + cell.begin(), + cell.end(), + *(edge->m_indexedEdge)); + if(it != cell.end()) + { + cell.erase(it); + cell.shrink_to_fit(); + } + } + edge->destroy(); +} + +void Hashtable::getPossibleCollisions( + Edge movement, std::set* result) +{ + if(movement.m_first.x == 0 && movement.m_first.y == 0) + { + DPSerial::sendInstantDebugLog( + "Skipping god object movement from zero position."); + return; + } + const auto startX = + get1dIndex(movement.m_first.x, rangeMinX, hashtableStepSizeX); + const auto startY = + get1dIndex(movement.m_first.y, rangeMinY, hashtableStepSizeY); + const auto startIndex = startX * hashtableStepsY + startY; + ASSERT_GE(startIndex, 0); + + ASSERT_LT(startIndex, hashtableNumCells); + const auto endX = + get1dIndex(movement.m_second.x, rangeMinX, hashtableStepSizeX); + const auto endY = + get1dIndex(movement.m_second.y, rangeMinY, hashtableStepSizeY); + const auto endIndex = endX * hashtableStepsY + endY; + ASSERT_GE(endIndex, 0); + ASSERT_LT(endIndex, hashtableNumCells); + auto dist = (uint8_t)(startX != endX) + (uint8_t)(startY != endY); + const auto* begin = &m_cells[0]; + if(dist == 0) + { + const auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + } + else if(dist == 1) + { + auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + cell = begin + endIndex; + result->insert(cell->begin(), cell->end()); + } + else + { + for(auto&& cellIndex : getCellIndices(movement)) + { + const auto* cell = begin + cellIndex; + result->insert(cell->begin(), cell->end()); + } + } +} + +void Hashtable::print() +{ + DPSerial::sendQueuedDebugLog("Printing hashtable..."); + std::ostringstream str; + for(auto y = 0; y < hashtableStepsY; ++y) + { + for(auto x = 0; x < hashtableStepsX; x++) + { + str << (m_cells[x * hashtableStepsY + y].empty() ? '-' : '#'); + } + DPSerial::sendQueuedDebugLog(str.str().c_str()); + str.str(""); + } + DPSerial::sendQueuedDebugLog("Printing complete."); +} diff --git a/firmware/haptics/force field/firmware/src/physics/indexedEdge.cpp b/firmware/haptics/force field/firmware/src/physics/indexedEdge.cpp new file mode 100644 index 0000000..c1f5f09 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/indexedEdge.cpp @@ -0,0 +1,26 @@ +#include "physics/indexedEdge.hpp" + +#include "physics/obstacle.hpp" + +IndexedEdge::IndexedEdge(Obstacle* obstacle, uint32_t index) +: m_obstacle(obstacle) +, m_index(index) +{ +} + +bool IndexedEdge::operator==(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle && m_index == other.m_index; +} + +bool IndexedEdge::operator<(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle ? + m_index < other.m_index : + m_obstacle < other.m_obstacle; +} + +Edge IndexedEdge::getEdge() const +{ + return m_obstacle->getEdge(m_index); +} diff --git a/firmware/haptics/force field/firmware/src/physics/obstacle.cpp b/firmware/haptics/force field/firmware/src/physics/obstacle.cpp new file mode 100644 index 0000000..9ef21a4 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/obstacle.cpp @@ -0,0 +1,40 @@ +#include "physics/obstacle.hpp" + +#include "physics/indexedEdge.hpp" + +Obstacle::Obstacle(std::vector points) : Collider(points) { } + +Obstacle::Obstacle(std::vector points, bool passable) : Collider(points) +{ + this->passable = passable; +} + +bool Obstacle::enabled() +{ + return m_enabled; +} + +void Obstacle::enable(bool enable) +{ + m_enabled = enable; +} + +std::vector Obstacle::getIndexedEdges( + uint32_t first, uint32_t last +) +{ + if(first == -1) + { + first = m_points.size() - 1; + } + if(last == -1) + { + last = m_points.size() - 1; + } + std::vector result; + for(auto i = first; i <= last; ++i) + { + result.emplace_back(this, i); + } + return result; +} diff --git a/firmware/haptics/force field/firmware/src/physics/pantoPhysics.cpp b/firmware/haptics/force field/firmware/src/physics/pantoPhysics.cpp new file mode 100644 index 0000000..2f27f4d --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/pantoPhysics.cpp @@ -0,0 +1,61 @@ +#include "physics/pantoPhysics.hpp" + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantoPhysics; + +PantoPhysics::PantoPhysics(Panto* panto) : m_panto(panto) +{ + m_currentPosition = m_panto->getPosition(); + try + { + m_godObject = new GodObject(m_currentPosition); + } + catch(const std::bad_alloc& e) + { + DPSerial::sendInstantDebugLog("Error while creating god object - the hash table may be too big."); + DPSerial::sendInstantDebugLog("Error: %s", e.what()); + DPSerial::sendInstantDebugLog("Rebooting..."); + ESP.restart(); + } +} + +GodObject* PantoPhysics::godObject() +{ + return m_godObject; +} + +void PantoPhysics::step() +{ + // PERFMON_START("[ba] Physics::step"); + // PERFMON_START("[baa] Physics::step::prep"); + m_godObject->update(); + + m_currentPosition = m_panto->getPosition(); + + bool isTweening = m_panto->getInTransition(); + + auto difference = m_currentPosition - m_godObject->getPosition(); + m_godObject->setMovementDirection(difference); + // PERFMON_STOP("[baa] Physics::step::prep"); + // PERFMON_START("[bab] Physics::step::move"); + bool isForceActive = m_godObject->move(isTweening, m_panto->getIsFrozen()); + // PERFMON_STOP("[bab] Physics::step::move"); + // PERFMON_START("[bac] Physics::step::motor"); + + // force is active when frozen, tethering is enabled or collision is detected + if(isForceActive ) + { + m_panto->setTarget(m_godObject->getActiveForce(), true); + } + else + { + if (!isTweening){ + // TODO: is there actually another case? + m_panto->setTarget(Vector2D(NAN, NAN), false); + } + } + // PERFMON_STOP("[bac] Physics::step::motor"); + // PERFMON_STOP("[ba] Physics::step"); +} diff --git a/firmware/haptics/force field/firmware/src/physics/rail.cpp b/firmware/haptics/force field/firmware/src/physics/rail.cpp new file mode 100644 index 0000000..09613d3 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physics/rail.cpp @@ -0,0 +1,14 @@ +#include "physics/rail.hpp" + +Rail::Rail(std::vector points, double displacement) : Obstacle(points, true) +{ + this->displacement = displacement; +} + +bool Rail::contains(Vector2D point){ + //return true; + // if the point is within the displacement around the rail then it is contained and the godobject collides + double distLineToPoint = point.distancePointToLineSegment(m_points[0], m_points[1]); + bool inside = (distLineToPoint < this->displacement); + return inside; +} \ No newline at end of file diff --git a/firmware/haptics/force field/firmware/src/physicsMain.cpp b/firmware/haptics/force field/firmware/src/physicsMain.cpp new file mode 100644 index 0000000..7065eec --- /dev/null +++ b/firmware/haptics/force field/firmware/src/physicsMain.cpp @@ -0,0 +1,154 @@ +#include "physicsMain.hpp" + +#include "hardware/panto.hpp" +#include "hardware/spiEncoderChain.hpp" +#include "physics/pantoPhysics.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/serial.hpp" + +FramerateLimiter spiErrorLimiter = FramerateLimiter::fromSeconds(1); + +#ifdef LINKAGE_ENCODER_USE_SPI +SPIEncoderChain* spi; +#endif + +void physicsSetup() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + spi = new SPIEncoderChain(numberOfSpiEncoders); + #endif + + for (auto i = 0; i < pantoCount; ++i) + { + pantos.emplace_back(i); + } + delay(1000); + + xTaskNotifyGive(Tasks.at("I/O").getHandle()); + + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + + EEPROM.begin(sizeof(uint32_t)*numberOfSpiEncoders); + + //calibrateEncoders; Comment if not needed + // for (auto i = 0; i < pantoCount; ++i) + // { pantos[i].calibrateEncoders(i);} + + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); //calibrating only handle pulse encoder + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + + pantos[i].setAngleAccessor(j, spi->getAngleAccessor(index)); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + + for (unsigned char i = 0; i < pantoCount; ++i) + { + pantoPhysics.emplace_back(&pantos[i]); + } + +} + +void physicsLoop() +{ + PERFMON_START("[a] Read encoders"); + // PERFMON_START("[aa] Query SPI"); + #ifdef LINKAGE_ENCODER_USE_SPI + spi->update(); + #endif + // PERFMON_STOP("[aa] Query SPI"); + + // PERFMON_START("[ab] Calculation loop"); + for (auto i = 0; i < pantoCount; ++i) + { + // PERFMON_START("[aba] Actually read"); + pantos[i].readEncoders(); + // PERFMON_STOP("[aba] Actually read"); + PERFMON_START("[abb] Calc fwd kinematics"); + pantos[i].forwardKinematics(); + PERFMON_STOP("[abb] Calc fwd kinematics"); + } + // PERFMON_STOP("[ab] Calculation loop"); + PERFMON_STOP("[a] Read encoders"); + + PERFMON_START("[b] Calculate physics"); + for (auto i = 0; i < pantoCount; ++i) + { + pantoPhysics[i].step(); + } + PERFMON_STOP("[b] Calculate physics"); + + PERFMON_START("[c] Actuate motors"); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].actuateMotors(); + } + PERFMON_STOP("[c] Actuate motors"); + + if(spiErrorLimiter.step()) { + // DPSerial::sendQueuedDebugLog("SPI Errors: %i out of %i requests", spi->getErrors(), spi->getRequests()); + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[0][%i]: %i out of %i requests",i, + // pantos[0].getEncoderErrorCounts(i), pantos[0].getEncoderRequestsCounts(i)); + // } + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[1][%i]: %i out of %i requests",i, + // pantos[1].getEncoderErrorCounts(i), pantos[1].getEncoderRequestsCounts(i)); + // } + // spi->resetErrors(); + } + + PERFMON_START("[d] Calibrate Pantos"); + bool flag = false; + for(auto i = 0; i < pantoCount; ++i){ + if(pantos[i].getCalibrationState()){ + flag = true; + break; + } + } + if(flag){ + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + } + PERFMON_STOP("[d] Calibrate Pantos"); +} diff --git a/firmware/haptics/force field/firmware/src/tasks/task.cpp b/firmware/haptics/force field/firmware/src/tasks/task.cpp new file mode 100644 index 0000000..36e28ad --- /dev/null +++ b/firmware/haptics/force field/firmware/src/tasks/task.cpp @@ -0,0 +1,109 @@ +#include "tasks/task.hpp" + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::map Task::s_fpsMap; +FramerateLimiter loggingLimiter = FramerateLimiter::fromSeconds(1); + +void Task::taskLoop(void* parameters) +{ + Task* task = reinterpret_cast(parameters); + +#ifdef ENABLE_FPS + task->initFps(); +#endif + + task->m_setupFunc(); + +loopLabel: + task->m_loopFunc(); + +#ifdef ENABLE_FPS + task->checkFps(); +#endif + + TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; + TIMERG0.wdt_feed=1; + TIMERG0.wdt_wprotect=0; + goto loopLabel; +}; + +inline void Task::initFps() +{ + m_fpsCalcLimiter.reset(); + m_loopCount = 0; +}; + +inline void Task::checkFps() +{ + ++m_loopCount; + if (m_fpsCalcLimiter.step()) + { + s_fpsMap[m_handle] = m_loopCount * 1000 / m_fpsInterval; + m_loopCount = 0; + + if(m_logFps && loggingLimiter.step()) + { + for(const auto& entry : s_fpsMap) + { + DPSerial::sendQueuedDebugLog( + "Task \"%s\" fps: %i", + pcTaskGetTaskName(entry.first), + entry.second); + } + + if(ESP.getHeapSize() != 0){ + DPSerial::sendQueuedDebugLog("Free heap: %i of %i (%.3f %%). Largest block: %i", ESP.getFreeHeap(), ESP.getHeapSize(), 100*ESP.getFreeHeap()/(float)ESP.getHeapSize(), ESP.getMaxAllocHeap()); + } + + #ifdef ENABLE_PERFMON + for(const auto& entry : PerfMon.getResults()) + { + DPSerial::sendQueuedDebugLog(entry.c_str()); + } + #endif + } + } +}; + +Task::Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core) + : m_setupFunc(setupFunc), + m_loopFunc(loopFunc), + m_name(name), + m_stackDepth(c_defaultStackDepth), + m_priority(c_defaultPriority), + m_core(core), + m_fpsInterval(c_defaultFpsInterval), + m_fpsCalcLimiter(FramerateLimiter::fromMilliseconds(m_fpsInterval)){}; + +void Task::run() +{ + xTaskCreatePinnedToCore( + taskLoop, + m_name, + m_stackDepth, + this, + m_priority, + &m_handle, + m_core); + DPSerial::sendInstantDebugLog("Started task \"%s\" on core %i.", m_name, m_core); +}; + +void Task::setLogFps(bool logFps) +{ + m_logFps = logFps; +} + +TaskHandle_t Task::getHandle() +{ + return m_handle; +} diff --git a/firmware/haptics/force field/firmware/src/tasks/taskRegistry.cpp b/firmware/haptics/force field/firmware/src/tasks/taskRegistry.cpp new file mode 100644 index 0000000..4414b0a --- /dev/null +++ b/firmware/haptics/force field/firmware/src/tasks/taskRegistry.cpp @@ -0,0 +1,3 @@ +#include "tasks/taskRegistry.hpp" + +TaskRegistry Tasks; diff --git a/firmware/haptics/force field/firmware/src/utils/crashDump.cpp b/firmware/haptics/force field/firmware/src/utils/crashDump.cpp new file mode 100644 index 0000000..26ab153 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/utils/crashDump.cpp @@ -0,0 +1,19 @@ +#include "utils/crashDump.hpp" + +#include "utils/serial.hpp" + +CrashDump physicsDump; + +CrashDump::CrashDump() : m_stream() {} + +void CrashDump::clear() +{ + m_stream.clear(); + m_stream.str(""); +} + +void CrashDump::dump() +{ + DPSerial::sendQueuedDebugLog(m_stream.str().c_str()); + clear(); +} diff --git a/firmware/haptics/force field/firmware/src/utils/framerateLimiter.cpp b/firmware/haptics/force field/firmware/src/utils/framerateLimiter.cpp new file mode 100644 index 0000000..050c2ee --- /dev/null +++ b/firmware/haptics/force field/firmware/src/utils/framerateLimiter.cpp @@ -0,0 +1,57 @@ +#include "utils/framerateLimiter.hpp" + +#include "utils/serial.hpp" + +uint64_t FramerateLimiter::s_period = 0; +uint64_t FramerateLimiter::s_lastNow = 0; + +uint64_t FramerateLimiter::now() +{ + uint64_t now = micros(); + if(now < s_lastNow) + { + ++s_period; + DPSerial::sendQueuedDebugLog("FramerateLimiter: micros() rolled over."); + } + return s_period * c_periodLength + now; +} + +FramerateLimiter::FramerateLimiter(uint64_t microseconds) +: m_delta(microseconds) +, m_last(0) { } + +FramerateLimiter FramerateLimiter::fromFPS(double fps) +{ + return FramerateLimiter(1e6 / fps); +} + +FramerateLimiter FramerateLimiter::fromSeconds(uint64_t seconds) +{ + return FramerateLimiter(1000000 * seconds); +} + +FramerateLimiter FramerateLimiter::fromMilliseconds(uint64_t milliseconds) +{ + return FramerateLimiter(1000 * milliseconds); +} + +FramerateLimiter FramerateLimiter::fromMicroseconds(uint64_t microseconds) +{ + return FramerateLimiter(microseconds); +} + +bool FramerateLimiter::step() +{ + uint64_t now = FramerateLimiter::now(); + if(m_last + m_delta > now) + { + return false; + } + m_last = now; + return true; +} + +void FramerateLimiter::reset() +{ + m_last = FramerateLimiter::now(); +} diff --git a/firmware/haptics/force field/firmware/src/utils/performanceMonitor.cpp b/firmware/haptics/force field/firmware/src/utils/performanceMonitor.cpp new file mode 100644 index 0000000..050e401 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/utils/performanceMonitor.cpp @@ -0,0 +1,158 @@ +#include "utils/performanceMonitor.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PerformanceMonitor::PerformanceEntry::PerformanceEntry() +{ + m_values.reserve(c_measurementCount); +} + +void PerformanceMonitor::start(std::string label) +{ + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(entry.m_running) + { + return; + } + + entry.m_running = true; + entry.m_start = ESP.getCycleCount(); +} + +void PerformanceMonitor::stop(std::string label) +{ + uint64_t end = ESP.getCycleCount(); + + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(!entry.m_running) + { + return; + } + + uint64_t duration = + entry.m_start < end ? + (end - entry.m_start) : + (end + UINT32_MAX - entry.m_start); + + if(entry.m_values.size() == c_measurementCount) + { + auto old = entry.m_values[entry.m_index]; + entry.m_sum -= old; + + entry.m_values[entry.m_index] = duration; + entry.m_sum += duration; + entry.m_index = (entry.m_index + 1) % c_measurementCount; + } + else + { + entry.m_values.push_back(duration); + entry.m_sum += duration; + } + + entry.m_running = false; +} + +std::vector PerformanceMonitor::getResults() +{ + std::vector> temp; + + const std::string labelHeader("label"); + const std::string cycleHeader("avg cycles"); + const std::string timeHeader("avg ns"); + uint32_t maxLabelSize = labelHeader.size(); + uint32_t maxCycleSize = cycleHeader.size(); + uint32_t maxTimeSize = timeHeader.size(); + + for(const auto& core : m_entries) + { + auto coreId = core.first; + + for(const auto& entry : core.second) + { + const auto& label = entry.first; + const auto& data = entry.second; + + const auto avgCycles = (double)data.m_sum / data.m_values.size(); + const auto avgTime = avgCycles / ESP.getCpuFreqMHz(); + + maxLabelSize = std::max( + maxLabelSize, + label.size() + 4); + maxCycleSize = std::max( + maxCycleSize, + (uint32_t)std::log10(avgCycles) + 1 + 4); + maxTimeSize = std::max( + maxTimeSize, + (uint32_t)std::log10(avgTime) + 1 + 4); + + if(!data.m_values.empty()) + { + temp.emplace_back(coreId, label, avgCycles, avgTime); + } + } + } + + maxCycleSize = constrain(maxCycleSize, cycleHeader.size(), 30); + maxTimeSize = constrain(maxTimeSize, timeHeader.size(), 30); + + std::vector results; + std::ostringstream stream; + stream << std::fixed; + + stream + << "| " + << std::setw(maxLabelSize) << std::left + << labelHeader + << " | " + << std::setw(maxCycleSize) << std::left + << cycleHeader + << " | " + << std::setw(maxTimeSize) << std::left + << timeHeader + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + + stream + << "|-" + << std::string(maxLabelSize, '-') + << "-|-" + << std::string(maxCycleSize, '-') + << "-|-" + << std::string(maxTimeSize, '-') + << "-|"; + results.emplace_back(stream.str()); + stream.seekp(0); + + for(const auto& tuple : temp) + { + stream + << "| " + << "["<< std::get<0>(tuple) << "] " + << std::setw(maxLabelSize - 4) << std::left + << std::get<1>(tuple) + << " | " + << std::setw(maxCycleSize) << std::right << std::setprecision(3) + << std::get<2>(tuple) + << " | " + << std::setw(maxTimeSize) << std::right << std::setprecision(3) + << std::get<3>(tuple) + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + } + + return results; +} + +PerformanceMonitor PerfMon; diff --git a/firmware/haptics/force field/firmware/src/utils/serial.cpp b/firmware/haptics/force field/firmware/src/utils/serial.cpp new file mode 100644 index 0000000..9d8623e --- /dev/null +++ b/firmware/haptics/force field/firmware/src/utils/serial.cpp @@ -0,0 +1,759 @@ +#include "utils/serial.hpp" + +#include + +#include "hardware/panto.hpp" +#include "physics/pantoPhysics.hpp" +#include "utils/vector.hpp" + +bool DPSerial::s_rxBufferCritical = false; +Header DPSerial::s_header = Header(); +uint8_t DPSerial::s_debugLogBuffer[c_debugLogBufferSize]; +std::queue DPSerial::s_debugLogQueue; +portMUX_TYPE DPSerial::s_serialMutex = portMUX_INITIALIZER_UNLOCKED; +ReceiveState DPSerial::s_receiveState = NONE; +uint8_t DPSerial::s_expectedPacketId = 1; +bool DPSerial::s_connected = false; +unsigned long DPSerial::s_lastHeartbeatTime = 0; +uint16_t DPSerial::s_unacknowledgedHeartbeats = 0; +std::map + DPSerial::s_receiveHandlers = { + {SYNC_ACK, DPSerial::receiveSyncAck}, + {HEARTBEAT_ACK, DPSerial::receiveHearbeatAck}, + {MOTOR, DPSerial::receiveMotor}, + {PID, DPSerial::receivePID}, + {SPEED, DPSerial::receiveSpeed}, + {CREATE_OBSTACLE, DPSerial::receiveCreateObstacle}, + {ADD_TO_OBSTACLE, DPSerial::receiveAddToObstacle}, + {REMOVE_OBSTACLE, DPSerial::receiveRemoveObstacle}, + {ENABLE_OBSTACLE, DPSerial::receiveEnableObstacle}, + {DISABLE_OBSTACLE, DPSerial::receiveDisableObstacle}, + {CALIBRATE_PANTO, DPSerial::receiveCalibrationRequest}, + {DUMP_HASHTABLE, DPSerial::receiveDumpHashtable}, + {CREATE_PASSABLE_OBSTACLE, DPSerial::receiveCreatePassableObstacle}, + {CREATE_RAIL, DPSerial::receiveCreateRail}, + {FREEZE, DPSerial::receiveFreeze}, + {FREE, DPSerial::receiveFree}, + {SPEED_CONTROL, DPSerial::receiveSpeedControl}}; + +// === private === + +// send helper + +void DPSerial::sendUInt8(uint8_t data) +{ + Serial.write(data); +} + +void DPSerial::sendInt16(int16_t data) +{ + Serial.write(static_cast(data >> 8)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt16(uint16_t data) +{ + sendInt16(reinterpret_cast(data)); +} + +void DPSerial::sendInt32(int32_t data) +{ + Serial.write(static_cast(data >> 24)); + Serial.write(static_cast((data >> 16) & 255)); + Serial.write(static_cast((data >> 8) & 255)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt32(uint32_t data) +{ + sendInt32(reinterpret_cast(data)); +} + +void DPSerial::sendFloat(float data) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + sendInt32(reinterpret_cast(data)); +#pragma GCC diagnostic pop +} + +void DPSerial::sendMessageType(MessageType data) +{ + Serial.write(data); +} + +void DPSerial::sendMagicNumber() +{ + for (auto i = 0; i < c_magicNumberSize; ++i) + { + sendUInt8(c_magicNumber[i]); + } +}; + +void DPSerial::sendHeader(MessageType messageType, uint16_t payloadSize) +{ + sendMessageType(messageType); + sendUInt8(0); // no panto -> fw tracked packages + sendUInt16(payloadSize); +}; + +// send + +void DPSerial::sendSync() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(SYNC, 4); + sendUInt32(c_revision); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendHeartbeat() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(HEARTBEAT, 0); + s_unacknowledgedHeartbeats++; + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferCritical() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_CRITICAL, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferReady() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_READY, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendPacketAck(uint8_t id) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(PACKET_ACK, 1); + sendUInt8(id); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidPacketId(uint8_t expected, uint8_t received) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_PACKET_ID, 2); + sendUInt8(expected); + sendUInt8(received); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidData() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_DATA, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive helper + +uint8_t DPSerial::receiveUInt8() +{ + return static_cast(Serial.read()); +} + +int16_t DPSerial::receiveInt16() +{ + return Serial.read() << 8 | Serial.read(); +} + +uint16_t DPSerial::receiveUInt16() +{ + auto temp = receiveInt16(); + return reinterpret_cast(temp); +} + +int32_t DPSerial::receiveInt32() +{ + return Serial.read() << 24 | Serial.read() << 16 | Serial.read() << 8 | Serial.read(); +} + +uint32_t DPSerial::receiveUInt32() +{ + auto temp = receiveInt32(); + return reinterpret_cast(temp); +} + +float DPSerial::receiveFloat() +{ + auto temp = receiveInt32(); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + return reinterpret_cast(temp); +#pragma GCC diagnostic pop +} + +MessageType DPSerial::receiveMessageType() +{ + return static_cast(Serial.read()); +} + +void DPSerial::checkBuffer() +{ + const auto available = Serial.available(); + if (available > c_rxBufferCriticalThreshold && !s_rxBufferCritical) + { + s_rxBufferCritical = true; + sendBufferCritical(); + } + if (available < c_rxBufferReadyThreshold && s_rxBufferCritical) + { + s_rxBufferCritical = false; + sendBufferReady(); + } +} + +bool DPSerial::receiveMagicNumber() +{ + int magicNumberProgress = 0; + + bool invalidData = false; + + // as long as enough data is available to find the magic number + while (Serial.available() >= c_magicNumberSize) + { + const uint8_t read = Serial.read(); + const uint8_t expected = c_magicNumber[magicNumberProgress]; + // does next byte fit expected by of magic number? + if (read == expected) + { + // yes - increase index. If check complete, return true. + if (++magicNumberProgress == c_magicNumberSize) + { + s_receiveState = FOUND_MAGIC; + return true; + } + } + else + { + // no - reset search progress + // sendInstantDebugLog( + // "Error: expected %x, read %x. State might by faulty!", + // expected, + // read); + invalidData = true; + magicNumberProgress = 0; + } + } + + if (invalidData) + { + sendInvalidData(); + } + + // ran out of available data before finding complete number - return false + return false; +}; + +bool DPSerial::receiveHeader() +{ + // make sure enough data is available + if (Serial.available() < c_headerSize) + { + return false; + } + + s_header.MessageType = receiveUInt8(); + s_header.PacketId = receiveUInt8(); + s_header.PayloadSize = receiveUInt16(); + s_receiveState = FOUND_HEADER; + return true; +}; + +bool DPSerial::payloadReady() +{ + return Serial.available() >= s_header.PayloadSize; +}; + +// receive + +void DPSerial::receiveSyncAck() +{ + s_connected = true; +}; + +void DPSerial::receiveHearbeatAck() +{ + s_unacknowledgedHeartbeats = 0; +}; + +void DPSerial::receiveMotor() +{ + const auto controlMethod = receiveUInt8(); + const auto pantoIndex = receiveUInt8(); + + const auto target = Vector2D(receiveFloat(), receiveFloat()); + if (!isnan(target.x) && !isnan(target.y)) + { + pantos[pantoIndex].setInTransition(true); + DPSerial::sendInstantDebugLog("In Transition"); + } + pantos[pantoIndex].setRotation(receiveFloat()); + pantos[pantoIndex].setTarget(target, controlMethod == 1); +}; + +void DPSerial::receivePID() +{ + auto motorIndex = receiveUInt8(); + + for (auto i = 0; i < 3; ++i) + { + pidFactor[motorIndex][i] = receiveFloat(); + } +}; + +void DPSerial::receiveSpeed() +{ + auto pantoIndex = receiveUInt8(); //0 or 1 + auto speed = receiveFloat(); + pantos[pantoIndex].setSpeed(speed); +}; + +void DPSerial::receiveCreateObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, false); + } + } +} + +void DPSerial::receiveCreatePassableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, true); + } + } +} + +void DPSerial::receiveCreateRail() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + auto displacement = (double)receiveFloat(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createRail(id, path, displacement); + } + } +} + +void DPSerial::receiveAddToObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->addToObstacle(id, path); + } + } +} + +void DPSerial::receiveRemoveObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->removeObstacle(id); + } + } +} + +void DPSerial::receiveEnableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id); + } + } +} + +void DPSerial::receiveDisableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id, false); + } + } +} + +void DPSerial::receiveFreeze() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + const auto target = pantos[i].getPosition(); + pantos[i].setTarget(target, 0); + pantos[i].setRotation(pantos[i].getRotation()); + pantos[i].setIsFrozen(true); + } + } +} + +void DPSerial::receiveFree() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantos[i].setTarget(Vector2D(NAN, NAN), 0); + pantos[i].setRotation(NAN); + pantos[i].setInTransition(false); + pantos[i].setIsFrozen(false); + } + } +} + +void DPSerial::receiveSpeedControl() +{ + auto tethered = receiveUInt8(); //0 or 1 + auto tetherFactor = receiveFloat(); + auto tetherInnerRadius = receiveFloat(); + auto tetherOuterRadius = receiveFloat(); + auto tetherStrategy = receiveUInt8(); // 0 for MaxSpeed, 1 for Exploration, 2 for Leash + OutOfTetherStrategy strategy; + switch (tetherStrategy) + { + case 0: + strategy = MaxSpeed; + break; + case 1: + strategy = Exploration; + break; + case 2: + strategy = Leash; + break; + default: + break; + } + auto pockEnabled = receiveUInt8(); //0 or 1 + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + pantoPhysics[i].godObject()->setSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); + } +} + +void DPSerial::receiveCalibrationRequest() +{ + DPSerial::sendInstantDebugLog("=== Calibration Request received ==="); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibratePanto(); + } +} + +void DPSerial::receiveDumpHashtable() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->dumpHashtable(); + } + } +} + +void DPSerial::receiveInvalid() +{ + sendQueuedDebugLog( + "Received invalid message: [%02X, %02X, %04X] %", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); +}; + +// === public === + +// setup + +void DPSerial::init() +{ + Serial.flush(); + Serial.begin(c_baudRate); + Serial.setRxBufferSize(c_rxBufferSize); +} + +bool DPSerial::ensureConnection() +{ + if (!s_connected) + { + sendSync(); + // delay to avoid spamming sync messages + delay(10); + return false; + } + + if (s_unacknowledgedHeartbeats > c_maxUnacklowledgedHeartbeats) + { + sendQueuedDebugLog("Disconnected due to too many unacklowledged heartbeats."); + s_unacknowledgedHeartbeats = 0; + s_connected = false; + return false; + } + + if (millis() > s_lastHeartbeatTime + c_heartbeatIntervalMs || s_lastHeartbeatTime == 0) + { + sendHeartbeat(); + s_lastHeartbeatTime = millis(); + } + + return true; +} + +// send + +void DPSerial::sendPosition() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(POSITION, pantoCount * 5 * 4); // five values per panto, 4 bytes each + + for (auto i = 0; i < pantoCount; ++i) + { + const auto panto = pantos[i]; + const auto pos = panto.getPosition(); + sendFloat(pos.x); + sendFloat(pos.y); + sendFloat(panto.getRotation()); + auto goPos = pantoPhysics[i].godObject()->getPosition(); + sendFloat(goPos.x); + sendFloat(goPos.y); + } + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInstantDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length + 1, 0, c_debugLogBufferSize); + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write(s_debugLogBuffer, length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendQueuedDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length, 0, c_debugLogBufferSize); + s_debugLogQueue.emplace(reinterpret_cast(s_debugLogBuffer), length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendTransitionEnded(uint8_t panto) +{ + // signal when tweening is over + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(TRANSITION_ENDED, 1); // 1 byte for the panto index is enough + sendUInt8(panto); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::processDebugLogQueue() +{ + portENTER_CRITICAL(&s_serialMutex); + // quick check to avoid loop if not necessary + if (!s_debugLogQueue.empty()) + { + for (auto i = 0; i < c_processedQueuedMessagesPerFrame; ++i) + { + if (!s_debugLogQueue.empty()) + { + auto &msg = s_debugLogQueue.front(); + auto length = msg.length() + 1; + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write( + reinterpret_cast(msg.c_str()), + length); + s_debugLogQueue.pop(); + } + } + } + portEXIT_CRITICAL(&s_serialMutex); +} + +void DPSerial::sendDebugData() +{ + const auto pos0 = pantos[0].getPosition(); + const auto pos1 = pantos[1].getPosition(); + portENTER_CRITICAL(&s_serialMutex); + sendInstantDebugLog( + "[ang/0] %+08.3f | %+08.3f | %+08.3f [ang/1] %+08.3f | %+08.3f | %+08.3f [pos/0] %+08.3f | %+08.3f | %+08.3f [pos/1] %+08.3f | %+08.3f | %+08.3f", + degrees(pantos[0].getActuationAngle(0)), + degrees(pantos[0].getActuationAngle(1)), + degrees(pantos[0].getActuationAngle(2)), + degrees(pantos[1].getActuationAngle(0)), + degrees(pantos[1].getActuationAngle(1)), + degrees(pantos[1].getActuationAngle(2)), + pos0.x, + pos0.y, + degrees(pantos[0].getRotation()), + pos1.x, + pos1.y, + degrees(pantos[1].getRotation())); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive + +void DPSerial::receive() +{ + checkBuffer(); + + if (s_receiveState == NONE && !receiveMagicNumber()) + { + return; + } + + if (s_receiveState == FOUND_MAGIC && !receiveHeader()) + { + return; + } + + if (s_receiveState == FOUND_HEADER && !payloadReady()) + { + return; + } + + if (!s_connected && s_header.MessageType != SYNC_ACK) + { + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + Serial.read(); + } + sendInstantDebugLog( + "Not connected, ignoring [%X %i %i]", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); + return; + } + + s_receiveState = NONE; + + if (s_header.PacketId > 0) + { + if (s_header.PacketId != s_expectedPacketId) + { + sendInvalidPacketId(s_expectedPacketId, s_header.PacketId); + return; + } + + sendPacketAck(s_header.PacketId); + s_expectedPacketId++; + if (s_expectedPacketId == 0) + { + s_expectedPacketId++; + } + } + + auto handler = s_receiveHandlers.find((MessageType)(s_header.MessageType)); + + if (handler == s_receiveHandlers.end()) + { + receiveInvalid(); + } + else + { + handler->second(); + } +}; diff --git a/firmware/haptics/force field/firmware/src/utils/vector.cpp b/firmware/haptics/force field/firmware/src/utils/vector.cpp new file mode 100644 index 0000000..34826d6 --- /dev/null +++ b/firmware/haptics/force field/firmware/src/utils/vector.cpp @@ -0,0 +1,85 @@ +#include "utils/vector.hpp" + +#include + +Vector2D Vector2D::fromPolar(double angle, double length) +{ + return Vector2D(cos(angle) * length, sin(angle) * length); +}; + +double Vector2D::determinant(const Vector2D& first, const Vector2D& second) +{ + return (first.x * second.y) - (first.y * second.x); +}; + +double Vector2D::length() const +{ + return sqrt(x * x + y * y); +}; + +double Vector2D::angle() const +{ + return atan2(y, x); +}; + +Vector2D Vector2D::normalize() const +{ + return Vector2D(x / length(), y / length()); +} + +Vector2D Vector2D::operator+(const Vector2D& other) const +{ + return Vector2D(x + other.x, y + other.y); +}; + +Vector2D Vector2D::operator-(const Vector2D& other) const +{ + return Vector2D(x - other.x, y - other.y); +}; + +double Vector2D::operator*(const Vector2D& other) const +{ + return x * other.x + y * other.y; +}; + +Vector2D Vector2D::operator*(const double scale) const +{ + return Vector2D(x * scale, y * scale); +}; + +bool Vector2D::operator==(const Vector2D &other) const +{ + return x == other.x && y == other.y; +}; + +bool Vector2D::operator!=(const Vector2D &other) const +{ + return x != other.x || y != other.y; +}; + +double Vector2D::distancePointToLineSegment(Vector2D a, Vector2D b) { + // a is the start of the line segment, b is the end + // distance is measured from the point p + Vector2D ab = b - a; + Vector2D ae = *this - a; + Vector2D be = *this - b; + + double ab_ae = ab * ae; + double ab_be = ab * be; + + if (ab_be > 0){ + // Point is above line segment + return be.length(); + } else if (ab_ae < 0) { + // Point is below line segment + return ae.length(); + } else { + // Finding the perpendicular distance + double x1 = ab.x; + double y1 = ab.y; + double x2 = ae.x; + double y2 = ae.y; + double mod = sqrt(x1 * x1 + y1 * y1); + return abs(x1 * y2 - y1 * x2) / mod; + } +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/backtrace/backtrace.sh b/firmware/haptics/force field/utils/backtrace/backtrace.sh new file mode 100644 index 0000000..4b75654 --- /dev/null +++ b/firmware/haptics/force field/utils/backtrace/backtrace.sh @@ -0,0 +1,26 @@ +#!/bin/bash +# Use this to analyze the ESP32's backtrace in case of a crash. +# Requires gdb to be accessable from bash. +# In order to find line numbers, add -g3 to the PlatformIO build flags. +# The default target is set for running this script from the base framework dir. +# Alternatively, you may pass an path to the elf as the first argument. +# Default usage: ./utils/backtrace/backtrace.sh "0x40085698:0x3ffb5e80 [...]" +# Alternative usage example for running from this dir: ./backtrace.sh ./../../firmware/.pioenvs/esp32dev/firmware.elf "0x40085698:0x3ffb5e80 [...]" + +if [ "$#" -eq 1 ]; then + target=./firmware/.pioenvs/esp32dev/firmware.elf + backtrace=$1 + echo "Using default target $target" +else + target=$1 + backtrace=$2 + echo "Using custom target $1" +fi + +file=$(mktemp) +echo $backtrace \ +| sed -r 's/:0x[[:xdigit:]]{8}\s?/\n/g' \ +| sed '/^[[:space:]]*$/d' \ +| sed 's/^.*$/echo === \0 ===\\n\ninfo symbol \0\ninfo line *\0/g' > $file +gdb -batch -x $file $target +rm $file diff --git a/firmware/haptics/force field/utils/protocol/include/protocol/header.hpp b/firmware/haptics/force field/utils/protocol/include/protocol/header.hpp new file mode 100644 index 0000000..f6b1d51 --- /dev/null +++ b/firmware/haptics/force field/utils/protocol/include/protocol/header.hpp @@ -0,0 +1,14 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +struct Header +{ + uint8_t MessageType = 0; + uint8_t PacketId = 0; + uint16_t PayloadSize = 0; +}; diff --git a/firmware/haptics/force field/utils/protocol/include/protocol/messageType.hpp b/firmware/haptics/force field/utils/protocol/include/protocol/messageType.hpp new file mode 100644 index 0000000..0e9566e --- /dev/null +++ b/firmware/haptics/force field/utils/protocol/include/protocol/messageType.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +enum MessageType +{ + SYNC = 0x00, + HEARTBEAT = 0x01, + BUFFER_CRITICAL = 0x02, + BUFFER_READY = 0x03, + PACKET_ACK = 0x04, + INVALID_PACKET_ID = 0x05, + INVALID_DATA = 0x06, + POSITION = 0x10, + DEBUG_LOG = 0x20, + SYNC_ACK = 0x80, + HEARTBEAT_ACK = 0x81, + MOTOR = 0x90, + PID = 0x91, + SPEED = 0x92, + TRANSITION_ENDED = 0x93, + CREATE_OBSTACLE = 0xA0, + ADD_TO_OBSTACLE = 0xA1, + REMOVE_OBSTACLE = 0xA2, + ENABLE_OBSTACLE = 0xA3, + DISABLE_OBSTACLE = 0xA4, + CALIBRATE_PANTO = 0xA5, + CREATE_PASSABLE_OBSTACLE = 0xA6, + CREATE_RAIL = 0xA7, + FREEZE = 0xA8, + FREE = 0xA9, + SPEED_CONTROL = 0xAA, + DUMP_HASHTABLE = 0xC0, +}; + +extern std::set TrackedMessageTypes; diff --git a/firmware/haptics/force field/utils/protocol/include/protocol/protocol.hpp b/firmware/haptics/force field/utils/protocol/include/protocol/protocol.hpp new file mode 100644 index 0000000..a2e41c3 --- /dev/null +++ b/firmware/haptics/force field/utils/protocol/include/protocol/protocol.hpp @@ -0,0 +1,29 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +class DPProtocol +{ +protected: + // revision + static const uint32_t c_revision = 6; + + // connection info + static const uint32_t c_baudRate = 115200; + + // magic number + static const uint8_t c_magicNumber[]; + static const uint8_t c_magicNumberSize = 2; + + // data size + static const uint8_t c_headerSize = 4; + static const uint16_t c_maxPayloadSize = 256; + + // combined maximal size of packet + static const uint16_t c_maxPacketSize = + c_maxPayloadSize + c_magicNumberSize + c_headerSize; +}; diff --git a/firmware/haptics/force field/utils/protocol/src/protocol/messageType.cpp b/firmware/haptics/force field/utils/protocol/src/protocol/messageType.cpp new file mode 100644 index 0000000..4e57c18 --- /dev/null +++ b/firmware/haptics/force field/utils/protocol/src/protocol/messageType.cpp @@ -0,0 +1,10 @@ +#include "protocol/messageType.hpp" + +std::set TrackedMessageTypes = { + CREATE_OBSTACLE, + ADD_TO_OBSTACLE, + REMOVE_OBSTACLE, + ENABLE_OBSTACLE, + DISABLE_OBSTACLE, + CREATE_PASSABLE_OBSTACLE, + CREATE_RAIL}; diff --git a/firmware/haptics/force field/utils/protocol/src/protocol/protocol.cpp b/firmware/haptics/force field/utils/protocol/src/protocol/protocol.cpp new file mode 100644 index 0000000..6871f88 --- /dev/null +++ b/firmware/haptics/force field/utils/protocol/src/protocol/protocol.cpp @@ -0,0 +1,3 @@ +#include "protocol/protocol.hpp" + +const uint8_t DPProtocol::c_magicNumber[] = {0x44, 0x50}; \ No newline at end of file diff --git a/firmware/haptics/force field/utils/scripts/generateHardwareConfig.js b/firmware/haptics/force field/utils/scripts/generateHardwareConfig.js new file mode 100644 index 0000000..563e783 --- /dev/null +++ b/firmware/haptics/force field/utils/scripts/generateHardwareConfig.js @@ -0,0 +1,269 @@ +// just assume the input config file is correct and we don't need this check +/* eslint-disable guard-for-in */ +// also, this is just a helper script and does not need documentation +/* eslint-disable require-jsdoc */ +// the template strings can't ne broken into multiple lines +/* eslint-disable max-len */ +const input = require('../../hardware/'+process.argv[2]+'.json'); +const fs = require('fs'); +const crypto = require('crypto'); +const hash = crypto.createHash('md5').update(JSON.stringify(input)).digest(); +const aggregates = {}; + +function insert(index, categoryName, valueName, value) { + const aggregate = categoryName+'_'+valueName; + if (!aggregates[aggregate]) { + aggregates[aggregate] = []; + } + aggregates[aggregate][index] = value; +} + +let index = 0; let pantoCount = 0; +for (const pantoName in input.pantos) { + ++pantoCount; + const panto = input.pantos[pantoName]; + for (const dofName in panto) { + const dof = panto[dofName]; + for (const categoryName in dof) { + const category = dof[categoryName]; + for (const valueName in category) { + const value = category[valueName]; + insert(index, categoryName, valueName, value); + } + } + ++index; + } +} + +function aggregate(name, valueIfUndefined = 0, map = ((x) => x)) { + let array = aggregates[name]; + if (!array) { + array = []; + } + for (let i = 0; i < index; ++i) { + if (array[i] == undefined) { + array[i] = valueIfUndefined; + } + if (array[i] instanceof Array) { + array[i] = `{${array[i].join(', ')}}`; + } + } + return array.map(map).join(', '); +} + +function count(name) { + const array = aggregates[name]; + if (!array) { + return 0; + } + let count = 0; + for (let i = 0; i < index; ++i) { + if (array[i] != undefined) { + count++; + } + } + return count; +} + +function getRangeForMotor(motor) { + const centerX = motor.linkage.baseX; + const centerY = motor.linkage.baseY; + const range = motor.linkage.innerLength + motor.linkage.outerLength; + return { + minX: centerX - range, + minY: centerY - range, + maxX: centerX + range, + maxY: centerY + range + }; +} + +function getRangeForPanto(panto) { + const left = getRangeForMotor(panto.left); + const right = getRangeForMotor(panto.right); + return { + minX: Math.min(left.minX, right.minX), + minY: Math.min(left.minY, right.minY), + maxX: Math.max(left.maxX, right.maxX), + maxY: Math.max(left.maxY, right.maxY) + }; +} + +function getRange() { + const upper = getRangeForPanto(input.pantos.upper); + const lower = getRangeForPanto(input.pantos.lower); + return { + minX: Math.min(upper.minX, lower.minX), + minY: Math.min(upper.minY, lower.minY), + maxX: Math.max(upper.maxX, lower.maxX), + maxY: Math.max(upper.maxY, lower.maxY) + }; +} + +let range; +if (input.range) { + range = input.range; +} else { + range = getRange(); +} + +// calculate these now because sqrt isn't a constexpr +function getHashtableSettings() { + const rangeX = range.maxX - range.minX; + const rangeY = range.maxY - range.minY; + const maxMemory = input.hashtableMemory / pantoCount; + const sizeofVector = 12; // = sizeof(std::vector) + const maxCells = Math.floor(maxMemory / sizeofVector); + const targetStepSize = Math.sqrt(rangeX * rangeY / maxCells); + const stepsX = Math.floor(rangeX / targetStepSize); + const stepsY = Math.floor(rangeY / targetStepSize); + const stepSizeX = rangeX / stepsX; + const stepSizeY = rangeY / stepsY; + const numCells = stepsX * stepsY; + const usedMemory = numCells * sizeofVector; + + return { + maxMemory, + usedMemory, + maxCells, + numCells, + stepsX, + stepsY, + stepSizeX, + stepSizeY + }; +} + +const hashtable = getHashtableSettings(); + +const headerOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is ${input.dummyPin}. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {${Array.from(hash).map((x) => '0x'+('0'+(Number(x).toString(16))).slice(-2).toUpperCase()).join(', ')}}; +const float opMinDist = ${input.opMinDist}, + opMaxDist = ${input.opMaxDist}, + opAngle = ${input.opAngle}; +extern float forceFactor; +const uint8_t pantoCount = ${pantoCount}; +const uint8_t dummyPin = ${input.dummyPin}; +${input.usesSpi ? '#define LINKAGE_ENCODER_USE_SPI' : ''} +const uint32_t numberOfSpiEncoders = ${count('encoder_spiIndex')}; +const float linkageBaseX[] = { + ${aggregate('linkage_baseX')} +}; +const float linkageBaseY[] = { + ${aggregate('linkage_baseY')} +}; +const float linkageInnerLength[] = { + ${aggregate('linkage_innerLength')} +}; +const float linkageOuterLength[] = { + ${aggregate('linkage_outerLength')} +}; +const uint8_t linkageHandleMount[] = { + ${aggregate('linkage_handleMount')} +}; +const float motorPowerLimit[] = { + ${aggregate('motor_powerLimit')} +}; +const float motor_powerLimitForce[] = { + ${aggregate('motor_powerLimitForce')} +}; +extern float pidFactor[${pantoCount*3}][3]; +const float forceP = ${input.forcePidFactor[0]}; +const float forceI = ${input.forcePidFactor[1]}; +const float forceD = ${input.forcePidFactor[2]}; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + ${aggregate('motor_pwmPin', input.dummyPin)} +}; +const uint8_t motorPwmPinForwards[] = { + ${aggregate('motor_pwmPinForwards', input.dummyPin)} +}; +const uint8_t motorPwmPinBackwards[] = { + ${aggregate('motor_pwmPinBackwards', input.dummyPin)} +}; +const uint8_t motorDirAPin[] = { + ${aggregate('motor_dirAPin', input.dummyPin)} +}; +const uint8_t motorDirBPin[] = { + ${aggregate('motor_dirBPin', input.dummyPin)} +}; +const bool motorFlipped[] = { + ${aggregate('motor_flipped')} +}; +const uint8_t encoderAPin[] = { + ${aggregate('encoder_aPin', input.dummyPin)} +}; +const uint8_t encoderBPin[] = { + ${aggregate('encoder_bPin', input.dummyPin)} +}; +const uint8_t encoderIndexPin[] = { + ${aggregate('encoder_indexPin', input.dummyPin)} +}; +const uint32_t encoderSteps[] = { + ${aggregate('encoder_steps')} +}; +const uint32_t encoderSpiIndex[] = { + ${aggregate('encoder_spiIndex', 0xffffffff)} +}; +const float encoderFlipped[] = { + ${aggregate('encoder_flipped', false, (x) => x ? -1 : 1)} +}; +const float setupAngle[] = { + ${aggregate('encoder_setup')} +}; +constexpr float rangeMinX = ${range.minX}; +constexpr float rangeMinY = ${range.minY}; +constexpr float rangeMaxX = ${range.maxX}; +constexpr float rangeMaxY = ${range.maxY}; +constexpr uint32_t hashtableMaxMemory = ${hashtable.maxMemory}; +constexpr uint32_t hashtableUsedMemory = ${hashtable.usedMemory}; +constexpr uint32_t hashtableMaxCells = ${hashtable.maxCells}; +constexpr uint32_t hashtableNumCells = ${hashtable.numCells}; +constexpr uint32_t hashtableStepsX = ${hashtable.stepsX}; +constexpr uint32_t hashtableStepsY = ${hashtable.stepsY}; +constexpr double hashtableStepSizeX = ${hashtable.stepSizeX}; +constexpr double hashtableStepSizeY = ${hashtable.stepSizeY}; +const uint32_t obstacleChangesPerFrame = ${input.obstacleChangesPerFrame};`; + +console.log(headerOutput); +const headerDir = 'firmware/include/config/'; +if (!fs.existsSync(headerDir)) { + fs.mkdirSync(headerDir, {recursive: true}); +} +fs.writeFileSync(headerDir + 'config.hpp', headerOutput); + +const sourceOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = ${input.forceFactor}; +float pidFactor[${pantoCount*3}][3] = { + ${aggregate('motor_pidFactor')} +};`; + +console.log(sourceOutput); +const sourceDir = 'firmware/src/config/'; +if (!fs.existsSync(sourceDir)) { + fs.mkdirSync(sourceDir, {recursive: true}); +} +fs.writeFileSync(sourceDir + 'config.cpp', sourceOutput); diff --git a/firmware/haptics/force field/utils/scripts/run.js b/firmware/haptics/force field/utils/scripts/run.js new file mode 100644 index 0000000..96fae21 --- /dev/null +++ b/firmware/haptics/force field/utils/scripts/run.js @@ -0,0 +1,222 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const os = require('os'); +const path = require('path'); +const {exec, remove, color, escape} = require('./tools'); + +function log(message, messageColor) { + console.log(`${messageColor}${message}${color.reset}`); +} + +const buildHandlers = { + 'framework': () => { + return build('voice-command') + & build('serial-plugin') + & build('serial-standalone'); + }, + 'voice-command': () => { + return exec('node', ['./utils/voiceCommand/build/build-release.js']); + }, + 'serial-plugin': () => { + const gypDef = '--cppdefs="NODE_GYP ' + escape(cppDefines.join(' ')) + '"'; + return exec('node-gyp', ['configure', '-C utils/serial', gypDef]) + & exec('node-gyp', ['build', '-C utils/serial']); + }, + 'serial-standalone': () => { + return exec( + cppExec, + cppArgs.concat(cppDefines.map((d) => cppDefinePrefix + d)).concat([ + 'utils/serial/src/standalone/main.cpp', + 'utils/serial/src/standalone/standalone.cpp', + 'utils/serial/src/serial/shared.cpp', + 'utils/serial/src/cppLib/lib.cpp', + 'utils/serial/src/crashAnalyzer/analyze.cpp', + 'utils/serial/src/crashAnalyzer/buffer.cpp', + process.platform == 'win32' ? + 'utils/serial/src/serial/win.cpp' : + 'utils/serial/src/serial/unix.cpp', + 'utils/protocol/src/protocol/protocol.cpp', + '-Iutils/serial/include', + '-Iutils/protocol/include', + '-o utils/serial/serial'])); + }, + 'unity-serial': () =>{ + return unity(); + }, + 'firmware': () => { + return config(process.argv[4]) + & platformio('build'); + } +}; + +function build(target) { + if (target === undefined) { + return build('firmware'); + // 14.5.21: The js framework is deprecated since this + // commit: a045c86fa3754810a9a68b1bc89dcf990d883579 + // To make it work again we have to implement the acknowledgement + // logic, similar to the way it's done in this + // commit: faa30310e884784b5d431ac65c05e3186b9bafab + return build('framework') + & build('firmware'); + } + + if (!buildHandlers.hasOwnProperty(target)) { + log(`Invalid build target ${target}`, color.red); + return false; + } + + log(`Building ${target}`, color.green); + const result = buildHandlers[target](); + if (result) { + log(`Building ${target} successful`, color.green); + } else { + log(`Building ${target} failed`, color.red); + } + return result; +} + +const cleanHandlers = { + 'framework': () => { + return clean('voice-command') + & clean('serial-plugin') + & clean('serial-standalone'); + }, + 'voice-command': () => { + return remove('./utils/voiceCommand/.bin'); + }, + 'serial-plugin': () => { + return remove('./utils/serial/build'); + }, + 'serial-standalone': () => { + log('Clean serial-standalone not implemented yet', color.yellow); + return true; + }, + 'firmware': () => { + return remove('./firmware/src/config/config.cpp') + & remove('./firmware/include/config/config.hpp') + & platformio('clean'); + } +}; + +function clean(target) { + if (target === undefined) { + return clean('framework') + & clean('firmware'); + } + + if (!cleanHandlers.hasOwnProperty(target)) { + log(`Invalid clean target ${target}`, color.red); + return false; + } + + log(`Cleaning ${target}`, color.green); + const result = cleanHandlers[target](); + if (result) { + log(`Cleaning ${target} successful`, color.green); + } else { + log(`Cleaning ${target} failed`, color.red); + } + return result; +} + +function config(target) { + if (target === undefined) { + target = 'fiona'; + } + log(`Generating config ${target}`, color.green); + return exec('node', ['utils/scripts/generateHardwareConfig.js', target]); +} + +function platformio(command) { + if (command == 'build' || command === undefined) { + command = '.'; + } + + + log(`Running platformio ${command}`, color.green); + return exec(platformioExec, ['run', '-d firmware', `-t ${command}`]); +} + +function plotter() { + return exec('http-server', ['utils/plotter/']); +} + +function docs() { + log(`Building docs`, color.green); + return exec('node', ['utils/scripts/docs.js']); +} + +function unity() { + if (process.platform == 'win32') { + return exec('utils\\serial\\unity\\win.bat'); + } else if (process.platform == 'darwin') { + const unityDir = './utils/serial/unity/'; + return exec(unityDir+'mac.sh'); + } else { + return exec('echo "Linux is not supported for building unity framework."'); + } +} + +const handlers = { + 'build': build, + 'clean': clean, + 'config': config, + 'platformio': platformio, + 'plotter': plotter, + 'docs': docs, + 'unity': unity +}; + +const platformioDir = os.homedir() + '/.platformio'; +const xtensaUtilDir = platformioDir + '/packages/toolchain-xtensa32/bin/'; +const addr2linePath = path.join(xtensaUtilDir, 'xtensa-esp32-elf-addr2line'); + +let platformioExec; +let cppExec; +let cppDefinePrefix; +let cppArgs; +const cppDefines = [ + escape('ADDR2LINE_PATH=\"' + addr2linePath + '\"') +]; + +if (process.platform == 'win32') { + platformioExec = path.join(platformioDir, '/penv/Scripts/platformio'); + cppExec = 'cl'; + cppDefinePrefix = '/D'; + cppArgs = ['/Fo:Utils\\Serial\\']; + cppDefines.push('WINDOWS'); +} else { + if (exec('which', ['platformio'])) { + platformioExec = 'platformio'; + } else { + platformioExec = platformioDir + '/penv/bin/platformio'; + } + if (process.platform == 'linux') { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } else { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } +} + +const command = process.argv[2]; +if (!handlers.hasOwnProperty(command)) { + log(`Unknown command ${command}`, color.red); + process.exitCode = 1; + return; +} + +log(`=== Running ${command} ===`, color.green); +const result = handlers[command](process.argv[3]); +if (result) { + log(`=== ${command} successful ===`, color.green); + process.exitCode = 0; +} else { + log(`=== ${command} failed ===`, color.red); + process.exitCode = 1; +} diff --git a/firmware/haptics/force field/utils/scripts/tools.js b/firmware/haptics/force field/utils/scripts/tools.js new file mode 100644 index 0000000..d125380 --- /dev/null +++ b/firmware/haptics/force field/utils/scripts/tools.js @@ -0,0 +1,54 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const childProcess = require('child_process'); +const path = require('path'); +const fs = require('fs'); + +const color = { + red: '\x1b[31m', + green: '\x1b[32m', + yellow: '\x1b[33m', + reset: '\x1b[0m' +}; + +function exec(cmd, args) { + return childProcess.spawnSync( + cmd, + args, + { + shell: true, + stdio: ['ignore', process.stdout, process.stderr] + }).status == 0; +}; + +function remove(target) { + if (!fs.existsSync(target)) { + console.log(`Could not find ${target}`, color.yellow); + return true; + } + if (fs.statSync(target).isDirectory()) { + const content = fs.readdirSync(target); + for (const entry in content) { + if (content.hasOwnProperty(entry)) { + remove(path.join(target, content[entry])); + } + } + fs.rmdirSync(target); + } else { + fs.unlinkSync(target); + } + console.log(`Removed ${target}`); + return true; +} + +function escape(string) { + return string.replace(/\\/g, '\\\\').replace(/"/g, '\\\"'); +} + +module.exports = { + exec, + remove, + color, + escape +}; diff --git a/firmware/haptics/force field/utils/serial/CMakeLists.txt b/firmware/haptics/force field/utils/serial/CMakeLists.txt new file mode 100644 index 0000000..edd3b8a --- /dev/null +++ b/firmware/haptics/force field/utils/serial/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.9) + +# project settings + +project(serial CXX) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# sources + +set(serial_source_dir src) +set(protocol_source_dir ../protocol/src/protocol) +set(serial_include_dir include) +set(protocol_include_dir ../protocol/include) +set(base_include_dir include/serial) +set(generated_header_dir ${CMAKE_BINARY_DIR}/generated) + +set(export_header + ${generated_header_dir}/serial_export.hpp) + +set(public_headers + ${serial_include_dir}/libInterface.hpp + ${export_header}) + +set(private_headers + ${serial_include_dir}/serial.hpp + ${serial_include_dir}/packet.hpp + ${serial_include_dir}/crashAnalyzer.hpp + ${protocol_include_dir}/protocol/protocol.hpp + ${protocol_include_dir}/protocol/header.hpp + ${protocol_include_dir}/protocol/messageType.hpp) + +set(shared_sources + ${serial_source_dir}/cppLib/lib.cpp + ${serial_source_dir}/crashAnalyzer/analyze.cpp + ${serial_source_dir}/crashAnalyzer/buffer.cpp + ${serial_source_dir}/serial/shared.cpp + ${serial_source_dir}/serial/packet.cpp + ${protocol_source_dir}/protocol.cpp + ${protocol_source_dir}/messageType.cpp) + +if (WIN32) +# set path vars +set(addr2line "$ENV{USERPROFILE}\\.platformio\\packages\\toolchain-xtensa32\\bin\\xtensa-esp32-elf-addr2line.exe") +set(firmware_rel "..\\..\\firmware\\.pio\\build\\esp32dev\\firmware.elf") +get_filename_component(firmware_abs ${firmware_rel} ABSOLUTE) +# escape backslashes +string(REPLACE "\\" "\\\\" addr2line ${addr2line}) +string(REPLACE "/" "\\\\" firmware_abs ${firmware_abs}) +message(STATUS "addr2line: ${addr2line}") +message(STATUS "firmware (rel): ${firmware_rel}") +message(STATUS "firmware (abs): ${firmware_abs}") +# set defines +add_compile_definitions(WINDOWS) +add_compile_definitions(ADDR2LINE_PATH="${addr2line}") +add_compile_definitions(FIRMWARE_PATH="${firmware_abs}") +set(sources + ${shared_sources} + ${serial_source_dir}/serial/win.cpp) +elseif (UNIX) +set(sources + ${shared_sources} + ${serial_source_dir}/serial/unix.cpp) +else() + message(FATAL_ERROR "System is neither WIN32 nor UNIX.") +endif() + +# lib target + +include_directories(${generated_header_dir}) +include_directories(${serial_include_dir}) +include_directories(${protocol_include_dir}) +#add_compile_definitions(ADDR2LINE_PATH) +add_library(serial SHARED ${public_headers} ${private_headers} ${sources}) + +include(GenerateExportHeader) +generate_export_header(serial + BASE_NAME serial + EXPORT_MACRO_NAME SERIAL_EXPORT + EXPORT_FILE_NAME ${export_header} + STATIC_DEFINE SERIAL_BUILT_AS_STATIC) + +# install settings + +set_target_properties(serial PROPERTIES PUBLIC_HEADER "${public_headers}") + +include(GNUInstallDirs) +install(TARGETS serial + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/firmware/haptics/force field/utils/serial/binding.gyp b/firmware/haptics/force field/utils/serial/binding.gyp new file mode 100644 index 0000000..17c9986 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/binding.gyp @@ -0,0 +1,36 @@ +{ + "variables": { + "cppdefs": "NODE_GYP", + }, + "targets": [{ + "target_name": "serial", + "sources": [ + "./src/node/main.cpp", + "./src/node/setup.cpp", + "./src/node/poll.cpp", + "./src/node/send.cpp", + "./src/node/receiveHelpers.cpp", + "./src/node/sendHelpers.cpp", + "./src/serial/shared.cpp", + "./src/cppLib/lib.cpp", + "./src/crashAnalyzer/buffer.cpp", + "./src/crashAnalyzer/analyze.cpp", + "../protocol/src/protocol/protocol.cpp" + ], + "conditions": [ + ["OS=='win'", { + "sources": ["./src/serial/win.cpp"] + }], + ["OS!='win'", { + "sources": ["./src/serial/unix.cpp"] + }], + ], + "include_dirs": [ + "./include", + "../protocol/include" + ], + "defines": [ + "<@(cppdefs)" + ] + }] +} diff --git a/firmware/haptics/force field/utils/serial/include/crashAnalyzer.hpp b/firmware/haptics/force field/utils/serial/include/crashAnalyzer.hpp new file mode 100644 index 0000000..dea6f74 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/crashAnalyzer.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#define SAFEMOD(a, b) ((a) % (b) + (b)) % (b) + +class CrashAnalyzer +{ +private: + static const uint16_t c_bufferLength = 1024; + static const uint16_t c_dumpLineWidth = 32; + static uint8_t s_buffer[c_bufferLength]; + static uint16_t s_length; + static uint16_t s_index; + + static void clearBuffer(); + static uint8_t getChar(uint16_t offset); + + static const std::string c_rebootString; + static const std::string c_backtraceString; + + static bool findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t& foundOffset); + static std::vector getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset); + static void addr2line(std::vector addresses); + + static void checkOutput(); +public: + static void push_back(const uint8_t character); +}; diff --git a/firmware/haptics/force field/utils/serial/include/libInterface.hpp b/firmware/haptics/force field/utils/serial/include/libInterface.hpp new file mode 100644 index 0000000..40b2846 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/libInterface.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include + +#include "serial.hpp" +#include "serial_export.hpp" + +// class stuff + +class CppLib : DPSerial +{ +public: + static uint32_t getRevision(); + static uint64_t open(char* port); + static void setActiveHandle(uint64_t handle); + static void close(); + static void poll(); + static void sendSyncAck(); + static void sendHeartbeatAck(); + static void sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + static void sendSpeed(uint8_t pantoIndex, float speed); + static void createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + static void addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void removeObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void enableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void disableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void sendFree(uint8_t pantoIndex); + static void sendFreeze(uint8_t pantoIndex); + static void sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + static uint32_t checkSendQueue(uint32_t maxPackets); + static void reset(); +}; + +// handlers + +typedef void (*syncHandler_t)(uint64_t); +extern syncHandler_t syncHandler; +typedef void (*heartbeatHandler_t)(uint64_t); +extern heartbeatHandler_t heartbeatHandler; +typedef void (*positionHandler_t)(uint64_t, double*); +extern positionHandler_t positionHandler; +typedef void (*loggingHandler_t)(char*); +extern loggingHandler_t loggingHandler; +typedef void (*transitionHandler_t)(uint8_t); +extern transitionHandler_t transitionHandler; + +void logString(char* msg); + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +extern "C" +{ + uint32_t SERIAL_EXPORT GetRevision(); + void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler); + void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler); + void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler); + void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler); + void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler); + uint64_t SERIAL_EXPORT Open(char* port); + void SERIAL_EXPORT Close(uint64_t handle); + void SERIAL_EXPORT Poll(uint64_t handle); + void SERIAL_EXPORT SendSyncAck(uint64_t handle); + void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle); + void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed); + void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets); + void SERIAL_EXPORT Reset(); +}; diff --git a/firmware/haptics/force field/utils/serial/include/node.hpp b/firmware/haptics/force field/utils/serial/include/node.hpp new file mode 100644 index 0000000..5941e9b --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "serial.hpp" + +#if __has_include("node_api.h") +#include +#elif __has_include("node/node_api.h") +#include +#endif +#define NAPI_CHECK(code) \ + if (code != napi_ok) \ + std::cerr << "NOT OK: " << __FILE__ << ":" << __LINE__ << std::endl; + +// enable for debugging +// #define DEBUG_LOGGING + +class Node : public DPSerial +{ +private: + static napi_value receiveUInt8(napi_env env, uint16_t& offset); + static napi_value receiveInt16(napi_env env, uint16_t& offset); + static napi_value receiveUInt16(napi_env env, uint16_t& offset); + static napi_value receiveInt32(napi_env env, uint16_t& offset); + static napi_value receiveUInt32(napi_env env, uint16_t& offset); + static napi_value receiveFloat(napi_env env, uint16_t& offset); + + static void sendUInt8(napi_env env, napi_value value, uint16_t& offset); + static void sendInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendFloat(napi_env env, napi_value value, uint16_t& offset); + +public: + static napi_value open(napi_env env, napi_callback_info info); + static napi_value close(napi_env env, napi_callback_info info); + static napi_value poll(napi_env env, napi_callback_info info); + static napi_value send(napi_env env, napi_callback_info info); +}; diff --git a/firmware/haptics/force field/utils/serial/include/packet.hpp b/firmware/haptics/force field/utils/serial/include/packet.hpp new file mode 100644 index 0000000..f885582 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/packet.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +class Packet +{ +public: + Header header; + uint8_t payload[256]; + uint8_t payloadIndex = 0; + + Packet(uint8_t messageType, uint16_t payloadSize); + + uint8_t receiveUInt8(); + int16_t receiveInt16(); + uint16_t receiveUInt16(); + int32_t receiveInt32(); + uint32_t receiveUInt32(); + float receiveFloat(); + + void sendUInt8(uint8_t value); + void sendInt16(int16_t value); + void sendUInt16(uint16_t value); + void sendInt32(int32_t value); + void sendUInt32(uint32_t value); + void sendFloat(float value); +}; diff --git a/firmware/haptics/force field/utils/serial/include/serial.hpp b/firmware/haptics/force field/utils/serial/include/serial.hpp new file mode 100644 index 0000000..6ff8e07 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/serial.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "packet.hpp" + +#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) +#include +#define FILEHANDLE HANDLE +#else +#include +#include +#include +#include +#include +#define FILEHANDLE FILE * +#endif + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; + +class DPSerial : public DPProtocol +{ +protected: + static std::string s_path; + static const uint32_t c_packetSize = 0xFF; + static FILEHANDLE s_handle; + static std::thread s_worker; + static bool s_workerRunning; + + static std::queue s_highPrioSendQueue; + static std::queue s_lowPrioSendQueue; + static std::queue s_receiveQueue; + + static uint8_t s_nextTrackedPacketId; + static bool s_haveUnacknowledgedTrackedPacket; + static Packet s_lastTrackedPacket; + static std::chrono::time_point + s_lastTrackedPacketSendTime; + static const std::chrono::milliseconds c_trackedPacketTimeout; + static bool s_pantoReportedInvalidData; + + static bool s_pantoReady; + static uint32_t s_magicReceiveIndex; + static ReceiveState s_receiveState; + static Header s_receiveHeader; + + static void startWorker(); + static void stopWorker(); + static void tearDown(); + static void reset(); + static void update(); + static void processOutput(); + static bool processInput(); + + static bool isTracked(uint8_t t); + static bool checkQueue(std::queue &q); + static void sendPacket(Packet p); + static void sendInstantPacket(Packet p); + static void write(const uint8_t *const data, const uint32_t length); + + static uint32_t getAvailableByteCount(FILEHANDLE s_handle); + static bool readBytesFromSerial(void *target, uint32_t length); + static bool readBytesIfAvailable(void *target, uint32_t length); + static bool readMagicNumber(); + static bool readHeader(); + static bool readPayload(); + +public: + static bool setup(std::string path); +}; diff --git a/firmware/haptics/force field/utils/serial/include/serial_export.hpp b/firmware/haptics/force field/utils/serial/include/serial_export.hpp new file mode 100644 index 0000000..09738b0 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/serial_export.hpp @@ -0,0 +1,59 @@ + +#ifndef SERIAL_EXPORT_H +#define SERIAL_EXPORT_H + +// figure out the correct attributes +#if defined _WIN32 || defined __CYGWIN__ + #define HELPER_DLL_IMPORT __declspec(dllimport) + #define HELPER_DLL_EXPORT __declspec(dllexport) + #define HELPER_DLL_LOCAL +#else + #if __GNUC__ >= 4 + #define HELPER_DLL_IMPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_EXPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden"))) + #else + #define HELPER_DLL_IMPORT + #define HELPER_DLL_EXPORT + #define HELPER_DLL_LOCAL + #endif +#endif + +#ifdef SERIAL_BUILT_AS_STATIC +# define SERIAL_EXPORT +# define SERIAL_NO_EXPORT +#else +# ifndef SERIAL_EXPORT +# ifdef serial_EXPORTS + /* We are building this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# else + /* We are using this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# endif +# endif + +# ifndef SERIAL_NO_EXPORT +# define SERIAL_NO_EXPORT HELPER_DLL_LOCAL +# endif +#endif + +#ifndef SERIAL_DEPRECATED +# define SERIAL_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef SERIAL_DEPRECATED_EXPORT +# define SERIAL_DEPRECATED_EXPORT SERIAL_EXPORT SERIAL_DEPRECATED +#endif + +#ifndef SERIAL_DEPRECATED_NO_EXPORT +# define SERIAL_DEPRECATED_NO_EXPORT SERIAL_NO_EXPORT SERIAL_DEPRECATED +#endif + +#if 0 /* DEFINE_NO_DEPRECATED */ +# ifndef SERIAL_NO_DEPRECATED +# define SERIAL_NO_DEPRECATED +# endif +#endif + +#endif /* SERIAL_EXPORT_H */ diff --git a/firmware/haptics/force field/utils/serial/include/standalone.hpp b/firmware/haptics/force field/utils/serial/include/standalone.hpp new file mode 100644 index 0000000..4bf5992 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/include/standalone.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include "serial.hpp" + +class Standalone : public DPSerial +{ +public: + static void terminate(int signal); + static void printPacket(); +}; diff --git a/firmware/haptics/force field/utils/serial/src/cppLib/lib.cpp b/firmware/haptics/force field/utils/serial/src/cppLib/lib.cpp new file mode 100644 index 0000000..446b894 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/cppLib/lib.cpp @@ -0,0 +1,445 @@ +#include "libInterface.hpp" + +#include +#include + +#include "packet.hpp" + +#ifdef _WIN32 +#define FILEPTR void * +#else +#define FILEPTR FILE * +#endif + +// class stuff + +uint32_t CppLib::getRevision() +{ + return c_revision; +} + +uint64_t CppLib::open(char *port) +{ + if (!setup(port)) + { + logString("Open failed"); + return 0; + } + logString("Open successfull"); + return (uint64_t)s_handle; +} + +void CppLib::setActiveHandle(uint64_t handle) +{ + s_handle = (FILEPTR)handle; +} + +void CppLib::close() +{ + tearDown(); +} + +void CppLib::poll() +{ + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + bool receivedTransition = false; + double positionCoords[2 * 5]; + uint8_t pantoIndex; + + while (s_receiveQueue.size() > 0) + { + auto packet = s_receiveQueue.front(); + s_receiveQueue.pop(); + + if (packet.header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + switch (packet.header.MessageType) + { + case SYNC: + { + auto receivedRevision = packet.receiveUInt32(); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::ostringstream oss; + oss << "Revision id not matching. Expected " << c_revision + << ", received " << receivedRevision << "." << std::endl; + logString((char *)oss.str().c_str()); + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (packet.payloadIndex < packet.header.PayloadSize) + { + uint8_t index = packet.payloadIndex / 4; + positionCoords[index] = packet.receiveFloat(); + } + break; + case DEBUG_LOG: + logString((char *)packet.payload); + break; + case TRANSITION_ENDED: + receivedTransition = true; + pantoIndex = packet.receiveUInt8(); + break; + default: + break; + } + } + + if (receivedSync) + { + if (syncHandler == nullptr) + { + logString("Received sync, but handler not set up"); + } + else + { + syncHandler((uint64_t)s_handle); + } + } + + if (receivedHeartbeat) + { + if (heartbeatHandler == nullptr) + { + logString("Received heartbeat, but handler not set up"); + } + else + { + heartbeatHandler((uint64_t)s_handle); + } + } + + if (receivedPosition) + { + if (positionHandler == nullptr) + { + logString("Received position, but handler not set up"); + } + else + { + positionHandler((uint64_t)s_handle, positionCoords); + } + } + + if (receivedTransition) + { + // transition (tweening) ended + if (transitionHandler == nullptr) + { + logString("Received transition ended, but handler not set up"); + } + else + { + transitionHandler(pantoIndex); + } + } +} + +void CppLib::sendSyncAck() +{ + DPSerial::sendInstantPacket(Packet(SYNC_ACK, 0)); +} + +void CppLib::sendHeartbeatAck() +{ + DPSerial::sendInstantPacket(Packet(HEARTBEAT_ACK, 0)); +} + +void CppLib::sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + auto p = Packet(MOTOR, 14); // 1 for control, 1 for index, 3 * 4 for pos + p.sendUInt8(controlMethod); + p.sendUInt8(pantoIndex); + p.sendFloat(positionX); + p.sendFloat(positionY); + p.sendFloat(rotation); + sendPacket(p); +} + +void CppLib::sendSpeed(uint8_t pantoIndex, float speed) +{ + auto p = Packet(SPEED, 5); // 1 for index, 1 * 4 for position + p.sendUInt8(pantoIndex); + p.sendFloat(speed); + sendPacket(p); +} + +void CppLib::sendFree(uint8_t pantoIndex) +{ + auto p = Packet(FREE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendFreeze(uint8_t pantoIndex) +{ + auto p = Packet(FREEZE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + auto p = Packet(SPEED_CONTROL, 15); // 1 for index, 4 for tether factor, 4 each for the tether radii, 1 for tether strategy and 1 for pock + p.sendUInt8(tethered); + p.sendFloat(tetherFactor); + p.sendFloat(tetherInnerRadius); + p.sendFloat(tetherOuterRadius); + p.sendUInt8(strategy); + p.sendUInt8(pockEnabled); + sendPacket(p); +} + +void CppLib::createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_PASSABLE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + auto p = Packet(CREATE_RAIL, 23); // 1 for index, 2 for id, 2 * 2 * 4 for vectors, 4 for displacement + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + p.sendFloat(displacement); + sendPacket(p); +} + +void CppLib::addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(ADD_TO_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::removeObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(REMOVE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::enableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(ENABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::disableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(DISABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::reset() +{ + DPSerial::reset(); +} + +// handlers +syncHandler_t syncHandler; +heartbeatHandler_t heartbeatHandler; +positionHandler_t positionHandler; +loggingHandler_t loggingHandler; +transitionHandler_t transitionHandler; + +void logString(char *msg) +{ + if (loggingHandler != nullptr) + { + loggingHandler(msg); + } +} + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +uint32_t GetRevision() +{ + return CppLib::getRevision(); +} + +void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler) +{ + syncHandler = handler; + logString("Sync handler set"); +} + +void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler) +{ + heartbeatHandler = handler; + logString("Heartbeat handler set"); +} + +void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler) +{ + positionHandler = handler; + logString("Position handler set"); +} + +void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler) +{ + loggingHandler = handler; + loggingHandler("Logging from plugin is enabled"); +} + +void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler) +{ + transitionHandler = handler; + logString("Transition handler set"); +} + +uint64_t SERIAL_EXPORT Open(char *port) +{ + return CppLib::open(port); +} + +void SERIAL_EXPORT Close(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::close(); +} + +void SERIAL_EXPORT Poll(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::poll(); +} + +void SERIAL_EXPORT SendSyncAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSyncAck(); +} + +void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendHeartbeatAck(); +} + +void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + CppLib::setActiveHandle(handle); + CppLib::sendMotor(controlMethod, pantoIndex, positionX, positionY, rotation); +} + +void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSpeed(pantoIndex, speed); +} + +void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFree(pantoIndex); +} + +void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFreeze(pantoIndex); +} + +void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createPassableObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + CppLib::setActiveHandle(handle); + CppLib::createRail(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y, displacement); +} + +void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::addToObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::removeObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::enableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::disableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + CppLib::sendSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); +} + +uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets) +{ + return 0; +} + +void SERIAL_EXPORT Reset() +{ + CppLib::reset(); +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/src/crashAnalyzer/analyze.cpp b/firmware/haptics/force field/utils/serial/src/crashAnalyzer/analyze.cpp new file mode 100644 index 0000000..9569fdb --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/crashAnalyzer/analyze.cpp @@ -0,0 +1,183 @@ +#include "crashAnalyzer.hpp" + +#include +#include +#include +#include +#include +#include +#include "libInterface.hpp" + +#ifdef WINDOWS +#include +#define popen _popen +#define pclose _pclose +#endif + +#ifdef __APPLE__ +#define popen popen +#define pclose pclose +#endif + +const std::string CrashAnalyzer::c_rebootString = "Rebooting..."; +const std::string CrashAnalyzer::c_backtraceString = "Backtrace:"; + +bool CrashAnalyzer::findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t &foundOffset) +{ + const auto length = string.length(); + int32_t index = length - 1; + auto offset = startOffset; + + while (index > -1 && offset <= endOffset) + { + if (getChar(offset) == string.at(index)) + { + index--; + } + else + { + index = length - 1; + } + offset++; + } + + if (index == -1) + { + foundOffset = offset; + return true; + } + + return false; +} + +std::vector CrashAnalyzer::getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset) +{ + std::string data(startOffset - endOffset + 1, 'a'); + for (auto i = startOffset; i >= endOffset; --i) + { + data.at(startOffset - i) = getChar(i); + } + std::regex regex("(0x.{8}):0x.{8}"); + auto it = std::sregex_iterator(data.begin(), data.end(), regex); + auto end = std::sregex_iterator(); + + std::vector result; + while (it != end) + { + result.push_back((*it++).str(1)); + } + + return result; +} + +char *exec(const char *cmd) +{ + loggingHandler((char *)cmd); + + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd, "r"), pclose); + if (!pipe) + { + loggingHandler("Popen failed"); + return "popen() failed!"; + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != NULL) + { + loggingHandler(buffer.data()); + //result += buffer.data(); + } + char *cstr = new char[result.length() + 1]; + strcpy(cstr, result.c_str()); + // do stuff + delete[] cstr; + return cstr; +} + +void CrashAnalyzer::addr2line(std::vector addresses) +{ + std::cout << std::endl + << "===== STACKTRACE BEGIN =====" << std::endl; + + loggingHandler("===== STACKTRACE BEGIN ====="); +#ifdef __APPLE__ +#define ADDR2LINE_PATH "~/.platformio/packages/toolchain-xtensa32/bin/xtensa-esp32-elf-addr2line" +#define FIRMWARE_PATH "/Users/julio/Documents/Uni/5_Master/HCI_Project_Seminar/dualpantoframework/firmware/.pio/build/esp32dev/firmware.elf" +#endif +#ifdef ADDR2LINE_PATH + std::ostringstream command; + command + << ADDR2LINE_PATH + << " -e " << FIRMWARE_PATH + << " -fpCis"; // see https://linux.die.net/man/1/addr2line + for (const auto &address : addresses) + { + command << " " << address; + } + + command << " 2>&1"; + auto result = exec(command.str().c_str()); + + std::ostringstream out; + out << "Stacktrace (most recent call first):" << std::endl + << result; +#else + loggingHandler("Path to addr2line executable not set. Can't analyze stacktrace."); + std::cout + << "Path to addr2line executable not set. Can't analyze stacktrace." + << std::endl; + +#endif + + std::cout << "====== STACKTRACE END ======" << std::endl; +} + +void CrashAnalyzer::checkOutput() +{ + uint16_t rebootOffset; + if (!findString( + 0, + c_rebootString.length(), + c_rebootString, + rebootOffset)) + { + return; + } + + uint16_t backtraceOffset; + if (!findString( + rebootOffset, + s_length, + c_backtraceString, + backtraceOffset)) + { + std::cout << "Reboot detected, but no backtrace found." << std::endl; + return; + } + char out[c_bufferLength + 1]; + std::memcpy(out, s_buffer, c_bufferLength); + out[c_bufferLength] = '\0'; + for (int i = 0; i < c_bufferLength; i++) + { + char x = s_buffer[i]; + if (x < 32 || x > 126) + { + // space + x = 32; + } + out[i] = x; + } + loggingHandler("ERROR!"); + loggingHandler(out); + auto addresses = getBacktraceAddresses( + backtraceOffset - c_backtraceString.length() - 1, + rebootOffset); + + addr2line(addresses); + clearBuffer(); +} diff --git a/firmware/haptics/force field/utils/serial/src/crashAnalyzer/buffer.cpp b/firmware/haptics/force field/utils/serial/src/crashAnalyzer/buffer.cpp new file mode 100644 index 0000000..8107795 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/crashAnalyzer/buffer.cpp @@ -0,0 +1,28 @@ +#include "crashAnalyzer.hpp" + +#include +#include + +uint8_t CrashAnalyzer::s_buffer[c_bufferLength]; +uint16_t CrashAnalyzer::s_length = 0; +uint16_t CrashAnalyzer::s_index = 0; + +void CrashAnalyzer::clearBuffer() +{ + s_index = 0; + s_length = 0; +} + +uint8_t CrashAnalyzer::getChar(uint16_t offset) +{ + return s_buffer[SAFEMOD((s_index - offset), c_bufferLength)]; +} + +void CrashAnalyzer::push_back(const uint8_t character) +{ + s_buffer[s_index] = character; + s_index = (s_index + 1) % c_bufferLength; + s_length = (s_length >= c_bufferLength) ? c_bufferLength : (s_length + 1); + + checkOutput(); +} diff --git a/firmware/haptics/force field/utils/serial/src/node/main.cpp b/firmware/haptics/force field/utils/serial/src/node/main.cpp new file mode 100644 index 0000000..fbbcadf --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/main.cpp @@ -0,0 +1,23 @@ +#include "node.hpp" + +#define defFunc(name, ptr) \ + if (napi_create_function(env, NULL, 0, ptr, NULL, &fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to wrap native function"); \ + } \ + if (napi_set_named_property(env, exports, name, fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to populate exports"); \ + } + +napi_value Init(napi_env env, napi_value exports) +{ + napi_value fn; + defFunc("open", Node::open); + defFunc("close", Node::close); + defFunc("poll", Node::poll); + defFunc("send", Node::send); + return exports; +} + +NAPI_MODULE(NODE_GYP_MODULE_NAME, Init) diff --git a/firmware/haptics/force field/utils/serial/src/node/poll.cpp b/firmware/haptics/force field/utils/serial/src/node/poll.cpp new file mode 100644 index 0000000..fd20886 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/poll.cpp @@ -0,0 +1,121 @@ +#include "node.hpp" + +#include + +napi_value Node::poll(napi_env env, napi_callback_info info) +{ + // argv[0]: handle + // argv[1]: device + // argv[2]: vector constructor + // argv[3]: sync cb + // argv[4]: heartbeat cb + // argv[5]: position cb + // argv[6]: debug log cb + size_t argc = 7; + napi_value argv[7]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + // only keep binary state for packages where only the newest counts + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + double positionCoords[2 * 5]; + + while (getAvailableByteCount(s_handle)) + { + receivePacket(); + + if (s_header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + uint16_t offset = 0; + switch (s_header.MessageType) + { + case SYNC: + { + auto receivedRevision = DPSerial::receiveUInt32(offset); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::cout + << "Received invalid revision id " << receivedRevision + << " (expected " << c_revision << "). Maybe try reset the device?" << std::endl; + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (offset < s_header.PayloadSize) + { + uint8_t index = offset / 4; + positionCoords[index] = DPSerial::receiveFloat(offset); + } + break; + case DEBUG_LOG: + napi_value result; + napi_create_string_utf8(env, reinterpret_cast(s_packetBuffer), s_header.PayloadSize, &result); + napi_call_function(env, argv[1], argv[6], 1, &result, NULL); + break; + default: + break; + } + } + + if(receivedSync) + { + napi_call_function(env, argv[1], argv[3], 0, NULL, NULL); + } + + if(receivedHeartbeat) + { + napi_call_function(env, argv[1], argv[4], 0, NULL, NULL); + } + + if (receivedPosition) + { + napi_value result; + napi_create_array(env, &result); + + const uint32_t posArgc = 3; + const uint32_t goArgc = 2; + for (auto i = 0u; i < 2; ++i) + { + napi_value posArgv[posArgc]; + + for (auto j = 0u; j < posArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + j], &(posArgv[j])); + } + + napi_value vector; + NAPI_CHECK(napi_new_instance(env, argv[2], posArgc, posArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2, vector)); + + napi_value goArgv[goArgc]; + + for (auto j = 0u; j < goArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + 3 + j], &(goArgv[j])); + } + + NAPI_CHECK(napi_new_instance(env, argv[2], goArgc, goArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2 + 1, vector)); + } + + NAPI_CHECK(napi_call_function(env, argv[1], argv[5], 1, &result, NULL)); + } + + return NULL; +} diff --git a/firmware/haptics/force field/utils/serial/src/node/receiveHelpers.cpp b/firmware/haptics/force field/utils/serial/src/node/receiveHelpers.cpp new file mode 100644 index 0000000..2450200 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/receiveHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +napi_value Node::receiveUInt8(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt8(offset), &result); + return result; +} + +napi_value Node::receiveInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt16(offset), &result); + return result; +} + +napi_value Node::receiveUInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt16(offset), &result); + return result; +} + +napi_value Node::receiveInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt32(offset), &result); + return result; +} + +napi_value Node::receiveUInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt32(offset), &result); + return result; +} + +napi_value Node::receiveFloat(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_double(env, DPSerial::receiveFloat(offset), &result); + return result; +} diff --git a/firmware/haptics/force field/utils/serial/src/node/send.cpp b/firmware/haptics/force field/utils/serial/src/node/send.cpp new file mode 100644 index 0000000..90f854e --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/send.cpp @@ -0,0 +1,198 @@ +#include "node.hpp" + +#include + +napi_value Node::send(napi_env env, napi_callback_info info) +{ + size_t argc = 3; + napi_value argv[3]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + uint32_t messageType; + napi_get_value_uint32(env, argv[1], &messageType); + + s_header.MessageType = static_cast(messageType); + + uint16_t offset = 0; + switch (messageType) + { + case SYNC_ACK: + break; + case HEARTBEAT_ACK: + break; + case MOTOR: + { + napi_value tempNapiValue; + uint32_t tempUInt32; + double tempDouble; + uint32_t index = 0; + + // control method + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // panto index + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // position + while (index < 5) + { + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_double(env, tempNapiValue, &tempDouble); + DPSerial::sendFloat(static_cast(tempDouble), offset); + } + break; + } + case PID: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "motorIndex", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "pid", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_value pidNapiValue; + double pidDouble; + for (auto i = 0; i < 3; ++i) + { + napi_get_element(env, tempNapiValue, i, &pidNapiValue); + napi_get_value_double(env, pidNapiValue, &pidDouble); + DPSerial::sendFloat(static_cast(pidDouble), offset); + } + break; + } + case CREATE_OBSTACLE: + case CREATE_PASSABLE_OBSTACLE: + case ADD_TO_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + break; + } + case CREATE_RAIL: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + double displacement; + napi_get_value_double(env, tempNapiValue, &displacement); + DPSerial::sendFloat(static_cast(displacement), offset); + break; + } + case REMOVE_OBSTACLE: + case ENABLE_OBSTACLE: + case DISABLE_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + break; + } + case CALIBRATE_PANTO:{ + break; + } + case DUMP_HASHTABLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + break; + } + default: + napi_throw_error(env, NULL, (std::string("Invalid message type") + std::to_string(messageType)).c_str()); + return NULL; + } + + #ifdef DEBUG_LOGGING + std::cout << "Payload size: 0x" << std::setw(2) << offset << std::endl; + #endif + + if(offset > c_maxPayloadSize) + { + std::cout << "Error: payload size of " << offset << " exceeds max payload size (" << c_maxPayloadSize << ")." << std::endl; + return NULL; + } + + s_header.PayloadSize = offset; + sendPacket(); + + #ifdef DEBUG_LOGGING + dumpBuffers(); + #endif + + return NULL; +} diff --git a/firmware/haptics/force field/utils/serial/src/node/sendHelpers.cpp b/firmware/haptics/force field/utils/serial/src/node/sendHelpers.cpp new file mode 100644 index 0000000..f1b062f --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/sendHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +void Node::sendUInt8(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt8(static_cast(temp), offset); +} + +void Node::sendInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendInt16(static_cast(temp), offset); +} + +void Node::sendUInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt16(static_cast(temp), offset); +} + +void Node::sendInt32(napi_env env, napi_value value, uint16_t& offset) +{ + int32_t temp; + napi_get_value_int32(env, value, &temp); + DPSerial::sendInt32(temp, offset); +} + +void Node::sendUInt32(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt32(temp, offset); +} + +void Node::sendFloat(napi_env env, napi_value value, uint16_t& offset) +{ + double temp; + napi_get_value_double(env, value, &temp); + DPSerial::sendFloat(static_cast(temp), offset); +} diff --git a/firmware/haptics/force field/utils/serial/src/node/setup.cpp b/firmware/haptics/force field/utils/serial/src/node/setup.cpp new file mode 100644 index 0000000..0c72735 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/node/setup.cpp @@ -0,0 +1,34 @@ +#include "node.hpp" + +napi_value Node::open(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + size_t length; + char buffer[64]; + napi_get_value_string_utf8(env, argv[0], buffer, sizeof(buffer), &length); + if (!setup(buffer)) + { + napi_throw_error(env, NULL, "open failed"); + } + napi_value result; + napi_create_int64(env, reinterpret_cast(s_handle), &result); + return result; +} + +napi_value Node::close(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + tearDown(); + return NULL; +} diff --git a/firmware/haptics/force field/utils/serial/src/serial/packet.cpp b/firmware/haptics/force field/utils/serial/src/serial/packet.cpp new file mode 100644 index 0000000..380f276 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/serial/packet.cpp @@ -0,0 +1,86 @@ +#include "packet.hpp" + +Packet::Packet(uint8_t messageType, uint16_t payloadSize) +{ + header.MessageType = messageType; + header.PayloadSize = payloadSize; +} + +uint8_t Packet::receiveUInt8() +{ + return payload[payloadIndex++]; +} + +int16_t Packet::receiveInt16() +{ + uint8_t temp[2]; + for (auto i = 0; i < 2; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 2; + return temp[0] << 8 | temp[1]; +} + +uint16_t Packet::receiveUInt16() +{ + auto temp = receiveInt16(); + return *reinterpret_cast(&temp); +} + +int32_t Packet::receiveInt32() +{ + uint8_t temp[4]; + for (auto i = 0; i < 4; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 4; + return temp[0] << 24 | temp[1] << 16 | temp[2] << 8 | temp[3]; +} + +uint32_t Packet::receiveUInt32() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +float Packet::receiveFloat() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +void Packet::sendUInt8(uint8_t value) +{ + payload[payloadIndex++] = value; +} + +void Packet::sendInt16(int16_t value) +{ + payload[payloadIndex++] = value >> 8; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt16(uint16_t value) +{ + sendInt16(*reinterpret_cast(&value)); +} + +void Packet::sendInt32(int32_t value) +{ + payload[payloadIndex++] = value >> 24; + payload[payloadIndex++] = (value >> 16) & 255; + payload[payloadIndex++] = (value >> 8) & 255; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt32(uint32_t value) +{ + sendInt32(*reinterpret_cast(&value)); +} + +void Packet::sendFloat(float value) +{ + sendInt32(*reinterpret_cast(&value)); +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/src/serial/shared.cpp b/firmware/haptics/force field/utils/serial/src/serial/shared.cpp new file mode 100644 index 0000000..30a72ac --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/serial/shared.cpp @@ -0,0 +1,311 @@ +#include "serial.hpp" + +#include +#include +#include +#include +#include + +#include "crashAnalyzer.hpp" +#include "libInterface.hpp" + +std::string DPSerial::s_path; +FILEHANDLE DPSerial::s_handle; +std::thread DPSerial::s_worker; +bool DPSerial::s_workerRunning = false; + +std::queue DPSerial::s_highPrioSendQueue; +std::queue DPSerial::s_lowPrioSendQueue; +std::queue DPSerial::s_receiveQueue; +uint8_t DPSerial::s_nextTrackedPacketId = 1; +bool DPSerial::s_haveUnacknowledgedTrackedPacket = false; +Packet DPSerial::s_lastTrackedPacket(0, 0); +std::chrono::time_point + DPSerial::s_lastTrackedPacketSendTime; +bool DPSerial::s_pantoReportedInvalidData = false; +const std::chrono::milliseconds DPSerial::c_trackedPacketTimeout(10); + +bool DPSerial::s_pantoReady = true; +uint32_t DPSerial::s_magicReceiveIndex = 0; +ReceiveState DPSerial::s_receiveState = NONE; +Header DPSerial::s_receiveHeader = {0, 0}; + +void DPSerial::startWorker() +{ + reset(); + s_workerRunning = true; + s_worker = std::thread(update); +} + +void DPSerial::stopWorker() +{ + s_workerRunning = false; + s_worker.join(); +} + +void DPSerial::sendInstantPacket(Packet p) +{ + s_highPrioSendQueue.push(p); +} + +void DPSerial::sendPacket(Packet p) +{ + s_lowPrioSendQueue.push(p); +} + +void DPSerial::reset() +{ + std::queue emptyHP; + std::swap(s_highPrioSendQueue, emptyHP); + std::queue emptyLP; + std::swap(s_lowPrioSendQueue, emptyLP); + std::queue emptyRec; + std::swap(s_receiveQueue, emptyRec); + s_nextTrackedPacketId = 1; + s_haveUnacknowledgedTrackedPacket = false; + s_lastTrackedPacketSendTime; + s_pantoReportedInvalidData = false; + s_pantoReady = true; + s_magicReceiveIndex = 0; + s_receiveState = NONE; + s_receiveHeader = {0, 0}; +} + +void DPSerial::update() +{ + while (s_workerRunning) + { + while (processInput()) + ; + processOutput(); + } +} + +bool DPSerial::isTracked(uint8_t t) +{ + auto it = TrackedMessageTypes.find((MessageType)t); + return it != TrackedMessageTypes.end(); +} + +bool DPSerial::checkQueue(std::queue &q) +{ + if (q.empty()) + { + return false; + } + auto tracked = isTracked(q.front().header.MessageType); + return !tracked || !s_haveUnacknowledgedTrackedPacket; +} + +void DPSerial::processOutput() +{ + bool resend = false; + Packet packet(255, 0); + + // send high prio packets (sync/heartbeat) + if (checkQueue(s_highPrioSendQueue)) + { + packet = s_highPrioSendQueue.front(); + s_highPrioSendQueue.pop(); + } + // otherwise, check if panto buffer is critical + else if (!s_pantoReady) + { + return; + } + // check if the last tracked message has to be resent + else if ( + s_haveUnacknowledgedTrackedPacket && + std::chrono::steady_clock::now() - s_lastTrackedPacketSendTime > + c_trackedPacketTimeout) + { + logString("Packet timed out, resending"); + packet = s_lastTrackedPacket; + } + // otherwise, send a low prio packet + else if (checkQueue(s_lowPrioSendQueue)) + { + packet = s_lowPrioSendQueue.front(); + s_lowPrioSendQueue.pop(); + } + // no packets, nothing to do + else + { + return; + } + + if (packet.payloadIndex != packet.header.PayloadSize) + { + logString("INVALID PACKET"); + return; + } + + if (isTracked(packet.header.MessageType)) + { + if (packet.header.PacketId == 0) + { + packet.header.PacketId = s_nextTrackedPacketId++; + if (s_nextTrackedPacketId == 0) + { + s_nextTrackedPacketId++; + } + } + s_haveUnacknowledgedTrackedPacket = true; + s_lastTrackedPacket = packet; + s_lastTrackedPacketSendTime = std::chrono::steady_clock::now(); + } + + uint8_t header[c_headerSize]; + header[0] = packet.header.MessageType; + header[1] = packet.header.PacketId; + header[2] = packet.header.PayloadSize >> 8; + header[3] = packet.header.PayloadSize & 255; + + write(c_magicNumber, c_magicNumberSize); + write(header, c_headerSize); + write(packet.payload, packet.header.PayloadSize); +} + +bool DPSerial::processInput() +{ + switch (s_receiveState) + { + case NONE: + return readMagicNumber(); + case FOUND_MAGIC: + return readHeader(); + case FOUND_HEADER: + return readPayload(); + default: + return false; + } +} + +bool DPSerial::readMagicNumber() +{ + uint8_t received; + while (s_magicReceiveIndex < c_magicNumberSize && + readBytesIfAvailable(&received, 1)) + { + if (received == c_magicNumber[s_magicReceiveIndex]) + { + ++s_magicReceiveIndex; + } + else + { + std::cout << received; + s_magicReceiveIndex = 0; +#ifndef SKIP_ANALYZER + CrashAnalyzer::push_back(received); +#endif + } + } + if (s_magicReceiveIndex != c_magicNumberSize) + { + return false; + }; + s_receiveState = FOUND_MAGIC; + s_magicReceiveIndex = 0; + return true; +} + +bool DPSerial::readHeader() +{ + uint8_t received[c_headerSize]; + if (!readBytesFromSerial(received, c_headerSize)) + { + return false; + } + + s_receiveHeader.MessageType = received[0]; + s_receiveHeader.PacketId = received[1]; + s_receiveHeader.PayloadSize = received[2] << 8 | received[3]; + + // handle no-payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case BUFFER_CRITICAL: + s_pantoReady = false; + s_receiveState = NONE; + logString("Panto buffer critical"); + break; + case BUFFER_READY: + s_pantoReady = true; + s_receiveState = NONE; + logString("Panto buffer ready"); + break; + case INVALID_DATA: + s_receiveState = NONE; + s_pantoReportedInvalidData = true; + logString("Panto received invalid data"); + break; + default: + s_receiveState = FOUND_HEADER; + break; + } + return true; +} + +bool DPSerial::readPayload() +{ + const uint16_t size = s_receiveHeader.PayloadSize; + std::vector received; + received.reserve(size); + if (!readBytesFromSerial(received.data(), size)) + { + return false; + } + + auto packet = Packet(s_receiveHeader.MessageType, size); + memcpy(packet.payload, received.data(), size); + s_receiveState = NONE; + + // handle payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case PACKET_ACK: + { + auto id = packet.receiveUInt8(); + if (!s_haveUnacknowledgedTrackedPacket) + { + logString("Received unexpected PACKET_ACK"); + } + else if (id != s_lastTrackedPacket.header.PacketId) + { + std::ostringstream oss; + oss << "Received PACKET_ACK for wrong packet. Expected " + << (int)s_lastTrackedPacket.header.PacketId << ", received " + << (int)id << std::endl; + logString((char *)oss.str().c_str()); + } + else + { + s_haveUnacknowledgedTrackedPacket = false; + } + break; + } + case INVALID_PACKET_ID: + { + std::ostringstream oss; + oss << "Panto reports INVALID_PACKET_ID. Expected " + << (int)packet.receiveUInt8() << ", received " + << (int)packet.receiveUInt8() << std::endl; + logString((char *)oss.str().c_str()); + break; + } + default: + s_receiveQueue.push(packet); + break; + } + + return true; +} + +bool DPSerial::readBytesIfAvailable(void *target, uint32_t length) +{ + if (getAvailableByteCount(s_handle) < length) + { + return false; + } + return readBytesFromSerial(target, length); +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/src/serial/unix.cpp b/firmware/haptics/force field/utils/serial/src/serial/unix.cpp new file mode 100644 index 0000000..eedd2c4 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/serial/unix.cpp @@ -0,0 +1,77 @@ +#include "serial.hpp" + +#include +#include + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + uint32_t available = 0; + if (ioctl(fileno(s_handle), FIONREAD, &available) < 0) + { + return 0; + } + return available; +} + +void DPSerial::tearDown() +{ + stopWorker(); + fclose(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + const uint32_t result = fread(target, 1, length, s_handle); + const bool valid = result == length; + if (!valid) + { + if (feof(s_handle)) + { + std::cout + << "Read end of file from serial, trying to reconnect." + << std::endl; + tearDown(); + setup(s_path); + } + else if (ferror(s_handle)) + { + perror("Error while reading from serial"); + } + } + return valid; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + ::write(fileno(s_handle), data, length); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + int fd = open(path.c_str(), O_RDWR | O_NOCTTY); + if (fd < 0) + { + return false; + } + struct termios tty; + std::memset(&tty, 0, sizeof(tty)); + if (tcgetattr(fd, &tty) < 0) + { + return false; + } + const speed_t speed = c_baudRate; + cfsetospeed(&tty, speed); + cfsetispeed(&tty, speed); + cfmakeraw(&tty); + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + if (tcsetattr(fd, TCSANOW, &tty) < 0) + { + return false; + } + s_handle = fdopen(fd, "rw"); + + startWorker(); + return true; +} diff --git a/firmware/haptics/force field/utils/serial/src/serial/win.cpp b/firmware/haptics/force field/utils/serial/src/serial/win.cpp new file mode 100644 index 0000000..51adc05 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/serial/win.cpp @@ -0,0 +1,71 @@ +#include "serial.hpp" + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + DWORD commerr; + COMSTAT comstat; + if (!ClearCommError(s_handle, &commerr, &comstat)) + { + return 0; + } + return comstat.cbInQue; +} + +void DPSerial::tearDown() +{ + stopWorker(); + CloseHandle(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + DWORD bytesRead; + ReadFile(s_handle, target, length, &bytesRead, NULL); + return bytesRead == length; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + DWORD bytesWritten = 0; + WriteFile(s_handle, data, length, &bytesWritten, NULL); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + s_handle = CreateFileA(path.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + if (s_handle == INVALID_HANDLE_VALUE) + { + return false; + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!GetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + dcbSerialParams.BaudRate = c_baudRate; + dcbSerialParams.ByteSize = 8; + dcbSerialParams.StopBits = ONESTOPBIT; + dcbSerialParams.Parity = NOPARITY; + if (!SetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + + COMMTIMEOUTS timeouts = {0}; + timeouts.ReadIntervalTimeout = 50; + timeouts.ReadTotalTimeoutConstant = 50; + timeouts.ReadTotalTimeoutMultiplier = 10; + timeouts.WriteTotalTimeoutConstant = 50; + timeouts.WriteTotalTimeoutMultiplier = 10; + if (!SetCommTimeouts(s_handle, &timeouts) || + !SetCommMask(s_handle, EV_RXCHAR)) + { + return false; + } + + startWorker(); + return true; +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/src/standalone/main.cpp b/firmware/haptics/force field/utils/serial/src/standalone/main.cpp new file mode 100644 index 0000000..9e4da35 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/standalone/main.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +#include "standalone.hpp" + +int main(int argc, char** argv) +{ + if (argc != 2) + { + fprintf(stderr, "Expected /dev/serialport\n"); + return -1; + } + if (!Standalone::setup(argv[1])) + { + fprintf(stderr, "Could not open %s\n", argv[1]); + return -2; + } + signal(SIGINT, &(Standalone::terminate)); + + // cin/cout to hex mode + std::cout << std::hex << std::uppercase << std::setfill('0'); + + while (true) + { + Standalone::printPacket(); + } + + return 0; +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/src/standalone/standalone.cpp b/firmware/haptics/force field/utils/serial/src/standalone/standalone.cpp new file mode 100644 index 0000000..d177bb4 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/src/standalone/standalone.cpp @@ -0,0 +1,27 @@ +#include +#include +#include +#include + +#include "standalone.hpp" + +void Standalone::terminate(int signal) +{ + tearDown(); + exit(0); +} + +void Standalone::printPacket() +{ + receivePacket(); + for (auto i = 0; i < c_headerSize; ++i) + { + std::cout << std::setw(2) << (int)s_headerBuffer[i] << " "; + } + std::cout << "| "; + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + std::cout << std::setw(2) << (int)s_packetBuffer[i] << " "; + } + std::cout << std::endl; +} \ No newline at end of file diff --git a/firmware/haptics/force field/utils/serial/unity/Serial.cs b/firmware/haptics/force field/utils/serial/unity/Serial.cs new file mode 100644 index 0000000..4936765 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/unity/Serial.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using UnityEngine; + +public class Serial : MonoBehaviour { + public delegate void SyncDelegate(ulong handle); + public delegate void HeartbeatDelegate(ulong handle); + public delegate void PositionDelegate(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions); + public delegate void LoggingDelegate(IntPtr msg); + + protected ulong Handle; + + [DllImport("serial")] + private static extern uint GetRevision(); + [DllImport("serial")] + private static extern void SetSyncHandler(SyncDelegate func); + [DllImport("serial")] + private static extern void SetHeartbeatHandler(HeartbeatDelegate func); + [DllImport("serial")] + private static extern void SetPositionHandler(PositionDelegate func); + [DllImport("serial")] + private static extern void SetLoggingHandler(LoggingDelegate func); + [DllImport("serial")] + private static extern ulong Open(IntPtr port); + [DllImport("serial")] + private static extern void Close(ulong handle); + [DllImport("serial")] + private static extern void Poll(ulong handle); + [DllImport("serial")] + private static extern void SendSyncAck(ulong handle); + [DllImport("serial")] + private static extern void SendHeartbeatAck(ulong handle); + + private static void SyncHandler(ulong handle) + { + Debug.Log("Received sync"); + SendSyncAck(handle); + } + + private static void HeartbeatHandler(ulong handle) + { + Debug.Log("Received heartbeat"); + SendHeartbeatAck(handle); + } + + private static void PositionHandler(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions) + { + Debug.Log("Received positions: (" + positions[0] + "|" + positions[1] + ")"); + //Debug.Log("Received positions"); + } + + private static void LogHandler(IntPtr msg) + { + Debug.Log(Marshal.PtrToStringAnsi(msg)); + } + + private static ulong OpenPort(string port) + { + return Open(Marshal.StringToHGlobalAnsi(port)); + } + + void Start () + { + Debug.Log("Serial protocol revision: " + GetRevision()); + SetLoggingHandler(LogHandler); + SetSyncHandler(SyncHandler); + SetHeartbeatHandler(HeartbeatHandler); + SetPositionHandler(PositionHandler); + // should be discovered automatically + Handle = OpenPort("//.//COM3"); + } + + void Update () + { + Poll(Handle); + } + + void OnDestroy() + { + Close(Handle); + } +} diff --git a/firmware/haptics/force field/utils/serial/unity/linux.sh b/firmware/haptics/force field/utils/serial/unity/linux.sh new file mode 100755 index 0000000..8d673ed --- /dev/null +++ b/firmware/haptics/force field/utils/serial/unity/linux.sh @@ -0,0 +1,3 @@ +#!/bin/sh +cmake .. -G"Unix Makefiles" -S".." -B"../cppLibBuild" -DCMAKE_BUILD_TYPE=Release +cmake --build "../cppLibBuild" --config Release diff --git a/firmware/haptics/force field/utils/serial/unity/mac.sh b/firmware/haptics/force field/utils/serial/unity/mac.sh new file mode 100755 index 0000000..33b47e9 --- /dev/null +++ b/firmware/haptics/force field/utils/serial/unity/mac.sh @@ -0,0 +1,5 @@ +SOURCE_DIR="./utils/serial" +BUILD_DIR="./utils/serial/cppLibBuild" + +cmake $SOURCE_DIR -G"Xcode" -S $SOURCE_DIR -B $BUILD_DIR +cmake --build $BUILD_DIR --config Release diff --git a/firmware/haptics/force field/utils/serial/unity/win.bat b/firmware/haptics/force field/utils/serial/unity/win.bat new file mode 100755 index 0000000..d18e6aa --- /dev/null +++ b/firmware/haptics/force field/utils/serial/unity/win.bat @@ -0,0 +1,5 @@ +SET SOURCE_DIR=utils\serial +SET BUILD_DIR=utils\serial\cppLibBuild + +cmake %SOURCE_DIR% -G"Visual Studio 15 2017 Win64" -S %SOURCE_DIR% -B %BUILD_DIR% +cmake --build %BUILD_DIR% --config RelWithDebInfo \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/.gitignore b/firmware/haptics/line wall/firmware/.gitignore new file mode 100644 index 0000000..6106fab --- /dev/null +++ b/firmware/haptics/line wall/firmware/.gitignore @@ -0,0 +1,4 @@ +.vscode +.pio +.pioenvs +.piolibdeps diff --git a/firmware/haptics/line wall/firmware/.idea/.gitignore b/firmware/haptics/line wall/firmware/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/firmware/haptics/line wall/firmware/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/firmware/haptics/line wall/firmware/.idea/misc.xml b/firmware/haptics/line wall/firmware/.idea/misc.xml new file mode 100644 index 0000000..d858eb1 --- /dev/null +++ b/firmware/haptics/line wall/firmware/.idea/misc.xml @@ -0,0 +1,17 @@ + + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/.idea/vcs.xml b/firmware/haptics/line wall/firmware/.idea/vcs.xml new file mode 100644 index 0000000..8aefe6c --- /dev/null +++ b/firmware/haptics/line wall/firmware/.idea/vcs.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/include/config/config.hpp b/firmware/haptics/line wall/firmware/include/config/config.hpp new file mode 100644 index 0000000..ab0218a --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/config/config.hpp @@ -0,0 +1,103 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is 40. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {0x8F, 0x37, 0xA2, 0x7A, 0x29, 0xDA, 0x7D, 0xB4, 0xBB, 0x54, 0x0F, 0x13, 0x43, 0x71, 0xBC, 0xE9}; +const float opMinDist = 65, + opMaxDist = 210, + opAngle = 2.2; +extern float forceFactor; +const uint8_t pantoCount = 2; +const uint8_t dummyPin = 40; +#define LINKAGE_ENCODER_USE_SPI +const uint32_t numberOfSpiEncoders = 4; +const float linkageBaseX[] = { + -23.7, 61.1, 0, -61.1, 23.7, 0 +}; +const float linkageBaseY[] = { + 0, 0, 0, 0, 0, 0 +}; +const float linkageInnerLength[] = { + 95.07, 57.71, 0, 57.71, 95.07, 0 +}; +const float linkageOuterLength[] = { + 119.79, 119.79, 0, 119.79, 119.79, 0 +}; +const uint8_t linkageHandleMount[] = { + 0, 0, 1, 0, 0, 0 +}; +const float motorPowerLimit[] = { + 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 +}; +const float motor_powerLimitForce[] = { + 0.6, 0.6, 0.2, 0.6, 0.6, 0.2 +}; +extern float pidFactor[6][3]; +const float forceP = 0.375; +const float forceI = 0; +const float forceD = 0; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + 40, 40, 25, 40, 40, 23 +}; +const uint8_t motorPwmPinForwards[] = { + 16, 2, 40, 22, 19, 40 +}; +const uint8_t motorPwmPinBackwards[] = { + 4, 17, 40, 21, 18, 40 +}; +const uint8_t motorDirAPin[] = { + 40, 40, 32, 40, 40, 26 +}; +const uint8_t motorDirBPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const bool motorFlipped[] = { + false, false, true, false, false, true +}; +const uint8_t encoderAPin[] = { + 40, 40, 35, 40, 40, 36 +}; +const uint8_t encoderBPin[] = { + 40, 40, 34, 40, 40, 39 +}; +const uint8_t encoderIndexPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const uint32_t encoderSteps[] = { + 16384, 16384, 260, 16384, 16384, 260 +}; +const uint32_t encoderSpiIndex[] = { + 3, 2, 4294967295, 0, 1, 4294967295 +}; +const float encoderFlipped[] = { + 1, 1, 1, -1, -1, -1 +}; +const float setupAngle[] = { + -0.5, 0.001, 0, -0.5, 0.001, 0 +}; +constexpr float rangeMinX = -180; +constexpr float rangeMinY = -205; +constexpr float rangeMaxX = 180; +constexpr float rangeMaxY = 5; +constexpr uint32_t hashtableMaxMemory = 100000; +constexpr uint32_t hashtableUsedMemory = 98532; +constexpr uint32_t hashtableMaxCells = 8333; +constexpr uint32_t hashtableNumCells = 8211; +constexpr uint32_t hashtableStepsX = 119; +constexpr uint32_t hashtableStepsY = 69; +constexpr double hashtableStepSizeX = 3.0252100840336134; +constexpr double hashtableStepSizeY = 3.0434782608695654; +const uint32_t obstacleChangesPerFrame = 1; \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/include/hardware/angleAccessor.hpp b/firmware/haptics/line wall/firmware/include/hardware/angleAccessor.hpp new file mode 100644 index 0000000..fc7e636 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/angleAccessor.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function AngleAccessor; diff --git a/firmware/haptics/line wall/firmware/include/hardware/panto.hpp b/firmware/haptics/line wall/firmware/include/hardware/panto.hpp new file mode 100644 index 0000000..143d308 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/panto.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include + +#include "config/config.hpp" +#include "hardware/angleAccessor.hpp" +#include "utils/vector.hpp" +#include + +// make sure results are in range -270° ~ 0° ~ +90° +#define ensureAngleRange(angle) \ + angle > HALF_PI ? \ + angle - TWO_PI : \ + angle < -(PI + HALF_PI) ? \ + angle + TWO_PI : \ + angle + +class Panto +{ +private: + static const uint16_t c_ledcFrequency = 20000; + static const uint16_t c_ledcResolution = 12; + static const uint16_t PWM_MAX = 4095; // (2^12)-1 + static const uint8_t c_dofCount = 3; + const uint8_t c_localLeftIndex = 0; + const uint8_t c_localRightIndex = 1; + const uint8_t c_localHandleIndex = 2; + + const uint8_t c_pantoIndex; + const uint8_t c_globalIndexOffset; + const uint8_t c_globalLeftIndex; + const uint8_t c_globalRightIndex; + const uint8_t c_globalHandleIndex; + const float c_leftInnerLength; + const float c_rightInnerLength; + const float c_leftOuterLength; + const float c_rightOuterLength; + const float c_leftInnerLengthDoubled; + const float c_rightInnerLengthDoubled; + const float c_leftInnerLengthSquared; + const float c_rightInnerLengthSquared; + const float c_leftOuterLengthSquared; + const float c_rightOuterLengthSquared; + const float c_leftInnerLengthSquaredMinusLeftOuterLengthSquared; + const float c_rightInnerLengthSquaredMinusRightOuterLengthSquared; + const float c_leftOuterLengthSquaredMinusRightOuterLengthSquared; + const bool c_handleMountedOnRightArm; + const float c_leftBaseX; + const float c_leftBaseY; + const float c_rightBaseX; + const float c_rightBaseY; + + #ifdef LINKAGE_ENCODER_USE_SPI + AngleAccessor m_angleAccessors[2]; + #endif + Encoder* m_encoder[c_dofCount]; + float m_actuationAngle[c_dofCount]; + float m_previousAngle[c_dofCount]; + float m_previousAngles[c_dofCount][5]; + float m_targetAngle[c_dofCount]; + float m_previousDiff[c_dofCount]; + float m_integral[c_dofCount]; + uint32_t m_prevTime = 0; + + int m_previousAnglesCount = 0; + int m_encoderErrorCount = 0; + int m_encoderErrorCounts[4] = {0,0,0,0}; + int m_encoderRequestCount = 0; + int m_encoderRequestCounts[4] = {0,0,0,0}; + float m_leftInnerAngle = 0; + float m_rightInnerAngle = 0; + float m_pointingAngle = 0; + float m_handleX = 0; + float m_handleY = 0; + float m_targetX = 0; + float m_targetY = 0; + float m_startX = 0; + float m_startY = 0; + float m_filteredX = 0; + float m_filteredY = 0; + float m_tweeningValue = 0.0f; + float m_tweeningStep = 0.00001f; + float m_tweeningSpeed = 1.0f; + uint32_t m_tweeningPrevtime = 0; + float m_dt = 0.0001f; + float delta = 0.01; + float velocity = 1.0f; + bool m_isforceRendering = false; + bool m_inTransition = false; + float m_jacobian[2][2] = {{0.0, 0.0}, {0.0, 0.0}}; + bool m_isFrozen = false; + bool m_isCalibrating = false; + + void inverseKinematics(); + void setMotor( + const uint8_t& localIndex, const bool& dir, const float& power); + void disengageMotors(); +public: + Panto(uint8_t pantoIndex); + float getActuationAngle(const uint8_t index) const; + Vector2D getPosition() const; + float getRotation() const; + void setAngleAccessor(const uint8_t index, const AngleAccessor accessor); + void setTarget(const Vector2D target, const bool isForceRendering); + void setSpeed(const float speed); + void setRotation(const float rotation); + bool getCalibrationState(); + void calibrateEncoders(); + void calibratePanto(); + void calibrationEnd(); + void resetActuationAngle(); + void readEncoders(); + void forwardKinematics(); + void actuateMotors(); + int getEncoderErrorCount(); + int getEncoderRequests(); + int getEncoderErrorCounts(int i); + int getEncoderRequestsCounts(int i); + uint8_t getPantoIndex(); + bool getInTransition(); + void setInTransition(bool inTransition); + bool getIsFrozen(); + void setIsFrozen(bool isFrozen); +}; + +extern std::vector pantos; \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/include/hardware/spiCommands.hpp b/firmware/haptics/line wall/firmware/include/hardware/spiCommands.hpp new file mode 100644 index 0000000..4f80dbe --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/spiCommands.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace SPICommands +{ + extern const uint16_t c_readAngle; + extern const uint16_t c_clearError; + extern const uint16_t c_nop; + extern const uint16_t c_highZeroRead; + extern const uint16_t c_lowZeroRead; + extern const uint16_t c_highZeroWrite; + extern const uint16_t c_lowZeroWrite; +}; diff --git a/firmware/haptics/line wall/firmware/include/hardware/spiEncoder.hpp b/firmware/haptics/line wall/firmware/include/hardware/spiEncoder.hpp new file mode 100644 index 0000000..21e9575 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/spiEncoder.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include "hardware/spiPacket.hpp" + +class SPIEncoder +{ + friend class SPIEncoderChain; +private: + SPIPacket m_lastPacket; + uint16_t m_lastValidAngle; + SPIClass* m_spi; + SPIEncoder() = default; + SPIEncoder(SPIClass* spi); + void transfer(uint16_t transmisson); +public: + uint32_t getAngle(); +}; diff --git a/firmware/haptics/line wall/firmware/include/hardware/spiEncoderChain.hpp b/firmware/haptics/line wall/firmware/include/hardware/spiEncoderChain.hpp new file mode 100644 index 0000000..0b5bb86 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/spiEncoderChain.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include "hardware/angleAccessor.hpp" +#include "hardware/spiEncoder.hpp" + +class SPIEncoderChain +{ +private: + SPISettings m_settings; + SPIClass m_spi; + uint32_t m_numberOfEncoders; + std::vector m_encoders; + std::vector m_values; + std::vector m_zeros; + uint32_t errors = 0; + uint32_t requests = 0; + uint8_t m_maxTries = 4; + uint8_t m_currentTry = 0; + // static const uint32_t c_hspiSsPin = 15; + + static const uint32_t c_hspiSsPin1 = 15; + static const uint32_t c_hspiSsPin2 = 5; + static const uint16_t c_nop = 0x0; + static const uint16_t c_clearError = 0x4001; + static const uint16_t c_readAngle = 0xFFFF; + static const uint16_t c_dataMask = 0x3FFF; + + void begin(); + void end(); + std::vector getZero(); + void transfer(uint16_t transmission); + void setZero(std::vector newZero); +public: + SPIEncoderChain(uint32_t numberOfEncoders); + void update(int channel); + void update(); + void clearError(); + void setZero(); + bool needsZero(); + void wakeUp(); + void __setZeros(); + void setPosition(std::vector positions); + uint32_t getErrors(); + uint32_t getRequests(); + void resetErrors(); + AngleAccessor getAngleAccessor(uint32_t index); +}; \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/include/hardware/spiPacket.hpp b/firmware/haptics/line wall/firmware/include/hardware/spiPacket.hpp new file mode 100644 index 0000000..73750d6 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/hardware/spiPacket.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +struct SPIPacket +{ + bool m_parity; + bool m_flag; + uint16_t m_data; + bool m_valid; + uint16_t m_transmission; + static const uint16_t c_parityMask = 0x8000; + static const uint16_t c_flagMask = 0x4000; + static const uint16_t c_dataMask = 0x3FFF; + SPIPacket(bool flag, uint16_t data); + SPIPacket(uint16_t transmisson); + SPIPacket() = default; + bool calcParity(); + bool parityError(); + bool commandInvalidError(); + bool framingError(); +}; diff --git a/firmware/haptics/line wall/firmware/include/ioMain.hpp b/firmware/haptics/line wall/firmware/include/ioMain.hpp new file mode 100644 index 0000000..1641622 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/ioMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void ioSetup(); +void ioLoop(); diff --git a/firmware/haptics/line wall/firmware/include/physics/annotatedEdge.hpp b/firmware/haptics/line wall/firmware/include/physics/annotatedEdge.hpp new file mode 100644 index 0000000..d2c8370 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/annotatedEdge.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +struct Edge; +struct IndexedEdge; +class Obstacle; + +struct AnnotatedEdge +{ + IndexedEdge* m_indexedEdge; + Edge* m_edge; + AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge); + AnnotatedEdge(const AnnotatedEdge& other); + void destroy(); +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/collider.hpp b/firmware/haptics/line wall/firmware/include/physics/collider.hpp new file mode 100644 index 0000000..5d78845 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/collider.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" +#include "utils/vector.hpp" + +class Collider +{ +protected: + std::vector m_points; +public: + Collider(std::vector points); + void add(std::vector points); + virtual bool contains(Vector2D point); + Edge getEdge(uint32_t index); +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/edge.hpp b/firmware/haptics/line wall/firmware/include/physics/edge.hpp new file mode 100644 index 0000000..35b00ff --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/edge.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "utils/vector.hpp" + +struct Edge +{ + Vector2D m_first; + Vector2D m_second; + Edge(Vector2D first, Vector2D second) : m_first(first), m_second(second) {}; + Edge() : m_first(Vector2D()), m_second(Vector2D()) {}; +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/godObject.hpp b/firmware/haptics/line wall/firmware/include/physics/godObject.hpp new file mode 100644 index 0000000..919353c --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/godObject.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include + +#include "hardware/panto.hpp" +#include "physics/godObjectAction.hpp" +#include "physics/indexedEdge.hpp" +#include "physics/obstacle.hpp" +#include "physics/rail.hpp" +#include "physics/hashtable.hpp" +#include "utils/vector.hpp" + +/* +The speed control is in detail described in our Dropbox (Interaction Techniques Haptic in Unity): +https://www.dropbox.com/scl/fi/uljoe140fet2b53bjhr4y/DualPanto-Speed-Control.pptx?dl=0&rlkey=6k77wrfnb3oaxg186489tpinj +*/ + +enum TetherState {Inner, Active, Outer}; +/* the handle can be in 3 states: + 1. Inner: the handle can freely move within a close radius around the god object without triggering speed control (otherwise there would always be force rendered, even if the player was just standing around). + 2. Active: the god object is under speed control. Its acceleration is proportional to the distance between god object and handle. + 3. Outer: out of outer tether radius. Multiple strategies to treat this scenario are described below. +*/ + +enum OutOfTetherStrategy {MaxSpeed, Exploration, Leash}; +/* possible strategies when the handle is pushed out of the outer tether radius. + MaxSpeed: the god object continues to move at max speed and the handle gets pushed back to the new god object position player. On the Unity side there could be an additional penalization (with auditory cues), e.g. that the player loses health points when moving out of the tether (in FPS). + Exploration: the god object position doesn't update anymore and the game is paused (speed control is disabled). The handle can still collide with obstacles and is in exploration mode. + Leash: the handle is "on leash" and can collide with obstacles. The god object is moving at max speed to the position of the handle (along the direct vector between both points). A weak constant pulling force indicates where the god object is. +*/ + +class GodObject +{ +private: + static constexpr double c_resolveDistance = 0.00001; + static constexpr double c_tetherForcePullingBack = -0.5; + static constexpr double c_tetherPockDistance = 500; // when tethered and collision with wall happens push the handle this far into the wall (along the direction of movement) + static constexpr double c_railsTetherFactor = 0.75; // if speed control is enabled and the god object collides with a wall: move the god object by this factor along the vector between handle and god object. This enables jumping rails under speed control. + + Vector2D m_position; + Vector2D m_tetherPosition; + Vector2D m_movementDirection; + Vector2D m_activeForce; + std::map m_obstacles; + Hashtable *m_hashtable = nullptr; + portMUX_TYPE m_obstacleMutex; + bool m_processingObstacleCollision; + u_short m_numCollisions = 0; // for speed control; when the speed is controlled and a collision with multiple walls occurs (in a corner) then the 2nd collision must also be feelable + bool m_doneColliding; + Vector2D m_lastError; + std::set* m_possibleCollisions; + std::deque m_actionQueue; + + // tether related properties + bool m_tethered = false; + double m_tetherFactor = 0; + Vector2D m_lastErrorTether; + double m_tetherInnerRadius = 0; + double m_tetherOuterRadius = 0; + TetherState m_tetherState = Inner; + double m_tetherSafeZonePadding = 0; // padding on the inner border to avoid that the tether gets pushed into the free moving zone immediately once the inner radius is passed + OutOfTetherStrategy m_tetherStrategy = Leash; + bool m_tetherPockEnabled = false; + + Hashtable& hashtable(); + +public: + GodObject(Vector2D position = Vector2D()); + ~GodObject(); + void setMovementDirection(Vector2D movementDirection); + void update(); + void dumpHashtable(); + bool move(bool isTweening, bool isFrozen); + Vector2D checkCollisions(Vector2D targetPoint, Vector2D currentPosition); + void createObstacle(uint16_t id, std::vector points, bool passable); + void createRail(uint16_t id, std::vector points, double displacement); + void addToObstacle(uint16_t id, std::vector points); + void removeObstacle(uint16_t id); + void enableObstacle(uint16_t id, bool enable = true); + Vector2D getPosition(); + Vector2D getActiveForce(); + Vector2D getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition); + Vector2D getTetherForce(Vector2D error); + void renderForce(Vector2D collisionForce, Vector2D tetherForce); + bool processTetheringForce(Vector2D handlePosition, bool lastCollisionState); + bool getProcessingObstacleCollision(); + bool getDoneColliding(); + bool tethered(); + void setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled); +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/godObjectAction.hpp b/firmware/haptics/line wall/firmware/include/physics/godObjectAction.hpp new file mode 100644 index 0000000..97c9c5d --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/godObjectAction.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "physics/annotatedEdge.hpp" +#include "physics/godObjectActionData.hpp" +#include "physics/godObjectActionType.hpp" + +struct GodObjectAction +{ + GodObjectActionType m_type; + GodObjectActionData m_data; + GodObjectAction( + GodObjectActionType type, + AnnotatedEdge* data) + : m_type(type) + , m_data{.m_annotatedEdge = data} { }; + GodObjectAction( + GodObjectActionType type, + uint16_t data) + : m_type(type) + // Intellisense might show an error here, but that's a bug. + // https://github.com/Microsoft/vscode-cpptools/issues/3491 + , m_data{.m_obstacleId = data} { }; +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/godObjectActionData.hpp b/firmware/haptics/line wall/firmware/include/physics/godObjectActionData.hpp new file mode 100644 index 0000000..1a23cac --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/godObjectActionData.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include + +#include "physics/annotatedEdge.hpp" +#include "physics/obstacle.hpp" + +union GodObjectActionData +{ + AnnotatedEdge* m_annotatedEdge; + uint16_t m_obstacleId; +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/godObjectActionType.hpp b/firmware/haptics/line wall/firmware/include/physics/godObjectActionType.hpp new file mode 100644 index 0000000..9b31a78 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/godObjectActionType.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum GodObjectActionType +{ + HT_ENABLE_EDGE, + HT_DISABLE_EDGE, + GO_REMOVE_OBSTACLE +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/hashtable.hpp b/firmware/haptics/line wall/firmware/include/physics/hashtable.hpp new file mode 100644 index 0000000..2a272d0 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/hashtable.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include "config/config.hpp" +#include "hardware/panto.hpp" +#include "physics/annotatedEdge.hpp" +#include "physics/indexedEdge.hpp" + +class Hashtable +{ +private: + static int32_t get1dIndex(double value, double min, double step); + + std::vector m_cells[hashtableNumCells]; + std::vector getCellIndices(Edge edge); + std::set expand(const std::vector& edges); +public: + Hashtable(); + void add(AnnotatedEdge* edge); + void remove(AnnotatedEdge* edge); + void getPossibleCollisions(Edge movement, std::set* result); + void print(); +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/indexedEdge.hpp b/firmware/haptics/line wall/firmware/include/physics/indexedEdge.hpp new file mode 100644 index 0000000..4b20339 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/indexedEdge.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" + +class Obstacle; + +struct IndexedEdge +{ + Obstacle* m_obstacle; + uint32_t m_index; + IndexedEdge(Obstacle* obstacle, uint32_t index); + bool operator==(const IndexedEdge& other) const; + bool operator<(const IndexedEdge& other) const; + Edge getEdge() const; +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/obstacle.hpp b/firmware/haptics/line wall/firmware/include/physics/obstacle.hpp new file mode 100644 index 0000000..1f67687 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/obstacle.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +#include "physics/indexedEdge.hpp" +#include "physics/collider.hpp" + +class Obstacle : public Collider +{ +private: + bool m_enabled = false; + +public: + bool passable = false; + Obstacle(std::vector points); + Obstacle(std::vector points, bool passable); + bool enabled(); + void enable(bool enable = true); + std::vector getIndexedEdges( + uint32_t first = 0, uint32_t last = -1); +}; diff --git a/firmware/haptics/line wall/firmware/include/physics/pantoPhysics.hpp b/firmware/haptics/line wall/firmware/include/physics/pantoPhysics.hpp new file mode 100644 index 0000000..8b5bded --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/pantoPhysics.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include "hardware/panto.hpp" +#include "physics/godObject.hpp" +#include "utils/vector.hpp" + +class PantoPhysics +{ +private: + Panto* m_panto; + Vector2D m_currentPosition; + GodObject* m_godObject; +public: + PantoPhysics(Panto* panto); + GodObject* godObject(); + void step(); +}; + +extern std::vector pantoPhysics; diff --git a/firmware/haptics/line wall/firmware/include/physics/rail.hpp b/firmware/haptics/line wall/firmware/include/physics/rail.hpp new file mode 100644 index 0000000..00f2631 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physics/rail.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "physics/obstacle.hpp" + +class Rail : public Obstacle +{ + +private: + uint32_t displacement; // orthogonal distance to the rail until where the user still feels the applied force of the rail + +public: + Rail(std::vector points, double displacement); + virtual bool contains(Vector2D point); +}; diff --git a/firmware/haptics/line wall/firmware/include/physicsMain.hpp b/firmware/haptics/line wall/firmware/include/physicsMain.hpp new file mode 100644 index 0000000..e98e807 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/physicsMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void physicsSetup(); +void physicsLoop(); diff --git a/firmware/haptics/line wall/firmware/include/tasks/task.hpp b/firmware/haptics/line wall/firmware/include/tasks/task.hpp new file mode 100644 index 0000000..ec2acf9 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/tasks/task.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "tasks/taskFunction.hpp" + +class Task +{ +private: + // default values + static const uint32_t c_defaultStackDepth = 8192; + static const uint32_t c_defaultPriority = 1; + static const uint32_t c_defaultFpsInterval = 1000; + + // task data + TaskFunction m_setupFunc; + TaskFunction m_loopFunc; + const char* m_name; + uint32_t m_stackDepth; + uint32_t m_priority; + TaskHandle_t m_handle; + int m_core; + + // task function + static void taskLoop(void* parameters); + + // fps counter data + uint32_t m_fpsInterval; + FramerateLimiter m_fpsCalcLimiter; + uint32_t m_loopCount; + static std::map s_fpsMap; + bool m_logFps; + + // fps counter funcs + inline void initFps(); + inline void checkFps(); +public: + Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core); + void run(); + void setLogFps(bool logFps = true); + TaskHandle_t getHandle(); +}; diff --git a/firmware/haptics/line wall/firmware/include/tasks/taskFunction.hpp b/firmware/haptics/line wall/firmware/include/tasks/taskFunction.hpp new file mode 100644 index 0000000..e3fb3dc --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/tasks/taskFunction.hpp @@ -0,0 +1,3 @@ +#pragma once + +typedef void (*TaskFunction)(); diff --git a/firmware/haptics/line wall/firmware/include/tasks/taskRegistry.hpp b/firmware/haptics/line wall/firmware/include/tasks/taskRegistry.hpp new file mode 100644 index 0000000..8bcf229 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/tasks/taskRegistry.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include +#include + +#include "tasks/task.hpp" + +typedef std::map TaskRegistry; +extern TaskRegistry Tasks; diff --git a/firmware/haptics/line wall/firmware/include/utils/assert.hpp b/firmware/haptics/line wall/firmware/include/utils/assert.hpp new file mode 100644 index 0000000..70f3d82 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/assert.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "utils/serial.hpp" + +void __fail( + const char* check, std::string val1, std::string val2, + const char* file, int line, const char* func) +{ + DPSerial::sendInstantDebugLog( + "Assertion %s failed with values %s and %s: file \"%s\", line %i, %s", + check, val1.c_str(), val2.c_str(), file, line, func); + abort(); +} + +#define __CHECK(operator, val1, val2) (val1 operator val2) ?\ + (void)0 :\ + __fail(\ + #val1 " " #operator " " #val2,\ + std::to_string(val1), std::to_string(val2),\ + __FILE__, __LINE__, __PRETTY_FUNCTION__); + +#define ASSERT_EQ(val1, val2) __CHECK(==, val1, val2) +#define ASSERT_LT(val1, val2) __CHECK(<, val1, val2) +#define ASSERT_LE(val1, val2) __CHECK(<=, val1, val2) +#define ASSERT_GT(val1, val2) __CHECK(>, val1, val2) +#define ASSERT_GE(val1, val2) __CHECK(>=, val1, val2) +#define ASSERT_NE(val1, val2) __CHECK(!=, val1, val2) diff --git a/firmware/haptics/line wall/firmware/include/utils/crashDump.hpp b/firmware/haptics/line wall/firmware/include/utils/crashDump.hpp new file mode 100644 index 0000000..d526096 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/crashDump.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +class CrashDump +{ +private: + std::ostringstream m_stream; +public: + CrashDump(); + void clear(); + void dump(); + template CrashDump& operator<<(T value) + { + m_stream << value; + return *this; + } +}; + +#ifdef ENABLE_CRASHDUMP +#define CRASHDUMP(dump, call) dump call +#else +#define CRASHDUMP(dump, call) +#endif + +extern CrashDump physicsDump; diff --git a/firmware/haptics/line wall/firmware/include/utils/framerateLimiter.hpp b/firmware/haptics/line wall/firmware/include/utils/framerateLimiter.hpp new file mode 100644 index 0000000..94d43b2 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/framerateLimiter.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +class FramerateLimiter +{ +private: + static const uint64_t c_periodLength = ULONG_MAX; + static uint64_t s_period; + static uint64_t s_lastNow; + static uint64_t now(); + uint64_t m_delta; + uint64_t m_last; + FramerateLimiter(uint64_t microseconds); +public: + static FramerateLimiter fromFPS(double fps); + static FramerateLimiter fromSeconds(uint64_t seconds); + static FramerateLimiter fromMilliseconds(uint64_t milliseconds); + static FramerateLimiter fromMicroseconds(uint64_t microseconds); + bool step(); + void reset(); +}; diff --git a/firmware/haptics/line wall/firmware/include/utils/performanceMonitor.hpp b/firmware/haptics/line wall/firmware/include/utils/performanceMonitor.hpp new file mode 100644 index 0000000..eff8a4f --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/performanceMonitor.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include + +class PerformanceMonitor +{ +private: + static const uint32_t c_measurementCount = 1000; + struct PerformanceEntry + { + std::vector m_values; + uint32_t m_index = 0; + uint64_t m_sum = 0; + bool m_running = false; + uint64_t m_start = 0; + PerformanceEntry(); + }; + std::map> m_entries; +public: + void start(std::string label); + void stop(std::string label); + std::vector getResults(); +}; + +#ifdef ENABLE_PERFMON +#define PERFMON_START(label) PerfMon.start(label); +#define PERFMON_STOP(label) PerfMon.stop(label); +#else +#define PERFMON_START(label) +#define PERFMON_STOP(label) +#endif + +extern PerformanceMonitor PerfMon; diff --git a/firmware/haptics/line wall/firmware/include/utils/receiveHandler.hpp b/firmware/haptics/line wall/firmware/include/utils/receiveHandler.hpp new file mode 100644 index 0000000..3b33431 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/receiveHandler.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function ReceiveHandler; diff --git a/firmware/haptics/line wall/firmware/include/utils/receiveState.hpp b/firmware/haptics/line wall/firmware/include/utils/receiveState.hpp new file mode 100644 index 0000000..abb521f --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/receiveState.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; diff --git a/firmware/haptics/line wall/firmware/include/utils/serial.hpp b/firmware/haptics/line wall/firmware/include/utils/serial.hpp new file mode 100644 index 0000000..accc846 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/serial.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "utils/receiveHandler.hpp" +#include "utils/receiveState.hpp" + +class DPSerial : DPProtocol +{ +private: + // receive buffer limits + static const uint16_t c_rxBufferSize = 10 * c_maxPacketSize; + static const uint16_t c_rxBufferCriticalThreshold = 5 * c_maxPacketSize; + static const uint16_t c_rxBufferReadyThreshold = 2 * c_maxPacketSize; + static bool s_rxBufferCritical; + + // data storage + static Header s_header; + static const uint16_t c_debugLogBufferSize = 256; + static uint8_t s_debugLogBuffer[c_debugLogBufferSize]; + static std::queue s_debugLogQueue; + static const uint16_t c_processedQueuedMessagesPerFrame = 4; + + // multithreading safety + static portMUX_TYPE s_serialMutex; + + static ReceiveState s_receiveState; + static uint8_t s_expectedPacketId; + + // connection + static bool s_connected; + static const uint16_t c_heartbeatIntervalMs = 1000; + static unsigned long s_lastHeartbeatTime; + static const uint16_t c_maxUnacklowledgedHeartbeats = 5; + static uint16_t s_unacknowledgedHeartbeats; + + // send helper + static void sendUInt8(uint8_t data); + static void sendInt16(int16_t data); + static void sendUInt16(uint16_t data); + static void sendInt32(int32_t data); + static void sendUInt32(uint32_t data); + static void sendFloat(float data); + static void sendMessageType(MessageType data); + static void sendMagicNumber(); + static void sendHeader(MessageType messageType, uint16_t payloadSize); + + // send + static void sendSync(); + static void sendHeartbeat(); + static void sendBufferCritical(); + static void sendBufferReady(); + static void sendPacketAck(uint8_t id); + static void sendInvalidPacketId(uint8_t expected, uint8_t received); + static void sendInvalidData(); + + // receive helper + static uint8_t receiveUInt8(); + static int16_t receiveInt16(); + static uint16_t receiveUInt16(); + static int32_t receiveInt32(); + static uint32_t receiveUInt32(); + static float receiveFloat(); + static MessageType receiveMessageType(); + static void checkBuffer(); + static bool receiveMagicNumber(); + static bool receiveHeader(); + static bool payloadReady(); + + // receive + static void receiveSyncAck(); + static void receiveHearbeatAck(); + static void receiveMotor(); + static void receiveSpeed(); + static void receivePID(); + static void receiveCreateObstacle(); + static void receiveCreatePassableObstacle(); + static void receiveCreateRail(); + static void receiveAddToObstacle(); + static void receiveRemoveObstacle(); + static void receiveEnableObstacle(); + static void receiveDisableObstacle(); + static void receiveCalibrationRequest(); + static void receiveDumpHashtable(); + static void receiveInvalid(); + static void receiveFreeze(); + static void receiveFree(); + static void receiveSpeedControl(); + + // map of receive handlers + static std::map s_receiveHandlers; + +public: + // delete contructor - this class only contains static members + DPSerial() = delete; + + // setup + static void init(); + static bool ensureConnection(); + + // send + static void sendPosition(); + static void sendTransitionEnded(uint8_t panto); + static void sendInstantDebugLog(const char *message, ...); + static void sendQueuedDebugLog(const char *message, ...); + static void processDebugLogQueue(); + static void sendDebugData(); + + // receive + static void receive(); +}; diff --git a/firmware/haptics/line wall/firmware/include/utils/utils.hpp b/firmware/haptics/line wall/firmware/include/utils/utils.hpp new file mode 100644 index 0000000..1eaca9e --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/utils.hpp @@ -0,0 +1,7 @@ +#pragma once + +// template func must be in header to compile all necessary variants +template int sgn(T val) +{ + return (T(0) < val) - (val < T(0)); +}; diff --git a/firmware/haptics/line wall/firmware/include/utils/vector.hpp b/firmware/haptics/line wall/firmware/include/utils/vector.hpp new file mode 100644 index 0000000..02bf456 --- /dev/null +++ b/firmware/haptics/line wall/firmware/include/utils/vector.hpp @@ -0,0 +1,22 @@ +#pragma once + +struct Vector2D +{ + double x, y; + + Vector2D() {} + Vector2D(double _x, double _y) : x(_x), y(_y) {} + + static Vector2D fromPolar(double angle, double length); + static double determinant(const Vector2D& first, const Vector2D& second); + double length() const; + double angle() const; + Vector2D normalize() const; + Vector2D operator+(const Vector2D& other) const; + Vector2D operator-(const Vector2D& other) const; + double operator*(const Vector2D& other) const; + Vector2D operator*(const double scale) const; + bool operator==(const Vector2D& other) const; + bool operator!=(const Vector2D &other) const; + double distancePointToLineSegment(Vector2D a, Vector2D b); +}; diff --git a/firmware/haptics/line wall/firmware/platformio.ini b/firmware/haptics/line wall/firmware/platformio.ini new file mode 100644 index 0000000..3cb404a --- /dev/null +++ b/firmware/haptics/line wall/firmware/platformio.ini @@ -0,0 +1,32 @@ +[platformio] +description = Dualpanto firmware + +[protocol] +base_dir = ../utils/protocol +include_dir = ${protocol.base_dir}/include +src_dir = ${protocol.base_dir}/src +src_filter = +<../${protocol.src_dir}> + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +build_flags = + -I${protocol.include_dir} + -D_GLIBCXX_USE_C99 + -DENABLE_FPS + -g + -frtti + -mtext-section-literals +build_unflags = + # use this while debugging + # -Os +src_filter = + +<*> + ${protocol.src_filter} +monitor_speed = 115200 +lib_deps = https://github.com/PaulStoffregen/Encoder.git + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +upload_port = /dev/cu.SLAB_USBtoUART diff --git a/firmware/haptics/line wall/firmware/src/config/config.cpp b/firmware/haptics/line wall/firmware/src/config/config.cpp new file mode 100644 index 0000000..202b2c5 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/config/config.cpp @@ -0,0 +1,12 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = 0.0085; +float pidFactor[6][3] = { + {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30}, {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30} +}; \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/src/hardware/panto.cpp b/firmware/haptics/line wall/firmware/src/hardware/panto.cpp new file mode 100644 index 0000000..063b680 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/hardware/panto.cpp @@ -0,0 +1,618 @@ +#include "hardware/panto.hpp" + +#include + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantos; + +void Panto::forwardKinematics() +{ + // base angles + // PERFMON_START("[abba] base angles"); + const auto leftBaseAngle = m_actuationAngle[c_localLeftIndex]; + const auto rightBaseAngle = m_actuationAngle[c_localRightIndex]; + const auto handleAngle = m_actuationAngle[c_localHandleIndex]; + // PERFMON_STOP("[abba] base angles"); + + // base angle sin / cos + // PERFMON_START("[abbb] base angle sin / cos"); + const auto leftBaseAngleSin = std::sin(leftBaseAngle); + const auto leftBaseAngleCos = std::cos(leftBaseAngle); + // PERFMON_STOP("[abbb] base angle sin / cos"); + + // calculate inner positions + // PERFMON_START("[abbc] calculate inner positions"); + const auto leftInnerX = + fmaf(leftBaseAngleCos, c_leftInnerLength, c_leftBaseX); + const auto leftInnerY = + fmaf(leftBaseAngleSin, c_leftInnerLength, c_leftBaseY); + const auto rightInnerX = + fmaf(std::cos(rightBaseAngle), c_rightInnerLength, c_rightBaseX); + const auto rightInnerY = + fmaf(std::sin(rightBaseAngle), c_rightInnerLength, c_rightBaseY); + // PERFMON_STOP("[abbc] calculate inner positions"); + + // diagonal between inner positions + // PERFMON_START("[abbd] diagonal between inner positions"); + const auto diagonalX = rightInnerX - leftInnerX; + const auto diagonalY = rightInnerY - leftInnerY; + const auto diagonalSquared = diagonalX * diagonalX + diagonalY * diagonalY; + const auto diagonalLength = std::sqrt(diagonalSquared); + // PERFMON_STOP("[abbd] diagonal between inner positions"); + + // left elbow angles + // - inside is between diagonal and linkage + // - offset is between zero and diagonal + // - total is between zero and linkage + // PERFMON_START("[abbe] left elbow angles"); + const auto leftElbowInsideAngleCos = + (diagonalSquared + + c_leftOuterLengthSquaredMinusRightOuterLengthSquared) / + (2 * diagonalLength * c_leftOuterLength); + const auto leftElbowInsideAngle = -std::acos(leftElbowInsideAngleCos); + const auto leftElbowOffsetAngle = std::atan2(diagonalY, diagonalX); + const auto leftElbowTotalAngle = + leftElbowInsideAngle + leftElbowOffsetAngle; + // PERFMON_STOP("[abbe] left elbow angles"); + + // left elbow angle sin / cos + // PERFMON_START("[abbf] left elbow angle sin / cos"); + const auto leftElbowTotalAngleSin = + std::sin(leftElbowTotalAngle); + const auto leftElbowTotalAngleCos = + std::cos(leftElbowTotalAngle); + // PERFMON_STOP("[abbf] left elbow angle sin / cos"); + + // handle position + // PERFMON_START("[abbg] handle position"); + m_handleX = + fmaf(leftElbowTotalAngleCos, c_leftOuterLength, leftInnerX); + m_handleY = + fmaf(leftElbowTotalAngleSin, c_leftOuterLength, leftInnerY); + // PERFMON_STOP("[abbg] handle position"); + + // right elbow angles + // PERFMON_START("[abbh] right elbow angles"); + const auto rightDiffX = m_handleX - rightInnerX; + const auto rightDiffY = m_handleY - rightInnerY; + const auto rightElbowTotalAngle = std::atan2(rightDiffY, rightDiffX); + // PERFMON_STOP("[abbh] right elbow angles"); + + // store angles + // PERFMON_START("[abbi] store angles"); + m_leftInnerAngle = leftElbowTotalAngle; + m_rightInnerAngle = rightElbowTotalAngle; + m_pointingAngle = + handleAngle + + (encoderFlipped[c_globalHandleIndex]==1? -1 : 1)* //sign changes when encoder is flipped + (c_handleMountedOnRightArm==1 ? + (-rightElbowTotalAngle) : + (leftElbowTotalAngle)); + + // PERFMON_STOP("[abbi] store angles"); + + // some weird diffs and their sinuses + // PERFMON_START("[abbj] some weird diffs and their sinuses"); + const auto rightElbowTotalAngleMinusLeftBaseAngle = + rightElbowTotalAngle - leftBaseAngle; + const auto rightElbowTotalAngleMinusLeftBaseAngleSin = + std::sin(rightElbowTotalAngleMinusLeftBaseAngle); + const auto rightElbowTotalAngleMinusRightBaseAngle = + rightElbowTotalAngle - rightBaseAngle; + const auto rightElbowTotalAngleMinusRightBaseAngleSin = + std::sin(rightElbowTotalAngleMinusRightBaseAngle); + const auto leftElbowTotalAngleMinusRightElbowTotalAngle = + leftElbowTotalAngle - rightElbowTotalAngle; + const auto leftElbowTotalAngleMinusRightElbowTotalAngleSin = + std::sin(leftElbowTotalAngleMinusRightElbowTotalAngle); + // PERFMON_STOP("[abbj] some weird diffs and their sinuses"); + + // shared factors for rows/columns + // PERFMON_START("[abbk] shared factors for rows/columns"); + const auto upperRow = + c_leftInnerLength * rightElbowTotalAngleMinusLeftBaseAngleSin; + const auto lowerRow = + c_rightInnerLength * rightElbowTotalAngleMinusRightBaseAngleSin; + const auto leftColumn = + leftElbowTotalAngleSin / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + const auto rightColumn = + leftElbowTotalAngleCos / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + // PERFMON_STOP("[abbk] shared factors for rows/columns"); + + // set jacobian matrix + // PERFMON_START("[abbl] set jacobian matrix"); + m_jacobian[0][0] = + (-c_leftInnerLength * leftBaseAngleSin) - (upperRow * leftColumn); + m_jacobian[0][1] = + (c_leftInnerLength * leftBaseAngleCos) + (upperRow * rightColumn); + m_jacobian[1][0] = + lowerRow * leftColumn; + m_jacobian[1][1] = + -lowerRow * rightColumn; + // PERFMON_STOP("[abbl] set jacobian matrix"); + inverseKinematics(); +} + +void Panto::inverseKinematics() +{ + + //update tweening delta micro here + unsigned long now = micros(); + float tweening_dt = now - m_tweeningPrevtime; + m_tweeningPrevtime = now; + + if (isnan(m_targetX) || isnan(m_targetY)) + { + m_targetAngle[c_localLeftIndex] = NAN; + m_targetAngle[c_localRightIndex] = NAN; + } + else if (m_isforceRendering) + { + m_targetAngle[c_localLeftIndex] = + m_jacobian[0][0] * m_targetX + + m_jacobian[0][1] * m_targetY; + m_targetAngle[c_localRightIndex] = + m_jacobian[1][0] * m_targetX + + m_jacobian[1][1] * m_targetY; + } + else + { + // tweening + const auto leftBaseToTargetX = m_filteredX - c_leftBaseX; + const auto leftBaseToTargetY = m_filteredY - c_leftBaseY; + const auto rightBaseToTargetX = m_filteredX - c_rightBaseX; + const auto rightBaseToTargetY = m_filteredY - c_rightBaseY; + const auto leftBaseToTargetSquared = + leftBaseToTargetX * leftBaseToTargetX + + leftBaseToTargetY * leftBaseToTargetY; + const auto rightBaseToTargetSquared = + rightBaseToTargetX * rightBaseToTargetX + + rightBaseToTargetY * rightBaseToTargetY; + const auto leftBaseToTargetLength = + std::sqrt(leftBaseToTargetSquared); + const auto rightBaseToTargetLength = + std::sqrt(rightBaseToTargetSquared); + + const auto leftInnerAngleCos = + (leftBaseToTargetSquared + + c_leftInnerLengthSquaredMinusLeftOuterLengthSquared) / + (c_leftInnerLengthDoubled * leftBaseToTargetLength); + const auto rightInnerAngleCos = + (rightBaseToTargetSquared + + c_rightInnerLengthSquaredMinusRightOuterLengthSquared) / + (c_rightInnerLengthDoubled * rightBaseToTargetLength); + const auto leftInnerAngle = std::acos(leftInnerAngleCos); + const auto rightInnerAngle = std::acos(rightInnerAngleCos); + const auto leftOffsetAngle = + std::atan2(leftBaseToTargetY, leftBaseToTargetX); + const auto rightOffsetAngle = + std::atan2(rightBaseToTargetY, rightBaseToTargetX); + + const auto leftAngle = leftOffsetAngle - leftInnerAngle; + const auto rightAngle = rightOffsetAngle + rightInnerAngle; + + m_targetAngle[c_localLeftIndex] = ensureAngleRange(leftAngle); + m_targetAngle[c_localRightIndex] = ensureAngleRange(rightAngle); + + if(abs(m_filteredX - m_targetX) + abs(m_filteredY - m_targetY) < 0.01f && m_inTransition){ + m_inTransition = false; + DPSerial::sendTransitionEnded(getPantoIndex()); + } + + m_filteredX = (m_targetX-m_startX)*m_tweeningValue+m_startX; + m_filteredY = (m_targetY-m_startY)*m_tweeningValue+m_startY; + float stepValue = 0.000001 * tweening_dt * m_tweeningSpeed; + m_tweeningValue=min(m_tweeningValue+stepValue, 1.0f); + + } +}; + +void Panto::setMotor( + const uint8_t& localIndex, const bool& dir, const float& power) +{ + const auto globalIndex = c_globalIndexOffset + localIndex; + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] == dummyPin) + { + return; + } + + const auto flippedDir = dir ^ motorFlipped[globalIndex]; + + if(motorPwmPinForwards[globalIndex] != dummyPin) + { + if(!flippedDir) { + ledcWrite(globalIndex+6, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + else { + ledcWrite(globalIndex, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex+6, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + return; + } + + + digitalWrite(motorDirAPin[globalIndex], flippedDir); + digitalWrite(motorDirBPin[globalIndex], !flippedDir); + ledcWrite(globalIndex, min(power, motorPowerLimit[globalIndex]) * PWM_MAX); +}; + +void Panto::readEncoders() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_previousAngle[localIndex] = + ensureAngleRange( + encoderFlipped[globalIndex] * + TWO_PI * m_angleAccessors[localIndex]() / + encoderSteps[globalIndex]); + m_encoderRequestCount++; + m_encoderRequestCounts[localIndex]++; + } + m_actuationAngle[c_localHandleIndex] = + (m_encoder[c_localHandleIndex]) ? + (encoderFlipped[c_globalHandleIndex] * + TWO_PI * m_encoder[c_localHandleIndex]->read() / + encoderSteps[c_globalHandleIndex]) : + NAN; + #else + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = + ensureAngleRange( + (m_encoder[localIndex]) ? + (encoderFlipped[globalIndex] * + TWO_PI * m_encoder[localIndex]->read() / + encoderSteps[globalIndex]) : + NAN); + } + #endif + + m_previousAngle[c_localHandleIndex] = m_actuationAngle[c_localHandleIndex]; + m_actuationAngle[c_localHandleIndex] = fmod(m_actuationAngle[c_localHandleIndex], TWO_PI); + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + if(m_previousAngle[localIndex]==0)return; + } + if(m_previousAnglesCount>4){ + m_previousAnglesCount=0; + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + float std = 0.0f; + float mean = 0.0f; + for(int i = 0; i < 5; i++){ + mean+=m_previousAngles[localIndex][i]; + }mean/=5.0f; + for(int i = 0; i < 5; i++){ + std+=(m_previousAngles[localIndex][i]-mean)*(m_previousAngles[localIndex][i]-mean); + }std /=5.0f; + if(std < 1.0f){ + m_actuationAngle[localIndex] = m_previousAngles[localIndex][4]; + } + else{ + m_encoderErrorCounts[localIndex]++; + // DPSerial::sendQueuedDebugLog("jumps at [panto %d][motor %d] (std>1.0f) mean = %f",c_pantoIndex, localIndex, mean); + // for(int i = 0; i < 5; i++){ + // DPSerial::sendQueuedDebugLog("previousAngles[%d][%d]=%f",localIndex, i, m_previousAngles[localIndex][i]); + // } + // m_actuationAngle[localIndex] = m_previousAngle[localIndex]; + } + } + } + else{ + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + m_previousAngles[localIndex][m_previousAnglesCount] = m_previousAngle[localIndex]; + } + } + m_previousAnglesCount++; +}; + +void Panto::actuateMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + if (isnan(m_targetAngle[localIndex])) + { + // free motor + setMotor(localIndex, false, 0); + } else if (m_isforceRendering) + { + setMotor( + localIndex, + m_targetAngle[localIndex] < 0, + fabs(m_targetAngle[localIndex]) * forceFactor); + } else + { + auto error = + m_targetAngle[localIndex] - m_actuationAngle[localIndex]; + if (localIndex == c_localHandleIndex) + { + // Linkage offsets handle + error -= + c_handleMountedOnRightArm ? + m_rightInnerAngle : + m_leftInnerAngle; + if (error > PI) + { + error -= TWO_PI; + } + else if (error < -PI) + { + error += TWO_PI; + } + if(encoderFlipped[c_globalHandleIndex]==1) error*=-1; + } + unsigned char dir = error < 0; + unsigned long now = micros(); + float dt = now - m_prevTime; + m_prevTime = now; + error = fabs(error); + // Power: PID + m_integral[localIndex] = min(0.5f, m_integral[localIndex] + error * dt); + float derivative = (error - m_previousDiff[localIndex]) / dt; + m_previousDiff[localIndex] = error; + const auto globalIndex = c_globalIndexOffset + localIndex; + const auto& pid = pidFactor[globalIndex]; + float pVal = pid[0] * error; + float dVal = pid[2] * derivative; + dVal = pVal + dVal > 0 ? dVal : 0; + setMotor( + localIndex, + dir, + pVal + + pid[1] * m_integral[localIndex] + + dVal); + } + } +}; + +void Panto::disengageMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + m_targetAngle[localIndex] = NAN; + setMotor(localIndex, false, 0); + } + m_targetX = NAN; + m_targetY = NAN; +}; + +Panto::Panto(uint8_t pantoIndex) +: c_pantoIndex(pantoIndex) +, c_globalIndexOffset(c_pantoIndex * c_dofCount) +, c_globalLeftIndex(c_globalIndexOffset + c_localLeftIndex) +, c_globalRightIndex(c_globalIndexOffset + c_localRightIndex) +, c_globalHandleIndex(c_globalIndexOffset + c_localHandleIndex) +, c_leftInnerLength(linkageInnerLength[c_globalLeftIndex]) +, c_rightInnerLength(linkageInnerLength[c_globalRightIndex]) +, c_leftOuterLength(linkageOuterLength[c_globalLeftIndex]) +, c_rightOuterLength(linkageOuterLength[c_globalRightIndex]) +, c_leftInnerLengthDoubled(2 * c_leftInnerLength) +, c_rightInnerLengthDoubled(2 * c_rightInnerLength) +, c_leftInnerLengthSquared(c_leftInnerLength * c_leftInnerLength) +, c_rightInnerLengthSquared(c_rightInnerLength * c_rightInnerLength) +, c_leftOuterLengthSquared(c_leftOuterLength * c_leftOuterLength) +, c_rightOuterLengthSquared(c_rightOuterLength * c_rightOuterLength) +, c_leftInnerLengthSquaredMinusLeftOuterLengthSquared( + c_leftInnerLengthSquared - c_leftOuterLengthSquared) +, c_rightInnerLengthSquaredMinusRightOuterLengthSquared( + c_rightInnerLengthSquared - c_rightOuterLengthSquared) +, c_leftOuterLengthSquaredMinusRightOuterLengthSquared( + c_leftOuterLengthSquared - c_rightOuterLengthSquared) +, c_handleMountedOnRightArm(linkageHandleMount[c_globalHandleIndex] == 1) +, c_leftBaseX(linkageBaseX[c_globalLeftIndex]) +, c_leftBaseY(linkageBaseY[c_globalLeftIndex]) +, c_rightBaseX(linkageBaseX[c_globalRightIndex]) +, c_rightBaseY(linkageBaseY[c_globalRightIndex]) +{ + m_targetX = NAN; + m_targetY = NAN; + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + m_targetAngle[localIndex] = NAN; + m_previousDiff[localIndex] = 0.0; + m_integral[localIndex] = 0.0; + if (encoderAPin[globalIndex] != dummyPin && + encoderBPin[globalIndex] != dummyPin) + { + m_encoder[localIndex] = new Encoder( + encoderAPin[globalIndex], encoderBPin[globalIndex]); + } + else + { + m_encoder[localIndex] = NULL; + } + + // we don't need additional checks aroud these - if the dummyPin is set properly, the ESP lib will check this anyway + pinMode(encoderIndexPin[globalIndex], INPUT); + pinMode(motorDirAPin[globalIndex], OUTPUT); + pinMode(motorDirBPin[globalIndex], OUTPUT); + + if(motorPwmPinForwards[globalIndex] == dummyPin) { + pinMode(motorPwmPin[globalIndex], OUTPUT); + + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcAttachPin(motorPwmPin[globalIndex], globalIndex); + } + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] != dummyPin) { + pinMode(motorPwmPinForwards[globalIndex], OUTPUT); + pinMode(motorPwmPinBackwards[globalIndex], OUTPUT); + + // TODO: initiate the PWM channels independent from globalIndex + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcSetup(globalIndex+6, c_ledcFrequency, c_ledcResolution); + + //DPSerial::sendInstantDebugLog("attaching gi %i to pwm %i and pwm %i\n", globalIndex, motorPwmPinForwards[globalIndex], motorPwmPinBackwards[globalIndex]); + + ledcAttachPin(motorPwmPinForwards[globalIndex], globalIndex); + ledcAttachPin(motorPwmPinBackwards[globalIndex], globalIndex+6); + + ledcWrite(globalIndex, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex, 0); + delay(10); + ledcWrite(globalIndex+6, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex+6, 0); + /* + ledcWrite(globalIndex, 0.2*PWM_MAX); + delay(2000); + ledcWrite(globalIndex, 0); + */ + + } + // TODO: Calibration + // Use encoder index pin and actuate the motors to reach it + setMotor(localIndex, false, 0); + } +}; + +void Panto::calibrateEncoders(){ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + //Write encoder values to EEPROM + EEPROM.writeInt((3*c_pantoIndex*sizeof(uint32_t)+localIndex*sizeof(uint32_t)),m_angleAccessors[localIndex]()); + } + #endif +} + +void Panto::resetActuationAngle(){ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex){ + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + } +} + +bool Panto::getCalibrationState(){ + return m_isCalibrating; +} + +void Panto::calibratePanto(){ + m_isCalibrating = true; +} + +void Panto::calibrationEnd() +{ + for (auto localIndex = 0; localIndex < 3; ++localIndex) + { + if (m_encoder[localIndex]) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_encoder[localIndex]->write( + m_actuationAngle[localIndex] / + (TWO_PI) * + encoderSteps[globalIndex]); + } + } + resetActuationAngle(); + m_isCalibrating = false; +}; + +float Panto::getActuationAngle(const uint8_t localIndex) const +{ + return m_actuationAngle[localIndex]; +}; + +Vector2D Panto::getPosition() const +{ + return Vector2D(m_handleX, m_handleY); +}; + +float Panto::getRotation() const +{ + return m_pointingAngle; +}; + +void Panto::setAngleAccessor( + const uint8_t localIndex, + const AngleAccessor accessor) +{ + m_angleAccessors[localIndex] = accessor; +}; + +void Panto::setTarget(const Vector2D target, const bool isForceRendering) +{ + m_isforceRendering = isForceRendering; + m_targetX = target.x; + m_targetY = target.y; + m_startX = m_handleX; + m_startY = m_handleY; + m_filteredX = m_startX; + m_filteredY = m_startY; + m_tweeningValue = 0.0f; + + float dx = (m_targetX - m_startX); + float dy = (m_targetY - m_startY); + float d = max((float)sqrt(dx*dx + dy*dy), 1.0f); //distance to target: avoiding 0 division + + const float velocity = 0.001 * m_tweeningSpeed; //[mm / s] maybe? + + m_tweeningStep = velocity / d; + inverseKinematics(); +}; + +void Panto::setSpeed(const float _speed){ + m_tweeningSpeed = _speed; +} + +void Panto::setRotation(const float rotation) +{ + m_targetAngle[c_localHandleIndex] = rotation; +}; + +int Panto::getEncoderErrorCount(){ + int res= m_encoderErrorCount; + m_encoderErrorCount =0; + return res; +} + +int Panto::getEncoderErrorCounts(int i){ + int res= m_encoderErrorCounts[i]; + m_encoderErrorCounts[i] =0; + return res; +} +int Panto::getEncoderRequests(){ + int res= m_encoderRequestCount; + m_encoderRequestCount =0; + return res; +} + +int Panto::getEncoderRequestsCounts(int i){ + int res= m_encoderRequestCounts[i]; + m_encoderRequestCounts[i] =0; + return res; +} + +uint8_t Panto::getPantoIndex(){ + return c_pantoIndex; +} + +void Panto::setInTransition(bool inTransition){ + m_inTransition = inTransition; +} + +bool Panto::getInTransition(){ + return m_inTransition; +} + +bool Panto::getIsFrozen(){ + return m_isFrozen; +} +void Panto::setIsFrozen(bool isFrozen){ + m_isFrozen = isFrozen; +} diff --git a/firmware/haptics/line wall/firmware/src/hardware/spiCommands.cpp b/firmware/haptics/line wall/firmware/src/hardware/spiCommands.cpp new file mode 100644 index 0000000..b2093cd --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/hardware/spiCommands.cpp @@ -0,0 +1,11 @@ +#include "hardware/spiCommands.hpp" + +#include "hardware/spiPacket.hpp" + +const uint16_t SPICommands::c_readAngle = SPIPacket(1, 0x3FFF).m_transmission; +const uint16_t SPICommands::c_clearError = SPIPacket(1, 0x1).m_transmission; +const uint16_t SPICommands::c_nop = SPIPacket(0, 0x0).m_transmission; +const uint16_t SPICommands::c_highZeroRead = SPIPacket(1, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroRead = SPIPacket(1, 0x17).m_transmission; +const uint16_t SPICommands::c_highZeroWrite = SPIPacket(0, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroWrite = SPIPacket(0, 0x17).m_transmission; diff --git a/firmware/haptics/line wall/firmware/src/hardware/spiEncoder.cpp b/firmware/haptics/line wall/firmware/src/hardware/spiEncoder.cpp new file mode 100644 index 0000000..6a73ea7 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/hardware/spiEncoder.cpp @@ -0,0 +1,18 @@ +#include + +SPIEncoder::SPIEncoder(SPIClass* spi) : m_spi(spi) { }; + +void SPIEncoder::transfer(uint16_t transmission) +{ + m_lastPacket = SPIPacket(m_spi->transfer16(transmission)); +} + +uint32_t SPIEncoder::getAngle() +{ + return m_lastValidAngle; + +} +// uint32_t SPIEncoder::getAngle(uint32_t index) +// { +// return m_values[index];// & c_dataMask; +// } diff --git a/firmware/haptics/line wall/firmware/src/hardware/spiEncoderChain.cpp b/firmware/haptics/line wall/firmware/src/hardware/spiEncoderChain.cpp new file mode 100644 index 0000000..5c53e57 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/hardware/spiEncoderChain.cpp @@ -0,0 +1,329 @@ +#include "hardware/spiEncoderChain.hpp" + +#include "hardware/spiCommands.hpp" +#include "utils/serial.hpp" +#include + +void SPIEncoderChain::begin() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); +} + +void SPIEncoderChain::end() +{ + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + + delay(1); +} + +// void SPIEncoderChain::transfer(uint16_t transmission) +// { +// begin(); +// for(auto i = 0; i < m_numberOfEncoders; ++i) +// { +// m_encoders[i].transfer(transmission); +// } +// end(); +// } + +void SPIEncoderChain::transfer(uint16_t transmission) +{ + + //upperhandle + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); +} + +void SPIEncoderChain::setZero(std::vector newZero) +{ + transfer(SPICommands::c_highZeroWrite); + + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + // } + // end(); + //upperhandle + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + transfer(SPICommands::c_lowZeroWrite); + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + // } + // end(); + + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // EEPROM.writeInt((i*sizeof(int32_t)),newZero[i] & 0x3fff); + // } + // EEPROM.commit(); + transfer(SPICommands::c_readAngle); +} + +SPIEncoderChain::SPIEncoderChain(uint32_t numberOfEncoders) +: m_settings(10000000, SPI_MSBFIRST, SPI_MODE1) //was 100000 +, m_spi(HSPI) +, m_numberOfEncoders(numberOfEncoders) +, m_encoders(numberOfEncoders, &m_spi) +, m_values(2 * 2) +, m_zeros(m_numberOfEncoders) +{ + m_spi.begin(); + pinMode(c_hspiSsPin1, OUTPUT); + pinMode(c_hspiSsPin2, OUTPUT); + pinMode(13, OUTPUT); + clearError(); + __setZeros(); + update(); +} + +void SPIEncoderChain::__setZeros(){ + for(int i=0 ; i < 4; i++)m_zeros[i]=0; + update(); + for(int i=0 ; i < 4; i++){ + m_zeros[i] = m_encoders[i].m_lastValidAngle; + } + // Serial.println("Zeros"); + + DPSerial::sendQueuedDebugLog("Zeros"); + + for(int i=0 ; i < 4; i++){ + // Serial.println(m_zeros[i]); + DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::update() +{ + update(0); + update(1); +} + +void SPIEncoderChain::update(int channel) +{ + uint16_t buf = 0; + m_spi.beginTransaction(m_settings); + //pinMode(13, OUTPUT); + digitalWrite(13, HIGH); + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + m_spi.transfer16(c_readAngle); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + delayMicroseconds(1); + + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + buf = m_spi.transfer16(c_nop); + m_values[i + channel*2] = buf & c_dataMask; + // Serial.printf("m_values%d\t%d\n", i, m_values[i + channel*2]); + // Serial.println(); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + m_spi.endTransaction(); + + for(int i=0; i < 4; i++){ + m_encoders[i].m_lastValidAngle = m_values[i]; + // DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::clearError() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_clearError); + } + digitalWrite(c_hspiSsPin1, HIGH); + + + //Serial.println("Error registers:"); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_nop); + m_spi.transfer16(c_nop); + //Serial.println(m_spi.transfer16(c_nop)); + //Serial.println(m_spi.transfer16(c_nop)); + } + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + +} + +std::vector SPIEncoderChain::getZero() //getZero returns 0 everytime power == off. +{ + // first pass - request high part of zero, don't care about current return value + transfer(SPICommands::c_highZeroRead); + + // second pass - request low part of zero, the return value contains the high part + transfer(SPICommands::c_lowZeroRead); + + std::vector result(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = m_encoders[i].m_lastPacket.m_data << 6; + } + + // third pass - jsut send nop, the return value contains the low part + transfer(SPICommands::c_nop); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] |= m_encoders[i].m_lastPacket.m_data & 0b111111; + } + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + // } + + return result; +} + +void SPIEncoderChain::setZero() +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + } + + setZero(newZero); +} + +bool SPIEncoderChain::needsZero() +{ + auto currentZero = getZero(); + + bool allZero = true; + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + allZero &= currentZero[i] == 0; + } + + transfer(SPICommands::c_readAngle); + + return allZero; +} + +void SPIEncoderChain::setPosition(std::vector positions) +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + newZero[i] -= positions[i]; + } + + setZero(newZero); +} + +void SPIEncoderChain::wakeUp(){ //call setZero(EEPROM_VALUE) + std::vector result(m_numberOfEncoders); + update(); + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + } + setZero(result); +} + +AngleAccessor SPIEncoderChain::getAngleAccessor(uint32_t index) +{ + return std::bind(&SPIEncoder::getAngle, &m_encoders[index]); +} + +uint32_t SPIEncoderChain::getErrors() { + return errors; +} + +uint32_t SPIEncoderChain::getRequests() { + return requests; +} + +void SPIEncoderChain::resetErrors() { + errors = 0; + requests = 0; +} \ No newline at end of file diff --git a/firmware/haptics/line wall/firmware/src/hardware/spiPacket.cpp b/firmware/haptics/line wall/firmware/src/hardware/spiPacket.cpp new file mode 100644 index 0000000..c4a5080 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/hardware/spiPacket.cpp @@ -0,0 +1,44 @@ +#include "hardware/spiPacket.hpp" + +SPIPacket::SPIPacket(bool flag, uint16_t data) +: m_flag(flag) +, m_data(data & c_dataMask) +, m_valid(true) +{ + m_parity = calcParity(); + m_transmission = m_data | (m_flag << 14) | (m_parity << 15); +} + +SPIPacket::SPIPacket(uint16_t transmission) +: m_transmission(transmission) +{ + m_parity = m_transmission & c_parityMask; + m_flag = m_transmission & c_flagMask; + m_data = m_transmission & c_dataMask; + m_valid = m_parity == calcParity(); +} + +bool SPIPacket::calcParity() +{ + uint16_t temp = (m_data & c_dataMask) | (m_flag << 14); + // calculate https://en.wikipedia.org/wiki/Hamming_weight + temp -= (temp >> 1) & 0b0101010101010101; + temp = (temp & 0b0011001100110011) + ((temp >> 2) & 0b0011001100110011); + temp = (temp + (temp >> 4)) & 0b0000111100001111; + return ((temp * 0b0000000100000001) >> 8) % 2; +} + +bool SPIPacket::parityError() +{ + return m_data & 0b100; +} + +bool SPIPacket::commandInvalidError() +{ + return m_data & 0b10; +} + +bool SPIPacket::framingError() +{ + return m_data & 0b1; +} diff --git a/firmware/haptics/line wall/firmware/src/ioMain.cpp b/firmware/haptics/line wall/firmware/src/ioMain.cpp new file mode 100644 index 0000000..14917df --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/ioMain.cpp @@ -0,0 +1,39 @@ +#include "ioMain.hpp" + +#include "hardware/panto.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +FramerateLimiter sendLimiter = FramerateLimiter::fromFPS(60); +//todo copy limiter to physics physicsMain.cpp for SPI error logging + +void ioSetup() +{ + ulTaskNotifyTake(true, portMAX_DELAY); + delay(100); // TODO: patch: wait until pantoPhysics instantiates godobject (physicsMain); +} + +void ioLoop() +{ + PERFMON_START("[a] Receive serial"); +// DPSerial::receive(); + auto connected = DPSerial::ensureConnection(); + PERFMON_STOP("[a] Receive serial"); + + PERFMON_START("[b] Send positions"); + //DPSerial::sendDebugData(); + //DPSerial::sendInstantDebugLog("\n"); + if (connected && sendLimiter.step()) + { + DPSerial::sendPosition(); + } + PERFMON_STOP("[b] Send positions"); + + PERFMON_START("[c] Send debug logs"); + if (connected) + { + DPSerial::processDebugLogQueue(); + } + PERFMON_STOP("[c] Send debug logs"); +} diff --git a/firmware/haptics/line wall/firmware/src/main.cpp b/firmware/haptics/line wall/firmware/src/main.cpp new file mode 100644 index 0000000..3393809 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/main.cpp @@ -0,0 +1,37 @@ +#include + +#include "ioMain.hpp" +#include "physicsMain.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/serial.hpp" + +void setup() +{ + DPSerial::init(); + + DPSerial::sendInstantDebugLog("========== START =========="); + + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("I/O"), + std::forward_as_tuple(&ioSetup, &ioLoop, "I/O", 0)); + Tasks.at("I/O").run(); + Tasks.at("I/O").setLogFps(); + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("Physics"), + std::forward_as_tuple(&physicsSetup, &physicsLoop, "Physics", 1)); + Tasks.at("Physics").run(); + + TaskHandle_t defaultTask = xTaskGetCurrentTaskHandle(); + DPSerial::sendInstantDebugLog("default task handle is %i", defaultTask); + vTaskSuspend(NULL); + taskYIELD(); + DPSerial::sendInstantDebugLog("setup - this should not be printed"); +} + +void loop() +{ + DPSerial::sendInstantDebugLog("loop - this should not be printed"); + delay(1000); +} diff --git a/firmware/haptics/line wall/firmware/src/physics/annotatedEdge.cpp b/firmware/haptics/line wall/firmware/src/physics/annotatedEdge.cpp new file mode 100644 index 0000000..8072350 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/annotatedEdge.cpp @@ -0,0 +1,25 @@ +#include "physics/annotatedEdge.hpp" + +#include "utils/serial.hpp" + +#include "physics/edge.hpp" +#include "physics/indexedEdge.hpp" + +AnnotatedEdge::AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge) +: m_indexedEdge{indexedEdge} +, m_edge{edge} +{ +} + +AnnotatedEdge::AnnotatedEdge(const AnnotatedEdge& other) +: m_indexedEdge{other.m_indexedEdge} +, m_edge{other.m_edge} +{ +} + +void AnnotatedEdge::destroy() +{ + delete m_indexedEdge; + delete m_edge; + delete this; +} diff --git a/firmware/haptics/line wall/firmware/src/physics/collider.cpp b/firmware/haptics/line wall/firmware/src/physics/collider.cpp new file mode 100644 index 0000000..ae51266 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/collider.cpp @@ -0,0 +1,46 @@ +#include "physics/collider.hpp" + +#include "utils/serial.hpp" + +Collider::Collider(std::vector points) : m_points(points) { } + +void Collider::add(std::vector points) +{ + m_points.insert(m_points.end(), points.begin(), points.end()); +} + +bool Collider::contains(Vector2D point) +{ + // will contain result + auto inside = false; + // loop vars + auto edgeCount = m_points.size(); + auto j = edgeCount - 1; + // pre-allocate + Vector2D first, second; + + for(auto i = 0; i < edgeCount; ++i) + { + first = m_points[i]; + second = m_points[j]; + if ((first.y > point.y) != (second.y > point.y) && + (point.x < + first.x + + (second.x - first.x) * + (point.y - first.y) / + (second.y - first.y))) + { + inside = !inside; + } + j = i; + } + + return inside; +} + +Edge Collider::getEdge(uint32_t index) +{ + return Edge( + m_points[index % m_points.size()], + m_points[(index + 1) % m_points.size()]); +} diff --git a/firmware/haptics/line wall/firmware/src/physics/godObject.cpp b/firmware/haptics/line wall/firmware/src/physics/godObject.cpp new file mode 100644 index 0000000..3b2b8c9 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/godObject.cpp @@ -0,0 +1,473 @@ +#include "physics/godObject.hpp" + +#include "config/config.hpp" +#include "utils/serial.hpp" + +GodObject::GodObject(Vector2D position) + : m_position(position), m_tetherPosition(position), m_obstacleMutex(portMUX_INITIALIZER_UNLOCKED), m_possibleCollisions(new std::set()) +{ +} + +GodObject::~GodObject() +{ + delete m_possibleCollisions; +} + +void GodObject::setMovementDirection(Vector2D movementDirection) +{ + m_movementDirection = movementDirection; +} + +Hashtable& GodObject::hashtable() +{ + if (!m_hashtable) + { + m_hashtable = new Hashtable(); + } + return *m_hashtable; +} + +void GodObject::update() +{ + if (m_actionQueue.empty()) + { + return; + } + + portENTER_CRITICAL(&m_obstacleMutex); + for (auto i = 0; i < obstacleChangesPerFrame && !m_actionQueue.empty(); ++i) + { + auto action = m_actionQueue.front(); + m_actionQueue.pop_front(); + switch (action->m_type) + { + case HT_ENABLE_EDGE: + { + hashtable().add(action->m_data.m_annotatedEdge); + break; + } + case HT_DISABLE_EDGE: + { + hashtable().remove(action->m_data.m_annotatedEdge); + break; + } + case GO_REMOVE_OBSTACLE: + { + try{ + + delete m_obstacles.at(action->m_data.m_obstacleId); + m_obstacles.erase(action->m_data.m_obstacleId); + } catch (const std::out_of_range &oor){ + DPSerial::sendInstantDebugLog("Could not remove obstacle"); + } + break; + } + default: + { + break; + } + } + delete action; + } + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::dumpHashtable() +{ + portENTER_CRITICAL(&m_obstacleMutex); + hashtable().print(); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +// returns if force is applied or not +bool GodObject::move(bool isTweening, bool isFrozen) +{ + auto lastState = m_processingObstacleCollision; + // if the number of collisions increased since the last frame then we ran into a corner + auto lastNumCollisions = m_numCollisions; + + m_processingObstacleCollision = false; + + + Vector2D nextGoPosition; + Vector2D handlePosition = m_position + m_movementDirection; + if (isFrozen){ + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + return true; + } + if (isTweening) { + m_position = handlePosition; + if (m_tethered) { + m_tetherPosition = handlePosition; + } + return false; + } + float movementStepLength = m_movementDirection.length(); // only used for tethering + if (m_tethered && !isTweening) { + double distHandleToGo = m_movementDirection.length(); + if ((distHandleToGo < m_tetherInnerRadius) || (m_tetherState!=Outer && (distHandleToGo > 10))) { + // the latter condition can happen at startup of the device. + // in this case we don't want to apply forces. + m_tetherState = Inner; + return false; + } + // find out the current tether state + if (distHandleToGo > m_tetherInnerRadius && distHandleToGo < m_tetherOuterRadius){ + m_tetherState = Active; + } else { + m_tetherState = Outer; + } + // the speed of the god object increases proportionally with the distance bw handle and go. The max speed of the go is dependent on the outer tether radius. + movementStepLength = min(m_tetherOuterRadius, distHandleToGo); + + // this is the movement of the god object that follows the tether + if (distHandleToGo != 0) + { + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * m_tetherFactor); + } else { + nextGoPosition = handlePosition; + } + } else { + nextGoPosition = handlePosition; + } + + // no matter what the tether state is we need to check if the god object is colliding with an obstacle + Vector2D godObjectPos; + portENTER_CRITICAL(&m_obstacleMutex); + if (m_tethered && m_tetherState == Outer && m_tetherStrategy == Exploration){ + // pausing the game means that the god object position doesn't update. + godObjectPos = m_position; + } else { + // this is the default case + godObjectPos = checkCollisions(nextGoPosition, m_position); + } + if (m_tethered && m_processingObstacleCollision && m_tetherState == Outer && m_tetherStrategy != MaxSpeed) { + // for the Exploration and Leash mode the tether state can't be outer once the god object collides + // (otherwise the wall force will be weaker when the handle moves further into the wall) + m_tetherState = Active; + } + + // check if collision with a passable obstacle or a haptic guide is present + if (m_tethered && m_processingObstacleCollision && m_tetherState == Active) { + m_processingObstacleCollision = false; + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * c_railsTetherFactor); + auto pos = checkCollisions(nextGoPosition, m_position); + // if the handle is already past the obstacle (no more collision present) then the god object jumps to its position + if (!m_processingObstacleCollision) { + m_position = pos; + } else { + m_position = godObjectPos; + } + } else { + m_position = godObjectPos; + } + + if (m_tethered && m_tetherState == Outer && !isTweening && m_tetherStrategy != MaxSpeed) + { + m_tetherPosition = checkCollisions(handlePosition, m_tetherPosition); + } else { + m_tetherPosition = handlePosition; + } + portEXIT_CRITICAL(&m_obstacleMutex); + + m_doneColliding = lastState && !m_processingObstacleCollision; + + if (!m_tethered) { + if (m_processingObstacleCollision) + { + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + } + return m_processingObstacleCollision; + } else { + // newCollision is only important for the pock + bool newCollision = lastNumCollisions < m_numCollisions; + return processTetheringForce(handlePosition, newCollision); + } +} + +Vector2D GodObject::getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition){ + // the PID error is the difference between the virtual object and the handle position + // the virtual object can either be a) the godobject or b) the tether + auto error = godObjectPosition - handlePosition; + auto force = error * forcePidFactor[0][0] + (error - m_lastError) * forcePidFactor[0][2]; + m_lastError = error; + return force; +} + +Vector2D GodObject::getTetherForce(Vector2D error){ + auto force = error * forcePidFactor[0][0] + (error - m_lastErrorTether) * forcePidFactor[0][2]; + m_lastErrorTether = error; + return force; +} + +void GodObject::renderForce(Vector2D collisionForce, Vector2D tetherForce) { + m_activeForce = collisionForce + tetherForce; +} + +bool GodObject::processTetheringForce(Vector2D handlePosition, bool newCollision) { + // returns if force is active or handle is freely moving + if (m_tetherState==Outer && m_tetherStrategy!=MaxSpeed) { + // for the 2 tether strategies where the god object is pulled on a virtual leash towards the tether position + auto error = m_movementDirection.normalize() * c_tetherForcePullingBack; + auto tetherForce = getTetherForce(error); //TODO: if we had a collision previously in Active state and moved from there into Outer state then the rendered force should be the sum of the previous force and the collision force + if (m_processingObstacleCollision) { + // think about adding a second pock here as well + renderForce(getCollisionForce(m_tetherPosition, handlePosition), tetherForce); + } else { + // weak constant force pulling the tether back to the god object + renderForce(Vector2D(0,0), tetherForce); + } + return true; + } else { + auto movementLength = (m_tetherPosition - m_position).length(); + auto error = m_movementDirection.normalize() * (m_tetherInnerRadius - movementLength); + if (newCollision && m_tetherPockEnabled) { + // weak constant force pushing the handle into the the wall so that the user gets force feedback at their fingertip + error = m_movementDirection.normalize() * c_tetherPockDistance; // this can't work because we include the last tether error + } + auto tetherForce = getTetherForce(error); + + if (m_processingObstacleCollision) { + // god object collision + renderForce(getCollisionForce(m_position, handlePosition), tetherForce); + return true; + } else { + if (!m_doneColliding) { + // regular tether force active that pushes the handle back to the inner tether radius + + renderForce(Vector2D(0,0), tetherForce); + } + return !m_doneColliding; + } + } + return false; +} + +Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPosition) +{ + /* + Collision detection works in 3 stages: + 1. Select collision candidates using a 2D lookup table. Every cell in that table contains the edges that go through it. That's why only particular edges have to be checked for collisions. + 2. The actual collision detection. + 3. If a collision is detected calculate the new god object position. + + If a collision is detected the collision detection is repeated with the new target position. This way we can check if the new position is accessible at all or not. + + Added by Julius on 30.09.20 + For more information check Lukas Wagners MT (section 4.3.1): https://www.dropbox.com/home/2018%20CHI%20Dueling%20Pantographs/Layer%202%20Firmware%20(Lukas%20Wagner)?preview=2019_09_07+ESP+Firmware+for+God+Haptic+Objects+%3D+Masterarbeit+(Lukas+Wagner).pdf + */ + + if (currentPosition == targetPoint || !m_hashtable) + { + return targetPoint; + } + // 1. select collision candidates + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + if (m_possibleCollisions->empty()) + { + return targetPoint; + } + + bool foundCollision; + u_short numCollisions = 0; + // 2. check if collisions are present between 2 vectors + // first vector goes from god object to handle and the + // second vector is the potential collision edge + do + { + // result vars + foundCollision = false; + double shortestMovementRatio = 0; + Vector2D closestEdgeFirst; + Vector2D closestEdgeFirstMinusSecond; + + // direction of movement: value is constant for loop + const auto posMinusTarget = currentPosition - targetPoint; + + if (posMinusTarget == Vector2D(0, 0)) + { + return targetPoint; + } + + for (auto&& indexedEdge : *m_possibleCollisions) + { + auto edge = indexedEdge.m_obstacle->getEdge(indexedEdge.m_index); + auto edgeFirst = edge.m_first; + auto firstMinusPos = edgeFirst - currentPosition; + auto firstMinusSecond = edgeFirst - edge.m_second; + auto divisor = + Vector2D::determinant(firstMinusSecond, posMinusTarget); + + auto movementRatio = + -Vector2D::determinant(firstMinusSecond, firstMinusPos) / + divisor; + if (movementRatio < 0 || movementRatio > 1) + { + continue; + } + + auto edgeRatio = + Vector2D::determinant(firstMinusPos, posMinusTarget) / divisor; + if (edgeRatio < 0 || edgeRatio > 1) + { + continue; + } + + // we have a collision! + if (!foundCollision || movementRatio < shortestMovementRatio) // I think the second condition never gets called because the movementRatio loop + // would already continue if the movementRatio was below 0 (which is the shortestMovementRatio) + { + // if a collision with a passable object is detected (e.g. a haptic rail) and the handle is not within the bounds of the colliding object, + // discard the collision and continue + auto ob = indexedEdge.m_obstacle; + if (ob->passable && !ob->contains(targetPoint)) + { + continue; + } + foundCollision = true; + if (!ob->passable){ + numCollisions++; + } + shortestMovementRatio = movementRatio; + closestEdgeFirst = edgeFirst; + closestEdgeFirstMinusSecond = firstMinusSecond; + } + } + + // calculate new god object position + if (foundCollision) + { + m_processingObstacleCollision = true; + + // god object slides along the colliding edge according to the handle movement but with tethered speed + + if (m_tethered) { + // if the movement is tethered the targetPoint can not be further away from the current position than the outer tether radius + const Vector2D movementVector = targetPoint - currentPosition; + double movementLength = min(m_tetherOuterRadius, movementVector.length()); + targetPoint = currentPosition + (movementVector.normalize() * movementLength); + } + + auto perpendicular = Vector2D( + -closestEdgeFirstMinusSecond.y, + closestEdgeFirstMinusSecond.x); + auto resolveRatio = + -Vector2D::determinant( + closestEdgeFirstMinusSecond, + closestEdgeFirst - targetPoint) / + Vector2D::determinant( + closestEdgeFirstMinusSecond, + perpendicular); + auto resolveVec = perpendicular * resolveRatio; + auto resolveLength = resolveVec.length(); + // c_resolveDistance is super small --> we need to add a tiny padding between the godobject and the edge so that it's not getting stuck in the edge + targetPoint = targetPoint - (resolveVec * ((resolveLength + c_resolveDistance) / resolveLength)); + + + // check for the new point if there is another collision with any other edge + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + } + // there can be multiple collisions, that's why we have to loop as well over the other possible collisions + } while (foundCollision); + m_numCollisions = numCollisions; + return targetPoint; +} + +void GodObject::createObstacle(uint16_t id, std::vector points, bool passable) +{ + // create obstacle or passable obstacle + auto ob = new Obstacle(points, passable); + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.emplace(id, ob); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::createRail(uint16_t id, std::vector points, double displacement) +{ + portENTER_CRITICAL(&m_obstacleMutex); + Rail* rail = new Rail(points, displacement); + m_obstacles.emplace(id, rail); + portEXIT_CRITICAL(&m_obstacleMutex); + return; + +} + +void GodObject::addToObstacle(uint16_t id, std::vector points) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.at(id)->add(points); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +void GodObject::removeObstacle(uint16_t id) +{ + enableObstacle(id, false); + m_actionQueue.push_back(new GodObjectAction(GO_REMOVE_OBSTACLE, id)); +} + +void GodObject::enableObstacle(uint16_t id, bool enable) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + if (enable != it->second->enabled()) + { + const auto edges = it->second->getIndexedEdges(); + const auto action = enable ? HT_ENABLE_EDGE : HT_DISABLE_EDGE; + for (const auto& edge : edges) + { + m_actionQueue.push_back(new GodObjectAction( + action, + new AnnotatedEdge( + new IndexedEdge(edge.m_obstacle, edge.m_index), + new Edge(edge.getEdge())))); + } + } + it->second->enable(enable); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +Vector2D GodObject::getPosition() +{ + return m_position; +} + +Vector2D GodObject::getActiveForce() +{ + return m_activeForce; +} + +bool GodObject::getProcessingObstacleCollision() +{ + return m_processingObstacleCollision; +} + +bool GodObject::getDoneColliding() +{ + return m_doneColliding; +} + +bool GodObject::tethered() +{ + return m_tethered; +} + +void GodObject::setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled) +{ + m_tethered = active; + m_tetherFactor = tetherFactor; + m_tetherInnerRadius = innerTetherRadius; + m_tetherOuterRadius = outerTetherRadius; + m_tetherStrategy = strategy; + m_tetherPockEnabled = pockEnabled; +} diff --git a/firmware/haptics/line wall/firmware/src/physics/hashtable.cpp b/firmware/haptics/line wall/firmware/src/physics/hashtable.cpp new file mode 100644 index 0000000..0a2e4bc --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/hashtable.cpp @@ -0,0 +1,224 @@ +#include "physics/hashtable.hpp" + +#include +#include + +#include "physics/edge.hpp" +#include "utils/assert.hpp" +#include "utils/serial.hpp" +#include "utils/utils.hpp" + + +int32_t Hashtable::get1dIndex(double value, double min, double step) +{ + return (int32_t)std::floor((value - min) / step); +} + +std::vector Hashtable::getCellIndices(Edge edge) +{ + std::vector result; + + // http://www.cse.yorku.ca/~amana/research/grid.pdf + const auto startX = edge.m_first.x; + const auto startY = edge.m_first.y; + const auto endX = edge.m_second.x; + const auto endY = edge.m_second.y; + auto x = get1dIndex(startX, rangeMinX, hashtableStepSizeX); + auto y = get1dIndex(startY, rangeMinY, hashtableStepSizeY); + const auto lastX = get1dIndex(endX, rangeMinX, hashtableStepSizeX); + const auto lastY = get1dIndex(endY, rangeMinY, hashtableStepSizeY); + const auto diffX = endX - startX; + const auto diffY = endY - startY; + const auto stepX = sgn(diffX); + const auto stepY = sgn(diffY); + const auto slopeX = diffY / diffX; + const auto slopeY = diffX / diffY; + auto nextX = stepX < 0 + ? (edge.m_first.x - (rangeMinX + x * hashtableStepSizeX)) + : ((rangeMinX + (x + 1) * hashtableStepSizeX) - edge.m_first.x); + nextX = hypot(nextX, nextX * slopeX); + auto nextY = stepY < 0 + ? (edge.m_first.y - (rangeMinY + y * hashtableStepSizeY)) + : ((rangeMinY + (y + 1) * hashtableStepSizeY) - edge.m_first.y); + nextY = hypot(nextY, nextY * slopeY); + const auto deltaX = + hypot(hashtableStepSizeX, hashtableStepSizeX * slopeX); + const auto deltaY = + hypot(hashtableStepSizeY * slopeY, hashtableStepSizeY); + + while(x != lastX || y != lastY) + { + // ignore cells outside the physical range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + if(nextX < nextY) + { + nextX += deltaX; + x += stepX; + } + else + { + nextY += deltaY; + y += stepY; + } + } + + // add end cell if inside range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + return result; +} + +std::set Hashtable::expand(const std::vector& edges) +{ + std::set result; + uint32_t x, y; + for (const auto& edge : edges) + { + x = edge % hashtableStepsY; + y = edge / hashtableStepsY; + + for(int32_t i = -1; i < 2; ++i) + { + if((x == 0 && i == -1) || (x == hashtableStepsX - 1 && i == 1)) + { + continue; + } + + for(int32_t j = -1; j < 2; ++j) + { + if((y == 0 && j == -1) || (y == hashtableStepsY - 1 && j == 1)) + { + continue; + } + + result.insert((y + j) * hashtableStepsY + (x + i)); + } + } + } + return result; +} + +Hashtable::Hashtable() +{ + DPSerial::sendQueuedDebugLog( + "Hashtable settings:"); + DPSerial::sendQueuedDebugLog( + "Available memory of %i bytes can hold %i cells", + hashtableMaxMemory, + hashtableMaxCells); + DPSerial::sendQueuedDebugLog( + "Possible range of values is %3f by %3f mm", + rangeMaxX - rangeMinX, + rangeMaxY - rangeMinY); + DPSerial::sendQueuedDebugLog( + "Horizontal step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeX, + hashtableStepsX); + DPSerial::sendQueuedDebugLog( + "Vertical step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeY, + hashtableStepsY); + DPSerial::sendQueuedDebugLog( + "Resulting step count is %i, using %i bytes", + hashtableNumCells, + hashtableUsedMemory); +} + +void Hashtable::add(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + m_cells[cellIndex].emplace_back( + edge->m_indexedEdge->m_obstacle, edge->m_indexedEdge->m_index); + } + edge->destroy(); +} + +void Hashtable::remove(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + auto& cell = m_cells[cellIndex]; + auto it = std::find( + cell.begin(), + cell.end(), + *(edge->m_indexedEdge)); + if(it != cell.end()) + { + cell.erase(it); + cell.shrink_to_fit(); + } + } + edge->destroy(); +} + +void Hashtable::getPossibleCollisions( + Edge movement, std::set* result) +{ + if(movement.m_first.x == 0 && movement.m_first.y == 0) + { + DPSerial::sendInstantDebugLog( + "Skipping god object movement from zero position."); + return; + } + const auto startX = + get1dIndex(movement.m_first.x, rangeMinX, hashtableStepSizeX); + const auto startY = + get1dIndex(movement.m_first.y, rangeMinY, hashtableStepSizeY); + const auto startIndex = startX * hashtableStepsY + startY; + ASSERT_GE(startIndex, 0); + + ASSERT_LT(startIndex, hashtableNumCells); + const auto endX = + get1dIndex(movement.m_second.x, rangeMinX, hashtableStepSizeX); + const auto endY = + get1dIndex(movement.m_second.y, rangeMinY, hashtableStepSizeY); + const auto endIndex = endX * hashtableStepsY + endY; + ASSERT_GE(endIndex, 0); + ASSERT_LT(endIndex, hashtableNumCells); + auto dist = (uint8_t)(startX != endX) + (uint8_t)(startY != endY); + const auto* begin = &m_cells[0]; + if(dist == 0) + { + const auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + } + else if(dist == 1) + { + auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + cell = begin + endIndex; + result->insert(cell->begin(), cell->end()); + } + else + { + for(auto&& cellIndex : getCellIndices(movement)) + { + const auto* cell = begin + cellIndex; + result->insert(cell->begin(), cell->end()); + } + } +} + +void Hashtable::print() +{ + DPSerial::sendQueuedDebugLog("Printing hashtable..."); + std::ostringstream str; + for(auto y = 0; y < hashtableStepsY; ++y) + { + for(auto x = 0; x < hashtableStepsX; x++) + { + str << (m_cells[x * hashtableStepsY + y].empty() ? '-' : '#'); + } + DPSerial::sendQueuedDebugLog(str.str().c_str()); + str.str(""); + } + DPSerial::sendQueuedDebugLog("Printing complete."); +} diff --git a/firmware/haptics/line wall/firmware/src/physics/indexedEdge.cpp b/firmware/haptics/line wall/firmware/src/physics/indexedEdge.cpp new file mode 100644 index 0000000..c1f5f09 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/indexedEdge.cpp @@ -0,0 +1,26 @@ +#include "physics/indexedEdge.hpp" + +#include "physics/obstacle.hpp" + +IndexedEdge::IndexedEdge(Obstacle* obstacle, uint32_t index) +: m_obstacle(obstacle) +, m_index(index) +{ +} + +bool IndexedEdge::operator==(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle && m_index == other.m_index; +} + +bool IndexedEdge::operator<(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle ? + m_index < other.m_index : + m_obstacle < other.m_obstacle; +} + +Edge IndexedEdge::getEdge() const +{ + return m_obstacle->getEdge(m_index); +} diff --git a/firmware/haptics/line wall/firmware/src/physics/obstacle.cpp b/firmware/haptics/line wall/firmware/src/physics/obstacle.cpp new file mode 100644 index 0000000..9ef21a4 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/obstacle.cpp @@ -0,0 +1,40 @@ +#include "physics/obstacle.hpp" + +#include "physics/indexedEdge.hpp" + +Obstacle::Obstacle(std::vector points) : Collider(points) { } + +Obstacle::Obstacle(std::vector points, bool passable) : Collider(points) +{ + this->passable = passable; +} + +bool Obstacle::enabled() +{ + return m_enabled; +} + +void Obstacle::enable(bool enable) +{ + m_enabled = enable; +} + +std::vector Obstacle::getIndexedEdges( + uint32_t first, uint32_t last +) +{ + if(first == -1) + { + first = m_points.size() - 1; + } + if(last == -1) + { + last = m_points.size() - 1; + } + std::vector result; + for(auto i = first; i <= last; ++i) + { + result.emplace_back(this, i); + } + return result; +} diff --git a/firmware/haptics/line wall/firmware/src/physics/pantoPhysics.cpp b/firmware/haptics/line wall/firmware/src/physics/pantoPhysics.cpp new file mode 100644 index 0000000..2f27f4d --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/pantoPhysics.cpp @@ -0,0 +1,61 @@ +#include "physics/pantoPhysics.hpp" + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantoPhysics; + +PantoPhysics::PantoPhysics(Panto* panto) : m_panto(panto) +{ + m_currentPosition = m_panto->getPosition(); + try + { + m_godObject = new GodObject(m_currentPosition); + } + catch(const std::bad_alloc& e) + { + DPSerial::sendInstantDebugLog("Error while creating god object - the hash table may be too big."); + DPSerial::sendInstantDebugLog("Error: %s", e.what()); + DPSerial::sendInstantDebugLog("Rebooting..."); + ESP.restart(); + } +} + +GodObject* PantoPhysics::godObject() +{ + return m_godObject; +} + +void PantoPhysics::step() +{ + // PERFMON_START("[ba] Physics::step"); + // PERFMON_START("[baa] Physics::step::prep"); + m_godObject->update(); + + m_currentPosition = m_panto->getPosition(); + + bool isTweening = m_panto->getInTransition(); + + auto difference = m_currentPosition - m_godObject->getPosition(); + m_godObject->setMovementDirection(difference); + // PERFMON_STOP("[baa] Physics::step::prep"); + // PERFMON_START("[bab] Physics::step::move"); + bool isForceActive = m_godObject->move(isTweening, m_panto->getIsFrozen()); + // PERFMON_STOP("[bab] Physics::step::move"); + // PERFMON_START("[bac] Physics::step::motor"); + + // force is active when frozen, tethering is enabled or collision is detected + if(isForceActive ) + { + m_panto->setTarget(m_godObject->getActiveForce(), true); + } + else + { + if (!isTweening){ + // TODO: is there actually another case? + m_panto->setTarget(Vector2D(NAN, NAN), false); + } + } + // PERFMON_STOP("[bac] Physics::step::motor"); + // PERFMON_STOP("[ba] Physics::step"); +} diff --git a/firmware/haptics/line wall/firmware/src/physics/rail.cpp b/firmware/haptics/line wall/firmware/src/physics/rail.cpp new file mode 100644 index 0000000..09613d3 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/physics/rail.cpp @@ -0,0 +1,14 @@ +#include "physics/rail.hpp" + +Rail::Rail(std::vector points, double displacement) : Obstacle(points, true) +{ + this->displacement = displacement; +} + +bool Rail::contains(Vector2D point){ + //return true; + // if the point is within the displacement around the rail then it is contained and the godobject collides + double distLineToPoint = point.distancePointToLineSegment(m_points[0], m_points[1]); + bool inside = (distLineToPoint < this->displacement); + return inside; +} \ No newline at end of file diff --git a/firmware/09 god object/firmware/src/physicsMain.cpp b/firmware/haptics/line wall/firmware/src/physicsMain.cpp similarity index 97% rename from firmware/09 god object/firmware/src/physicsMain.cpp rename to firmware/haptics/line wall/firmware/src/physicsMain.cpp index a1ac1bf..1dad6d4 100644 --- a/firmware/09 god object/firmware/src/physicsMain.cpp +++ b/firmware/haptics/line wall/firmware/src/physicsMain.cpp @@ -70,11 +70,11 @@ void physicsSetup() { std::vector path; - path.emplace_back(-70.15f, -81.37f); - path.emplace_back(38.73f, -88.67f); - - auto id = 20; + // render line wall + path.emplace_back(-40.0f, -70.0f); + path.emplace_back(40.0f, -70.0f); + auto id = 1; pantoPhysics[i].godObject()->createObstacle(id, path, false); pantoPhysics[i].godObject()->enableObstacle(id, true); } diff --git a/firmware/haptics/line wall/firmware/src/tasks/task.cpp b/firmware/haptics/line wall/firmware/src/tasks/task.cpp new file mode 100644 index 0000000..36e28ad --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/tasks/task.cpp @@ -0,0 +1,109 @@ +#include "tasks/task.hpp" + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::map Task::s_fpsMap; +FramerateLimiter loggingLimiter = FramerateLimiter::fromSeconds(1); + +void Task::taskLoop(void* parameters) +{ + Task* task = reinterpret_cast(parameters); + +#ifdef ENABLE_FPS + task->initFps(); +#endif + + task->m_setupFunc(); + +loopLabel: + task->m_loopFunc(); + +#ifdef ENABLE_FPS + task->checkFps(); +#endif + + TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; + TIMERG0.wdt_feed=1; + TIMERG0.wdt_wprotect=0; + goto loopLabel; +}; + +inline void Task::initFps() +{ + m_fpsCalcLimiter.reset(); + m_loopCount = 0; +}; + +inline void Task::checkFps() +{ + ++m_loopCount; + if (m_fpsCalcLimiter.step()) + { + s_fpsMap[m_handle] = m_loopCount * 1000 / m_fpsInterval; + m_loopCount = 0; + + if(m_logFps && loggingLimiter.step()) + { + for(const auto& entry : s_fpsMap) + { + DPSerial::sendQueuedDebugLog( + "Task \"%s\" fps: %i", + pcTaskGetTaskName(entry.first), + entry.second); + } + + if(ESP.getHeapSize() != 0){ + DPSerial::sendQueuedDebugLog("Free heap: %i of %i (%.3f %%). Largest block: %i", ESP.getFreeHeap(), ESP.getHeapSize(), 100*ESP.getFreeHeap()/(float)ESP.getHeapSize(), ESP.getMaxAllocHeap()); + } + + #ifdef ENABLE_PERFMON + for(const auto& entry : PerfMon.getResults()) + { + DPSerial::sendQueuedDebugLog(entry.c_str()); + } + #endif + } + } +}; + +Task::Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core) + : m_setupFunc(setupFunc), + m_loopFunc(loopFunc), + m_name(name), + m_stackDepth(c_defaultStackDepth), + m_priority(c_defaultPriority), + m_core(core), + m_fpsInterval(c_defaultFpsInterval), + m_fpsCalcLimiter(FramerateLimiter::fromMilliseconds(m_fpsInterval)){}; + +void Task::run() +{ + xTaskCreatePinnedToCore( + taskLoop, + m_name, + m_stackDepth, + this, + m_priority, + &m_handle, + m_core); + DPSerial::sendInstantDebugLog("Started task \"%s\" on core %i.", m_name, m_core); +}; + +void Task::setLogFps(bool logFps) +{ + m_logFps = logFps; +} + +TaskHandle_t Task::getHandle() +{ + return m_handle; +} diff --git a/firmware/haptics/line wall/firmware/src/tasks/taskRegistry.cpp b/firmware/haptics/line wall/firmware/src/tasks/taskRegistry.cpp new file mode 100644 index 0000000..4414b0a --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/tasks/taskRegistry.cpp @@ -0,0 +1,3 @@ +#include "tasks/taskRegistry.hpp" + +TaskRegistry Tasks; diff --git a/firmware/haptics/line wall/firmware/src/utils/crashDump.cpp b/firmware/haptics/line wall/firmware/src/utils/crashDump.cpp new file mode 100644 index 0000000..26ab153 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/utils/crashDump.cpp @@ -0,0 +1,19 @@ +#include "utils/crashDump.hpp" + +#include "utils/serial.hpp" + +CrashDump physicsDump; + +CrashDump::CrashDump() : m_stream() {} + +void CrashDump::clear() +{ + m_stream.clear(); + m_stream.str(""); +} + +void CrashDump::dump() +{ + DPSerial::sendQueuedDebugLog(m_stream.str().c_str()); + clear(); +} diff --git a/firmware/haptics/line wall/firmware/src/utils/framerateLimiter.cpp b/firmware/haptics/line wall/firmware/src/utils/framerateLimiter.cpp new file mode 100644 index 0000000..050c2ee --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/utils/framerateLimiter.cpp @@ -0,0 +1,57 @@ +#include "utils/framerateLimiter.hpp" + +#include "utils/serial.hpp" + +uint64_t FramerateLimiter::s_period = 0; +uint64_t FramerateLimiter::s_lastNow = 0; + +uint64_t FramerateLimiter::now() +{ + uint64_t now = micros(); + if(now < s_lastNow) + { + ++s_period; + DPSerial::sendQueuedDebugLog("FramerateLimiter: micros() rolled over."); + } + return s_period * c_periodLength + now; +} + +FramerateLimiter::FramerateLimiter(uint64_t microseconds) +: m_delta(microseconds) +, m_last(0) { } + +FramerateLimiter FramerateLimiter::fromFPS(double fps) +{ + return FramerateLimiter(1e6 / fps); +} + +FramerateLimiter FramerateLimiter::fromSeconds(uint64_t seconds) +{ + return FramerateLimiter(1000000 * seconds); +} + +FramerateLimiter FramerateLimiter::fromMilliseconds(uint64_t milliseconds) +{ + return FramerateLimiter(1000 * milliseconds); +} + +FramerateLimiter FramerateLimiter::fromMicroseconds(uint64_t microseconds) +{ + return FramerateLimiter(microseconds); +} + +bool FramerateLimiter::step() +{ + uint64_t now = FramerateLimiter::now(); + if(m_last + m_delta > now) + { + return false; + } + m_last = now; + return true; +} + +void FramerateLimiter::reset() +{ + m_last = FramerateLimiter::now(); +} diff --git a/firmware/haptics/line wall/firmware/src/utils/performanceMonitor.cpp b/firmware/haptics/line wall/firmware/src/utils/performanceMonitor.cpp new file mode 100644 index 0000000..050e401 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/utils/performanceMonitor.cpp @@ -0,0 +1,158 @@ +#include "utils/performanceMonitor.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PerformanceMonitor::PerformanceEntry::PerformanceEntry() +{ + m_values.reserve(c_measurementCount); +} + +void PerformanceMonitor::start(std::string label) +{ + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(entry.m_running) + { + return; + } + + entry.m_running = true; + entry.m_start = ESP.getCycleCount(); +} + +void PerformanceMonitor::stop(std::string label) +{ + uint64_t end = ESP.getCycleCount(); + + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(!entry.m_running) + { + return; + } + + uint64_t duration = + entry.m_start < end ? + (end - entry.m_start) : + (end + UINT32_MAX - entry.m_start); + + if(entry.m_values.size() == c_measurementCount) + { + auto old = entry.m_values[entry.m_index]; + entry.m_sum -= old; + + entry.m_values[entry.m_index] = duration; + entry.m_sum += duration; + entry.m_index = (entry.m_index + 1) % c_measurementCount; + } + else + { + entry.m_values.push_back(duration); + entry.m_sum += duration; + } + + entry.m_running = false; +} + +std::vector PerformanceMonitor::getResults() +{ + std::vector> temp; + + const std::string labelHeader("label"); + const std::string cycleHeader("avg cycles"); + const std::string timeHeader("avg ns"); + uint32_t maxLabelSize = labelHeader.size(); + uint32_t maxCycleSize = cycleHeader.size(); + uint32_t maxTimeSize = timeHeader.size(); + + for(const auto& core : m_entries) + { + auto coreId = core.first; + + for(const auto& entry : core.second) + { + const auto& label = entry.first; + const auto& data = entry.second; + + const auto avgCycles = (double)data.m_sum / data.m_values.size(); + const auto avgTime = avgCycles / ESP.getCpuFreqMHz(); + + maxLabelSize = std::max( + maxLabelSize, + label.size() + 4); + maxCycleSize = std::max( + maxCycleSize, + (uint32_t)std::log10(avgCycles) + 1 + 4); + maxTimeSize = std::max( + maxTimeSize, + (uint32_t)std::log10(avgTime) + 1 + 4); + + if(!data.m_values.empty()) + { + temp.emplace_back(coreId, label, avgCycles, avgTime); + } + } + } + + maxCycleSize = constrain(maxCycleSize, cycleHeader.size(), 30); + maxTimeSize = constrain(maxTimeSize, timeHeader.size(), 30); + + std::vector results; + std::ostringstream stream; + stream << std::fixed; + + stream + << "| " + << std::setw(maxLabelSize) << std::left + << labelHeader + << " | " + << std::setw(maxCycleSize) << std::left + << cycleHeader + << " | " + << std::setw(maxTimeSize) << std::left + << timeHeader + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + + stream + << "|-" + << std::string(maxLabelSize, '-') + << "-|-" + << std::string(maxCycleSize, '-') + << "-|-" + << std::string(maxTimeSize, '-') + << "-|"; + results.emplace_back(stream.str()); + stream.seekp(0); + + for(const auto& tuple : temp) + { + stream + << "| " + << "["<< std::get<0>(tuple) << "] " + << std::setw(maxLabelSize - 4) << std::left + << std::get<1>(tuple) + << " | " + << std::setw(maxCycleSize) << std::right << std::setprecision(3) + << std::get<2>(tuple) + << " | " + << std::setw(maxTimeSize) << std::right << std::setprecision(3) + << std::get<3>(tuple) + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + } + + return results; +} + +PerformanceMonitor PerfMon; diff --git a/firmware/haptics/line wall/firmware/src/utils/serial.cpp b/firmware/haptics/line wall/firmware/src/utils/serial.cpp new file mode 100644 index 0000000..9d8623e --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/utils/serial.cpp @@ -0,0 +1,759 @@ +#include "utils/serial.hpp" + +#include + +#include "hardware/panto.hpp" +#include "physics/pantoPhysics.hpp" +#include "utils/vector.hpp" + +bool DPSerial::s_rxBufferCritical = false; +Header DPSerial::s_header = Header(); +uint8_t DPSerial::s_debugLogBuffer[c_debugLogBufferSize]; +std::queue DPSerial::s_debugLogQueue; +portMUX_TYPE DPSerial::s_serialMutex = portMUX_INITIALIZER_UNLOCKED; +ReceiveState DPSerial::s_receiveState = NONE; +uint8_t DPSerial::s_expectedPacketId = 1; +bool DPSerial::s_connected = false; +unsigned long DPSerial::s_lastHeartbeatTime = 0; +uint16_t DPSerial::s_unacknowledgedHeartbeats = 0; +std::map + DPSerial::s_receiveHandlers = { + {SYNC_ACK, DPSerial::receiveSyncAck}, + {HEARTBEAT_ACK, DPSerial::receiveHearbeatAck}, + {MOTOR, DPSerial::receiveMotor}, + {PID, DPSerial::receivePID}, + {SPEED, DPSerial::receiveSpeed}, + {CREATE_OBSTACLE, DPSerial::receiveCreateObstacle}, + {ADD_TO_OBSTACLE, DPSerial::receiveAddToObstacle}, + {REMOVE_OBSTACLE, DPSerial::receiveRemoveObstacle}, + {ENABLE_OBSTACLE, DPSerial::receiveEnableObstacle}, + {DISABLE_OBSTACLE, DPSerial::receiveDisableObstacle}, + {CALIBRATE_PANTO, DPSerial::receiveCalibrationRequest}, + {DUMP_HASHTABLE, DPSerial::receiveDumpHashtable}, + {CREATE_PASSABLE_OBSTACLE, DPSerial::receiveCreatePassableObstacle}, + {CREATE_RAIL, DPSerial::receiveCreateRail}, + {FREEZE, DPSerial::receiveFreeze}, + {FREE, DPSerial::receiveFree}, + {SPEED_CONTROL, DPSerial::receiveSpeedControl}}; + +// === private === + +// send helper + +void DPSerial::sendUInt8(uint8_t data) +{ + Serial.write(data); +} + +void DPSerial::sendInt16(int16_t data) +{ + Serial.write(static_cast(data >> 8)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt16(uint16_t data) +{ + sendInt16(reinterpret_cast(data)); +} + +void DPSerial::sendInt32(int32_t data) +{ + Serial.write(static_cast(data >> 24)); + Serial.write(static_cast((data >> 16) & 255)); + Serial.write(static_cast((data >> 8) & 255)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt32(uint32_t data) +{ + sendInt32(reinterpret_cast(data)); +} + +void DPSerial::sendFloat(float data) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + sendInt32(reinterpret_cast(data)); +#pragma GCC diagnostic pop +} + +void DPSerial::sendMessageType(MessageType data) +{ + Serial.write(data); +} + +void DPSerial::sendMagicNumber() +{ + for (auto i = 0; i < c_magicNumberSize; ++i) + { + sendUInt8(c_magicNumber[i]); + } +}; + +void DPSerial::sendHeader(MessageType messageType, uint16_t payloadSize) +{ + sendMessageType(messageType); + sendUInt8(0); // no panto -> fw tracked packages + sendUInt16(payloadSize); +}; + +// send + +void DPSerial::sendSync() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(SYNC, 4); + sendUInt32(c_revision); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendHeartbeat() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(HEARTBEAT, 0); + s_unacknowledgedHeartbeats++; + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferCritical() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_CRITICAL, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferReady() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_READY, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendPacketAck(uint8_t id) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(PACKET_ACK, 1); + sendUInt8(id); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidPacketId(uint8_t expected, uint8_t received) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_PACKET_ID, 2); + sendUInt8(expected); + sendUInt8(received); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidData() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_DATA, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive helper + +uint8_t DPSerial::receiveUInt8() +{ + return static_cast(Serial.read()); +} + +int16_t DPSerial::receiveInt16() +{ + return Serial.read() << 8 | Serial.read(); +} + +uint16_t DPSerial::receiveUInt16() +{ + auto temp = receiveInt16(); + return reinterpret_cast(temp); +} + +int32_t DPSerial::receiveInt32() +{ + return Serial.read() << 24 | Serial.read() << 16 | Serial.read() << 8 | Serial.read(); +} + +uint32_t DPSerial::receiveUInt32() +{ + auto temp = receiveInt32(); + return reinterpret_cast(temp); +} + +float DPSerial::receiveFloat() +{ + auto temp = receiveInt32(); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + return reinterpret_cast(temp); +#pragma GCC diagnostic pop +} + +MessageType DPSerial::receiveMessageType() +{ + return static_cast(Serial.read()); +} + +void DPSerial::checkBuffer() +{ + const auto available = Serial.available(); + if (available > c_rxBufferCriticalThreshold && !s_rxBufferCritical) + { + s_rxBufferCritical = true; + sendBufferCritical(); + } + if (available < c_rxBufferReadyThreshold && s_rxBufferCritical) + { + s_rxBufferCritical = false; + sendBufferReady(); + } +} + +bool DPSerial::receiveMagicNumber() +{ + int magicNumberProgress = 0; + + bool invalidData = false; + + // as long as enough data is available to find the magic number + while (Serial.available() >= c_magicNumberSize) + { + const uint8_t read = Serial.read(); + const uint8_t expected = c_magicNumber[magicNumberProgress]; + // does next byte fit expected by of magic number? + if (read == expected) + { + // yes - increase index. If check complete, return true. + if (++magicNumberProgress == c_magicNumberSize) + { + s_receiveState = FOUND_MAGIC; + return true; + } + } + else + { + // no - reset search progress + // sendInstantDebugLog( + // "Error: expected %x, read %x. State might by faulty!", + // expected, + // read); + invalidData = true; + magicNumberProgress = 0; + } + } + + if (invalidData) + { + sendInvalidData(); + } + + // ran out of available data before finding complete number - return false + return false; +}; + +bool DPSerial::receiveHeader() +{ + // make sure enough data is available + if (Serial.available() < c_headerSize) + { + return false; + } + + s_header.MessageType = receiveUInt8(); + s_header.PacketId = receiveUInt8(); + s_header.PayloadSize = receiveUInt16(); + s_receiveState = FOUND_HEADER; + return true; +}; + +bool DPSerial::payloadReady() +{ + return Serial.available() >= s_header.PayloadSize; +}; + +// receive + +void DPSerial::receiveSyncAck() +{ + s_connected = true; +}; + +void DPSerial::receiveHearbeatAck() +{ + s_unacknowledgedHeartbeats = 0; +}; + +void DPSerial::receiveMotor() +{ + const auto controlMethod = receiveUInt8(); + const auto pantoIndex = receiveUInt8(); + + const auto target = Vector2D(receiveFloat(), receiveFloat()); + if (!isnan(target.x) && !isnan(target.y)) + { + pantos[pantoIndex].setInTransition(true); + DPSerial::sendInstantDebugLog("In Transition"); + } + pantos[pantoIndex].setRotation(receiveFloat()); + pantos[pantoIndex].setTarget(target, controlMethod == 1); +}; + +void DPSerial::receivePID() +{ + auto motorIndex = receiveUInt8(); + + for (auto i = 0; i < 3; ++i) + { + pidFactor[motorIndex][i] = receiveFloat(); + } +}; + +void DPSerial::receiveSpeed() +{ + auto pantoIndex = receiveUInt8(); //0 or 1 + auto speed = receiveFloat(); + pantos[pantoIndex].setSpeed(speed); +}; + +void DPSerial::receiveCreateObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, false); + } + } +} + +void DPSerial::receiveCreatePassableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, true); + } + } +} + +void DPSerial::receiveCreateRail() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + auto displacement = (double)receiveFloat(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createRail(id, path, displacement); + } + } +} + +void DPSerial::receiveAddToObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->addToObstacle(id, path); + } + } +} + +void DPSerial::receiveRemoveObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->removeObstacle(id); + } + } +} + +void DPSerial::receiveEnableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id); + } + } +} + +void DPSerial::receiveDisableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id, false); + } + } +} + +void DPSerial::receiveFreeze() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + const auto target = pantos[i].getPosition(); + pantos[i].setTarget(target, 0); + pantos[i].setRotation(pantos[i].getRotation()); + pantos[i].setIsFrozen(true); + } + } +} + +void DPSerial::receiveFree() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantos[i].setTarget(Vector2D(NAN, NAN), 0); + pantos[i].setRotation(NAN); + pantos[i].setInTransition(false); + pantos[i].setIsFrozen(false); + } + } +} + +void DPSerial::receiveSpeedControl() +{ + auto tethered = receiveUInt8(); //0 or 1 + auto tetherFactor = receiveFloat(); + auto tetherInnerRadius = receiveFloat(); + auto tetherOuterRadius = receiveFloat(); + auto tetherStrategy = receiveUInt8(); // 0 for MaxSpeed, 1 for Exploration, 2 for Leash + OutOfTetherStrategy strategy; + switch (tetherStrategy) + { + case 0: + strategy = MaxSpeed; + break; + case 1: + strategy = Exploration; + break; + case 2: + strategy = Leash; + break; + default: + break; + } + auto pockEnabled = receiveUInt8(); //0 or 1 + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + pantoPhysics[i].godObject()->setSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); + } +} + +void DPSerial::receiveCalibrationRequest() +{ + DPSerial::sendInstantDebugLog("=== Calibration Request received ==="); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibratePanto(); + } +} + +void DPSerial::receiveDumpHashtable() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->dumpHashtable(); + } + } +} + +void DPSerial::receiveInvalid() +{ + sendQueuedDebugLog( + "Received invalid message: [%02X, %02X, %04X] %", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); +}; + +// === public === + +// setup + +void DPSerial::init() +{ + Serial.flush(); + Serial.begin(c_baudRate); + Serial.setRxBufferSize(c_rxBufferSize); +} + +bool DPSerial::ensureConnection() +{ + if (!s_connected) + { + sendSync(); + // delay to avoid spamming sync messages + delay(10); + return false; + } + + if (s_unacknowledgedHeartbeats > c_maxUnacklowledgedHeartbeats) + { + sendQueuedDebugLog("Disconnected due to too many unacklowledged heartbeats."); + s_unacknowledgedHeartbeats = 0; + s_connected = false; + return false; + } + + if (millis() > s_lastHeartbeatTime + c_heartbeatIntervalMs || s_lastHeartbeatTime == 0) + { + sendHeartbeat(); + s_lastHeartbeatTime = millis(); + } + + return true; +} + +// send + +void DPSerial::sendPosition() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(POSITION, pantoCount * 5 * 4); // five values per panto, 4 bytes each + + for (auto i = 0; i < pantoCount; ++i) + { + const auto panto = pantos[i]; + const auto pos = panto.getPosition(); + sendFloat(pos.x); + sendFloat(pos.y); + sendFloat(panto.getRotation()); + auto goPos = pantoPhysics[i].godObject()->getPosition(); + sendFloat(goPos.x); + sendFloat(goPos.y); + } + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInstantDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length + 1, 0, c_debugLogBufferSize); + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write(s_debugLogBuffer, length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendQueuedDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length, 0, c_debugLogBufferSize); + s_debugLogQueue.emplace(reinterpret_cast(s_debugLogBuffer), length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendTransitionEnded(uint8_t panto) +{ + // signal when tweening is over + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(TRANSITION_ENDED, 1); // 1 byte for the panto index is enough + sendUInt8(panto); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::processDebugLogQueue() +{ + portENTER_CRITICAL(&s_serialMutex); + // quick check to avoid loop if not necessary + if (!s_debugLogQueue.empty()) + { + for (auto i = 0; i < c_processedQueuedMessagesPerFrame; ++i) + { + if (!s_debugLogQueue.empty()) + { + auto &msg = s_debugLogQueue.front(); + auto length = msg.length() + 1; + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write( + reinterpret_cast(msg.c_str()), + length); + s_debugLogQueue.pop(); + } + } + } + portEXIT_CRITICAL(&s_serialMutex); +} + +void DPSerial::sendDebugData() +{ + const auto pos0 = pantos[0].getPosition(); + const auto pos1 = pantos[1].getPosition(); + portENTER_CRITICAL(&s_serialMutex); + sendInstantDebugLog( + "[ang/0] %+08.3f | %+08.3f | %+08.3f [ang/1] %+08.3f | %+08.3f | %+08.3f [pos/0] %+08.3f | %+08.3f | %+08.3f [pos/1] %+08.3f | %+08.3f | %+08.3f", + degrees(pantos[0].getActuationAngle(0)), + degrees(pantos[0].getActuationAngle(1)), + degrees(pantos[0].getActuationAngle(2)), + degrees(pantos[1].getActuationAngle(0)), + degrees(pantos[1].getActuationAngle(1)), + degrees(pantos[1].getActuationAngle(2)), + pos0.x, + pos0.y, + degrees(pantos[0].getRotation()), + pos1.x, + pos1.y, + degrees(pantos[1].getRotation())); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive + +void DPSerial::receive() +{ + checkBuffer(); + + if (s_receiveState == NONE && !receiveMagicNumber()) + { + return; + } + + if (s_receiveState == FOUND_MAGIC && !receiveHeader()) + { + return; + } + + if (s_receiveState == FOUND_HEADER && !payloadReady()) + { + return; + } + + if (!s_connected && s_header.MessageType != SYNC_ACK) + { + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + Serial.read(); + } + sendInstantDebugLog( + "Not connected, ignoring [%X %i %i]", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); + return; + } + + s_receiveState = NONE; + + if (s_header.PacketId > 0) + { + if (s_header.PacketId != s_expectedPacketId) + { + sendInvalidPacketId(s_expectedPacketId, s_header.PacketId); + return; + } + + sendPacketAck(s_header.PacketId); + s_expectedPacketId++; + if (s_expectedPacketId == 0) + { + s_expectedPacketId++; + } + } + + auto handler = s_receiveHandlers.find((MessageType)(s_header.MessageType)); + + if (handler == s_receiveHandlers.end()) + { + receiveInvalid(); + } + else + { + handler->second(); + } +}; diff --git a/firmware/haptics/line wall/firmware/src/utils/vector.cpp b/firmware/haptics/line wall/firmware/src/utils/vector.cpp new file mode 100644 index 0000000..34826d6 --- /dev/null +++ b/firmware/haptics/line wall/firmware/src/utils/vector.cpp @@ -0,0 +1,85 @@ +#include "utils/vector.hpp" + +#include + +Vector2D Vector2D::fromPolar(double angle, double length) +{ + return Vector2D(cos(angle) * length, sin(angle) * length); +}; + +double Vector2D::determinant(const Vector2D& first, const Vector2D& second) +{ + return (first.x * second.y) - (first.y * second.x); +}; + +double Vector2D::length() const +{ + return sqrt(x * x + y * y); +}; + +double Vector2D::angle() const +{ + return atan2(y, x); +}; + +Vector2D Vector2D::normalize() const +{ + return Vector2D(x / length(), y / length()); +} + +Vector2D Vector2D::operator+(const Vector2D& other) const +{ + return Vector2D(x + other.x, y + other.y); +}; + +Vector2D Vector2D::operator-(const Vector2D& other) const +{ + return Vector2D(x - other.x, y - other.y); +}; + +double Vector2D::operator*(const Vector2D& other) const +{ + return x * other.x + y * other.y; +}; + +Vector2D Vector2D::operator*(const double scale) const +{ + return Vector2D(x * scale, y * scale); +}; + +bool Vector2D::operator==(const Vector2D &other) const +{ + return x == other.x && y == other.y; +}; + +bool Vector2D::operator!=(const Vector2D &other) const +{ + return x != other.x || y != other.y; +}; + +double Vector2D::distancePointToLineSegment(Vector2D a, Vector2D b) { + // a is the start of the line segment, b is the end + // distance is measured from the point p + Vector2D ab = b - a; + Vector2D ae = *this - a; + Vector2D be = *this - b; + + double ab_ae = ab * ae; + double ab_be = ab * be; + + if (ab_be > 0){ + // Point is above line segment + return be.length(); + } else if (ab_ae < 0) { + // Point is below line segment + return ae.length(); + } else { + // Finding the perpendicular distance + double x1 = ab.x; + double y1 = ab.y; + double x2 = ae.x; + double y2 = ae.y; + double mod = sqrt(x1 * x1 + y1 * y1); + return abs(x1 * y2 - y1 * x2) / mod; + } +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/backtrace/backtrace.sh b/firmware/haptics/line wall/utils/backtrace/backtrace.sh new file mode 100644 index 0000000..4b75654 --- /dev/null +++ b/firmware/haptics/line wall/utils/backtrace/backtrace.sh @@ -0,0 +1,26 @@ +#!/bin/bash +# Use this to analyze the ESP32's backtrace in case of a crash. +# Requires gdb to be accessable from bash. +# In order to find line numbers, add -g3 to the PlatformIO build flags. +# The default target is set for running this script from the base framework dir. +# Alternatively, you may pass an path to the elf as the first argument. +# Default usage: ./utils/backtrace/backtrace.sh "0x40085698:0x3ffb5e80 [...]" +# Alternative usage example for running from this dir: ./backtrace.sh ./../../firmware/.pioenvs/esp32dev/firmware.elf "0x40085698:0x3ffb5e80 [...]" + +if [ "$#" -eq 1 ]; then + target=./firmware/.pioenvs/esp32dev/firmware.elf + backtrace=$1 + echo "Using default target $target" +else + target=$1 + backtrace=$2 + echo "Using custom target $1" +fi + +file=$(mktemp) +echo $backtrace \ +| sed -r 's/:0x[[:xdigit:]]{8}\s?/\n/g' \ +| sed '/^[[:space:]]*$/d' \ +| sed 's/^.*$/echo === \0 ===\\n\ninfo symbol \0\ninfo line *\0/g' > $file +gdb -batch -x $file $target +rm $file diff --git a/firmware/haptics/line wall/utils/protocol/include/protocol/header.hpp b/firmware/haptics/line wall/utils/protocol/include/protocol/header.hpp new file mode 100644 index 0000000..f6b1d51 --- /dev/null +++ b/firmware/haptics/line wall/utils/protocol/include/protocol/header.hpp @@ -0,0 +1,14 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +struct Header +{ + uint8_t MessageType = 0; + uint8_t PacketId = 0; + uint16_t PayloadSize = 0; +}; diff --git a/firmware/haptics/line wall/utils/protocol/include/protocol/messageType.hpp b/firmware/haptics/line wall/utils/protocol/include/protocol/messageType.hpp new file mode 100644 index 0000000..0e9566e --- /dev/null +++ b/firmware/haptics/line wall/utils/protocol/include/protocol/messageType.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +enum MessageType +{ + SYNC = 0x00, + HEARTBEAT = 0x01, + BUFFER_CRITICAL = 0x02, + BUFFER_READY = 0x03, + PACKET_ACK = 0x04, + INVALID_PACKET_ID = 0x05, + INVALID_DATA = 0x06, + POSITION = 0x10, + DEBUG_LOG = 0x20, + SYNC_ACK = 0x80, + HEARTBEAT_ACK = 0x81, + MOTOR = 0x90, + PID = 0x91, + SPEED = 0x92, + TRANSITION_ENDED = 0x93, + CREATE_OBSTACLE = 0xA0, + ADD_TO_OBSTACLE = 0xA1, + REMOVE_OBSTACLE = 0xA2, + ENABLE_OBSTACLE = 0xA3, + DISABLE_OBSTACLE = 0xA4, + CALIBRATE_PANTO = 0xA5, + CREATE_PASSABLE_OBSTACLE = 0xA6, + CREATE_RAIL = 0xA7, + FREEZE = 0xA8, + FREE = 0xA9, + SPEED_CONTROL = 0xAA, + DUMP_HASHTABLE = 0xC0, +}; + +extern std::set TrackedMessageTypes; diff --git a/firmware/haptics/line wall/utils/protocol/include/protocol/protocol.hpp b/firmware/haptics/line wall/utils/protocol/include/protocol/protocol.hpp new file mode 100644 index 0000000..a2e41c3 --- /dev/null +++ b/firmware/haptics/line wall/utils/protocol/include/protocol/protocol.hpp @@ -0,0 +1,29 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +class DPProtocol +{ +protected: + // revision + static const uint32_t c_revision = 6; + + // connection info + static const uint32_t c_baudRate = 115200; + + // magic number + static const uint8_t c_magicNumber[]; + static const uint8_t c_magicNumberSize = 2; + + // data size + static const uint8_t c_headerSize = 4; + static const uint16_t c_maxPayloadSize = 256; + + // combined maximal size of packet + static const uint16_t c_maxPacketSize = + c_maxPayloadSize + c_magicNumberSize + c_headerSize; +}; diff --git a/firmware/haptics/line wall/utils/protocol/src/protocol/messageType.cpp b/firmware/haptics/line wall/utils/protocol/src/protocol/messageType.cpp new file mode 100644 index 0000000..4e57c18 --- /dev/null +++ b/firmware/haptics/line wall/utils/protocol/src/protocol/messageType.cpp @@ -0,0 +1,10 @@ +#include "protocol/messageType.hpp" + +std::set TrackedMessageTypes = { + CREATE_OBSTACLE, + ADD_TO_OBSTACLE, + REMOVE_OBSTACLE, + ENABLE_OBSTACLE, + DISABLE_OBSTACLE, + CREATE_PASSABLE_OBSTACLE, + CREATE_RAIL}; diff --git a/firmware/haptics/line wall/utils/protocol/src/protocol/protocol.cpp b/firmware/haptics/line wall/utils/protocol/src/protocol/protocol.cpp new file mode 100644 index 0000000..6871f88 --- /dev/null +++ b/firmware/haptics/line wall/utils/protocol/src/protocol/protocol.cpp @@ -0,0 +1,3 @@ +#include "protocol/protocol.hpp" + +const uint8_t DPProtocol::c_magicNumber[] = {0x44, 0x50}; \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/scripts/generateHardwareConfig.js b/firmware/haptics/line wall/utils/scripts/generateHardwareConfig.js new file mode 100644 index 0000000..563e783 --- /dev/null +++ b/firmware/haptics/line wall/utils/scripts/generateHardwareConfig.js @@ -0,0 +1,269 @@ +// just assume the input config file is correct and we don't need this check +/* eslint-disable guard-for-in */ +// also, this is just a helper script and does not need documentation +/* eslint-disable require-jsdoc */ +// the template strings can't ne broken into multiple lines +/* eslint-disable max-len */ +const input = require('../../hardware/'+process.argv[2]+'.json'); +const fs = require('fs'); +const crypto = require('crypto'); +const hash = crypto.createHash('md5').update(JSON.stringify(input)).digest(); +const aggregates = {}; + +function insert(index, categoryName, valueName, value) { + const aggregate = categoryName+'_'+valueName; + if (!aggregates[aggregate]) { + aggregates[aggregate] = []; + } + aggregates[aggregate][index] = value; +} + +let index = 0; let pantoCount = 0; +for (const pantoName in input.pantos) { + ++pantoCount; + const panto = input.pantos[pantoName]; + for (const dofName in panto) { + const dof = panto[dofName]; + for (const categoryName in dof) { + const category = dof[categoryName]; + for (const valueName in category) { + const value = category[valueName]; + insert(index, categoryName, valueName, value); + } + } + ++index; + } +} + +function aggregate(name, valueIfUndefined = 0, map = ((x) => x)) { + let array = aggregates[name]; + if (!array) { + array = []; + } + for (let i = 0; i < index; ++i) { + if (array[i] == undefined) { + array[i] = valueIfUndefined; + } + if (array[i] instanceof Array) { + array[i] = `{${array[i].join(', ')}}`; + } + } + return array.map(map).join(', '); +} + +function count(name) { + const array = aggregates[name]; + if (!array) { + return 0; + } + let count = 0; + for (let i = 0; i < index; ++i) { + if (array[i] != undefined) { + count++; + } + } + return count; +} + +function getRangeForMotor(motor) { + const centerX = motor.linkage.baseX; + const centerY = motor.linkage.baseY; + const range = motor.linkage.innerLength + motor.linkage.outerLength; + return { + minX: centerX - range, + minY: centerY - range, + maxX: centerX + range, + maxY: centerY + range + }; +} + +function getRangeForPanto(panto) { + const left = getRangeForMotor(panto.left); + const right = getRangeForMotor(panto.right); + return { + minX: Math.min(left.minX, right.minX), + minY: Math.min(left.minY, right.minY), + maxX: Math.max(left.maxX, right.maxX), + maxY: Math.max(left.maxY, right.maxY) + }; +} + +function getRange() { + const upper = getRangeForPanto(input.pantos.upper); + const lower = getRangeForPanto(input.pantos.lower); + return { + minX: Math.min(upper.minX, lower.minX), + minY: Math.min(upper.minY, lower.minY), + maxX: Math.max(upper.maxX, lower.maxX), + maxY: Math.max(upper.maxY, lower.maxY) + }; +} + +let range; +if (input.range) { + range = input.range; +} else { + range = getRange(); +} + +// calculate these now because sqrt isn't a constexpr +function getHashtableSettings() { + const rangeX = range.maxX - range.minX; + const rangeY = range.maxY - range.minY; + const maxMemory = input.hashtableMemory / pantoCount; + const sizeofVector = 12; // = sizeof(std::vector) + const maxCells = Math.floor(maxMemory / sizeofVector); + const targetStepSize = Math.sqrt(rangeX * rangeY / maxCells); + const stepsX = Math.floor(rangeX / targetStepSize); + const stepsY = Math.floor(rangeY / targetStepSize); + const stepSizeX = rangeX / stepsX; + const stepSizeY = rangeY / stepsY; + const numCells = stepsX * stepsY; + const usedMemory = numCells * sizeofVector; + + return { + maxMemory, + usedMemory, + maxCells, + numCells, + stepsX, + stepsY, + stepSizeX, + stepSizeY + }; +} + +const hashtable = getHashtableSettings(); + +const headerOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is ${input.dummyPin}. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {${Array.from(hash).map((x) => '0x'+('0'+(Number(x).toString(16))).slice(-2).toUpperCase()).join(', ')}}; +const float opMinDist = ${input.opMinDist}, + opMaxDist = ${input.opMaxDist}, + opAngle = ${input.opAngle}; +extern float forceFactor; +const uint8_t pantoCount = ${pantoCount}; +const uint8_t dummyPin = ${input.dummyPin}; +${input.usesSpi ? '#define LINKAGE_ENCODER_USE_SPI' : ''} +const uint32_t numberOfSpiEncoders = ${count('encoder_spiIndex')}; +const float linkageBaseX[] = { + ${aggregate('linkage_baseX')} +}; +const float linkageBaseY[] = { + ${aggregate('linkage_baseY')} +}; +const float linkageInnerLength[] = { + ${aggregate('linkage_innerLength')} +}; +const float linkageOuterLength[] = { + ${aggregate('linkage_outerLength')} +}; +const uint8_t linkageHandleMount[] = { + ${aggregate('linkage_handleMount')} +}; +const float motorPowerLimit[] = { + ${aggregate('motor_powerLimit')} +}; +const float motor_powerLimitForce[] = { + ${aggregate('motor_powerLimitForce')} +}; +extern float pidFactor[${pantoCount*3}][3]; +const float forceP = ${input.forcePidFactor[0]}; +const float forceI = ${input.forcePidFactor[1]}; +const float forceD = ${input.forcePidFactor[2]}; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + ${aggregate('motor_pwmPin', input.dummyPin)} +}; +const uint8_t motorPwmPinForwards[] = { + ${aggregate('motor_pwmPinForwards', input.dummyPin)} +}; +const uint8_t motorPwmPinBackwards[] = { + ${aggregate('motor_pwmPinBackwards', input.dummyPin)} +}; +const uint8_t motorDirAPin[] = { + ${aggregate('motor_dirAPin', input.dummyPin)} +}; +const uint8_t motorDirBPin[] = { + ${aggregate('motor_dirBPin', input.dummyPin)} +}; +const bool motorFlipped[] = { + ${aggregate('motor_flipped')} +}; +const uint8_t encoderAPin[] = { + ${aggregate('encoder_aPin', input.dummyPin)} +}; +const uint8_t encoderBPin[] = { + ${aggregate('encoder_bPin', input.dummyPin)} +}; +const uint8_t encoderIndexPin[] = { + ${aggregate('encoder_indexPin', input.dummyPin)} +}; +const uint32_t encoderSteps[] = { + ${aggregate('encoder_steps')} +}; +const uint32_t encoderSpiIndex[] = { + ${aggregate('encoder_spiIndex', 0xffffffff)} +}; +const float encoderFlipped[] = { + ${aggregate('encoder_flipped', false, (x) => x ? -1 : 1)} +}; +const float setupAngle[] = { + ${aggregate('encoder_setup')} +}; +constexpr float rangeMinX = ${range.minX}; +constexpr float rangeMinY = ${range.minY}; +constexpr float rangeMaxX = ${range.maxX}; +constexpr float rangeMaxY = ${range.maxY}; +constexpr uint32_t hashtableMaxMemory = ${hashtable.maxMemory}; +constexpr uint32_t hashtableUsedMemory = ${hashtable.usedMemory}; +constexpr uint32_t hashtableMaxCells = ${hashtable.maxCells}; +constexpr uint32_t hashtableNumCells = ${hashtable.numCells}; +constexpr uint32_t hashtableStepsX = ${hashtable.stepsX}; +constexpr uint32_t hashtableStepsY = ${hashtable.stepsY}; +constexpr double hashtableStepSizeX = ${hashtable.stepSizeX}; +constexpr double hashtableStepSizeY = ${hashtable.stepSizeY}; +const uint32_t obstacleChangesPerFrame = ${input.obstacleChangesPerFrame};`; + +console.log(headerOutput); +const headerDir = 'firmware/include/config/'; +if (!fs.existsSync(headerDir)) { + fs.mkdirSync(headerDir, {recursive: true}); +} +fs.writeFileSync(headerDir + 'config.hpp', headerOutput); + +const sourceOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = ${input.forceFactor}; +float pidFactor[${pantoCount*3}][3] = { + ${aggregate('motor_pidFactor')} +};`; + +console.log(sourceOutput); +const sourceDir = 'firmware/src/config/'; +if (!fs.existsSync(sourceDir)) { + fs.mkdirSync(sourceDir, {recursive: true}); +} +fs.writeFileSync(sourceDir + 'config.cpp', sourceOutput); diff --git a/firmware/haptics/line wall/utils/scripts/run.js b/firmware/haptics/line wall/utils/scripts/run.js new file mode 100644 index 0000000..96fae21 --- /dev/null +++ b/firmware/haptics/line wall/utils/scripts/run.js @@ -0,0 +1,222 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const os = require('os'); +const path = require('path'); +const {exec, remove, color, escape} = require('./tools'); + +function log(message, messageColor) { + console.log(`${messageColor}${message}${color.reset}`); +} + +const buildHandlers = { + 'framework': () => { + return build('voice-command') + & build('serial-plugin') + & build('serial-standalone'); + }, + 'voice-command': () => { + return exec('node', ['./utils/voiceCommand/build/build-release.js']); + }, + 'serial-plugin': () => { + const gypDef = '--cppdefs="NODE_GYP ' + escape(cppDefines.join(' ')) + '"'; + return exec('node-gyp', ['configure', '-C utils/serial', gypDef]) + & exec('node-gyp', ['build', '-C utils/serial']); + }, + 'serial-standalone': () => { + return exec( + cppExec, + cppArgs.concat(cppDefines.map((d) => cppDefinePrefix + d)).concat([ + 'utils/serial/src/standalone/main.cpp', + 'utils/serial/src/standalone/standalone.cpp', + 'utils/serial/src/serial/shared.cpp', + 'utils/serial/src/cppLib/lib.cpp', + 'utils/serial/src/crashAnalyzer/analyze.cpp', + 'utils/serial/src/crashAnalyzer/buffer.cpp', + process.platform == 'win32' ? + 'utils/serial/src/serial/win.cpp' : + 'utils/serial/src/serial/unix.cpp', + 'utils/protocol/src/protocol/protocol.cpp', + '-Iutils/serial/include', + '-Iutils/protocol/include', + '-o utils/serial/serial'])); + }, + 'unity-serial': () =>{ + return unity(); + }, + 'firmware': () => { + return config(process.argv[4]) + & platformio('build'); + } +}; + +function build(target) { + if (target === undefined) { + return build('firmware'); + // 14.5.21: The js framework is deprecated since this + // commit: a045c86fa3754810a9a68b1bc89dcf990d883579 + // To make it work again we have to implement the acknowledgement + // logic, similar to the way it's done in this + // commit: faa30310e884784b5d431ac65c05e3186b9bafab + return build('framework') + & build('firmware'); + } + + if (!buildHandlers.hasOwnProperty(target)) { + log(`Invalid build target ${target}`, color.red); + return false; + } + + log(`Building ${target}`, color.green); + const result = buildHandlers[target](); + if (result) { + log(`Building ${target} successful`, color.green); + } else { + log(`Building ${target} failed`, color.red); + } + return result; +} + +const cleanHandlers = { + 'framework': () => { + return clean('voice-command') + & clean('serial-plugin') + & clean('serial-standalone'); + }, + 'voice-command': () => { + return remove('./utils/voiceCommand/.bin'); + }, + 'serial-plugin': () => { + return remove('./utils/serial/build'); + }, + 'serial-standalone': () => { + log('Clean serial-standalone not implemented yet', color.yellow); + return true; + }, + 'firmware': () => { + return remove('./firmware/src/config/config.cpp') + & remove('./firmware/include/config/config.hpp') + & platformio('clean'); + } +}; + +function clean(target) { + if (target === undefined) { + return clean('framework') + & clean('firmware'); + } + + if (!cleanHandlers.hasOwnProperty(target)) { + log(`Invalid clean target ${target}`, color.red); + return false; + } + + log(`Cleaning ${target}`, color.green); + const result = cleanHandlers[target](); + if (result) { + log(`Cleaning ${target} successful`, color.green); + } else { + log(`Cleaning ${target} failed`, color.red); + } + return result; +} + +function config(target) { + if (target === undefined) { + target = 'fiona'; + } + log(`Generating config ${target}`, color.green); + return exec('node', ['utils/scripts/generateHardwareConfig.js', target]); +} + +function platformio(command) { + if (command == 'build' || command === undefined) { + command = '.'; + } + + + log(`Running platformio ${command}`, color.green); + return exec(platformioExec, ['run', '-d firmware', `-t ${command}`]); +} + +function plotter() { + return exec('http-server', ['utils/plotter/']); +} + +function docs() { + log(`Building docs`, color.green); + return exec('node', ['utils/scripts/docs.js']); +} + +function unity() { + if (process.platform == 'win32') { + return exec('utils\\serial\\unity\\win.bat'); + } else if (process.platform == 'darwin') { + const unityDir = './utils/serial/unity/'; + return exec(unityDir+'mac.sh'); + } else { + return exec('echo "Linux is not supported for building unity framework."'); + } +} + +const handlers = { + 'build': build, + 'clean': clean, + 'config': config, + 'platformio': platformio, + 'plotter': plotter, + 'docs': docs, + 'unity': unity +}; + +const platformioDir = os.homedir() + '/.platformio'; +const xtensaUtilDir = platformioDir + '/packages/toolchain-xtensa32/bin/'; +const addr2linePath = path.join(xtensaUtilDir, 'xtensa-esp32-elf-addr2line'); + +let platformioExec; +let cppExec; +let cppDefinePrefix; +let cppArgs; +const cppDefines = [ + escape('ADDR2LINE_PATH=\"' + addr2linePath + '\"') +]; + +if (process.platform == 'win32') { + platformioExec = path.join(platformioDir, '/penv/Scripts/platformio'); + cppExec = 'cl'; + cppDefinePrefix = '/D'; + cppArgs = ['/Fo:Utils\\Serial\\']; + cppDefines.push('WINDOWS'); +} else { + if (exec('which', ['platformio'])) { + platformioExec = 'platformio'; + } else { + platformioExec = platformioDir + '/penv/bin/platformio'; + } + if (process.platform == 'linux') { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } else { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } +} + +const command = process.argv[2]; +if (!handlers.hasOwnProperty(command)) { + log(`Unknown command ${command}`, color.red); + process.exitCode = 1; + return; +} + +log(`=== Running ${command} ===`, color.green); +const result = handlers[command](process.argv[3]); +if (result) { + log(`=== ${command} successful ===`, color.green); + process.exitCode = 0; +} else { + log(`=== ${command} failed ===`, color.red); + process.exitCode = 1; +} diff --git a/firmware/haptics/line wall/utils/scripts/tools.js b/firmware/haptics/line wall/utils/scripts/tools.js new file mode 100644 index 0000000..d125380 --- /dev/null +++ b/firmware/haptics/line wall/utils/scripts/tools.js @@ -0,0 +1,54 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const childProcess = require('child_process'); +const path = require('path'); +const fs = require('fs'); + +const color = { + red: '\x1b[31m', + green: '\x1b[32m', + yellow: '\x1b[33m', + reset: '\x1b[0m' +}; + +function exec(cmd, args) { + return childProcess.spawnSync( + cmd, + args, + { + shell: true, + stdio: ['ignore', process.stdout, process.stderr] + }).status == 0; +}; + +function remove(target) { + if (!fs.existsSync(target)) { + console.log(`Could not find ${target}`, color.yellow); + return true; + } + if (fs.statSync(target).isDirectory()) { + const content = fs.readdirSync(target); + for (const entry in content) { + if (content.hasOwnProperty(entry)) { + remove(path.join(target, content[entry])); + } + } + fs.rmdirSync(target); + } else { + fs.unlinkSync(target); + } + console.log(`Removed ${target}`); + return true; +} + +function escape(string) { + return string.replace(/\\/g, '\\\\').replace(/"/g, '\\\"'); +} + +module.exports = { + exec, + remove, + color, + escape +}; diff --git a/firmware/haptics/line wall/utils/serial/CMakeLists.txt b/firmware/haptics/line wall/utils/serial/CMakeLists.txt new file mode 100644 index 0000000..edd3b8a --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.9) + +# project settings + +project(serial CXX) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# sources + +set(serial_source_dir src) +set(protocol_source_dir ../protocol/src/protocol) +set(serial_include_dir include) +set(protocol_include_dir ../protocol/include) +set(base_include_dir include/serial) +set(generated_header_dir ${CMAKE_BINARY_DIR}/generated) + +set(export_header + ${generated_header_dir}/serial_export.hpp) + +set(public_headers + ${serial_include_dir}/libInterface.hpp + ${export_header}) + +set(private_headers + ${serial_include_dir}/serial.hpp + ${serial_include_dir}/packet.hpp + ${serial_include_dir}/crashAnalyzer.hpp + ${protocol_include_dir}/protocol/protocol.hpp + ${protocol_include_dir}/protocol/header.hpp + ${protocol_include_dir}/protocol/messageType.hpp) + +set(shared_sources + ${serial_source_dir}/cppLib/lib.cpp + ${serial_source_dir}/crashAnalyzer/analyze.cpp + ${serial_source_dir}/crashAnalyzer/buffer.cpp + ${serial_source_dir}/serial/shared.cpp + ${serial_source_dir}/serial/packet.cpp + ${protocol_source_dir}/protocol.cpp + ${protocol_source_dir}/messageType.cpp) + +if (WIN32) +# set path vars +set(addr2line "$ENV{USERPROFILE}\\.platformio\\packages\\toolchain-xtensa32\\bin\\xtensa-esp32-elf-addr2line.exe") +set(firmware_rel "..\\..\\firmware\\.pio\\build\\esp32dev\\firmware.elf") +get_filename_component(firmware_abs ${firmware_rel} ABSOLUTE) +# escape backslashes +string(REPLACE "\\" "\\\\" addr2line ${addr2line}) +string(REPLACE "/" "\\\\" firmware_abs ${firmware_abs}) +message(STATUS "addr2line: ${addr2line}") +message(STATUS "firmware (rel): ${firmware_rel}") +message(STATUS "firmware (abs): ${firmware_abs}") +# set defines +add_compile_definitions(WINDOWS) +add_compile_definitions(ADDR2LINE_PATH="${addr2line}") +add_compile_definitions(FIRMWARE_PATH="${firmware_abs}") +set(sources + ${shared_sources} + ${serial_source_dir}/serial/win.cpp) +elseif (UNIX) +set(sources + ${shared_sources} + ${serial_source_dir}/serial/unix.cpp) +else() + message(FATAL_ERROR "System is neither WIN32 nor UNIX.") +endif() + +# lib target + +include_directories(${generated_header_dir}) +include_directories(${serial_include_dir}) +include_directories(${protocol_include_dir}) +#add_compile_definitions(ADDR2LINE_PATH) +add_library(serial SHARED ${public_headers} ${private_headers} ${sources}) + +include(GenerateExportHeader) +generate_export_header(serial + BASE_NAME serial + EXPORT_MACRO_NAME SERIAL_EXPORT + EXPORT_FILE_NAME ${export_header} + STATIC_DEFINE SERIAL_BUILT_AS_STATIC) + +# install settings + +set_target_properties(serial PROPERTIES PUBLIC_HEADER "${public_headers}") + +include(GNUInstallDirs) +install(TARGETS serial + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/firmware/haptics/line wall/utils/serial/binding.gyp b/firmware/haptics/line wall/utils/serial/binding.gyp new file mode 100644 index 0000000..17c9986 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/binding.gyp @@ -0,0 +1,36 @@ +{ + "variables": { + "cppdefs": "NODE_GYP", + }, + "targets": [{ + "target_name": "serial", + "sources": [ + "./src/node/main.cpp", + "./src/node/setup.cpp", + "./src/node/poll.cpp", + "./src/node/send.cpp", + "./src/node/receiveHelpers.cpp", + "./src/node/sendHelpers.cpp", + "./src/serial/shared.cpp", + "./src/cppLib/lib.cpp", + "./src/crashAnalyzer/buffer.cpp", + "./src/crashAnalyzer/analyze.cpp", + "../protocol/src/protocol/protocol.cpp" + ], + "conditions": [ + ["OS=='win'", { + "sources": ["./src/serial/win.cpp"] + }], + ["OS!='win'", { + "sources": ["./src/serial/unix.cpp"] + }], + ], + "include_dirs": [ + "./include", + "../protocol/include" + ], + "defines": [ + "<@(cppdefs)" + ] + }] +} diff --git a/firmware/haptics/line wall/utils/serial/include/crashAnalyzer.hpp b/firmware/haptics/line wall/utils/serial/include/crashAnalyzer.hpp new file mode 100644 index 0000000..dea6f74 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/crashAnalyzer.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#define SAFEMOD(a, b) ((a) % (b) + (b)) % (b) + +class CrashAnalyzer +{ +private: + static const uint16_t c_bufferLength = 1024; + static const uint16_t c_dumpLineWidth = 32; + static uint8_t s_buffer[c_bufferLength]; + static uint16_t s_length; + static uint16_t s_index; + + static void clearBuffer(); + static uint8_t getChar(uint16_t offset); + + static const std::string c_rebootString; + static const std::string c_backtraceString; + + static bool findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t& foundOffset); + static std::vector getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset); + static void addr2line(std::vector addresses); + + static void checkOutput(); +public: + static void push_back(const uint8_t character); +}; diff --git a/firmware/haptics/line wall/utils/serial/include/libInterface.hpp b/firmware/haptics/line wall/utils/serial/include/libInterface.hpp new file mode 100644 index 0000000..40b2846 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/libInterface.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include + +#include "serial.hpp" +#include "serial_export.hpp" + +// class stuff + +class CppLib : DPSerial +{ +public: + static uint32_t getRevision(); + static uint64_t open(char* port); + static void setActiveHandle(uint64_t handle); + static void close(); + static void poll(); + static void sendSyncAck(); + static void sendHeartbeatAck(); + static void sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + static void sendSpeed(uint8_t pantoIndex, float speed); + static void createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + static void addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void removeObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void enableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void disableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void sendFree(uint8_t pantoIndex); + static void sendFreeze(uint8_t pantoIndex); + static void sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + static uint32_t checkSendQueue(uint32_t maxPackets); + static void reset(); +}; + +// handlers + +typedef void (*syncHandler_t)(uint64_t); +extern syncHandler_t syncHandler; +typedef void (*heartbeatHandler_t)(uint64_t); +extern heartbeatHandler_t heartbeatHandler; +typedef void (*positionHandler_t)(uint64_t, double*); +extern positionHandler_t positionHandler; +typedef void (*loggingHandler_t)(char*); +extern loggingHandler_t loggingHandler; +typedef void (*transitionHandler_t)(uint8_t); +extern transitionHandler_t transitionHandler; + +void logString(char* msg); + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +extern "C" +{ + uint32_t SERIAL_EXPORT GetRevision(); + void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler); + void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler); + void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler); + void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler); + void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler); + uint64_t SERIAL_EXPORT Open(char* port); + void SERIAL_EXPORT Close(uint64_t handle); + void SERIAL_EXPORT Poll(uint64_t handle); + void SERIAL_EXPORT SendSyncAck(uint64_t handle); + void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle); + void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed); + void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets); + void SERIAL_EXPORT Reset(); +}; diff --git a/firmware/haptics/line wall/utils/serial/include/node.hpp b/firmware/haptics/line wall/utils/serial/include/node.hpp new file mode 100644 index 0000000..5941e9b --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "serial.hpp" + +#if __has_include("node_api.h") +#include +#elif __has_include("node/node_api.h") +#include +#endif +#define NAPI_CHECK(code) \ + if (code != napi_ok) \ + std::cerr << "NOT OK: " << __FILE__ << ":" << __LINE__ << std::endl; + +// enable for debugging +// #define DEBUG_LOGGING + +class Node : public DPSerial +{ +private: + static napi_value receiveUInt8(napi_env env, uint16_t& offset); + static napi_value receiveInt16(napi_env env, uint16_t& offset); + static napi_value receiveUInt16(napi_env env, uint16_t& offset); + static napi_value receiveInt32(napi_env env, uint16_t& offset); + static napi_value receiveUInt32(napi_env env, uint16_t& offset); + static napi_value receiveFloat(napi_env env, uint16_t& offset); + + static void sendUInt8(napi_env env, napi_value value, uint16_t& offset); + static void sendInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendFloat(napi_env env, napi_value value, uint16_t& offset); + +public: + static napi_value open(napi_env env, napi_callback_info info); + static napi_value close(napi_env env, napi_callback_info info); + static napi_value poll(napi_env env, napi_callback_info info); + static napi_value send(napi_env env, napi_callback_info info); +}; diff --git a/firmware/haptics/line wall/utils/serial/include/packet.hpp b/firmware/haptics/line wall/utils/serial/include/packet.hpp new file mode 100644 index 0000000..f885582 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/packet.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +class Packet +{ +public: + Header header; + uint8_t payload[256]; + uint8_t payloadIndex = 0; + + Packet(uint8_t messageType, uint16_t payloadSize); + + uint8_t receiveUInt8(); + int16_t receiveInt16(); + uint16_t receiveUInt16(); + int32_t receiveInt32(); + uint32_t receiveUInt32(); + float receiveFloat(); + + void sendUInt8(uint8_t value); + void sendInt16(int16_t value); + void sendUInt16(uint16_t value); + void sendInt32(int32_t value); + void sendUInt32(uint32_t value); + void sendFloat(float value); +}; diff --git a/firmware/haptics/line wall/utils/serial/include/serial.hpp b/firmware/haptics/line wall/utils/serial/include/serial.hpp new file mode 100644 index 0000000..6ff8e07 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/serial.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "packet.hpp" + +#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) +#include +#define FILEHANDLE HANDLE +#else +#include +#include +#include +#include +#include +#define FILEHANDLE FILE * +#endif + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; + +class DPSerial : public DPProtocol +{ +protected: + static std::string s_path; + static const uint32_t c_packetSize = 0xFF; + static FILEHANDLE s_handle; + static std::thread s_worker; + static bool s_workerRunning; + + static std::queue s_highPrioSendQueue; + static std::queue s_lowPrioSendQueue; + static std::queue s_receiveQueue; + + static uint8_t s_nextTrackedPacketId; + static bool s_haveUnacknowledgedTrackedPacket; + static Packet s_lastTrackedPacket; + static std::chrono::time_point + s_lastTrackedPacketSendTime; + static const std::chrono::milliseconds c_trackedPacketTimeout; + static bool s_pantoReportedInvalidData; + + static bool s_pantoReady; + static uint32_t s_magicReceiveIndex; + static ReceiveState s_receiveState; + static Header s_receiveHeader; + + static void startWorker(); + static void stopWorker(); + static void tearDown(); + static void reset(); + static void update(); + static void processOutput(); + static bool processInput(); + + static bool isTracked(uint8_t t); + static bool checkQueue(std::queue &q); + static void sendPacket(Packet p); + static void sendInstantPacket(Packet p); + static void write(const uint8_t *const data, const uint32_t length); + + static uint32_t getAvailableByteCount(FILEHANDLE s_handle); + static bool readBytesFromSerial(void *target, uint32_t length); + static bool readBytesIfAvailable(void *target, uint32_t length); + static bool readMagicNumber(); + static bool readHeader(); + static bool readPayload(); + +public: + static bool setup(std::string path); +}; diff --git a/firmware/haptics/line wall/utils/serial/include/serial_export.hpp b/firmware/haptics/line wall/utils/serial/include/serial_export.hpp new file mode 100644 index 0000000..09738b0 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/serial_export.hpp @@ -0,0 +1,59 @@ + +#ifndef SERIAL_EXPORT_H +#define SERIAL_EXPORT_H + +// figure out the correct attributes +#if defined _WIN32 || defined __CYGWIN__ + #define HELPER_DLL_IMPORT __declspec(dllimport) + #define HELPER_DLL_EXPORT __declspec(dllexport) + #define HELPER_DLL_LOCAL +#else + #if __GNUC__ >= 4 + #define HELPER_DLL_IMPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_EXPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden"))) + #else + #define HELPER_DLL_IMPORT + #define HELPER_DLL_EXPORT + #define HELPER_DLL_LOCAL + #endif +#endif + +#ifdef SERIAL_BUILT_AS_STATIC +# define SERIAL_EXPORT +# define SERIAL_NO_EXPORT +#else +# ifndef SERIAL_EXPORT +# ifdef serial_EXPORTS + /* We are building this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# else + /* We are using this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# endif +# endif + +# ifndef SERIAL_NO_EXPORT +# define SERIAL_NO_EXPORT HELPER_DLL_LOCAL +# endif +#endif + +#ifndef SERIAL_DEPRECATED +# define SERIAL_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef SERIAL_DEPRECATED_EXPORT +# define SERIAL_DEPRECATED_EXPORT SERIAL_EXPORT SERIAL_DEPRECATED +#endif + +#ifndef SERIAL_DEPRECATED_NO_EXPORT +# define SERIAL_DEPRECATED_NO_EXPORT SERIAL_NO_EXPORT SERIAL_DEPRECATED +#endif + +#if 0 /* DEFINE_NO_DEPRECATED */ +# ifndef SERIAL_NO_DEPRECATED +# define SERIAL_NO_DEPRECATED +# endif +#endif + +#endif /* SERIAL_EXPORT_H */ diff --git a/firmware/haptics/line wall/utils/serial/include/standalone.hpp b/firmware/haptics/line wall/utils/serial/include/standalone.hpp new file mode 100644 index 0000000..4bf5992 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/include/standalone.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include "serial.hpp" + +class Standalone : public DPSerial +{ +public: + static void terminate(int signal); + static void printPacket(); +}; diff --git a/firmware/haptics/line wall/utils/serial/src/cppLib/lib.cpp b/firmware/haptics/line wall/utils/serial/src/cppLib/lib.cpp new file mode 100644 index 0000000..446b894 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/cppLib/lib.cpp @@ -0,0 +1,445 @@ +#include "libInterface.hpp" + +#include +#include + +#include "packet.hpp" + +#ifdef _WIN32 +#define FILEPTR void * +#else +#define FILEPTR FILE * +#endif + +// class stuff + +uint32_t CppLib::getRevision() +{ + return c_revision; +} + +uint64_t CppLib::open(char *port) +{ + if (!setup(port)) + { + logString("Open failed"); + return 0; + } + logString("Open successfull"); + return (uint64_t)s_handle; +} + +void CppLib::setActiveHandle(uint64_t handle) +{ + s_handle = (FILEPTR)handle; +} + +void CppLib::close() +{ + tearDown(); +} + +void CppLib::poll() +{ + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + bool receivedTransition = false; + double positionCoords[2 * 5]; + uint8_t pantoIndex; + + while (s_receiveQueue.size() > 0) + { + auto packet = s_receiveQueue.front(); + s_receiveQueue.pop(); + + if (packet.header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + switch (packet.header.MessageType) + { + case SYNC: + { + auto receivedRevision = packet.receiveUInt32(); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::ostringstream oss; + oss << "Revision id not matching. Expected " << c_revision + << ", received " << receivedRevision << "." << std::endl; + logString((char *)oss.str().c_str()); + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (packet.payloadIndex < packet.header.PayloadSize) + { + uint8_t index = packet.payloadIndex / 4; + positionCoords[index] = packet.receiveFloat(); + } + break; + case DEBUG_LOG: + logString((char *)packet.payload); + break; + case TRANSITION_ENDED: + receivedTransition = true; + pantoIndex = packet.receiveUInt8(); + break; + default: + break; + } + } + + if (receivedSync) + { + if (syncHandler == nullptr) + { + logString("Received sync, but handler not set up"); + } + else + { + syncHandler((uint64_t)s_handle); + } + } + + if (receivedHeartbeat) + { + if (heartbeatHandler == nullptr) + { + logString("Received heartbeat, but handler not set up"); + } + else + { + heartbeatHandler((uint64_t)s_handle); + } + } + + if (receivedPosition) + { + if (positionHandler == nullptr) + { + logString("Received position, but handler not set up"); + } + else + { + positionHandler((uint64_t)s_handle, positionCoords); + } + } + + if (receivedTransition) + { + // transition (tweening) ended + if (transitionHandler == nullptr) + { + logString("Received transition ended, but handler not set up"); + } + else + { + transitionHandler(pantoIndex); + } + } +} + +void CppLib::sendSyncAck() +{ + DPSerial::sendInstantPacket(Packet(SYNC_ACK, 0)); +} + +void CppLib::sendHeartbeatAck() +{ + DPSerial::sendInstantPacket(Packet(HEARTBEAT_ACK, 0)); +} + +void CppLib::sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + auto p = Packet(MOTOR, 14); // 1 for control, 1 for index, 3 * 4 for pos + p.sendUInt8(controlMethod); + p.sendUInt8(pantoIndex); + p.sendFloat(positionX); + p.sendFloat(positionY); + p.sendFloat(rotation); + sendPacket(p); +} + +void CppLib::sendSpeed(uint8_t pantoIndex, float speed) +{ + auto p = Packet(SPEED, 5); // 1 for index, 1 * 4 for position + p.sendUInt8(pantoIndex); + p.sendFloat(speed); + sendPacket(p); +} + +void CppLib::sendFree(uint8_t pantoIndex) +{ + auto p = Packet(FREE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendFreeze(uint8_t pantoIndex) +{ + auto p = Packet(FREEZE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + auto p = Packet(SPEED_CONTROL, 15); // 1 for index, 4 for tether factor, 4 each for the tether radii, 1 for tether strategy and 1 for pock + p.sendUInt8(tethered); + p.sendFloat(tetherFactor); + p.sendFloat(tetherInnerRadius); + p.sendFloat(tetherOuterRadius); + p.sendUInt8(strategy); + p.sendUInt8(pockEnabled); + sendPacket(p); +} + +void CppLib::createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_PASSABLE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + auto p = Packet(CREATE_RAIL, 23); // 1 for index, 2 for id, 2 * 2 * 4 for vectors, 4 for displacement + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + p.sendFloat(displacement); + sendPacket(p); +} + +void CppLib::addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(ADD_TO_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::removeObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(REMOVE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::enableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(ENABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::disableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(DISABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::reset() +{ + DPSerial::reset(); +} + +// handlers +syncHandler_t syncHandler; +heartbeatHandler_t heartbeatHandler; +positionHandler_t positionHandler; +loggingHandler_t loggingHandler; +transitionHandler_t transitionHandler; + +void logString(char *msg) +{ + if (loggingHandler != nullptr) + { + loggingHandler(msg); + } +} + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +uint32_t GetRevision() +{ + return CppLib::getRevision(); +} + +void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler) +{ + syncHandler = handler; + logString("Sync handler set"); +} + +void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler) +{ + heartbeatHandler = handler; + logString("Heartbeat handler set"); +} + +void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler) +{ + positionHandler = handler; + logString("Position handler set"); +} + +void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler) +{ + loggingHandler = handler; + loggingHandler("Logging from plugin is enabled"); +} + +void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler) +{ + transitionHandler = handler; + logString("Transition handler set"); +} + +uint64_t SERIAL_EXPORT Open(char *port) +{ + return CppLib::open(port); +} + +void SERIAL_EXPORT Close(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::close(); +} + +void SERIAL_EXPORT Poll(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::poll(); +} + +void SERIAL_EXPORT SendSyncAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSyncAck(); +} + +void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendHeartbeatAck(); +} + +void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + CppLib::setActiveHandle(handle); + CppLib::sendMotor(controlMethod, pantoIndex, positionX, positionY, rotation); +} + +void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSpeed(pantoIndex, speed); +} + +void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFree(pantoIndex); +} + +void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFreeze(pantoIndex); +} + +void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createPassableObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + CppLib::setActiveHandle(handle); + CppLib::createRail(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y, displacement); +} + +void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::addToObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::removeObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::enableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::disableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + CppLib::sendSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); +} + +uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets) +{ + return 0; +} + +void SERIAL_EXPORT Reset() +{ + CppLib::reset(); +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/analyze.cpp b/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/analyze.cpp new file mode 100644 index 0000000..9569fdb --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/analyze.cpp @@ -0,0 +1,183 @@ +#include "crashAnalyzer.hpp" + +#include +#include +#include +#include +#include +#include +#include "libInterface.hpp" + +#ifdef WINDOWS +#include +#define popen _popen +#define pclose _pclose +#endif + +#ifdef __APPLE__ +#define popen popen +#define pclose pclose +#endif + +const std::string CrashAnalyzer::c_rebootString = "Rebooting..."; +const std::string CrashAnalyzer::c_backtraceString = "Backtrace:"; + +bool CrashAnalyzer::findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t &foundOffset) +{ + const auto length = string.length(); + int32_t index = length - 1; + auto offset = startOffset; + + while (index > -1 && offset <= endOffset) + { + if (getChar(offset) == string.at(index)) + { + index--; + } + else + { + index = length - 1; + } + offset++; + } + + if (index == -1) + { + foundOffset = offset; + return true; + } + + return false; +} + +std::vector CrashAnalyzer::getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset) +{ + std::string data(startOffset - endOffset + 1, 'a'); + for (auto i = startOffset; i >= endOffset; --i) + { + data.at(startOffset - i) = getChar(i); + } + std::regex regex("(0x.{8}):0x.{8}"); + auto it = std::sregex_iterator(data.begin(), data.end(), regex); + auto end = std::sregex_iterator(); + + std::vector result; + while (it != end) + { + result.push_back((*it++).str(1)); + } + + return result; +} + +char *exec(const char *cmd) +{ + loggingHandler((char *)cmd); + + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd, "r"), pclose); + if (!pipe) + { + loggingHandler("Popen failed"); + return "popen() failed!"; + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != NULL) + { + loggingHandler(buffer.data()); + //result += buffer.data(); + } + char *cstr = new char[result.length() + 1]; + strcpy(cstr, result.c_str()); + // do stuff + delete[] cstr; + return cstr; +} + +void CrashAnalyzer::addr2line(std::vector addresses) +{ + std::cout << std::endl + << "===== STACKTRACE BEGIN =====" << std::endl; + + loggingHandler("===== STACKTRACE BEGIN ====="); +#ifdef __APPLE__ +#define ADDR2LINE_PATH "~/.platformio/packages/toolchain-xtensa32/bin/xtensa-esp32-elf-addr2line" +#define FIRMWARE_PATH "/Users/julio/Documents/Uni/5_Master/HCI_Project_Seminar/dualpantoframework/firmware/.pio/build/esp32dev/firmware.elf" +#endif +#ifdef ADDR2LINE_PATH + std::ostringstream command; + command + << ADDR2LINE_PATH + << " -e " << FIRMWARE_PATH + << " -fpCis"; // see https://linux.die.net/man/1/addr2line + for (const auto &address : addresses) + { + command << " " << address; + } + + command << " 2>&1"; + auto result = exec(command.str().c_str()); + + std::ostringstream out; + out << "Stacktrace (most recent call first):" << std::endl + << result; +#else + loggingHandler("Path to addr2line executable not set. Can't analyze stacktrace."); + std::cout + << "Path to addr2line executable not set. Can't analyze stacktrace." + << std::endl; + +#endif + + std::cout << "====== STACKTRACE END ======" << std::endl; +} + +void CrashAnalyzer::checkOutput() +{ + uint16_t rebootOffset; + if (!findString( + 0, + c_rebootString.length(), + c_rebootString, + rebootOffset)) + { + return; + } + + uint16_t backtraceOffset; + if (!findString( + rebootOffset, + s_length, + c_backtraceString, + backtraceOffset)) + { + std::cout << "Reboot detected, but no backtrace found." << std::endl; + return; + } + char out[c_bufferLength + 1]; + std::memcpy(out, s_buffer, c_bufferLength); + out[c_bufferLength] = '\0'; + for (int i = 0; i < c_bufferLength; i++) + { + char x = s_buffer[i]; + if (x < 32 || x > 126) + { + // space + x = 32; + } + out[i] = x; + } + loggingHandler("ERROR!"); + loggingHandler(out); + auto addresses = getBacktraceAddresses( + backtraceOffset - c_backtraceString.length() - 1, + rebootOffset); + + addr2line(addresses); + clearBuffer(); +} diff --git a/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/buffer.cpp b/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/buffer.cpp new file mode 100644 index 0000000..8107795 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/crashAnalyzer/buffer.cpp @@ -0,0 +1,28 @@ +#include "crashAnalyzer.hpp" + +#include +#include + +uint8_t CrashAnalyzer::s_buffer[c_bufferLength]; +uint16_t CrashAnalyzer::s_length = 0; +uint16_t CrashAnalyzer::s_index = 0; + +void CrashAnalyzer::clearBuffer() +{ + s_index = 0; + s_length = 0; +} + +uint8_t CrashAnalyzer::getChar(uint16_t offset) +{ + return s_buffer[SAFEMOD((s_index - offset), c_bufferLength)]; +} + +void CrashAnalyzer::push_back(const uint8_t character) +{ + s_buffer[s_index] = character; + s_index = (s_index + 1) % c_bufferLength; + s_length = (s_length >= c_bufferLength) ? c_bufferLength : (s_length + 1); + + checkOutput(); +} diff --git a/firmware/haptics/line wall/utils/serial/src/node/main.cpp b/firmware/haptics/line wall/utils/serial/src/node/main.cpp new file mode 100644 index 0000000..fbbcadf --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/main.cpp @@ -0,0 +1,23 @@ +#include "node.hpp" + +#define defFunc(name, ptr) \ + if (napi_create_function(env, NULL, 0, ptr, NULL, &fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to wrap native function"); \ + } \ + if (napi_set_named_property(env, exports, name, fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to populate exports"); \ + } + +napi_value Init(napi_env env, napi_value exports) +{ + napi_value fn; + defFunc("open", Node::open); + defFunc("close", Node::close); + defFunc("poll", Node::poll); + defFunc("send", Node::send); + return exports; +} + +NAPI_MODULE(NODE_GYP_MODULE_NAME, Init) diff --git a/firmware/haptics/line wall/utils/serial/src/node/poll.cpp b/firmware/haptics/line wall/utils/serial/src/node/poll.cpp new file mode 100644 index 0000000..fd20886 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/poll.cpp @@ -0,0 +1,121 @@ +#include "node.hpp" + +#include + +napi_value Node::poll(napi_env env, napi_callback_info info) +{ + // argv[0]: handle + // argv[1]: device + // argv[2]: vector constructor + // argv[3]: sync cb + // argv[4]: heartbeat cb + // argv[5]: position cb + // argv[6]: debug log cb + size_t argc = 7; + napi_value argv[7]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + // only keep binary state for packages where only the newest counts + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + double positionCoords[2 * 5]; + + while (getAvailableByteCount(s_handle)) + { + receivePacket(); + + if (s_header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + uint16_t offset = 0; + switch (s_header.MessageType) + { + case SYNC: + { + auto receivedRevision = DPSerial::receiveUInt32(offset); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::cout + << "Received invalid revision id " << receivedRevision + << " (expected " << c_revision << "). Maybe try reset the device?" << std::endl; + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (offset < s_header.PayloadSize) + { + uint8_t index = offset / 4; + positionCoords[index] = DPSerial::receiveFloat(offset); + } + break; + case DEBUG_LOG: + napi_value result; + napi_create_string_utf8(env, reinterpret_cast(s_packetBuffer), s_header.PayloadSize, &result); + napi_call_function(env, argv[1], argv[6], 1, &result, NULL); + break; + default: + break; + } + } + + if(receivedSync) + { + napi_call_function(env, argv[1], argv[3], 0, NULL, NULL); + } + + if(receivedHeartbeat) + { + napi_call_function(env, argv[1], argv[4], 0, NULL, NULL); + } + + if (receivedPosition) + { + napi_value result; + napi_create_array(env, &result); + + const uint32_t posArgc = 3; + const uint32_t goArgc = 2; + for (auto i = 0u; i < 2; ++i) + { + napi_value posArgv[posArgc]; + + for (auto j = 0u; j < posArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + j], &(posArgv[j])); + } + + napi_value vector; + NAPI_CHECK(napi_new_instance(env, argv[2], posArgc, posArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2, vector)); + + napi_value goArgv[goArgc]; + + for (auto j = 0u; j < goArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + 3 + j], &(goArgv[j])); + } + + NAPI_CHECK(napi_new_instance(env, argv[2], goArgc, goArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2 + 1, vector)); + } + + NAPI_CHECK(napi_call_function(env, argv[1], argv[5], 1, &result, NULL)); + } + + return NULL; +} diff --git a/firmware/haptics/line wall/utils/serial/src/node/receiveHelpers.cpp b/firmware/haptics/line wall/utils/serial/src/node/receiveHelpers.cpp new file mode 100644 index 0000000..2450200 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/receiveHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +napi_value Node::receiveUInt8(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt8(offset), &result); + return result; +} + +napi_value Node::receiveInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt16(offset), &result); + return result; +} + +napi_value Node::receiveUInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt16(offset), &result); + return result; +} + +napi_value Node::receiveInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt32(offset), &result); + return result; +} + +napi_value Node::receiveUInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt32(offset), &result); + return result; +} + +napi_value Node::receiveFloat(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_double(env, DPSerial::receiveFloat(offset), &result); + return result; +} diff --git a/firmware/haptics/line wall/utils/serial/src/node/send.cpp b/firmware/haptics/line wall/utils/serial/src/node/send.cpp new file mode 100644 index 0000000..90f854e --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/send.cpp @@ -0,0 +1,198 @@ +#include "node.hpp" + +#include + +napi_value Node::send(napi_env env, napi_callback_info info) +{ + size_t argc = 3; + napi_value argv[3]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + uint32_t messageType; + napi_get_value_uint32(env, argv[1], &messageType); + + s_header.MessageType = static_cast(messageType); + + uint16_t offset = 0; + switch (messageType) + { + case SYNC_ACK: + break; + case HEARTBEAT_ACK: + break; + case MOTOR: + { + napi_value tempNapiValue; + uint32_t tempUInt32; + double tempDouble; + uint32_t index = 0; + + // control method + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // panto index + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // position + while (index < 5) + { + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_double(env, tempNapiValue, &tempDouble); + DPSerial::sendFloat(static_cast(tempDouble), offset); + } + break; + } + case PID: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "motorIndex", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "pid", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_value pidNapiValue; + double pidDouble; + for (auto i = 0; i < 3; ++i) + { + napi_get_element(env, tempNapiValue, i, &pidNapiValue); + napi_get_value_double(env, pidNapiValue, &pidDouble); + DPSerial::sendFloat(static_cast(pidDouble), offset); + } + break; + } + case CREATE_OBSTACLE: + case CREATE_PASSABLE_OBSTACLE: + case ADD_TO_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + break; + } + case CREATE_RAIL: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + double displacement; + napi_get_value_double(env, tempNapiValue, &displacement); + DPSerial::sendFloat(static_cast(displacement), offset); + break; + } + case REMOVE_OBSTACLE: + case ENABLE_OBSTACLE: + case DISABLE_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + break; + } + case CALIBRATE_PANTO:{ + break; + } + case DUMP_HASHTABLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + break; + } + default: + napi_throw_error(env, NULL, (std::string("Invalid message type") + std::to_string(messageType)).c_str()); + return NULL; + } + + #ifdef DEBUG_LOGGING + std::cout << "Payload size: 0x" << std::setw(2) << offset << std::endl; + #endif + + if(offset > c_maxPayloadSize) + { + std::cout << "Error: payload size of " << offset << " exceeds max payload size (" << c_maxPayloadSize << ")." << std::endl; + return NULL; + } + + s_header.PayloadSize = offset; + sendPacket(); + + #ifdef DEBUG_LOGGING + dumpBuffers(); + #endif + + return NULL; +} diff --git a/firmware/haptics/line wall/utils/serial/src/node/sendHelpers.cpp b/firmware/haptics/line wall/utils/serial/src/node/sendHelpers.cpp new file mode 100644 index 0000000..f1b062f --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/sendHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +void Node::sendUInt8(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt8(static_cast(temp), offset); +} + +void Node::sendInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendInt16(static_cast(temp), offset); +} + +void Node::sendUInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt16(static_cast(temp), offset); +} + +void Node::sendInt32(napi_env env, napi_value value, uint16_t& offset) +{ + int32_t temp; + napi_get_value_int32(env, value, &temp); + DPSerial::sendInt32(temp, offset); +} + +void Node::sendUInt32(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt32(temp, offset); +} + +void Node::sendFloat(napi_env env, napi_value value, uint16_t& offset) +{ + double temp; + napi_get_value_double(env, value, &temp); + DPSerial::sendFloat(static_cast(temp), offset); +} diff --git a/firmware/haptics/line wall/utils/serial/src/node/setup.cpp b/firmware/haptics/line wall/utils/serial/src/node/setup.cpp new file mode 100644 index 0000000..0c72735 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/node/setup.cpp @@ -0,0 +1,34 @@ +#include "node.hpp" + +napi_value Node::open(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + size_t length; + char buffer[64]; + napi_get_value_string_utf8(env, argv[0], buffer, sizeof(buffer), &length); + if (!setup(buffer)) + { + napi_throw_error(env, NULL, "open failed"); + } + napi_value result; + napi_create_int64(env, reinterpret_cast(s_handle), &result); + return result; +} + +napi_value Node::close(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + tearDown(); + return NULL; +} diff --git a/firmware/haptics/line wall/utils/serial/src/serial/packet.cpp b/firmware/haptics/line wall/utils/serial/src/serial/packet.cpp new file mode 100644 index 0000000..380f276 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/serial/packet.cpp @@ -0,0 +1,86 @@ +#include "packet.hpp" + +Packet::Packet(uint8_t messageType, uint16_t payloadSize) +{ + header.MessageType = messageType; + header.PayloadSize = payloadSize; +} + +uint8_t Packet::receiveUInt8() +{ + return payload[payloadIndex++]; +} + +int16_t Packet::receiveInt16() +{ + uint8_t temp[2]; + for (auto i = 0; i < 2; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 2; + return temp[0] << 8 | temp[1]; +} + +uint16_t Packet::receiveUInt16() +{ + auto temp = receiveInt16(); + return *reinterpret_cast(&temp); +} + +int32_t Packet::receiveInt32() +{ + uint8_t temp[4]; + for (auto i = 0; i < 4; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 4; + return temp[0] << 24 | temp[1] << 16 | temp[2] << 8 | temp[3]; +} + +uint32_t Packet::receiveUInt32() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +float Packet::receiveFloat() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +void Packet::sendUInt8(uint8_t value) +{ + payload[payloadIndex++] = value; +} + +void Packet::sendInt16(int16_t value) +{ + payload[payloadIndex++] = value >> 8; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt16(uint16_t value) +{ + sendInt16(*reinterpret_cast(&value)); +} + +void Packet::sendInt32(int32_t value) +{ + payload[payloadIndex++] = value >> 24; + payload[payloadIndex++] = (value >> 16) & 255; + payload[payloadIndex++] = (value >> 8) & 255; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt32(uint32_t value) +{ + sendInt32(*reinterpret_cast(&value)); +} + +void Packet::sendFloat(float value) +{ + sendInt32(*reinterpret_cast(&value)); +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/src/serial/shared.cpp b/firmware/haptics/line wall/utils/serial/src/serial/shared.cpp new file mode 100644 index 0000000..30a72ac --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/serial/shared.cpp @@ -0,0 +1,311 @@ +#include "serial.hpp" + +#include +#include +#include +#include +#include + +#include "crashAnalyzer.hpp" +#include "libInterface.hpp" + +std::string DPSerial::s_path; +FILEHANDLE DPSerial::s_handle; +std::thread DPSerial::s_worker; +bool DPSerial::s_workerRunning = false; + +std::queue DPSerial::s_highPrioSendQueue; +std::queue DPSerial::s_lowPrioSendQueue; +std::queue DPSerial::s_receiveQueue; +uint8_t DPSerial::s_nextTrackedPacketId = 1; +bool DPSerial::s_haveUnacknowledgedTrackedPacket = false; +Packet DPSerial::s_lastTrackedPacket(0, 0); +std::chrono::time_point + DPSerial::s_lastTrackedPacketSendTime; +bool DPSerial::s_pantoReportedInvalidData = false; +const std::chrono::milliseconds DPSerial::c_trackedPacketTimeout(10); + +bool DPSerial::s_pantoReady = true; +uint32_t DPSerial::s_magicReceiveIndex = 0; +ReceiveState DPSerial::s_receiveState = NONE; +Header DPSerial::s_receiveHeader = {0, 0}; + +void DPSerial::startWorker() +{ + reset(); + s_workerRunning = true; + s_worker = std::thread(update); +} + +void DPSerial::stopWorker() +{ + s_workerRunning = false; + s_worker.join(); +} + +void DPSerial::sendInstantPacket(Packet p) +{ + s_highPrioSendQueue.push(p); +} + +void DPSerial::sendPacket(Packet p) +{ + s_lowPrioSendQueue.push(p); +} + +void DPSerial::reset() +{ + std::queue emptyHP; + std::swap(s_highPrioSendQueue, emptyHP); + std::queue emptyLP; + std::swap(s_lowPrioSendQueue, emptyLP); + std::queue emptyRec; + std::swap(s_receiveQueue, emptyRec); + s_nextTrackedPacketId = 1; + s_haveUnacknowledgedTrackedPacket = false; + s_lastTrackedPacketSendTime; + s_pantoReportedInvalidData = false; + s_pantoReady = true; + s_magicReceiveIndex = 0; + s_receiveState = NONE; + s_receiveHeader = {0, 0}; +} + +void DPSerial::update() +{ + while (s_workerRunning) + { + while (processInput()) + ; + processOutput(); + } +} + +bool DPSerial::isTracked(uint8_t t) +{ + auto it = TrackedMessageTypes.find((MessageType)t); + return it != TrackedMessageTypes.end(); +} + +bool DPSerial::checkQueue(std::queue &q) +{ + if (q.empty()) + { + return false; + } + auto tracked = isTracked(q.front().header.MessageType); + return !tracked || !s_haveUnacknowledgedTrackedPacket; +} + +void DPSerial::processOutput() +{ + bool resend = false; + Packet packet(255, 0); + + // send high prio packets (sync/heartbeat) + if (checkQueue(s_highPrioSendQueue)) + { + packet = s_highPrioSendQueue.front(); + s_highPrioSendQueue.pop(); + } + // otherwise, check if panto buffer is critical + else if (!s_pantoReady) + { + return; + } + // check if the last tracked message has to be resent + else if ( + s_haveUnacknowledgedTrackedPacket && + std::chrono::steady_clock::now() - s_lastTrackedPacketSendTime > + c_trackedPacketTimeout) + { + logString("Packet timed out, resending"); + packet = s_lastTrackedPacket; + } + // otherwise, send a low prio packet + else if (checkQueue(s_lowPrioSendQueue)) + { + packet = s_lowPrioSendQueue.front(); + s_lowPrioSendQueue.pop(); + } + // no packets, nothing to do + else + { + return; + } + + if (packet.payloadIndex != packet.header.PayloadSize) + { + logString("INVALID PACKET"); + return; + } + + if (isTracked(packet.header.MessageType)) + { + if (packet.header.PacketId == 0) + { + packet.header.PacketId = s_nextTrackedPacketId++; + if (s_nextTrackedPacketId == 0) + { + s_nextTrackedPacketId++; + } + } + s_haveUnacknowledgedTrackedPacket = true; + s_lastTrackedPacket = packet; + s_lastTrackedPacketSendTime = std::chrono::steady_clock::now(); + } + + uint8_t header[c_headerSize]; + header[0] = packet.header.MessageType; + header[1] = packet.header.PacketId; + header[2] = packet.header.PayloadSize >> 8; + header[3] = packet.header.PayloadSize & 255; + + write(c_magicNumber, c_magicNumberSize); + write(header, c_headerSize); + write(packet.payload, packet.header.PayloadSize); +} + +bool DPSerial::processInput() +{ + switch (s_receiveState) + { + case NONE: + return readMagicNumber(); + case FOUND_MAGIC: + return readHeader(); + case FOUND_HEADER: + return readPayload(); + default: + return false; + } +} + +bool DPSerial::readMagicNumber() +{ + uint8_t received; + while (s_magicReceiveIndex < c_magicNumberSize && + readBytesIfAvailable(&received, 1)) + { + if (received == c_magicNumber[s_magicReceiveIndex]) + { + ++s_magicReceiveIndex; + } + else + { + std::cout << received; + s_magicReceiveIndex = 0; +#ifndef SKIP_ANALYZER + CrashAnalyzer::push_back(received); +#endif + } + } + if (s_magicReceiveIndex != c_magicNumberSize) + { + return false; + }; + s_receiveState = FOUND_MAGIC; + s_magicReceiveIndex = 0; + return true; +} + +bool DPSerial::readHeader() +{ + uint8_t received[c_headerSize]; + if (!readBytesFromSerial(received, c_headerSize)) + { + return false; + } + + s_receiveHeader.MessageType = received[0]; + s_receiveHeader.PacketId = received[1]; + s_receiveHeader.PayloadSize = received[2] << 8 | received[3]; + + // handle no-payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case BUFFER_CRITICAL: + s_pantoReady = false; + s_receiveState = NONE; + logString("Panto buffer critical"); + break; + case BUFFER_READY: + s_pantoReady = true; + s_receiveState = NONE; + logString("Panto buffer ready"); + break; + case INVALID_DATA: + s_receiveState = NONE; + s_pantoReportedInvalidData = true; + logString("Panto received invalid data"); + break; + default: + s_receiveState = FOUND_HEADER; + break; + } + return true; +} + +bool DPSerial::readPayload() +{ + const uint16_t size = s_receiveHeader.PayloadSize; + std::vector received; + received.reserve(size); + if (!readBytesFromSerial(received.data(), size)) + { + return false; + } + + auto packet = Packet(s_receiveHeader.MessageType, size); + memcpy(packet.payload, received.data(), size); + s_receiveState = NONE; + + // handle payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case PACKET_ACK: + { + auto id = packet.receiveUInt8(); + if (!s_haveUnacknowledgedTrackedPacket) + { + logString("Received unexpected PACKET_ACK"); + } + else if (id != s_lastTrackedPacket.header.PacketId) + { + std::ostringstream oss; + oss << "Received PACKET_ACK for wrong packet. Expected " + << (int)s_lastTrackedPacket.header.PacketId << ", received " + << (int)id << std::endl; + logString((char *)oss.str().c_str()); + } + else + { + s_haveUnacknowledgedTrackedPacket = false; + } + break; + } + case INVALID_PACKET_ID: + { + std::ostringstream oss; + oss << "Panto reports INVALID_PACKET_ID. Expected " + << (int)packet.receiveUInt8() << ", received " + << (int)packet.receiveUInt8() << std::endl; + logString((char *)oss.str().c_str()); + break; + } + default: + s_receiveQueue.push(packet); + break; + } + + return true; +} + +bool DPSerial::readBytesIfAvailable(void *target, uint32_t length) +{ + if (getAvailableByteCount(s_handle) < length) + { + return false; + } + return readBytesFromSerial(target, length); +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/src/serial/unix.cpp b/firmware/haptics/line wall/utils/serial/src/serial/unix.cpp new file mode 100644 index 0000000..eedd2c4 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/serial/unix.cpp @@ -0,0 +1,77 @@ +#include "serial.hpp" + +#include +#include + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + uint32_t available = 0; + if (ioctl(fileno(s_handle), FIONREAD, &available) < 0) + { + return 0; + } + return available; +} + +void DPSerial::tearDown() +{ + stopWorker(); + fclose(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + const uint32_t result = fread(target, 1, length, s_handle); + const bool valid = result == length; + if (!valid) + { + if (feof(s_handle)) + { + std::cout + << "Read end of file from serial, trying to reconnect." + << std::endl; + tearDown(); + setup(s_path); + } + else if (ferror(s_handle)) + { + perror("Error while reading from serial"); + } + } + return valid; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + ::write(fileno(s_handle), data, length); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + int fd = open(path.c_str(), O_RDWR | O_NOCTTY); + if (fd < 0) + { + return false; + } + struct termios tty; + std::memset(&tty, 0, sizeof(tty)); + if (tcgetattr(fd, &tty) < 0) + { + return false; + } + const speed_t speed = c_baudRate; + cfsetospeed(&tty, speed); + cfsetispeed(&tty, speed); + cfmakeraw(&tty); + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + if (tcsetattr(fd, TCSANOW, &tty) < 0) + { + return false; + } + s_handle = fdopen(fd, "rw"); + + startWorker(); + return true; +} diff --git a/firmware/haptics/line wall/utils/serial/src/serial/win.cpp b/firmware/haptics/line wall/utils/serial/src/serial/win.cpp new file mode 100644 index 0000000..51adc05 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/serial/win.cpp @@ -0,0 +1,71 @@ +#include "serial.hpp" + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + DWORD commerr; + COMSTAT comstat; + if (!ClearCommError(s_handle, &commerr, &comstat)) + { + return 0; + } + return comstat.cbInQue; +} + +void DPSerial::tearDown() +{ + stopWorker(); + CloseHandle(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + DWORD bytesRead; + ReadFile(s_handle, target, length, &bytesRead, NULL); + return bytesRead == length; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + DWORD bytesWritten = 0; + WriteFile(s_handle, data, length, &bytesWritten, NULL); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + s_handle = CreateFileA(path.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + if (s_handle == INVALID_HANDLE_VALUE) + { + return false; + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!GetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + dcbSerialParams.BaudRate = c_baudRate; + dcbSerialParams.ByteSize = 8; + dcbSerialParams.StopBits = ONESTOPBIT; + dcbSerialParams.Parity = NOPARITY; + if (!SetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + + COMMTIMEOUTS timeouts = {0}; + timeouts.ReadIntervalTimeout = 50; + timeouts.ReadTotalTimeoutConstant = 50; + timeouts.ReadTotalTimeoutMultiplier = 10; + timeouts.WriteTotalTimeoutConstant = 50; + timeouts.WriteTotalTimeoutMultiplier = 10; + if (!SetCommTimeouts(s_handle, &timeouts) || + !SetCommMask(s_handle, EV_RXCHAR)) + { + return false; + } + + startWorker(); + return true; +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/src/standalone/main.cpp b/firmware/haptics/line wall/utils/serial/src/standalone/main.cpp new file mode 100644 index 0000000..9e4da35 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/standalone/main.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +#include "standalone.hpp" + +int main(int argc, char** argv) +{ + if (argc != 2) + { + fprintf(stderr, "Expected /dev/serialport\n"); + return -1; + } + if (!Standalone::setup(argv[1])) + { + fprintf(stderr, "Could not open %s\n", argv[1]); + return -2; + } + signal(SIGINT, &(Standalone::terminate)); + + // cin/cout to hex mode + std::cout << std::hex << std::uppercase << std::setfill('0'); + + while (true) + { + Standalone::printPacket(); + } + + return 0; +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/src/standalone/standalone.cpp b/firmware/haptics/line wall/utils/serial/src/standalone/standalone.cpp new file mode 100644 index 0000000..d177bb4 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/src/standalone/standalone.cpp @@ -0,0 +1,27 @@ +#include +#include +#include +#include + +#include "standalone.hpp" + +void Standalone::terminate(int signal) +{ + tearDown(); + exit(0); +} + +void Standalone::printPacket() +{ + receivePacket(); + for (auto i = 0; i < c_headerSize; ++i) + { + std::cout << std::setw(2) << (int)s_headerBuffer[i] << " "; + } + std::cout << "| "; + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + std::cout << std::setw(2) << (int)s_packetBuffer[i] << " "; + } + std::cout << std::endl; +} \ No newline at end of file diff --git a/firmware/haptics/line wall/utils/serial/unity/Serial.cs b/firmware/haptics/line wall/utils/serial/unity/Serial.cs new file mode 100644 index 0000000..4936765 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/unity/Serial.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using UnityEngine; + +public class Serial : MonoBehaviour { + public delegate void SyncDelegate(ulong handle); + public delegate void HeartbeatDelegate(ulong handle); + public delegate void PositionDelegate(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions); + public delegate void LoggingDelegate(IntPtr msg); + + protected ulong Handle; + + [DllImport("serial")] + private static extern uint GetRevision(); + [DllImport("serial")] + private static extern void SetSyncHandler(SyncDelegate func); + [DllImport("serial")] + private static extern void SetHeartbeatHandler(HeartbeatDelegate func); + [DllImport("serial")] + private static extern void SetPositionHandler(PositionDelegate func); + [DllImport("serial")] + private static extern void SetLoggingHandler(LoggingDelegate func); + [DllImport("serial")] + private static extern ulong Open(IntPtr port); + [DllImport("serial")] + private static extern void Close(ulong handle); + [DllImport("serial")] + private static extern void Poll(ulong handle); + [DllImport("serial")] + private static extern void SendSyncAck(ulong handle); + [DllImport("serial")] + private static extern void SendHeartbeatAck(ulong handle); + + private static void SyncHandler(ulong handle) + { + Debug.Log("Received sync"); + SendSyncAck(handle); + } + + private static void HeartbeatHandler(ulong handle) + { + Debug.Log("Received heartbeat"); + SendHeartbeatAck(handle); + } + + private static void PositionHandler(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions) + { + Debug.Log("Received positions: (" + positions[0] + "|" + positions[1] + ")"); + //Debug.Log("Received positions"); + } + + private static void LogHandler(IntPtr msg) + { + Debug.Log(Marshal.PtrToStringAnsi(msg)); + } + + private static ulong OpenPort(string port) + { + return Open(Marshal.StringToHGlobalAnsi(port)); + } + + void Start () + { + Debug.Log("Serial protocol revision: " + GetRevision()); + SetLoggingHandler(LogHandler); + SetSyncHandler(SyncHandler); + SetHeartbeatHandler(HeartbeatHandler); + SetPositionHandler(PositionHandler); + // should be discovered automatically + Handle = OpenPort("//.//COM3"); + } + + void Update () + { + Poll(Handle); + } + + void OnDestroy() + { + Close(Handle); + } +} diff --git a/firmware/haptics/line wall/utils/serial/unity/linux.sh b/firmware/haptics/line wall/utils/serial/unity/linux.sh new file mode 100755 index 0000000..8d673ed --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/unity/linux.sh @@ -0,0 +1,3 @@ +#!/bin/sh +cmake .. -G"Unix Makefiles" -S".." -B"../cppLibBuild" -DCMAKE_BUILD_TYPE=Release +cmake --build "../cppLibBuild" --config Release diff --git a/firmware/haptics/line wall/utils/serial/unity/mac.sh b/firmware/haptics/line wall/utils/serial/unity/mac.sh new file mode 100755 index 0000000..33b47e9 --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/unity/mac.sh @@ -0,0 +1,5 @@ +SOURCE_DIR="./utils/serial" +BUILD_DIR="./utils/serial/cppLibBuild" + +cmake $SOURCE_DIR -G"Xcode" -S $SOURCE_DIR -B $BUILD_DIR +cmake --build $BUILD_DIR --config Release diff --git a/firmware/haptics/line wall/utils/serial/unity/win.bat b/firmware/haptics/line wall/utils/serial/unity/win.bat new file mode 100755 index 0000000..d18e6aa --- /dev/null +++ b/firmware/haptics/line wall/utils/serial/unity/win.bat @@ -0,0 +1,5 @@ +SET SOURCE_DIR=utils\serial +SET BUILD_DIR=utils\serial\cppLibBuild + +cmake %SOURCE_DIR% -G"Visual Studio 15 2017 Win64" -S %SOURCE_DIR% -B %BUILD_DIR% +cmake --build %BUILD_DIR% --config RelWithDebInfo \ No newline at end of file diff --git a/firmware/haptics/template/firmware/.gitignore b/firmware/haptics/template/firmware/.gitignore new file mode 100644 index 0000000..6106fab --- /dev/null +++ b/firmware/haptics/template/firmware/.gitignore @@ -0,0 +1,4 @@ +.vscode +.pio +.pioenvs +.piolibdeps diff --git a/firmware/haptics/template/firmware/.idea/.gitignore b/firmware/haptics/template/firmware/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/firmware/haptics/template/firmware/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/firmware/haptics/template/firmware/.idea/misc.xml b/firmware/haptics/template/firmware/.idea/misc.xml new file mode 100644 index 0000000..d858eb1 --- /dev/null +++ b/firmware/haptics/template/firmware/.idea/misc.xml @@ -0,0 +1,17 @@ + + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/template/firmware/.idea/vcs.xml b/firmware/haptics/template/firmware/.idea/vcs.xml new file mode 100644 index 0000000..8aefe6c --- /dev/null +++ b/firmware/haptics/template/firmware/.idea/vcs.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/firmware/haptics/template/firmware/include/config/config.hpp b/firmware/haptics/template/firmware/include/config/config.hpp new file mode 100644 index 0000000..ab0218a --- /dev/null +++ b/firmware/haptics/template/firmware/include/config/config.hpp @@ -0,0 +1,103 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is 40. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {0x8F, 0x37, 0xA2, 0x7A, 0x29, 0xDA, 0x7D, 0xB4, 0xBB, 0x54, 0x0F, 0x13, 0x43, 0x71, 0xBC, 0xE9}; +const float opMinDist = 65, + opMaxDist = 210, + opAngle = 2.2; +extern float forceFactor; +const uint8_t pantoCount = 2; +const uint8_t dummyPin = 40; +#define LINKAGE_ENCODER_USE_SPI +const uint32_t numberOfSpiEncoders = 4; +const float linkageBaseX[] = { + -23.7, 61.1, 0, -61.1, 23.7, 0 +}; +const float linkageBaseY[] = { + 0, 0, 0, 0, 0, 0 +}; +const float linkageInnerLength[] = { + 95.07, 57.71, 0, 57.71, 95.07, 0 +}; +const float linkageOuterLength[] = { + 119.79, 119.79, 0, 119.79, 119.79, 0 +}; +const uint8_t linkageHandleMount[] = { + 0, 0, 1, 0, 0, 0 +}; +const float motorPowerLimit[] = { + 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 +}; +const float motor_powerLimitForce[] = { + 0.6, 0.6, 0.2, 0.6, 0.6, 0.2 +}; +extern float pidFactor[6][3]; +const float forceP = 0.375; +const float forceI = 0; +const float forceD = 0; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + 40, 40, 25, 40, 40, 23 +}; +const uint8_t motorPwmPinForwards[] = { + 16, 2, 40, 22, 19, 40 +}; +const uint8_t motorPwmPinBackwards[] = { + 4, 17, 40, 21, 18, 40 +}; +const uint8_t motorDirAPin[] = { + 40, 40, 32, 40, 40, 26 +}; +const uint8_t motorDirBPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const bool motorFlipped[] = { + false, false, true, false, false, true +}; +const uint8_t encoderAPin[] = { + 40, 40, 35, 40, 40, 36 +}; +const uint8_t encoderBPin[] = { + 40, 40, 34, 40, 40, 39 +}; +const uint8_t encoderIndexPin[] = { + 40, 40, 40, 40, 40, 40 +}; +const uint32_t encoderSteps[] = { + 16384, 16384, 260, 16384, 16384, 260 +}; +const uint32_t encoderSpiIndex[] = { + 3, 2, 4294967295, 0, 1, 4294967295 +}; +const float encoderFlipped[] = { + 1, 1, 1, -1, -1, -1 +}; +const float setupAngle[] = { + -0.5, 0.001, 0, -0.5, 0.001, 0 +}; +constexpr float rangeMinX = -180; +constexpr float rangeMinY = -205; +constexpr float rangeMaxX = 180; +constexpr float rangeMaxY = 5; +constexpr uint32_t hashtableMaxMemory = 100000; +constexpr uint32_t hashtableUsedMemory = 98532; +constexpr uint32_t hashtableMaxCells = 8333; +constexpr uint32_t hashtableNumCells = 8211; +constexpr uint32_t hashtableStepsX = 119; +constexpr uint32_t hashtableStepsY = 69; +constexpr double hashtableStepSizeX = 3.0252100840336134; +constexpr double hashtableStepSizeY = 3.0434782608695654; +const uint32_t obstacleChangesPerFrame = 1; \ No newline at end of file diff --git a/firmware/haptics/template/firmware/include/hardware/angleAccessor.hpp b/firmware/haptics/template/firmware/include/hardware/angleAccessor.hpp new file mode 100644 index 0000000..fc7e636 --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/angleAccessor.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function AngleAccessor; diff --git a/firmware/haptics/template/firmware/include/hardware/panto.hpp b/firmware/haptics/template/firmware/include/hardware/panto.hpp new file mode 100644 index 0000000..143d308 --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/panto.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include + +#include "config/config.hpp" +#include "hardware/angleAccessor.hpp" +#include "utils/vector.hpp" +#include + +// make sure results are in range -270° ~ 0° ~ +90° +#define ensureAngleRange(angle) \ + angle > HALF_PI ? \ + angle - TWO_PI : \ + angle < -(PI + HALF_PI) ? \ + angle + TWO_PI : \ + angle + +class Panto +{ +private: + static const uint16_t c_ledcFrequency = 20000; + static const uint16_t c_ledcResolution = 12; + static const uint16_t PWM_MAX = 4095; // (2^12)-1 + static const uint8_t c_dofCount = 3; + const uint8_t c_localLeftIndex = 0; + const uint8_t c_localRightIndex = 1; + const uint8_t c_localHandleIndex = 2; + + const uint8_t c_pantoIndex; + const uint8_t c_globalIndexOffset; + const uint8_t c_globalLeftIndex; + const uint8_t c_globalRightIndex; + const uint8_t c_globalHandleIndex; + const float c_leftInnerLength; + const float c_rightInnerLength; + const float c_leftOuterLength; + const float c_rightOuterLength; + const float c_leftInnerLengthDoubled; + const float c_rightInnerLengthDoubled; + const float c_leftInnerLengthSquared; + const float c_rightInnerLengthSquared; + const float c_leftOuterLengthSquared; + const float c_rightOuterLengthSquared; + const float c_leftInnerLengthSquaredMinusLeftOuterLengthSquared; + const float c_rightInnerLengthSquaredMinusRightOuterLengthSquared; + const float c_leftOuterLengthSquaredMinusRightOuterLengthSquared; + const bool c_handleMountedOnRightArm; + const float c_leftBaseX; + const float c_leftBaseY; + const float c_rightBaseX; + const float c_rightBaseY; + + #ifdef LINKAGE_ENCODER_USE_SPI + AngleAccessor m_angleAccessors[2]; + #endif + Encoder* m_encoder[c_dofCount]; + float m_actuationAngle[c_dofCount]; + float m_previousAngle[c_dofCount]; + float m_previousAngles[c_dofCount][5]; + float m_targetAngle[c_dofCount]; + float m_previousDiff[c_dofCount]; + float m_integral[c_dofCount]; + uint32_t m_prevTime = 0; + + int m_previousAnglesCount = 0; + int m_encoderErrorCount = 0; + int m_encoderErrorCounts[4] = {0,0,0,0}; + int m_encoderRequestCount = 0; + int m_encoderRequestCounts[4] = {0,0,0,0}; + float m_leftInnerAngle = 0; + float m_rightInnerAngle = 0; + float m_pointingAngle = 0; + float m_handleX = 0; + float m_handleY = 0; + float m_targetX = 0; + float m_targetY = 0; + float m_startX = 0; + float m_startY = 0; + float m_filteredX = 0; + float m_filteredY = 0; + float m_tweeningValue = 0.0f; + float m_tweeningStep = 0.00001f; + float m_tweeningSpeed = 1.0f; + uint32_t m_tweeningPrevtime = 0; + float m_dt = 0.0001f; + float delta = 0.01; + float velocity = 1.0f; + bool m_isforceRendering = false; + bool m_inTransition = false; + float m_jacobian[2][2] = {{0.0, 0.0}, {0.0, 0.0}}; + bool m_isFrozen = false; + bool m_isCalibrating = false; + + void inverseKinematics(); + void setMotor( + const uint8_t& localIndex, const bool& dir, const float& power); + void disengageMotors(); +public: + Panto(uint8_t pantoIndex); + float getActuationAngle(const uint8_t index) const; + Vector2D getPosition() const; + float getRotation() const; + void setAngleAccessor(const uint8_t index, const AngleAccessor accessor); + void setTarget(const Vector2D target, const bool isForceRendering); + void setSpeed(const float speed); + void setRotation(const float rotation); + bool getCalibrationState(); + void calibrateEncoders(); + void calibratePanto(); + void calibrationEnd(); + void resetActuationAngle(); + void readEncoders(); + void forwardKinematics(); + void actuateMotors(); + int getEncoderErrorCount(); + int getEncoderRequests(); + int getEncoderErrorCounts(int i); + int getEncoderRequestsCounts(int i); + uint8_t getPantoIndex(); + bool getInTransition(); + void setInTransition(bool inTransition); + bool getIsFrozen(); + void setIsFrozen(bool isFrozen); +}; + +extern std::vector pantos; \ No newline at end of file diff --git a/firmware/haptics/template/firmware/include/hardware/spiCommands.hpp b/firmware/haptics/template/firmware/include/hardware/spiCommands.hpp new file mode 100644 index 0000000..4f80dbe --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/spiCommands.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +namespace SPICommands +{ + extern const uint16_t c_readAngle; + extern const uint16_t c_clearError; + extern const uint16_t c_nop; + extern const uint16_t c_highZeroRead; + extern const uint16_t c_lowZeroRead; + extern const uint16_t c_highZeroWrite; + extern const uint16_t c_lowZeroWrite; +}; diff --git a/firmware/haptics/template/firmware/include/hardware/spiEncoder.hpp b/firmware/haptics/template/firmware/include/hardware/spiEncoder.hpp new file mode 100644 index 0000000..21e9575 --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/spiEncoder.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +#include "hardware/spiPacket.hpp" + +class SPIEncoder +{ + friend class SPIEncoderChain; +private: + SPIPacket m_lastPacket; + uint16_t m_lastValidAngle; + SPIClass* m_spi; + SPIEncoder() = default; + SPIEncoder(SPIClass* spi); + void transfer(uint16_t transmisson); +public: + uint32_t getAngle(); +}; diff --git a/firmware/haptics/template/firmware/include/hardware/spiEncoderChain.hpp b/firmware/haptics/template/firmware/include/hardware/spiEncoderChain.hpp new file mode 100644 index 0000000..0b5bb86 --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/spiEncoderChain.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include "hardware/angleAccessor.hpp" +#include "hardware/spiEncoder.hpp" + +class SPIEncoderChain +{ +private: + SPISettings m_settings; + SPIClass m_spi; + uint32_t m_numberOfEncoders; + std::vector m_encoders; + std::vector m_values; + std::vector m_zeros; + uint32_t errors = 0; + uint32_t requests = 0; + uint8_t m_maxTries = 4; + uint8_t m_currentTry = 0; + // static const uint32_t c_hspiSsPin = 15; + + static const uint32_t c_hspiSsPin1 = 15; + static const uint32_t c_hspiSsPin2 = 5; + static const uint16_t c_nop = 0x0; + static const uint16_t c_clearError = 0x4001; + static const uint16_t c_readAngle = 0xFFFF; + static const uint16_t c_dataMask = 0x3FFF; + + void begin(); + void end(); + std::vector getZero(); + void transfer(uint16_t transmission); + void setZero(std::vector newZero); +public: + SPIEncoderChain(uint32_t numberOfEncoders); + void update(int channel); + void update(); + void clearError(); + void setZero(); + bool needsZero(); + void wakeUp(); + void __setZeros(); + void setPosition(std::vector positions); + uint32_t getErrors(); + uint32_t getRequests(); + void resetErrors(); + AngleAccessor getAngleAccessor(uint32_t index); +}; \ No newline at end of file diff --git a/firmware/haptics/template/firmware/include/hardware/spiPacket.hpp b/firmware/haptics/template/firmware/include/hardware/spiPacket.hpp new file mode 100644 index 0000000..73750d6 --- /dev/null +++ b/firmware/haptics/template/firmware/include/hardware/spiPacket.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +struct SPIPacket +{ + bool m_parity; + bool m_flag; + uint16_t m_data; + bool m_valid; + uint16_t m_transmission; + static const uint16_t c_parityMask = 0x8000; + static const uint16_t c_flagMask = 0x4000; + static const uint16_t c_dataMask = 0x3FFF; + SPIPacket(bool flag, uint16_t data); + SPIPacket(uint16_t transmisson); + SPIPacket() = default; + bool calcParity(); + bool parityError(); + bool commandInvalidError(); + bool framingError(); +}; diff --git a/firmware/haptics/template/firmware/include/ioMain.hpp b/firmware/haptics/template/firmware/include/ioMain.hpp new file mode 100644 index 0000000..1641622 --- /dev/null +++ b/firmware/haptics/template/firmware/include/ioMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void ioSetup(); +void ioLoop(); diff --git a/firmware/haptics/template/firmware/include/physics/annotatedEdge.hpp b/firmware/haptics/template/firmware/include/physics/annotatedEdge.hpp new file mode 100644 index 0000000..d2c8370 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/annotatedEdge.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +struct Edge; +struct IndexedEdge; +class Obstacle; + +struct AnnotatedEdge +{ + IndexedEdge* m_indexedEdge; + Edge* m_edge; + AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge); + AnnotatedEdge(const AnnotatedEdge& other); + void destroy(); +}; diff --git a/firmware/haptics/template/firmware/include/physics/collider.hpp b/firmware/haptics/template/firmware/include/physics/collider.hpp new file mode 100644 index 0000000..5d78845 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/collider.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" +#include "utils/vector.hpp" + +class Collider +{ +protected: + std::vector m_points; +public: + Collider(std::vector points); + void add(std::vector points); + virtual bool contains(Vector2D point); + Edge getEdge(uint32_t index); +}; diff --git a/firmware/haptics/template/firmware/include/physics/edge.hpp b/firmware/haptics/template/firmware/include/physics/edge.hpp new file mode 100644 index 0000000..35b00ff --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/edge.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include "utils/vector.hpp" + +struct Edge +{ + Vector2D m_first; + Vector2D m_second; + Edge(Vector2D first, Vector2D second) : m_first(first), m_second(second) {}; + Edge() : m_first(Vector2D()), m_second(Vector2D()) {}; +}; diff --git a/firmware/haptics/template/firmware/include/physics/godObject.hpp b/firmware/haptics/template/firmware/include/physics/godObject.hpp new file mode 100644 index 0000000..919353c --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/godObject.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include +#include + +#include "hardware/panto.hpp" +#include "physics/godObjectAction.hpp" +#include "physics/indexedEdge.hpp" +#include "physics/obstacle.hpp" +#include "physics/rail.hpp" +#include "physics/hashtable.hpp" +#include "utils/vector.hpp" + +/* +The speed control is in detail described in our Dropbox (Interaction Techniques Haptic in Unity): +https://www.dropbox.com/scl/fi/uljoe140fet2b53bjhr4y/DualPanto-Speed-Control.pptx?dl=0&rlkey=6k77wrfnb3oaxg186489tpinj +*/ + +enum TetherState {Inner, Active, Outer}; +/* the handle can be in 3 states: + 1. Inner: the handle can freely move within a close radius around the god object without triggering speed control (otherwise there would always be force rendered, even if the player was just standing around). + 2. Active: the god object is under speed control. Its acceleration is proportional to the distance between god object and handle. + 3. Outer: out of outer tether radius. Multiple strategies to treat this scenario are described below. +*/ + +enum OutOfTetherStrategy {MaxSpeed, Exploration, Leash}; +/* possible strategies when the handle is pushed out of the outer tether radius. + MaxSpeed: the god object continues to move at max speed and the handle gets pushed back to the new god object position player. On the Unity side there could be an additional penalization (with auditory cues), e.g. that the player loses health points when moving out of the tether (in FPS). + Exploration: the god object position doesn't update anymore and the game is paused (speed control is disabled). The handle can still collide with obstacles and is in exploration mode. + Leash: the handle is "on leash" and can collide with obstacles. The god object is moving at max speed to the position of the handle (along the direct vector between both points). A weak constant pulling force indicates where the god object is. +*/ + +class GodObject +{ +private: + static constexpr double c_resolveDistance = 0.00001; + static constexpr double c_tetherForcePullingBack = -0.5; + static constexpr double c_tetherPockDistance = 500; // when tethered and collision with wall happens push the handle this far into the wall (along the direction of movement) + static constexpr double c_railsTetherFactor = 0.75; // if speed control is enabled and the god object collides with a wall: move the god object by this factor along the vector between handle and god object. This enables jumping rails under speed control. + + Vector2D m_position; + Vector2D m_tetherPosition; + Vector2D m_movementDirection; + Vector2D m_activeForce; + std::map m_obstacles; + Hashtable *m_hashtable = nullptr; + portMUX_TYPE m_obstacleMutex; + bool m_processingObstacleCollision; + u_short m_numCollisions = 0; // for speed control; when the speed is controlled and a collision with multiple walls occurs (in a corner) then the 2nd collision must also be feelable + bool m_doneColliding; + Vector2D m_lastError; + std::set* m_possibleCollisions; + std::deque m_actionQueue; + + // tether related properties + bool m_tethered = false; + double m_tetherFactor = 0; + Vector2D m_lastErrorTether; + double m_tetherInnerRadius = 0; + double m_tetherOuterRadius = 0; + TetherState m_tetherState = Inner; + double m_tetherSafeZonePadding = 0; // padding on the inner border to avoid that the tether gets pushed into the free moving zone immediately once the inner radius is passed + OutOfTetherStrategy m_tetherStrategy = Leash; + bool m_tetherPockEnabled = false; + + Hashtable& hashtable(); + +public: + GodObject(Vector2D position = Vector2D()); + ~GodObject(); + void setMovementDirection(Vector2D movementDirection); + void update(); + void dumpHashtable(); + bool move(bool isTweening, bool isFrozen); + Vector2D checkCollisions(Vector2D targetPoint, Vector2D currentPosition); + void createObstacle(uint16_t id, std::vector points, bool passable); + void createRail(uint16_t id, std::vector points, double displacement); + void addToObstacle(uint16_t id, std::vector points); + void removeObstacle(uint16_t id); + void enableObstacle(uint16_t id, bool enable = true); + Vector2D getPosition(); + Vector2D getActiveForce(); + Vector2D getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition); + Vector2D getTetherForce(Vector2D error); + void renderForce(Vector2D collisionForce, Vector2D tetherForce); + bool processTetheringForce(Vector2D handlePosition, bool lastCollisionState); + bool getProcessingObstacleCollision(); + bool getDoneColliding(); + bool tethered(); + void setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled); +}; diff --git a/firmware/haptics/template/firmware/include/physics/godObjectAction.hpp b/firmware/haptics/template/firmware/include/physics/godObjectAction.hpp new file mode 100644 index 0000000..97c9c5d --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/godObjectAction.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "physics/annotatedEdge.hpp" +#include "physics/godObjectActionData.hpp" +#include "physics/godObjectActionType.hpp" + +struct GodObjectAction +{ + GodObjectActionType m_type; + GodObjectActionData m_data; + GodObjectAction( + GodObjectActionType type, + AnnotatedEdge* data) + : m_type(type) + , m_data{.m_annotatedEdge = data} { }; + GodObjectAction( + GodObjectActionType type, + uint16_t data) + : m_type(type) + // Intellisense might show an error here, but that's a bug. + // https://github.com/Microsoft/vscode-cpptools/issues/3491 + , m_data{.m_obstacleId = data} { }; +}; diff --git a/firmware/haptics/template/firmware/include/physics/godObjectActionData.hpp b/firmware/haptics/template/firmware/include/physics/godObjectActionData.hpp new file mode 100644 index 0000000..1a23cac --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/godObjectActionData.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include + +#include "physics/annotatedEdge.hpp" +#include "physics/obstacle.hpp" + +union GodObjectActionData +{ + AnnotatedEdge* m_annotatedEdge; + uint16_t m_obstacleId; +}; diff --git a/firmware/haptics/template/firmware/include/physics/godObjectActionType.hpp b/firmware/haptics/template/firmware/include/physics/godObjectActionType.hpp new file mode 100644 index 0000000..9b31a78 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/godObjectActionType.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum GodObjectActionType +{ + HT_ENABLE_EDGE, + HT_DISABLE_EDGE, + GO_REMOVE_OBSTACLE +}; diff --git a/firmware/haptics/template/firmware/include/physics/hashtable.hpp b/firmware/haptics/template/firmware/include/physics/hashtable.hpp new file mode 100644 index 0000000..2a272d0 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/hashtable.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include "config/config.hpp" +#include "hardware/panto.hpp" +#include "physics/annotatedEdge.hpp" +#include "physics/indexedEdge.hpp" + +class Hashtable +{ +private: + static int32_t get1dIndex(double value, double min, double step); + + std::vector m_cells[hashtableNumCells]; + std::vector getCellIndices(Edge edge); + std::set expand(const std::vector& edges); +public: + Hashtable(); + void add(AnnotatedEdge* edge); + void remove(AnnotatedEdge* edge); + void getPossibleCollisions(Edge movement, std::set* result); + void print(); +}; diff --git a/firmware/haptics/template/firmware/include/physics/indexedEdge.hpp b/firmware/haptics/template/firmware/include/physics/indexedEdge.hpp new file mode 100644 index 0000000..4b20339 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/indexedEdge.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "physics/edge.hpp" + +class Obstacle; + +struct IndexedEdge +{ + Obstacle* m_obstacle; + uint32_t m_index; + IndexedEdge(Obstacle* obstacle, uint32_t index); + bool operator==(const IndexedEdge& other) const; + bool operator<(const IndexedEdge& other) const; + Edge getEdge() const; +}; diff --git a/firmware/haptics/template/firmware/include/physics/obstacle.hpp b/firmware/haptics/template/firmware/include/physics/obstacle.hpp new file mode 100644 index 0000000..1f67687 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/obstacle.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +#include "physics/indexedEdge.hpp" +#include "physics/collider.hpp" + +class Obstacle : public Collider +{ +private: + bool m_enabled = false; + +public: + bool passable = false; + Obstacle(std::vector points); + Obstacle(std::vector points, bool passable); + bool enabled(); + void enable(bool enable = true); + std::vector getIndexedEdges( + uint32_t first = 0, uint32_t last = -1); +}; diff --git a/firmware/haptics/template/firmware/include/physics/pantoPhysics.hpp b/firmware/haptics/template/firmware/include/physics/pantoPhysics.hpp new file mode 100644 index 0000000..8b5bded --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/pantoPhysics.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include "hardware/panto.hpp" +#include "physics/godObject.hpp" +#include "utils/vector.hpp" + +class PantoPhysics +{ +private: + Panto* m_panto; + Vector2D m_currentPosition; + GodObject* m_godObject; +public: + PantoPhysics(Panto* panto); + GodObject* godObject(); + void step(); +}; + +extern std::vector pantoPhysics; diff --git a/firmware/haptics/template/firmware/include/physics/rail.hpp b/firmware/haptics/template/firmware/include/physics/rail.hpp new file mode 100644 index 0000000..00f2631 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physics/rail.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "physics/obstacle.hpp" + +class Rail : public Obstacle +{ + +private: + uint32_t displacement; // orthogonal distance to the rail until where the user still feels the applied force of the rail + +public: + Rail(std::vector points, double displacement); + virtual bool contains(Vector2D point); +}; diff --git a/firmware/haptics/template/firmware/include/physicsMain.hpp b/firmware/haptics/template/firmware/include/physicsMain.hpp new file mode 100644 index 0000000..e98e807 --- /dev/null +++ b/firmware/haptics/template/firmware/include/physicsMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void physicsSetup(); +void physicsLoop(); diff --git a/firmware/haptics/template/firmware/include/tasks/task.hpp b/firmware/haptics/template/firmware/include/tasks/task.hpp new file mode 100644 index 0000000..ec2acf9 --- /dev/null +++ b/firmware/haptics/template/firmware/include/tasks/task.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "tasks/taskFunction.hpp" + +class Task +{ +private: + // default values + static const uint32_t c_defaultStackDepth = 8192; + static const uint32_t c_defaultPriority = 1; + static const uint32_t c_defaultFpsInterval = 1000; + + // task data + TaskFunction m_setupFunc; + TaskFunction m_loopFunc; + const char* m_name; + uint32_t m_stackDepth; + uint32_t m_priority; + TaskHandle_t m_handle; + int m_core; + + // task function + static void taskLoop(void* parameters); + + // fps counter data + uint32_t m_fpsInterval; + FramerateLimiter m_fpsCalcLimiter; + uint32_t m_loopCount; + static std::map s_fpsMap; + bool m_logFps; + + // fps counter funcs + inline void initFps(); + inline void checkFps(); +public: + Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core); + void run(); + void setLogFps(bool logFps = true); + TaskHandle_t getHandle(); +}; diff --git a/firmware/haptics/template/firmware/include/tasks/taskFunction.hpp b/firmware/haptics/template/firmware/include/tasks/taskFunction.hpp new file mode 100644 index 0000000..e3fb3dc --- /dev/null +++ b/firmware/haptics/template/firmware/include/tasks/taskFunction.hpp @@ -0,0 +1,3 @@ +#pragma once + +typedef void (*TaskFunction)(); diff --git a/firmware/haptics/template/firmware/include/tasks/taskRegistry.hpp b/firmware/haptics/template/firmware/include/tasks/taskRegistry.hpp new file mode 100644 index 0000000..8bcf229 --- /dev/null +++ b/firmware/haptics/template/firmware/include/tasks/taskRegistry.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include +#include + +#include "tasks/task.hpp" + +typedef std::map TaskRegistry; +extern TaskRegistry Tasks; diff --git a/firmware/haptics/template/firmware/include/utils/assert.hpp b/firmware/haptics/template/firmware/include/utils/assert.hpp new file mode 100644 index 0000000..70f3d82 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/assert.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "utils/serial.hpp" + +void __fail( + const char* check, std::string val1, std::string val2, + const char* file, int line, const char* func) +{ + DPSerial::sendInstantDebugLog( + "Assertion %s failed with values %s and %s: file \"%s\", line %i, %s", + check, val1.c_str(), val2.c_str(), file, line, func); + abort(); +} + +#define __CHECK(operator, val1, val2) (val1 operator val2) ?\ + (void)0 :\ + __fail(\ + #val1 " " #operator " " #val2,\ + std::to_string(val1), std::to_string(val2),\ + __FILE__, __LINE__, __PRETTY_FUNCTION__); + +#define ASSERT_EQ(val1, val2) __CHECK(==, val1, val2) +#define ASSERT_LT(val1, val2) __CHECK(<, val1, val2) +#define ASSERT_LE(val1, val2) __CHECK(<=, val1, val2) +#define ASSERT_GT(val1, val2) __CHECK(>, val1, val2) +#define ASSERT_GE(val1, val2) __CHECK(>=, val1, val2) +#define ASSERT_NE(val1, val2) __CHECK(!=, val1, val2) diff --git a/firmware/haptics/template/firmware/include/utils/crashDump.hpp b/firmware/haptics/template/firmware/include/utils/crashDump.hpp new file mode 100644 index 0000000..d526096 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/crashDump.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +class CrashDump +{ +private: + std::ostringstream m_stream; +public: + CrashDump(); + void clear(); + void dump(); + template CrashDump& operator<<(T value) + { + m_stream << value; + return *this; + } +}; + +#ifdef ENABLE_CRASHDUMP +#define CRASHDUMP(dump, call) dump call +#else +#define CRASHDUMP(dump, call) +#endif + +extern CrashDump physicsDump; diff --git a/firmware/haptics/template/firmware/include/utils/framerateLimiter.hpp b/firmware/haptics/template/firmware/include/utils/framerateLimiter.hpp new file mode 100644 index 0000000..94d43b2 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/framerateLimiter.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include + +class FramerateLimiter +{ +private: + static const uint64_t c_periodLength = ULONG_MAX; + static uint64_t s_period; + static uint64_t s_lastNow; + static uint64_t now(); + uint64_t m_delta; + uint64_t m_last; + FramerateLimiter(uint64_t microseconds); +public: + static FramerateLimiter fromFPS(double fps); + static FramerateLimiter fromSeconds(uint64_t seconds); + static FramerateLimiter fromMilliseconds(uint64_t milliseconds); + static FramerateLimiter fromMicroseconds(uint64_t microseconds); + bool step(); + void reset(); +}; diff --git a/firmware/haptics/template/firmware/include/utils/performanceMonitor.hpp b/firmware/haptics/template/firmware/include/utils/performanceMonitor.hpp new file mode 100644 index 0000000..eff8a4f --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/performanceMonitor.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include + +class PerformanceMonitor +{ +private: + static const uint32_t c_measurementCount = 1000; + struct PerformanceEntry + { + std::vector m_values; + uint32_t m_index = 0; + uint64_t m_sum = 0; + bool m_running = false; + uint64_t m_start = 0; + PerformanceEntry(); + }; + std::map> m_entries; +public: + void start(std::string label); + void stop(std::string label); + std::vector getResults(); +}; + +#ifdef ENABLE_PERFMON +#define PERFMON_START(label) PerfMon.start(label); +#define PERFMON_STOP(label) PerfMon.stop(label); +#else +#define PERFMON_START(label) +#define PERFMON_STOP(label) +#endif + +extern PerformanceMonitor PerfMon; diff --git a/firmware/haptics/template/firmware/include/utils/receiveHandler.hpp b/firmware/haptics/template/firmware/include/utils/receiveHandler.hpp new file mode 100644 index 0000000..3b33431 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/receiveHandler.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function ReceiveHandler; diff --git a/firmware/haptics/template/firmware/include/utils/receiveState.hpp b/firmware/haptics/template/firmware/include/utils/receiveState.hpp new file mode 100644 index 0000000..abb521f --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/receiveState.hpp @@ -0,0 +1,8 @@ +#pragma once + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; diff --git a/firmware/haptics/template/firmware/include/utils/serial.hpp b/firmware/haptics/template/firmware/include/utils/serial.hpp new file mode 100644 index 0000000..accc846 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/serial.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "utils/receiveHandler.hpp" +#include "utils/receiveState.hpp" + +class DPSerial : DPProtocol +{ +private: + // receive buffer limits + static const uint16_t c_rxBufferSize = 10 * c_maxPacketSize; + static const uint16_t c_rxBufferCriticalThreshold = 5 * c_maxPacketSize; + static const uint16_t c_rxBufferReadyThreshold = 2 * c_maxPacketSize; + static bool s_rxBufferCritical; + + // data storage + static Header s_header; + static const uint16_t c_debugLogBufferSize = 256; + static uint8_t s_debugLogBuffer[c_debugLogBufferSize]; + static std::queue s_debugLogQueue; + static const uint16_t c_processedQueuedMessagesPerFrame = 4; + + // multithreading safety + static portMUX_TYPE s_serialMutex; + + static ReceiveState s_receiveState; + static uint8_t s_expectedPacketId; + + // connection + static bool s_connected; + static const uint16_t c_heartbeatIntervalMs = 1000; + static unsigned long s_lastHeartbeatTime; + static const uint16_t c_maxUnacklowledgedHeartbeats = 5; + static uint16_t s_unacknowledgedHeartbeats; + + // send helper + static void sendUInt8(uint8_t data); + static void sendInt16(int16_t data); + static void sendUInt16(uint16_t data); + static void sendInt32(int32_t data); + static void sendUInt32(uint32_t data); + static void sendFloat(float data); + static void sendMessageType(MessageType data); + static void sendMagicNumber(); + static void sendHeader(MessageType messageType, uint16_t payloadSize); + + // send + static void sendSync(); + static void sendHeartbeat(); + static void sendBufferCritical(); + static void sendBufferReady(); + static void sendPacketAck(uint8_t id); + static void sendInvalidPacketId(uint8_t expected, uint8_t received); + static void sendInvalidData(); + + // receive helper + static uint8_t receiveUInt8(); + static int16_t receiveInt16(); + static uint16_t receiveUInt16(); + static int32_t receiveInt32(); + static uint32_t receiveUInt32(); + static float receiveFloat(); + static MessageType receiveMessageType(); + static void checkBuffer(); + static bool receiveMagicNumber(); + static bool receiveHeader(); + static bool payloadReady(); + + // receive + static void receiveSyncAck(); + static void receiveHearbeatAck(); + static void receiveMotor(); + static void receiveSpeed(); + static void receivePID(); + static void receiveCreateObstacle(); + static void receiveCreatePassableObstacle(); + static void receiveCreateRail(); + static void receiveAddToObstacle(); + static void receiveRemoveObstacle(); + static void receiveEnableObstacle(); + static void receiveDisableObstacle(); + static void receiveCalibrationRequest(); + static void receiveDumpHashtable(); + static void receiveInvalid(); + static void receiveFreeze(); + static void receiveFree(); + static void receiveSpeedControl(); + + // map of receive handlers + static std::map s_receiveHandlers; + +public: + // delete contructor - this class only contains static members + DPSerial() = delete; + + // setup + static void init(); + static bool ensureConnection(); + + // send + static void sendPosition(); + static void sendTransitionEnded(uint8_t panto); + static void sendInstantDebugLog(const char *message, ...); + static void sendQueuedDebugLog(const char *message, ...); + static void processDebugLogQueue(); + static void sendDebugData(); + + // receive + static void receive(); +}; diff --git a/firmware/haptics/template/firmware/include/utils/utils.hpp b/firmware/haptics/template/firmware/include/utils/utils.hpp new file mode 100644 index 0000000..1eaca9e --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/utils.hpp @@ -0,0 +1,7 @@ +#pragma once + +// template func must be in header to compile all necessary variants +template int sgn(T val) +{ + return (T(0) < val) - (val < T(0)); +}; diff --git a/firmware/haptics/template/firmware/include/utils/vector.hpp b/firmware/haptics/template/firmware/include/utils/vector.hpp new file mode 100644 index 0000000..02bf456 --- /dev/null +++ b/firmware/haptics/template/firmware/include/utils/vector.hpp @@ -0,0 +1,22 @@ +#pragma once + +struct Vector2D +{ + double x, y; + + Vector2D() {} + Vector2D(double _x, double _y) : x(_x), y(_y) {} + + static Vector2D fromPolar(double angle, double length); + static double determinant(const Vector2D& first, const Vector2D& second); + double length() const; + double angle() const; + Vector2D normalize() const; + Vector2D operator+(const Vector2D& other) const; + Vector2D operator-(const Vector2D& other) const; + double operator*(const Vector2D& other) const; + Vector2D operator*(const double scale) const; + bool operator==(const Vector2D& other) const; + bool operator!=(const Vector2D &other) const; + double distancePointToLineSegment(Vector2D a, Vector2D b); +}; diff --git a/firmware/haptics/template/firmware/platformio.ini b/firmware/haptics/template/firmware/platformio.ini new file mode 100644 index 0000000..3cb404a --- /dev/null +++ b/firmware/haptics/template/firmware/platformio.ini @@ -0,0 +1,32 @@ +[platformio] +description = Dualpanto firmware + +[protocol] +base_dir = ../utils/protocol +include_dir = ${protocol.base_dir}/include +src_dir = ${protocol.base_dir}/src +src_filter = +<../${protocol.src_dir}> + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +build_flags = + -I${protocol.include_dir} + -D_GLIBCXX_USE_C99 + -DENABLE_FPS + -g + -frtti + -mtext-section-literals +build_unflags = + # use this while debugging + # -Os +src_filter = + +<*> + ${protocol.src_filter} +monitor_speed = 115200 +lib_deps = https://github.com/PaulStoffregen/Encoder.git + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +upload_port = /dev/cu.SLAB_USBtoUART diff --git a/firmware/haptics/template/firmware/src/config/config.cpp b/firmware/haptics/template/firmware/src/config/config.cpp new file mode 100644 index 0000000..202b2c5 --- /dev/null +++ b/firmware/haptics/template/firmware/src/config/config.cpp @@ -0,0 +1,12 @@ +/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = 0.0085; +float pidFactor[6][3] = { + {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30}, {6, 0, 600}, {6, 0, 600}, {0.5, 0, 30} +}; \ No newline at end of file diff --git a/firmware/haptics/template/firmware/src/hardware/panto.cpp b/firmware/haptics/template/firmware/src/hardware/panto.cpp new file mode 100644 index 0000000..063b680 --- /dev/null +++ b/firmware/haptics/template/firmware/src/hardware/panto.cpp @@ -0,0 +1,618 @@ +#include "hardware/panto.hpp" + +#include + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantos; + +void Panto::forwardKinematics() +{ + // base angles + // PERFMON_START("[abba] base angles"); + const auto leftBaseAngle = m_actuationAngle[c_localLeftIndex]; + const auto rightBaseAngle = m_actuationAngle[c_localRightIndex]; + const auto handleAngle = m_actuationAngle[c_localHandleIndex]; + // PERFMON_STOP("[abba] base angles"); + + // base angle sin / cos + // PERFMON_START("[abbb] base angle sin / cos"); + const auto leftBaseAngleSin = std::sin(leftBaseAngle); + const auto leftBaseAngleCos = std::cos(leftBaseAngle); + // PERFMON_STOP("[abbb] base angle sin / cos"); + + // calculate inner positions + // PERFMON_START("[abbc] calculate inner positions"); + const auto leftInnerX = + fmaf(leftBaseAngleCos, c_leftInnerLength, c_leftBaseX); + const auto leftInnerY = + fmaf(leftBaseAngleSin, c_leftInnerLength, c_leftBaseY); + const auto rightInnerX = + fmaf(std::cos(rightBaseAngle), c_rightInnerLength, c_rightBaseX); + const auto rightInnerY = + fmaf(std::sin(rightBaseAngle), c_rightInnerLength, c_rightBaseY); + // PERFMON_STOP("[abbc] calculate inner positions"); + + // diagonal between inner positions + // PERFMON_START("[abbd] diagonal between inner positions"); + const auto diagonalX = rightInnerX - leftInnerX; + const auto diagonalY = rightInnerY - leftInnerY; + const auto diagonalSquared = diagonalX * diagonalX + diagonalY * diagonalY; + const auto diagonalLength = std::sqrt(diagonalSquared); + // PERFMON_STOP("[abbd] diagonal between inner positions"); + + // left elbow angles + // - inside is between diagonal and linkage + // - offset is between zero and diagonal + // - total is between zero and linkage + // PERFMON_START("[abbe] left elbow angles"); + const auto leftElbowInsideAngleCos = + (diagonalSquared + + c_leftOuterLengthSquaredMinusRightOuterLengthSquared) / + (2 * diagonalLength * c_leftOuterLength); + const auto leftElbowInsideAngle = -std::acos(leftElbowInsideAngleCos); + const auto leftElbowOffsetAngle = std::atan2(diagonalY, diagonalX); + const auto leftElbowTotalAngle = + leftElbowInsideAngle + leftElbowOffsetAngle; + // PERFMON_STOP("[abbe] left elbow angles"); + + // left elbow angle sin / cos + // PERFMON_START("[abbf] left elbow angle sin / cos"); + const auto leftElbowTotalAngleSin = + std::sin(leftElbowTotalAngle); + const auto leftElbowTotalAngleCos = + std::cos(leftElbowTotalAngle); + // PERFMON_STOP("[abbf] left elbow angle sin / cos"); + + // handle position + // PERFMON_START("[abbg] handle position"); + m_handleX = + fmaf(leftElbowTotalAngleCos, c_leftOuterLength, leftInnerX); + m_handleY = + fmaf(leftElbowTotalAngleSin, c_leftOuterLength, leftInnerY); + // PERFMON_STOP("[abbg] handle position"); + + // right elbow angles + // PERFMON_START("[abbh] right elbow angles"); + const auto rightDiffX = m_handleX - rightInnerX; + const auto rightDiffY = m_handleY - rightInnerY; + const auto rightElbowTotalAngle = std::atan2(rightDiffY, rightDiffX); + // PERFMON_STOP("[abbh] right elbow angles"); + + // store angles + // PERFMON_START("[abbi] store angles"); + m_leftInnerAngle = leftElbowTotalAngle; + m_rightInnerAngle = rightElbowTotalAngle; + m_pointingAngle = + handleAngle + + (encoderFlipped[c_globalHandleIndex]==1? -1 : 1)* //sign changes when encoder is flipped + (c_handleMountedOnRightArm==1 ? + (-rightElbowTotalAngle) : + (leftElbowTotalAngle)); + + // PERFMON_STOP("[abbi] store angles"); + + // some weird diffs and their sinuses + // PERFMON_START("[abbj] some weird diffs and their sinuses"); + const auto rightElbowTotalAngleMinusLeftBaseAngle = + rightElbowTotalAngle - leftBaseAngle; + const auto rightElbowTotalAngleMinusLeftBaseAngleSin = + std::sin(rightElbowTotalAngleMinusLeftBaseAngle); + const auto rightElbowTotalAngleMinusRightBaseAngle = + rightElbowTotalAngle - rightBaseAngle; + const auto rightElbowTotalAngleMinusRightBaseAngleSin = + std::sin(rightElbowTotalAngleMinusRightBaseAngle); + const auto leftElbowTotalAngleMinusRightElbowTotalAngle = + leftElbowTotalAngle - rightElbowTotalAngle; + const auto leftElbowTotalAngleMinusRightElbowTotalAngleSin = + std::sin(leftElbowTotalAngleMinusRightElbowTotalAngle); + // PERFMON_STOP("[abbj] some weird diffs and their sinuses"); + + // shared factors for rows/columns + // PERFMON_START("[abbk] shared factors for rows/columns"); + const auto upperRow = + c_leftInnerLength * rightElbowTotalAngleMinusLeftBaseAngleSin; + const auto lowerRow = + c_rightInnerLength * rightElbowTotalAngleMinusRightBaseAngleSin; + const auto leftColumn = + leftElbowTotalAngleSin / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + const auto rightColumn = + leftElbowTotalAngleCos / + leftElbowTotalAngleMinusRightElbowTotalAngleSin; + // PERFMON_STOP("[abbk] shared factors for rows/columns"); + + // set jacobian matrix + // PERFMON_START("[abbl] set jacobian matrix"); + m_jacobian[0][0] = + (-c_leftInnerLength * leftBaseAngleSin) - (upperRow * leftColumn); + m_jacobian[0][1] = + (c_leftInnerLength * leftBaseAngleCos) + (upperRow * rightColumn); + m_jacobian[1][0] = + lowerRow * leftColumn; + m_jacobian[1][1] = + -lowerRow * rightColumn; + // PERFMON_STOP("[abbl] set jacobian matrix"); + inverseKinematics(); +} + +void Panto::inverseKinematics() +{ + + //update tweening delta micro here + unsigned long now = micros(); + float tweening_dt = now - m_tweeningPrevtime; + m_tweeningPrevtime = now; + + if (isnan(m_targetX) || isnan(m_targetY)) + { + m_targetAngle[c_localLeftIndex] = NAN; + m_targetAngle[c_localRightIndex] = NAN; + } + else if (m_isforceRendering) + { + m_targetAngle[c_localLeftIndex] = + m_jacobian[0][0] * m_targetX + + m_jacobian[0][1] * m_targetY; + m_targetAngle[c_localRightIndex] = + m_jacobian[1][0] * m_targetX + + m_jacobian[1][1] * m_targetY; + } + else + { + // tweening + const auto leftBaseToTargetX = m_filteredX - c_leftBaseX; + const auto leftBaseToTargetY = m_filteredY - c_leftBaseY; + const auto rightBaseToTargetX = m_filteredX - c_rightBaseX; + const auto rightBaseToTargetY = m_filteredY - c_rightBaseY; + const auto leftBaseToTargetSquared = + leftBaseToTargetX * leftBaseToTargetX + + leftBaseToTargetY * leftBaseToTargetY; + const auto rightBaseToTargetSquared = + rightBaseToTargetX * rightBaseToTargetX + + rightBaseToTargetY * rightBaseToTargetY; + const auto leftBaseToTargetLength = + std::sqrt(leftBaseToTargetSquared); + const auto rightBaseToTargetLength = + std::sqrt(rightBaseToTargetSquared); + + const auto leftInnerAngleCos = + (leftBaseToTargetSquared + + c_leftInnerLengthSquaredMinusLeftOuterLengthSquared) / + (c_leftInnerLengthDoubled * leftBaseToTargetLength); + const auto rightInnerAngleCos = + (rightBaseToTargetSquared + + c_rightInnerLengthSquaredMinusRightOuterLengthSquared) / + (c_rightInnerLengthDoubled * rightBaseToTargetLength); + const auto leftInnerAngle = std::acos(leftInnerAngleCos); + const auto rightInnerAngle = std::acos(rightInnerAngleCos); + const auto leftOffsetAngle = + std::atan2(leftBaseToTargetY, leftBaseToTargetX); + const auto rightOffsetAngle = + std::atan2(rightBaseToTargetY, rightBaseToTargetX); + + const auto leftAngle = leftOffsetAngle - leftInnerAngle; + const auto rightAngle = rightOffsetAngle + rightInnerAngle; + + m_targetAngle[c_localLeftIndex] = ensureAngleRange(leftAngle); + m_targetAngle[c_localRightIndex] = ensureAngleRange(rightAngle); + + if(abs(m_filteredX - m_targetX) + abs(m_filteredY - m_targetY) < 0.01f && m_inTransition){ + m_inTransition = false; + DPSerial::sendTransitionEnded(getPantoIndex()); + } + + m_filteredX = (m_targetX-m_startX)*m_tweeningValue+m_startX; + m_filteredY = (m_targetY-m_startY)*m_tweeningValue+m_startY; + float stepValue = 0.000001 * tweening_dt * m_tweeningSpeed; + m_tweeningValue=min(m_tweeningValue+stepValue, 1.0f); + + } +}; + +void Panto::setMotor( + const uint8_t& localIndex, const bool& dir, const float& power) +{ + const auto globalIndex = c_globalIndexOffset + localIndex; + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] == dummyPin) + { + return; + } + + const auto flippedDir = dir ^ motorFlipped[globalIndex]; + + if(motorPwmPinForwards[globalIndex] != dummyPin) + { + if(!flippedDir) { + ledcWrite(globalIndex+6, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + else { + ledcWrite(globalIndex, 0);//min(power, motorPowerLimit[globalIndex]) * PWM_MAX); + ledcWrite(globalIndex+6, min(power, + (m_isforceRendering) ? motor_powerLimitForce[globalIndex] : motorPowerLimit[globalIndex]) * PWM_MAX); + } + return; + } + + + digitalWrite(motorDirAPin[globalIndex], flippedDir); + digitalWrite(motorDirBPin[globalIndex], !flippedDir); + ledcWrite(globalIndex, min(power, motorPowerLimit[globalIndex]) * PWM_MAX); +}; + +void Panto::readEncoders() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_previousAngle[localIndex] = + ensureAngleRange( + encoderFlipped[globalIndex] * + TWO_PI * m_angleAccessors[localIndex]() / + encoderSteps[globalIndex]); + m_encoderRequestCount++; + m_encoderRequestCounts[localIndex]++; + } + m_actuationAngle[c_localHandleIndex] = + (m_encoder[c_localHandleIndex]) ? + (encoderFlipped[c_globalHandleIndex] * + TWO_PI * m_encoder[c_localHandleIndex]->read() / + encoderSteps[c_globalHandleIndex]) : + NAN; + #else + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = + ensureAngleRange( + (m_encoder[localIndex]) ? + (encoderFlipped[globalIndex] * + TWO_PI * m_encoder[localIndex]->read() / + encoderSteps[globalIndex]) : + NAN); + } + #endif + + m_previousAngle[c_localHandleIndex] = m_actuationAngle[c_localHandleIndex]; + m_actuationAngle[c_localHandleIndex] = fmod(m_actuationAngle[c_localHandleIndex], TWO_PI); + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + if(m_previousAngle[localIndex]==0)return; + } + if(m_previousAnglesCount>4){ + m_previousAnglesCount=0; + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + float std = 0.0f; + float mean = 0.0f; + for(int i = 0; i < 5; i++){ + mean+=m_previousAngles[localIndex][i]; + }mean/=5.0f; + for(int i = 0; i < 5; i++){ + std+=(m_previousAngles[localIndex][i]-mean)*(m_previousAngles[localIndex][i]-mean); + }std /=5.0f; + if(std < 1.0f){ + m_actuationAngle[localIndex] = m_previousAngles[localIndex][4]; + } + else{ + m_encoderErrorCounts[localIndex]++; + // DPSerial::sendQueuedDebugLog("jumps at [panto %d][motor %d] (std>1.0f) mean = %f",c_pantoIndex, localIndex, mean); + // for(int i = 0; i < 5; i++){ + // DPSerial::sendQueuedDebugLog("previousAngles[%d][%d]=%f",localIndex, i, m_previousAngles[localIndex][i]); + // } + // m_actuationAngle[localIndex] = m_previousAngle[localIndex]; + } + } + } + else{ + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + m_previousAngles[localIndex][m_previousAnglesCount] = m_previousAngle[localIndex]; + } + } + m_previousAnglesCount++; +}; + +void Panto::actuateMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + if (isnan(m_targetAngle[localIndex])) + { + // free motor + setMotor(localIndex, false, 0); + } else if (m_isforceRendering) + { + setMotor( + localIndex, + m_targetAngle[localIndex] < 0, + fabs(m_targetAngle[localIndex]) * forceFactor); + } else + { + auto error = + m_targetAngle[localIndex] - m_actuationAngle[localIndex]; + if (localIndex == c_localHandleIndex) + { + // Linkage offsets handle + error -= + c_handleMountedOnRightArm ? + m_rightInnerAngle : + m_leftInnerAngle; + if (error > PI) + { + error -= TWO_PI; + } + else if (error < -PI) + { + error += TWO_PI; + } + if(encoderFlipped[c_globalHandleIndex]==1) error*=-1; + } + unsigned char dir = error < 0; + unsigned long now = micros(); + float dt = now - m_prevTime; + m_prevTime = now; + error = fabs(error); + // Power: PID + m_integral[localIndex] = min(0.5f, m_integral[localIndex] + error * dt); + float derivative = (error - m_previousDiff[localIndex]) / dt; + m_previousDiff[localIndex] = error; + const auto globalIndex = c_globalIndexOffset + localIndex; + const auto& pid = pidFactor[globalIndex]; + float pVal = pid[0] * error; + float dVal = pid[2] * derivative; + dVal = pVal + dVal > 0 ? dVal : 0; + setMotor( + localIndex, + dir, + pVal + + pid[1] * m_integral[localIndex] + + dVal); + } + } +}; + +void Panto::disengageMotors() +{ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + m_targetAngle[localIndex] = NAN; + setMotor(localIndex, false, 0); + } + m_targetX = NAN; + m_targetY = NAN; +}; + +Panto::Panto(uint8_t pantoIndex) +: c_pantoIndex(pantoIndex) +, c_globalIndexOffset(c_pantoIndex * c_dofCount) +, c_globalLeftIndex(c_globalIndexOffset + c_localLeftIndex) +, c_globalRightIndex(c_globalIndexOffset + c_localRightIndex) +, c_globalHandleIndex(c_globalIndexOffset + c_localHandleIndex) +, c_leftInnerLength(linkageInnerLength[c_globalLeftIndex]) +, c_rightInnerLength(linkageInnerLength[c_globalRightIndex]) +, c_leftOuterLength(linkageOuterLength[c_globalLeftIndex]) +, c_rightOuterLength(linkageOuterLength[c_globalRightIndex]) +, c_leftInnerLengthDoubled(2 * c_leftInnerLength) +, c_rightInnerLengthDoubled(2 * c_rightInnerLength) +, c_leftInnerLengthSquared(c_leftInnerLength * c_leftInnerLength) +, c_rightInnerLengthSquared(c_rightInnerLength * c_rightInnerLength) +, c_leftOuterLengthSquared(c_leftOuterLength * c_leftOuterLength) +, c_rightOuterLengthSquared(c_rightOuterLength * c_rightOuterLength) +, c_leftInnerLengthSquaredMinusLeftOuterLengthSquared( + c_leftInnerLengthSquared - c_leftOuterLengthSquared) +, c_rightInnerLengthSquaredMinusRightOuterLengthSquared( + c_rightInnerLengthSquared - c_rightOuterLengthSquared) +, c_leftOuterLengthSquaredMinusRightOuterLengthSquared( + c_leftOuterLengthSquared - c_rightOuterLengthSquared) +, c_handleMountedOnRightArm(linkageHandleMount[c_globalHandleIndex] == 1) +, c_leftBaseX(linkageBaseX[c_globalLeftIndex]) +, c_leftBaseY(linkageBaseY[c_globalLeftIndex]) +, c_rightBaseX(linkageBaseX[c_globalRightIndex]) +, c_rightBaseY(linkageBaseY[c_globalRightIndex]) +{ + m_targetX = NAN; + m_targetY = NAN; + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + m_targetAngle[localIndex] = NAN; + m_previousDiff[localIndex] = 0.0; + m_integral[localIndex] = 0.0; + if (encoderAPin[globalIndex] != dummyPin && + encoderBPin[globalIndex] != dummyPin) + { + m_encoder[localIndex] = new Encoder( + encoderAPin[globalIndex], encoderBPin[globalIndex]); + } + else + { + m_encoder[localIndex] = NULL; + } + + // we don't need additional checks aroud these - if the dummyPin is set properly, the ESP lib will check this anyway + pinMode(encoderIndexPin[globalIndex], INPUT); + pinMode(motorDirAPin[globalIndex], OUTPUT); + pinMode(motorDirBPin[globalIndex], OUTPUT); + + if(motorPwmPinForwards[globalIndex] == dummyPin) { + pinMode(motorPwmPin[globalIndex], OUTPUT); + + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcAttachPin(motorPwmPin[globalIndex], globalIndex); + } + + if(motorPwmPin[globalIndex] == dummyPin && motorPwmPinForwards[globalIndex] != dummyPin) { + pinMode(motorPwmPinForwards[globalIndex], OUTPUT); + pinMode(motorPwmPinBackwards[globalIndex], OUTPUT); + + // TODO: initiate the PWM channels independent from globalIndex + ledcSetup(globalIndex, c_ledcFrequency, c_ledcResolution); + ledcSetup(globalIndex+6, c_ledcFrequency, c_ledcResolution); + + //DPSerial::sendInstantDebugLog("attaching gi %i to pwm %i and pwm %i\n", globalIndex, motorPwmPinForwards[globalIndex], motorPwmPinBackwards[globalIndex]); + + ledcAttachPin(motorPwmPinForwards[globalIndex], globalIndex); + ledcAttachPin(motorPwmPinBackwards[globalIndex], globalIndex+6); + + ledcWrite(globalIndex, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex, 0); + delay(10); + ledcWrite(globalIndex+6, 0.1*PWM_MAX); + delay(10); + ledcWrite(globalIndex+6, 0); + /* + ledcWrite(globalIndex, 0.2*PWM_MAX); + delay(2000); + ledcWrite(globalIndex, 0); + */ + + } + // TODO: Calibration + // Use encoder index pin and actuate the motors to reach it + setMotor(localIndex, false, 0); + } +}; + +void Panto::calibrateEncoders(){ + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto localIndex = 0; localIndex < c_dofCount - 1; ++localIndex) + { + //Write encoder values to EEPROM + EEPROM.writeInt((3*c_pantoIndex*sizeof(uint32_t)+localIndex*sizeof(uint32_t)),m_angleAccessors[localIndex]()); + } + #endif +} + +void Panto::resetActuationAngle(){ + for (auto localIndex = 0; localIndex < c_dofCount; ++localIndex){ + const auto globalIndex = c_globalIndexOffset + localIndex; + m_actuationAngle[localIndex] = setupAngle[globalIndex] * TWO_PI; + } +} + +bool Panto::getCalibrationState(){ + return m_isCalibrating; +} + +void Panto::calibratePanto(){ + m_isCalibrating = true; +} + +void Panto::calibrationEnd() +{ + for (auto localIndex = 0; localIndex < 3; ++localIndex) + { + if (m_encoder[localIndex]) + { + const auto globalIndex = c_globalIndexOffset + localIndex; + m_encoder[localIndex]->write( + m_actuationAngle[localIndex] / + (TWO_PI) * + encoderSteps[globalIndex]); + } + } + resetActuationAngle(); + m_isCalibrating = false; +}; + +float Panto::getActuationAngle(const uint8_t localIndex) const +{ + return m_actuationAngle[localIndex]; +}; + +Vector2D Panto::getPosition() const +{ + return Vector2D(m_handleX, m_handleY); +}; + +float Panto::getRotation() const +{ + return m_pointingAngle; +}; + +void Panto::setAngleAccessor( + const uint8_t localIndex, + const AngleAccessor accessor) +{ + m_angleAccessors[localIndex] = accessor; +}; + +void Panto::setTarget(const Vector2D target, const bool isForceRendering) +{ + m_isforceRendering = isForceRendering; + m_targetX = target.x; + m_targetY = target.y; + m_startX = m_handleX; + m_startY = m_handleY; + m_filteredX = m_startX; + m_filteredY = m_startY; + m_tweeningValue = 0.0f; + + float dx = (m_targetX - m_startX); + float dy = (m_targetY - m_startY); + float d = max((float)sqrt(dx*dx + dy*dy), 1.0f); //distance to target: avoiding 0 division + + const float velocity = 0.001 * m_tweeningSpeed; //[mm / s] maybe? + + m_tweeningStep = velocity / d; + inverseKinematics(); +}; + +void Panto::setSpeed(const float _speed){ + m_tweeningSpeed = _speed; +} + +void Panto::setRotation(const float rotation) +{ + m_targetAngle[c_localHandleIndex] = rotation; +}; + +int Panto::getEncoderErrorCount(){ + int res= m_encoderErrorCount; + m_encoderErrorCount =0; + return res; +} + +int Panto::getEncoderErrorCounts(int i){ + int res= m_encoderErrorCounts[i]; + m_encoderErrorCounts[i] =0; + return res; +} +int Panto::getEncoderRequests(){ + int res= m_encoderRequestCount; + m_encoderRequestCount =0; + return res; +} + +int Panto::getEncoderRequestsCounts(int i){ + int res= m_encoderRequestCounts[i]; + m_encoderRequestCounts[i] =0; + return res; +} + +uint8_t Panto::getPantoIndex(){ + return c_pantoIndex; +} + +void Panto::setInTransition(bool inTransition){ + m_inTransition = inTransition; +} + +bool Panto::getInTransition(){ + return m_inTransition; +} + +bool Panto::getIsFrozen(){ + return m_isFrozen; +} +void Panto::setIsFrozen(bool isFrozen){ + m_isFrozen = isFrozen; +} diff --git a/firmware/haptics/template/firmware/src/hardware/spiCommands.cpp b/firmware/haptics/template/firmware/src/hardware/spiCommands.cpp new file mode 100644 index 0000000..b2093cd --- /dev/null +++ b/firmware/haptics/template/firmware/src/hardware/spiCommands.cpp @@ -0,0 +1,11 @@ +#include "hardware/spiCommands.hpp" + +#include "hardware/spiPacket.hpp" + +const uint16_t SPICommands::c_readAngle = SPIPacket(1, 0x3FFF).m_transmission; +const uint16_t SPICommands::c_clearError = SPIPacket(1, 0x1).m_transmission; +const uint16_t SPICommands::c_nop = SPIPacket(0, 0x0).m_transmission; +const uint16_t SPICommands::c_highZeroRead = SPIPacket(1, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroRead = SPIPacket(1, 0x17).m_transmission; +const uint16_t SPICommands::c_highZeroWrite = SPIPacket(0, 0x16).m_transmission; +const uint16_t SPICommands::c_lowZeroWrite = SPIPacket(0, 0x17).m_transmission; diff --git a/firmware/haptics/template/firmware/src/hardware/spiEncoder.cpp b/firmware/haptics/template/firmware/src/hardware/spiEncoder.cpp new file mode 100644 index 0000000..6a73ea7 --- /dev/null +++ b/firmware/haptics/template/firmware/src/hardware/spiEncoder.cpp @@ -0,0 +1,18 @@ +#include + +SPIEncoder::SPIEncoder(SPIClass* spi) : m_spi(spi) { }; + +void SPIEncoder::transfer(uint16_t transmission) +{ + m_lastPacket = SPIPacket(m_spi->transfer16(transmission)); +} + +uint32_t SPIEncoder::getAngle() +{ + return m_lastValidAngle; + +} +// uint32_t SPIEncoder::getAngle(uint32_t index) +// { +// return m_values[index];// & c_dataMask; +// } diff --git a/firmware/haptics/template/firmware/src/hardware/spiEncoderChain.cpp b/firmware/haptics/template/firmware/src/hardware/spiEncoderChain.cpp new file mode 100644 index 0000000..5c53e57 --- /dev/null +++ b/firmware/haptics/template/firmware/src/hardware/spiEncoderChain.cpp @@ -0,0 +1,329 @@ +#include "hardware/spiEncoderChain.hpp" + +#include "hardware/spiCommands.hpp" +#include "utils/serial.hpp" +#include + +void SPIEncoderChain::begin() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); +} + +void SPIEncoderChain::end() +{ + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + + delay(1); +} + +// void SPIEncoderChain::transfer(uint16_t transmission) +// { +// begin(); +// for(auto i = 0; i < m_numberOfEncoders; ++i) +// { +// m_encoders[i].transfer(transmission); +// } +// end(); +// } + +void SPIEncoderChain::transfer(uint16_t transmission) +{ + + //upperhandle + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); +} + +void SPIEncoderChain::setZero(std::vector newZero) +{ + transfer(SPICommands::c_highZeroWrite); + + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + // } + // end(); + //upperhandle + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] >> 6).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + transfer(SPICommands::c_lowZeroWrite); + + // begin(); + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + // } + // end(); + + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_numberOfEncoders/2; ++i) + { + m_encoders[i].transfer(SPIPacket(0, newZero[i] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin1, HIGH); + // m_spi.endTransaction(); + + //lower handle + // m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin2, LOW); + for (auto i = 0; i < m_numberOfEncoders / 2; ++i) + { + m_encoders[i+2].transfer(SPIPacket(0, newZero[i+2] & 0b111111).m_transmission); + } + digitalWrite(c_hspiSsPin2, HIGH); + m_spi.endTransaction(); + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // EEPROM.writeInt((i*sizeof(int32_t)),newZero[i] & 0x3fff); + // } + // EEPROM.commit(); + transfer(SPICommands::c_readAngle); +} + +SPIEncoderChain::SPIEncoderChain(uint32_t numberOfEncoders) +: m_settings(10000000, SPI_MSBFIRST, SPI_MODE1) //was 100000 +, m_spi(HSPI) +, m_numberOfEncoders(numberOfEncoders) +, m_encoders(numberOfEncoders, &m_spi) +, m_values(2 * 2) +, m_zeros(m_numberOfEncoders) +{ + m_spi.begin(); + pinMode(c_hspiSsPin1, OUTPUT); + pinMode(c_hspiSsPin2, OUTPUT); + pinMode(13, OUTPUT); + clearError(); + __setZeros(); + update(); +} + +void SPIEncoderChain::__setZeros(){ + for(int i=0 ; i < 4; i++)m_zeros[i]=0; + update(); + for(int i=0 ; i < 4; i++){ + m_zeros[i] = m_encoders[i].m_lastValidAngle; + } + // Serial.println("Zeros"); + + DPSerial::sendQueuedDebugLog("Zeros"); + + for(int i=0 ; i < 4; i++){ + // Serial.println(m_zeros[i]); + DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::update() +{ + update(0); + update(1); +} + +void SPIEncoderChain::update(int channel) +{ + uint16_t buf = 0; + m_spi.beginTransaction(m_settings); + //pinMode(13, OUTPUT); + digitalWrite(13, HIGH); + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + m_spi.transfer16(c_readAngle); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + delayMicroseconds(1); + + if(channel == 0) digitalWrite(c_hspiSsPin1, LOW); + else if(channel == 1) digitalWrite(c_hspiSsPin2, LOW); + for(auto i = 0; i < m_values.size()/2; ++i) + { + buf = m_spi.transfer16(c_nop); + m_values[i + channel*2] = buf & c_dataMask; + // Serial.printf("m_values%d\t%d\n", i, m_values[i + channel*2]); + // Serial.println(); + } + digitalWrite(c_hspiSsPin1, HIGH); + digitalWrite(c_hspiSsPin2, HIGH); + + m_spi.endTransaction(); + + for(int i=0; i < 4; i++){ + m_encoders[i].m_lastValidAngle = m_values[i]; + // DPSerial::sendQueuedDebugLog("zero %u reported=%u", m_zeros[i], m_encoders[i].m_lastValidAngle); + } +} + +void SPIEncoderChain::clearError() +{ + m_spi.beginTransaction(m_settings); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_clearError); + } + digitalWrite(c_hspiSsPin1, HIGH); + + + //Serial.println("Error registers:"); + digitalWrite(c_hspiSsPin1, LOW); + for(auto i = 0; i < m_values.size(); ++i) + { + m_spi.transfer16(c_nop); + m_spi.transfer16(c_nop); + //Serial.println(m_spi.transfer16(c_nop)); + //Serial.println(m_spi.transfer16(c_nop)); + } + digitalWrite(c_hspiSsPin1, HIGH); + m_spi.endTransaction(); + +} + +std::vector SPIEncoderChain::getZero() //getZero returns 0 everytime power == off. +{ + // first pass - request high part of zero, don't care about current return value + transfer(SPICommands::c_highZeroRead); + + // second pass - request low part of zero, the return value contains the high part + transfer(SPICommands::c_lowZeroRead); + + std::vector result(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = m_encoders[i].m_lastPacket.m_data << 6; + } + + // third pass - jsut send nop, the return value contains the low part + transfer(SPICommands::c_nop); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] |= m_encoders[i].m_lastPacket.m_data & 0b111111; + } + + // for(auto i = 0; i < m_numberOfEncoders; ++i) + // { + // result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + // } + + return result; +} + +void SPIEncoderChain::setZero() +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + } + + setZero(newZero); +} + +bool SPIEncoderChain::needsZero() +{ + auto currentZero = getZero(); + + bool allZero = true; + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + allZero &= currentZero[i] == 0; + } + + transfer(SPICommands::c_readAngle); + + return allZero; +} + +void SPIEncoderChain::setPosition(std::vector positions) +{ + auto currentZero = getZero(); + update(); + + std::vector newZero(m_numberOfEncoders); + + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + newZero[i] = currentZero[i] + m_encoders[i].m_lastValidAngle; + newZero[i] -= positions[i]; + } + + setZero(newZero); +} + +void SPIEncoderChain::wakeUp(){ //call setZero(EEPROM_VALUE) + std::vector result(m_numberOfEncoders); + update(); + for(auto i = 0; i < m_numberOfEncoders; ++i) + { + result[i] = (uint16_t)EEPROM.readInt(i*sizeof(int32_t)); + } + setZero(result); +} + +AngleAccessor SPIEncoderChain::getAngleAccessor(uint32_t index) +{ + return std::bind(&SPIEncoder::getAngle, &m_encoders[index]); +} + +uint32_t SPIEncoderChain::getErrors() { + return errors; +} + +uint32_t SPIEncoderChain::getRequests() { + return requests; +} + +void SPIEncoderChain::resetErrors() { + errors = 0; + requests = 0; +} \ No newline at end of file diff --git a/firmware/haptics/template/firmware/src/hardware/spiPacket.cpp b/firmware/haptics/template/firmware/src/hardware/spiPacket.cpp new file mode 100644 index 0000000..c4a5080 --- /dev/null +++ b/firmware/haptics/template/firmware/src/hardware/spiPacket.cpp @@ -0,0 +1,44 @@ +#include "hardware/spiPacket.hpp" + +SPIPacket::SPIPacket(bool flag, uint16_t data) +: m_flag(flag) +, m_data(data & c_dataMask) +, m_valid(true) +{ + m_parity = calcParity(); + m_transmission = m_data | (m_flag << 14) | (m_parity << 15); +} + +SPIPacket::SPIPacket(uint16_t transmission) +: m_transmission(transmission) +{ + m_parity = m_transmission & c_parityMask; + m_flag = m_transmission & c_flagMask; + m_data = m_transmission & c_dataMask; + m_valid = m_parity == calcParity(); +} + +bool SPIPacket::calcParity() +{ + uint16_t temp = (m_data & c_dataMask) | (m_flag << 14); + // calculate https://en.wikipedia.org/wiki/Hamming_weight + temp -= (temp >> 1) & 0b0101010101010101; + temp = (temp & 0b0011001100110011) + ((temp >> 2) & 0b0011001100110011); + temp = (temp + (temp >> 4)) & 0b0000111100001111; + return ((temp * 0b0000000100000001) >> 8) % 2; +} + +bool SPIPacket::parityError() +{ + return m_data & 0b100; +} + +bool SPIPacket::commandInvalidError() +{ + return m_data & 0b10; +} + +bool SPIPacket::framingError() +{ + return m_data & 0b1; +} diff --git a/firmware/haptics/template/firmware/src/ioMain.cpp b/firmware/haptics/template/firmware/src/ioMain.cpp new file mode 100644 index 0000000..14917df --- /dev/null +++ b/firmware/haptics/template/firmware/src/ioMain.cpp @@ -0,0 +1,39 @@ +#include "ioMain.hpp" + +#include "hardware/panto.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +FramerateLimiter sendLimiter = FramerateLimiter::fromFPS(60); +//todo copy limiter to physics physicsMain.cpp for SPI error logging + +void ioSetup() +{ + ulTaskNotifyTake(true, portMAX_DELAY); + delay(100); // TODO: patch: wait until pantoPhysics instantiates godobject (physicsMain); +} + +void ioLoop() +{ + PERFMON_START("[a] Receive serial"); +// DPSerial::receive(); + auto connected = DPSerial::ensureConnection(); + PERFMON_STOP("[a] Receive serial"); + + PERFMON_START("[b] Send positions"); + //DPSerial::sendDebugData(); + //DPSerial::sendInstantDebugLog("\n"); + if (connected && sendLimiter.step()) + { + DPSerial::sendPosition(); + } + PERFMON_STOP("[b] Send positions"); + + PERFMON_START("[c] Send debug logs"); + if (connected) + { + DPSerial::processDebugLogQueue(); + } + PERFMON_STOP("[c] Send debug logs"); +} diff --git a/firmware/haptics/template/firmware/src/main.cpp b/firmware/haptics/template/firmware/src/main.cpp new file mode 100644 index 0000000..3393809 --- /dev/null +++ b/firmware/haptics/template/firmware/src/main.cpp @@ -0,0 +1,37 @@ +#include + +#include "ioMain.hpp" +#include "physicsMain.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/serial.hpp" + +void setup() +{ + DPSerial::init(); + + DPSerial::sendInstantDebugLog("========== START =========="); + + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("I/O"), + std::forward_as_tuple(&ioSetup, &ioLoop, "I/O", 0)); + Tasks.at("I/O").run(); + Tasks.at("I/O").setLogFps(); + Tasks.emplace( + std::piecewise_construct, + std::forward_as_tuple("Physics"), + std::forward_as_tuple(&physicsSetup, &physicsLoop, "Physics", 1)); + Tasks.at("Physics").run(); + + TaskHandle_t defaultTask = xTaskGetCurrentTaskHandle(); + DPSerial::sendInstantDebugLog("default task handle is %i", defaultTask); + vTaskSuspend(NULL); + taskYIELD(); + DPSerial::sendInstantDebugLog("setup - this should not be printed"); +} + +void loop() +{ + DPSerial::sendInstantDebugLog("loop - this should not be printed"); + delay(1000); +} diff --git a/firmware/haptics/template/firmware/src/physics/annotatedEdge.cpp b/firmware/haptics/template/firmware/src/physics/annotatedEdge.cpp new file mode 100644 index 0000000..8072350 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/annotatedEdge.cpp @@ -0,0 +1,25 @@ +#include "physics/annotatedEdge.hpp" + +#include "utils/serial.hpp" + +#include "physics/edge.hpp" +#include "physics/indexedEdge.hpp" + +AnnotatedEdge::AnnotatedEdge(IndexedEdge* indexedEdge, Edge* edge) +: m_indexedEdge{indexedEdge} +, m_edge{edge} +{ +} + +AnnotatedEdge::AnnotatedEdge(const AnnotatedEdge& other) +: m_indexedEdge{other.m_indexedEdge} +, m_edge{other.m_edge} +{ +} + +void AnnotatedEdge::destroy() +{ + delete m_indexedEdge; + delete m_edge; + delete this; +} diff --git a/firmware/haptics/template/firmware/src/physics/collider.cpp b/firmware/haptics/template/firmware/src/physics/collider.cpp new file mode 100644 index 0000000..ae51266 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/collider.cpp @@ -0,0 +1,46 @@ +#include "physics/collider.hpp" + +#include "utils/serial.hpp" + +Collider::Collider(std::vector points) : m_points(points) { } + +void Collider::add(std::vector points) +{ + m_points.insert(m_points.end(), points.begin(), points.end()); +} + +bool Collider::contains(Vector2D point) +{ + // will contain result + auto inside = false; + // loop vars + auto edgeCount = m_points.size(); + auto j = edgeCount - 1; + // pre-allocate + Vector2D first, second; + + for(auto i = 0; i < edgeCount; ++i) + { + first = m_points[i]; + second = m_points[j]; + if ((first.y > point.y) != (second.y > point.y) && + (point.x < + first.x + + (second.x - first.x) * + (point.y - first.y) / + (second.y - first.y))) + { + inside = !inside; + } + j = i; + } + + return inside; +} + +Edge Collider::getEdge(uint32_t index) +{ + return Edge( + m_points[index % m_points.size()], + m_points[(index + 1) % m_points.size()]); +} diff --git a/firmware/haptics/template/firmware/src/physics/godObject.cpp b/firmware/haptics/template/firmware/src/physics/godObject.cpp new file mode 100644 index 0000000..3b2b8c9 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/godObject.cpp @@ -0,0 +1,473 @@ +#include "physics/godObject.hpp" + +#include "config/config.hpp" +#include "utils/serial.hpp" + +GodObject::GodObject(Vector2D position) + : m_position(position), m_tetherPosition(position), m_obstacleMutex(portMUX_INITIALIZER_UNLOCKED), m_possibleCollisions(new std::set()) +{ +} + +GodObject::~GodObject() +{ + delete m_possibleCollisions; +} + +void GodObject::setMovementDirection(Vector2D movementDirection) +{ + m_movementDirection = movementDirection; +} + +Hashtable& GodObject::hashtable() +{ + if (!m_hashtable) + { + m_hashtable = new Hashtable(); + } + return *m_hashtable; +} + +void GodObject::update() +{ + if (m_actionQueue.empty()) + { + return; + } + + portENTER_CRITICAL(&m_obstacleMutex); + for (auto i = 0; i < obstacleChangesPerFrame && !m_actionQueue.empty(); ++i) + { + auto action = m_actionQueue.front(); + m_actionQueue.pop_front(); + switch (action->m_type) + { + case HT_ENABLE_EDGE: + { + hashtable().add(action->m_data.m_annotatedEdge); + break; + } + case HT_DISABLE_EDGE: + { + hashtable().remove(action->m_data.m_annotatedEdge); + break; + } + case GO_REMOVE_OBSTACLE: + { + try{ + + delete m_obstacles.at(action->m_data.m_obstacleId); + m_obstacles.erase(action->m_data.m_obstacleId); + } catch (const std::out_of_range &oor){ + DPSerial::sendInstantDebugLog("Could not remove obstacle"); + } + break; + } + default: + { + break; + } + } + delete action; + } + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::dumpHashtable() +{ + portENTER_CRITICAL(&m_obstacleMutex); + hashtable().print(); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +// returns if force is applied or not +bool GodObject::move(bool isTweening, bool isFrozen) +{ + auto lastState = m_processingObstacleCollision; + // if the number of collisions increased since the last frame then we ran into a corner + auto lastNumCollisions = m_numCollisions; + + m_processingObstacleCollision = false; + + + Vector2D nextGoPosition; + Vector2D handlePosition = m_position + m_movementDirection; + if (isFrozen){ + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + return true; + } + if (isTweening) { + m_position = handlePosition; + if (m_tethered) { + m_tetherPosition = handlePosition; + } + return false; + } + float movementStepLength = m_movementDirection.length(); // only used for tethering + if (m_tethered && !isTweening) { + double distHandleToGo = m_movementDirection.length(); + if ((distHandleToGo < m_tetherInnerRadius) || (m_tetherState!=Outer && (distHandleToGo > 10))) { + // the latter condition can happen at startup of the device. + // in this case we don't want to apply forces. + m_tetherState = Inner; + return false; + } + // find out the current tether state + if (distHandleToGo > m_tetherInnerRadius && distHandleToGo < m_tetherOuterRadius){ + m_tetherState = Active; + } else { + m_tetherState = Outer; + } + // the speed of the god object increases proportionally with the distance bw handle and go. The max speed of the go is dependent on the outer tether radius. + movementStepLength = min(m_tetherOuterRadius, distHandleToGo); + + // this is the movement of the god object that follows the tether + if (distHandleToGo != 0) + { + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * m_tetherFactor); + } else { + nextGoPosition = handlePosition; + } + } else { + nextGoPosition = handlePosition; + } + + // no matter what the tether state is we need to check if the god object is colliding with an obstacle + Vector2D godObjectPos; + portENTER_CRITICAL(&m_obstacleMutex); + if (m_tethered && m_tetherState == Outer && m_tetherStrategy == Exploration){ + // pausing the game means that the god object position doesn't update. + godObjectPos = m_position; + } else { + // this is the default case + godObjectPos = checkCollisions(nextGoPosition, m_position); + } + if (m_tethered && m_processingObstacleCollision && m_tetherState == Outer && m_tetherStrategy != MaxSpeed) { + // for the Exploration and Leash mode the tether state can't be outer once the god object collides + // (otherwise the wall force will be weaker when the handle moves further into the wall) + m_tetherState = Active; + } + + // check if collision with a passable obstacle or a haptic guide is present + if (m_tethered && m_processingObstacleCollision && m_tetherState == Active) { + m_processingObstacleCollision = false; + nextGoPosition = m_position + (m_movementDirection.normalize() * movementStepLength * c_railsTetherFactor); + auto pos = checkCollisions(nextGoPosition, m_position); + // if the handle is already past the obstacle (no more collision present) then the god object jumps to its position + if (!m_processingObstacleCollision) { + m_position = pos; + } else { + m_position = godObjectPos; + } + } else { + m_position = godObjectPos; + } + + if (m_tethered && m_tetherState == Outer && !isTweening && m_tetherStrategy != MaxSpeed) + { + m_tetherPosition = checkCollisions(handlePosition, m_tetherPosition); + } else { + m_tetherPosition = handlePosition; + } + portEXIT_CRITICAL(&m_obstacleMutex); + + m_doneColliding = lastState && !m_processingObstacleCollision; + + if (!m_tethered) { + if (m_processingObstacleCollision) + { + renderForce(getCollisionForce(m_position, handlePosition), Vector2D(0,0)); + } + return m_processingObstacleCollision; + } else { + // newCollision is only important for the pock + bool newCollision = lastNumCollisions < m_numCollisions; + return processTetheringForce(handlePosition, newCollision); + } +} + +Vector2D GodObject::getCollisionForce(Vector2D godObjectPosition, Vector2D handlePosition){ + // the PID error is the difference between the virtual object and the handle position + // the virtual object can either be a) the godobject or b) the tether + auto error = godObjectPosition - handlePosition; + auto force = error * forcePidFactor[0][0] + (error - m_lastError) * forcePidFactor[0][2]; + m_lastError = error; + return force; +} + +Vector2D GodObject::getTetherForce(Vector2D error){ + auto force = error * forcePidFactor[0][0] + (error - m_lastErrorTether) * forcePidFactor[0][2]; + m_lastErrorTether = error; + return force; +} + +void GodObject::renderForce(Vector2D collisionForce, Vector2D tetherForce) { + m_activeForce = collisionForce + tetherForce; +} + +bool GodObject::processTetheringForce(Vector2D handlePosition, bool newCollision) { + // returns if force is active or handle is freely moving + if (m_tetherState==Outer && m_tetherStrategy!=MaxSpeed) { + // for the 2 tether strategies where the god object is pulled on a virtual leash towards the tether position + auto error = m_movementDirection.normalize() * c_tetherForcePullingBack; + auto tetherForce = getTetherForce(error); //TODO: if we had a collision previously in Active state and moved from there into Outer state then the rendered force should be the sum of the previous force and the collision force + if (m_processingObstacleCollision) { + // think about adding a second pock here as well + renderForce(getCollisionForce(m_tetherPosition, handlePosition), tetherForce); + } else { + // weak constant force pulling the tether back to the god object + renderForce(Vector2D(0,0), tetherForce); + } + return true; + } else { + auto movementLength = (m_tetherPosition - m_position).length(); + auto error = m_movementDirection.normalize() * (m_tetherInnerRadius - movementLength); + if (newCollision && m_tetherPockEnabled) { + // weak constant force pushing the handle into the the wall so that the user gets force feedback at their fingertip + error = m_movementDirection.normalize() * c_tetherPockDistance; // this can't work because we include the last tether error + } + auto tetherForce = getTetherForce(error); + + if (m_processingObstacleCollision) { + // god object collision + renderForce(getCollisionForce(m_position, handlePosition), tetherForce); + return true; + } else { + if (!m_doneColliding) { + // regular tether force active that pushes the handle back to the inner tether radius + + renderForce(Vector2D(0,0), tetherForce); + } + return !m_doneColliding; + } + } + return false; +} + +Vector2D GodObject::checkCollisions(Vector2D targetPoint, Vector2D currentPosition) +{ + /* + Collision detection works in 3 stages: + 1. Select collision candidates using a 2D lookup table. Every cell in that table contains the edges that go through it. That's why only particular edges have to be checked for collisions. + 2. The actual collision detection. + 3. If a collision is detected calculate the new god object position. + + If a collision is detected the collision detection is repeated with the new target position. This way we can check if the new position is accessible at all or not. + + Added by Julius on 30.09.20 + For more information check Lukas Wagners MT (section 4.3.1): https://www.dropbox.com/home/2018%20CHI%20Dueling%20Pantographs/Layer%202%20Firmware%20(Lukas%20Wagner)?preview=2019_09_07+ESP+Firmware+for+God+Haptic+Objects+%3D+Masterarbeit+(Lukas+Wagner).pdf + */ + + if (currentPosition == targetPoint || !m_hashtable) + { + return targetPoint; + } + // 1. select collision candidates + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + if (m_possibleCollisions->empty()) + { + return targetPoint; + } + + bool foundCollision; + u_short numCollisions = 0; + // 2. check if collisions are present between 2 vectors + // first vector goes from god object to handle and the + // second vector is the potential collision edge + do + { + // result vars + foundCollision = false; + double shortestMovementRatio = 0; + Vector2D closestEdgeFirst; + Vector2D closestEdgeFirstMinusSecond; + + // direction of movement: value is constant for loop + const auto posMinusTarget = currentPosition - targetPoint; + + if (posMinusTarget == Vector2D(0, 0)) + { + return targetPoint; + } + + for (auto&& indexedEdge : *m_possibleCollisions) + { + auto edge = indexedEdge.m_obstacle->getEdge(indexedEdge.m_index); + auto edgeFirst = edge.m_first; + auto firstMinusPos = edgeFirst - currentPosition; + auto firstMinusSecond = edgeFirst - edge.m_second; + auto divisor = + Vector2D::determinant(firstMinusSecond, posMinusTarget); + + auto movementRatio = + -Vector2D::determinant(firstMinusSecond, firstMinusPos) / + divisor; + if (movementRatio < 0 || movementRatio > 1) + { + continue; + } + + auto edgeRatio = + Vector2D::determinant(firstMinusPos, posMinusTarget) / divisor; + if (edgeRatio < 0 || edgeRatio > 1) + { + continue; + } + + // we have a collision! + if (!foundCollision || movementRatio < shortestMovementRatio) // I think the second condition never gets called because the movementRatio loop + // would already continue if the movementRatio was below 0 (which is the shortestMovementRatio) + { + // if a collision with a passable object is detected (e.g. a haptic rail) and the handle is not within the bounds of the colliding object, + // discard the collision and continue + auto ob = indexedEdge.m_obstacle; + if (ob->passable && !ob->contains(targetPoint)) + { + continue; + } + foundCollision = true; + if (!ob->passable){ + numCollisions++; + } + shortestMovementRatio = movementRatio; + closestEdgeFirst = edgeFirst; + closestEdgeFirstMinusSecond = firstMinusSecond; + } + } + + // calculate new god object position + if (foundCollision) + { + m_processingObstacleCollision = true; + + // god object slides along the colliding edge according to the handle movement but with tethered speed + + if (m_tethered) { + // if the movement is tethered the targetPoint can not be further away from the current position than the outer tether radius + const Vector2D movementVector = targetPoint - currentPosition; + double movementLength = min(m_tetherOuterRadius, movementVector.length()); + targetPoint = currentPosition + (movementVector.normalize() * movementLength); + } + + auto perpendicular = Vector2D( + -closestEdgeFirstMinusSecond.y, + closestEdgeFirstMinusSecond.x); + auto resolveRatio = + -Vector2D::determinant( + closestEdgeFirstMinusSecond, + closestEdgeFirst - targetPoint) / + Vector2D::determinant( + closestEdgeFirstMinusSecond, + perpendicular); + auto resolveVec = perpendicular * resolveRatio; + auto resolveLength = resolveVec.length(); + // c_resolveDistance is super small --> we need to add a tiny padding between the godobject and the edge so that it's not getting stuck in the edge + targetPoint = targetPoint - (resolveVec * ((resolveLength + c_resolveDistance) / resolveLength)); + + + // check for the new point if there is another collision with any other edge + m_possibleCollisions->clear(); + hashtable().getPossibleCollisions( + Edge(currentPosition, targetPoint), m_possibleCollisions); + } + // there can be multiple collisions, that's why we have to loop as well over the other possible collisions + } while (foundCollision); + m_numCollisions = numCollisions; + return targetPoint; +} + +void GodObject::createObstacle(uint16_t id, std::vector points, bool passable) +{ + // create obstacle or passable obstacle + auto ob = new Obstacle(points, passable); + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.emplace(id, ob); + portEXIT_CRITICAL(&m_obstacleMutex); +} + +void GodObject::createRail(uint16_t id, std::vector points, double displacement) +{ + portENTER_CRITICAL(&m_obstacleMutex); + Rail* rail = new Rail(points, displacement); + m_obstacles.emplace(id, rail); + portEXIT_CRITICAL(&m_obstacleMutex); + return; + +} + +void GodObject::addToObstacle(uint16_t id, std::vector points) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + m_obstacles.at(id)->add(points); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +void GodObject::removeObstacle(uint16_t id) +{ + enableObstacle(id, false); + m_actionQueue.push_back(new GodObjectAction(GO_REMOVE_OBSTACLE, id)); +} + +void GodObject::enableObstacle(uint16_t id, bool enable) +{ + auto it = m_obstacles.find(id); + if (it != m_obstacles.end()) + { + portENTER_CRITICAL(&m_obstacleMutex); + if (enable != it->second->enabled()) + { + const auto edges = it->second->getIndexedEdges(); + const auto action = enable ? HT_ENABLE_EDGE : HT_DISABLE_EDGE; + for (const auto& edge : edges) + { + m_actionQueue.push_back(new GodObjectAction( + action, + new AnnotatedEdge( + new IndexedEdge(edge.m_obstacle, edge.m_index), + new Edge(edge.getEdge())))); + } + } + it->second->enable(enable); + portEXIT_CRITICAL(&m_obstacleMutex); + } +} + +Vector2D GodObject::getPosition() +{ + return m_position; +} + +Vector2D GodObject::getActiveForce() +{ + return m_activeForce; +} + +bool GodObject::getProcessingObstacleCollision() +{ + return m_processingObstacleCollision; +} + +bool GodObject::getDoneColliding() +{ + return m_doneColliding; +} + +bool GodObject::tethered() +{ + return m_tethered; +} + +void GodObject::setSpeedControl(bool active, double tetherFactor, double innerTetherRadius, double outerTetherRadius, OutOfTetherStrategy strategy, bool pockEnabled) +{ + m_tethered = active; + m_tetherFactor = tetherFactor; + m_tetherInnerRadius = innerTetherRadius; + m_tetherOuterRadius = outerTetherRadius; + m_tetherStrategy = strategy; + m_tetherPockEnabled = pockEnabled; +} diff --git a/firmware/haptics/template/firmware/src/physics/hashtable.cpp b/firmware/haptics/template/firmware/src/physics/hashtable.cpp new file mode 100644 index 0000000..0a2e4bc --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/hashtable.cpp @@ -0,0 +1,224 @@ +#include "physics/hashtable.hpp" + +#include +#include + +#include "physics/edge.hpp" +#include "utils/assert.hpp" +#include "utils/serial.hpp" +#include "utils/utils.hpp" + + +int32_t Hashtable::get1dIndex(double value, double min, double step) +{ + return (int32_t)std::floor((value - min) / step); +} + +std::vector Hashtable::getCellIndices(Edge edge) +{ + std::vector result; + + // http://www.cse.yorku.ca/~amana/research/grid.pdf + const auto startX = edge.m_first.x; + const auto startY = edge.m_first.y; + const auto endX = edge.m_second.x; + const auto endY = edge.m_second.y; + auto x = get1dIndex(startX, rangeMinX, hashtableStepSizeX); + auto y = get1dIndex(startY, rangeMinY, hashtableStepSizeY); + const auto lastX = get1dIndex(endX, rangeMinX, hashtableStepSizeX); + const auto lastY = get1dIndex(endY, rangeMinY, hashtableStepSizeY); + const auto diffX = endX - startX; + const auto diffY = endY - startY; + const auto stepX = sgn(diffX); + const auto stepY = sgn(diffY); + const auto slopeX = diffY / diffX; + const auto slopeY = diffX / diffY; + auto nextX = stepX < 0 + ? (edge.m_first.x - (rangeMinX + x * hashtableStepSizeX)) + : ((rangeMinX + (x + 1) * hashtableStepSizeX) - edge.m_first.x); + nextX = hypot(nextX, nextX * slopeX); + auto nextY = stepY < 0 + ? (edge.m_first.y - (rangeMinY + y * hashtableStepSizeY)) + : ((rangeMinY + (y + 1) * hashtableStepSizeY) - edge.m_first.y); + nextY = hypot(nextY, nextY * slopeY); + const auto deltaX = + hypot(hashtableStepSizeX, hashtableStepSizeX * slopeX); + const auto deltaY = + hypot(hashtableStepSizeY * slopeY, hashtableStepSizeY); + + while(x != lastX || y != lastY) + { + // ignore cells outside the physical range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + if(nextX < nextY) + { + nextX += deltaX; + x += stepX; + } + else + { + nextY += deltaY; + y += stepY; + } + } + + // add end cell if inside range + if(x >= 0 && x < hashtableStepsX && y >= 0 && y < hashtableStepsY) + { + result.push_back(x * hashtableStepsY + y); + } + + return result; +} + +std::set Hashtable::expand(const std::vector& edges) +{ + std::set result; + uint32_t x, y; + for (const auto& edge : edges) + { + x = edge % hashtableStepsY; + y = edge / hashtableStepsY; + + for(int32_t i = -1; i < 2; ++i) + { + if((x == 0 && i == -1) || (x == hashtableStepsX - 1 && i == 1)) + { + continue; + } + + for(int32_t j = -1; j < 2; ++j) + { + if((y == 0 && j == -1) || (y == hashtableStepsY - 1 && j == 1)) + { + continue; + } + + result.insert((y + j) * hashtableStepsY + (x + i)); + } + } + } + return result; +} + +Hashtable::Hashtable() +{ + DPSerial::sendQueuedDebugLog( + "Hashtable settings:"); + DPSerial::sendQueuedDebugLog( + "Available memory of %i bytes can hold %i cells", + hashtableMaxMemory, + hashtableMaxCells); + DPSerial::sendQueuedDebugLog( + "Possible range of values is %3f by %3f mm", + rangeMaxX - rangeMinX, + rangeMaxY - rangeMinY); + DPSerial::sendQueuedDebugLog( + "Horizontal step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeX, + hashtableStepsX); + DPSerial::sendQueuedDebugLog( + "Vertical step size is %5.3f mm, resulting in %i steps", + hashtableStepSizeY, + hashtableStepsY); + DPSerial::sendQueuedDebugLog( + "Resulting step count is %i, using %i bytes", + hashtableNumCells, + hashtableUsedMemory); +} + +void Hashtable::add(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + m_cells[cellIndex].emplace_back( + edge->m_indexedEdge->m_obstacle, edge->m_indexedEdge->m_index); + } + edge->destroy(); +} + +void Hashtable::remove(AnnotatedEdge* edge) +{ + for(auto&& cellIndex : expand(getCellIndices(*(edge->m_edge)))) + { + auto& cell = m_cells[cellIndex]; + auto it = std::find( + cell.begin(), + cell.end(), + *(edge->m_indexedEdge)); + if(it != cell.end()) + { + cell.erase(it); + cell.shrink_to_fit(); + } + } + edge->destroy(); +} + +void Hashtable::getPossibleCollisions( + Edge movement, std::set* result) +{ + if(movement.m_first.x == 0 && movement.m_first.y == 0) + { + DPSerial::sendInstantDebugLog( + "Skipping god object movement from zero position."); + return; + } + const auto startX = + get1dIndex(movement.m_first.x, rangeMinX, hashtableStepSizeX); + const auto startY = + get1dIndex(movement.m_first.y, rangeMinY, hashtableStepSizeY); + const auto startIndex = startX * hashtableStepsY + startY; + ASSERT_GE(startIndex, 0); + + ASSERT_LT(startIndex, hashtableNumCells); + const auto endX = + get1dIndex(movement.m_second.x, rangeMinX, hashtableStepSizeX); + const auto endY = + get1dIndex(movement.m_second.y, rangeMinY, hashtableStepSizeY); + const auto endIndex = endX * hashtableStepsY + endY; + ASSERT_GE(endIndex, 0); + ASSERT_LT(endIndex, hashtableNumCells); + auto dist = (uint8_t)(startX != endX) + (uint8_t)(startY != endY); + const auto* begin = &m_cells[0]; + if(dist == 0) + { + const auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + } + else if(dist == 1) + { + auto* cell = begin + startIndex; + result->insert(cell->begin(), cell->end()); + cell = begin + endIndex; + result->insert(cell->begin(), cell->end()); + } + else + { + for(auto&& cellIndex : getCellIndices(movement)) + { + const auto* cell = begin + cellIndex; + result->insert(cell->begin(), cell->end()); + } + } +} + +void Hashtable::print() +{ + DPSerial::sendQueuedDebugLog("Printing hashtable..."); + std::ostringstream str; + for(auto y = 0; y < hashtableStepsY; ++y) + { + for(auto x = 0; x < hashtableStepsX; x++) + { + str << (m_cells[x * hashtableStepsY + y].empty() ? '-' : '#'); + } + DPSerial::sendQueuedDebugLog(str.str().c_str()); + str.str(""); + } + DPSerial::sendQueuedDebugLog("Printing complete."); +} diff --git a/firmware/haptics/template/firmware/src/physics/indexedEdge.cpp b/firmware/haptics/template/firmware/src/physics/indexedEdge.cpp new file mode 100644 index 0000000..c1f5f09 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/indexedEdge.cpp @@ -0,0 +1,26 @@ +#include "physics/indexedEdge.hpp" + +#include "physics/obstacle.hpp" + +IndexedEdge::IndexedEdge(Obstacle* obstacle, uint32_t index) +: m_obstacle(obstacle) +, m_index(index) +{ +} + +bool IndexedEdge::operator==(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle && m_index == other.m_index; +} + +bool IndexedEdge::operator<(const IndexedEdge& other) const +{ + return m_obstacle == other.m_obstacle ? + m_index < other.m_index : + m_obstacle < other.m_obstacle; +} + +Edge IndexedEdge::getEdge() const +{ + return m_obstacle->getEdge(m_index); +} diff --git a/firmware/haptics/template/firmware/src/physics/obstacle.cpp b/firmware/haptics/template/firmware/src/physics/obstacle.cpp new file mode 100644 index 0000000..9ef21a4 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/obstacle.cpp @@ -0,0 +1,40 @@ +#include "physics/obstacle.hpp" + +#include "physics/indexedEdge.hpp" + +Obstacle::Obstacle(std::vector points) : Collider(points) { } + +Obstacle::Obstacle(std::vector points, bool passable) : Collider(points) +{ + this->passable = passable; +} + +bool Obstacle::enabled() +{ + return m_enabled; +} + +void Obstacle::enable(bool enable) +{ + m_enabled = enable; +} + +std::vector Obstacle::getIndexedEdges( + uint32_t first, uint32_t last +) +{ + if(first == -1) + { + first = m_points.size() - 1; + } + if(last == -1) + { + last = m_points.size() - 1; + } + std::vector result; + for(auto i = first; i <= last; ++i) + { + result.emplace_back(this, i); + } + return result; +} diff --git a/firmware/haptics/template/firmware/src/physics/pantoPhysics.cpp b/firmware/haptics/template/firmware/src/physics/pantoPhysics.cpp new file mode 100644 index 0000000..2f27f4d --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/pantoPhysics.cpp @@ -0,0 +1,61 @@ +#include "physics/pantoPhysics.hpp" + +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::vector pantoPhysics; + +PantoPhysics::PantoPhysics(Panto* panto) : m_panto(panto) +{ + m_currentPosition = m_panto->getPosition(); + try + { + m_godObject = new GodObject(m_currentPosition); + } + catch(const std::bad_alloc& e) + { + DPSerial::sendInstantDebugLog("Error while creating god object - the hash table may be too big."); + DPSerial::sendInstantDebugLog("Error: %s", e.what()); + DPSerial::sendInstantDebugLog("Rebooting..."); + ESP.restart(); + } +} + +GodObject* PantoPhysics::godObject() +{ + return m_godObject; +} + +void PantoPhysics::step() +{ + // PERFMON_START("[ba] Physics::step"); + // PERFMON_START("[baa] Physics::step::prep"); + m_godObject->update(); + + m_currentPosition = m_panto->getPosition(); + + bool isTweening = m_panto->getInTransition(); + + auto difference = m_currentPosition - m_godObject->getPosition(); + m_godObject->setMovementDirection(difference); + // PERFMON_STOP("[baa] Physics::step::prep"); + // PERFMON_START("[bab] Physics::step::move"); + bool isForceActive = m_godObject->move(isTweening, m_panto->getIsFrozen()); + // PERFMON_STOP("[bab] Physics::step::move"); + // PERFMON_START("[bac] Physics::step::motor"); + + // force is active when frozen, tethering is enabled or collision is detected + if(isForceActive ) + { + m_panto->setTarget(m_godObject->getActiveForce(), true); + } + else + { + if (!isTweening){ + // TODO: is there actually another case? + m_panto->setTarget(Vector2D(NAN, NAN), false); + } + } + // PERFMON_STOP("[bac] Physics::step::motor"); + // PERFMON_STOP("[ba] Physics::step"); +} diff --git a/firmware/haptics/template/firmware/src/physics/rail.cpp b/firmware/haptics/template/firmware/src/physics/rail.cpp new file mode 100644 index 0000000..09613d3 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physics/rail.cpp @@ -0,0 +1,14 @@ +#include "physics/rail.hpp" + +Rail::Rail(std::vector points, double displacement) : Obstacle(points, true) +{ + this->displacement = displacement; +} + +bool Rail::contains(Vector2D point){ + //return true; + // if the point is within the displacement around the rail then it is contained and the godobject collides + double distLineToPoint = point.distancePointToLineSegment(m_points[0], m_points[1]); + bool inside = (distLineToPoint < this->displacement); + return inside; +} \ No newline at end of file diff --git a/firmware/haptics/template/firmware/src/physicsMain.cpp b/firmware/haptics/template/firmware/src/physicsMain.cpp new file mode 100644 index 0000000..2eedae2 --- /dev/null +++ b/firmware/haptics/template/firmware/src/physicsMain.cpp @@ -0,0 +1,153 @@ +#include "physicsMain.hpp" + +#include "hardware/panto.hpp" +#include "hardware/spiEncoderChain.hpp" +#include "physics/pantoPhysics.hpp" +#include "tasks/taskRegistry.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/framerateLimiter.hpp" +#include "utils/serial.hpp" + +FramerateLimiter spiErrorLimiter = FramerateLimiter::fromSeconds(1); + +#ifdef LINKAGE_ENCODER_USE_SPI +SPIEncoderChain* spi; +#endif + +void physicsSetup() +{ + #ifdef LINKAGE_ENCODER_USE_SPI + spi = new SPIEncoderChain(numberOfSpiEncoders); + #endif + + for (auto i = 0; i < pantoCount; ++i) + { + pantos.emplace_back(i); + } + delay(1000); + + xTaskNotifyGive(Tasks.at("I/O").getHandle()); + + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + + EEPROM.begin(sizeof(uint32_t)*numberOfSpiEncoders); + + //calibrateEncoders; Comment if not needed + // for (auto i = 0; i < pantoCount; ++i) + // { pantos[i].calibrateEncoders(i);} + + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); //calibrating only handle pulse encoder + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + + pantos[i].setAngleAccessor(j, spi->getAngleAccessor(index)); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + + for (unsigned char i = 0; i < pantoCount; ++i) + { + pantoPhysics.emplace_back(&pantos[i]); + } +} + +void physicsLoop() +{ + PERFMON_START("[a] Read encoders"); + // PERFMON_START("[aa] Query SPI"); + #ifdef LINKAGE_ENCODER_USE_SPI + spi->update(); + #endif + // PERFMON_STOP("[aa] Query SPI"); + + // PERFMON_START("[ab] Calculation loop"); + for (auto i = 0; i < pantoCount; ++i) + { + // PERFMON_START("[aba] Actually read"); + pantos[i].readEncoders(); + // PERFMON_STOP("[aba] Actually read"); + PERFMON_START("[abb] Calc fwd kinematics"); + pantos[i].forwardKinematics(); + PERFMON_STOP("[abb] Calc fwd kinematics"); + } + // PERFMON_STOP("[ab] Calculation loop"); + PERFMON_STOP("[a] Read encoders"); + + PERFMON_START("[b] Calculate physics"); + for (auto i = 0; i < pantoCount; ++i) + { + pantoPhysics[i].step(); + } + PERFMON_STOP("[b] Calculate physics"); + + PERFMON_START("[c] Actuate motors"); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].actuateMotors(); + } + PERFMON_STOP("[c] Actuate motors"); + + if(spiErrorLimiter.step()) { + // DPSerial::sendQueuedDebugLog("SPI Errors: %i out of %i requests", spi->getErrors(), spi->getRequests()); + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[0][%i]: %i out of %i requests",i, + // pantos[0].getEncoderErrorCounts(i), pantos[0].getEncoderRequestsCounts(i)); + // } + // for(int i=0; i < 2; i++){ + // DPSerial::sendQueuedDebugLog("Encoder Errors panto[1][%i]: %i out of %i requests",i, + // pantos[1].getEncoderErrorCounts(i), pantos[1].getEncoderRequestsCounts(i)); + // } + // spi->resetErrors(); + } + + PERFMON_START("[d] Calibrate Pantos"); + bool flag = false; + for(auto i = 0; i < pantoCount; ++i){ + if(pantos[i].getCalibrationState()){ + flag = true; + break; + } + } + if(flag){ + #ifdef LINKAGE_ENCODER_USE_SPI + std::vector startPositions(numberOfSpiEncoders); + #endif + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibrationEnd(); + #ifdef LINKAGE_ENCODER_USE_SPI + for (auto j = 0; j < 3; ++j) // three encoders + { + auto index = encoderSpiIndex[i * 3 + j]; + if(index != 0xffffffff) // excluding it / me handle. + { + startPositions[index] = + ((uint16_t)(pantos[i].getActuationAngle(j) / + (TWO_PI) * + encoderSteps[i * 3 + j]) & 0x3fff); + } + } + #endif + } + #ifdef LINKAGE_ENCODER_USE_SPI + spi->setPosition(startPositions); + #endif + } + PERFMON_STOP("[d] Calibrate Pantos"); +} diff --git a/firmware/haptics/template/firmware/src/tasks/task.cpp b/firmware/haptics/template/firmware/src/tasks/task.cpp new file mode 100644 index 0000000..36e28ad --- /dev/null +++ b/firmware/haptics/template/firmware/src/tasks/task.cpp @@ -0,0 +1,109 @@ +#include "tasks/task.hpp" + +#include +#include + +#include "utils/framerateLimiter.hpp" +#include "utils/performanceMonitor.hpp" +#include "utils/serial.hpp" + +std::map Task::s_fpsMap; +FramerateLimiter loggingLimiter = FramerateLimiter::fromSeconds(1); + +void Task::taskLoop(void* parameters) +{ + Task* task = reinterpret_cast(parameters); + +#ifdef ENABLE_FPS + task->initFps(); +#endif + + task->m_setupFunc(); + +loopLabel: + task->m_loopFunc(); + +#ifdef ENABLE_FPS + task->checkFps(); +#endif + + TIMERG0.wdt_wprotect=TIMG_WDT_WKEY_VALUE; + TIMERG0.wdt_feed=1; + TIMERG0.wdt_wprotect=0; + goto loopLabel; +}; + +inline void Task::initFps() +{ + m_fpsCalcLimiter.reset(); + m_loopCount = 0; +}; + +inline void Task::checkFps() +{ + ++m_loopCount; + if (m_fpsCalcLimiter.step()) + { + s_fpsMap[m_handle] = m_loopCount * 1000 / m_fpsInterval; + m_loopCount = 0; + + if(m_logFps && loggingLimiter.step()) + { + for(const auto& entry : s_fpsMap) + { + DPSerial::sendQueuedDebugLog( + "Task \"%s\" fps: %i", + pcTaskGetTaskName(entry.first), + entry.second); + } + + if(ESP.getHeapSize() != 0){ + DPSerial::sendQueuedDebugLog("Free heap: %i of %i (%.3f %%). Largest block: %i", ESP.getFreeHeap(), ESP.getHeapSize(), 100*ESP.getFreeHeap()/(float)ESP.getHeapSize(), ESP.getMaxAllocHeap()); + } + + #ifdef ENABLE_PERFMON + for(const auto& entry : PerfMon.getResults()) + { + DPSerial::sendQueuedDebugLog(entry.c_str()); + } + #endif + } + } +}; + +Task::Task( + TaskFunction setupFunc, + TaskFunction loopFunc, + const char* name, + int core) + : m_setupFunc(setupFunc), + m_loopFunc(loopFunc), + m_name(name), + m_stackDepth(c_defaultStackDepth), + m_priority(c_defaultPriority), + m_core(core), + m_fpsInterval(c_defaultFpsInterval), + m_fpsCalcLimiter(FramerateLimiter::fromMilliseconds(m_fpsInterval)){}; + +void Task::run() +{ + xTaskCreatePinnedToCore( + taskLoop, + m_name, + m_stackDepth, + this, + m_priority, + &m_handle, + m_core); + DPSerial::sendInstantDebugLog("Started task \"%s\" on core %i.", m_name, m_core); +}; + +void Task::setLogFps(bool logFps) +{ + m_logFps = logFps; +} + +TaskHandle_t Task::getHandle() +{ + return m_handle; +} diff --git a/firmware/haptics/template/firmware/src/tasks/taskRegistry.cpp b/firmware/haptics/template/firmware/src/tasks/taskRegistry.cpp new file mode 100644 index 0000000..4414b0a --- /dev/null +++ b/firmware/haptics/template/firmware/src/tasks/taskRegistry.cpp @@ -0,0 +1,3 @@ +#include "tasks/taskRegistry.hpp" + +TaskRegistry Tasks; diff --git a/firmware/haptics/template/firmware/src/utils/crashDump.cpp b/firmware/haptics/template/firmware/src/utils/crashDump.cpp new file mode 100644 index 0000000..26ab153 --- /dev/null +++ b/firmware/haptics/template/firmware/src/utils/crashDump.cpp @@ -0,0 +1,19 @@ +#include "utils/crashDump.hpp" + +#include "utils/serial.hpp" + +CrashDump physicsDump; + +CrashDump::CrashDump() : m_stream() {} + +void CrashDump::clear() +{ + m_stream.clear(); + m_stream.str(""); +} + +void CrashDump::dump() +{ + DPSerial::sendQueuedDebugLog(m_stream.str().c_str()); + clear(); +} diff --git a/firmware/haptics/template/firmware/src/utils/framerateLimiter.cpp b/firmware/haptics/template/firmware/src/utils/framerateLimiter.cpp new file mode 100644 index 0000000..050c2ee --- /dev/null +++ b/firmware/haptics/template/firmware/src/utils/framerateLimiter.cpp @@ -0,0 +1,57 @@ +#include "utils/framerateLimiter.hpp" + +#include "utils/serial.hpp" + +uint64_t FramerateLimiter::s_period = 0; +uint64_t FramerateLimiter::s_lastNow = 0; + +uint64_t FramerateLimiter::now() +{ + uint64_t now = micros(); + if(now < s_lastNow) + { + ++s_period; + DPSerial::sendQueuedDebugLog("FramerateLimiter: micros() rolled over."); + } + return s_period * c_periodLength + now; +} + +FramerateLimiter::FramerateLimiter(uint64_t microseconds) +: m_delta(microseconds) +, m_last(0) { } + +FramerateLimiter FramerateLimiter::fromFPS(double fps) +{ + return FramerateLimiter(1e6 / fps); +} + +FramerateLimiter FramerateLimiter::fromSeconds(uint64_t seconds) +{ + return FramerateLimiter(1000000 * seconds); +} + +FramerateLimiter FramerateLimiter::fromMilliseconds(uint64_t milliseconds) +{ + return FramerateLimiter(1000 * milliseconds); +} + +FramerateLimiter FramerateLimiter::fromMicroseconds(uint64_t microseconds) +{ + return FramerateLimiter(microseconds); +} + +bool FramerateLimiter::step() +{ + uint64_t now = FramerateLimiter::now(); + if(m_last + m_delta > now) + { + return false; + } + m_last = now; + return true; +} + +void FramerateLimiter::reset() +{ + m_last = FramerateLimiter::now(); +} diff --git a/firmware/haptics/template/firmware/src/utils/performanceMonitor.cpp b/firmware/haptics/template/firmware/src/utils/performanceMonitor.cpp new file mode 100644 index 0000000..050e401 --- /dev/null +++ b/firmware/haptics/template/firmware/src/utils/performanceMonitor.cpp @@ -0,0 +1,158 @@ +#include "utils/performanceMonitor.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PerformanceMonitor::PerformanceEntry::PerformanceEntry() +{ + m_values.reserve(c_measurementCount); +} + +void PerformanceMonitor::start(std::string label) +{ + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(entry.m_running) + { + return; + } + + entry.m_running = true; + entry.m_start = ESP.getCycleCount(); +} + +void PerformanceMonitor::stop(std::string label) +{ + uint64_t end = ESP.getCycleCount(); + + auto& entry = m_entries[xPortGetCoreID()][label]; + + if(!entry.m_running) + { + return; + } + + uint64_t duration = + entry.m_start < end ? + (end - entry.m_start) : + (end + UINT32_MAX - entry.m_start); + + if(entry.m_values.size() == c_measurementCount) + { + auto old = entry.m_values[entry.m_index]; + entry.m_sum -= old; + + entry.m_values[entry.m_index] = duration; + entry.m_sum += duration; + entry.m_index = (entry.m_index + 1) % c_measurementCount; + } + else + { + entry.m_values.push_back(duration); + entry.m_sum += duration; + } + + entry.m_running = false; +} + +std::vector PerformanceMonitor::getResults() +{ + std::vector> temp; + + const std::string labelHeader("label"); + const std::string cycleHeader("avg cycles"); + const std::string timeHeader("avg ns"); + uint32_t maxLabelSize = labelHeader.size(); + uint32_t maxCycleSize = cycleHeader.size(); + uint32_t maxTimeSize = timeHeader.size(); + + for(const auto& core : m_entries) + { + auto coreId = core.first; + + for(const auto& entry : core.second) + { + const auto& label = entry.first; + const auto& data = entry.second; + + const auto avgCycles = (double)data.m_sum / data.m_values.size(); + const auto avgTime = avgCycles / ESP.getCpuFreqMHz(); + + maxLabelSize = std::max( + maxLabelSize, + label.size() + 4); + maxCycleSize = std::max( + maxCycleSize, + (uint32_t)std::log10(avgCycles) + 1 + 4); + maxTimeSize = std::max( + maxTimeSize, + (uint32_t)std::log10(avgTime) + 1 + 4); + + if(!data.m_values.empty()) + { + temp.emplace_back(coreId, label, avgCycles, avgTime); + } + } + } + + maxCycleSize = constrain(maxCycleSize, cycleHeader.size(), 30); + maxTimeSize = constrain(maxTimeSize, timeHeader.size(), 30); + + std::vector results; + std::ostringstream stream; + stream << std::fixed; + + stream + << "| " + << std::setw(maxLabelSize) << std::left + << labelHeader + << " | " + << std::setw(maxCycleSize) << std::left + << cycleHeader + << " | " + << std::setw(maxTimeSize) << std::left + << timeHeader + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + + stream + << "|-" + << std::string(maxLabelSize, '-') + << "-|-" + << std::string(maxCycleSize, '-') + << "-|-" + << std::string(maxTimeSize, '-') + << "-|"; + results.emplace_back(stream.str()); + stream.seekp(0); + + for(const auto& tuple : temp) + { + stream + << "| " + << "["<< std::get<0>(tuple) << "] " + << std::setw(maxLabelSize - 4) << std::left + << std::get<1>(tuple) + << " | " + << std::setw(maxCycleSize) << std::right << std::setprecision(3) + << std::get<2>(tuple) + << " | " + << std::setw(maxTimeSize) << std::right << std::setprecision(3) + << std::get<3>(tuple) + << " |"; + results.emplace_back(stream.str()); + stream.seekp(0); + } + + return results; +} + +PerformanceMonitor PerfMon; diff --git a/firmware/haptics/template/firmware/src/utils/serial.cpp b/firmware/haptics/template/firmware/src/utils/serial.cpp new file mode 100644 index 0000000..9d8623e --- /dev/null +++ b/firmware/haptics/template/firmware/src/utils/serial.cpp @@ -0,0 +1,759 @@ +#include "utils/serial.hpp" + +#include + +#include "hardware/panto.hpp" +#include "physics/pantoPhysics.hpp" +#include "utils/vector.hpp" + +bool DPSerial::s_rxBufferCritical = false; +Header DPSerial::s_header = Header(); +uint8_t DPSerial::s_debugLogBuffer[c_debugLogBufferSize]; +std::queue DPSerial::s_debugLogQueue; +portMUX_TYPE DPSerial::s_serialMutex = portMUX_INITIALIZER_UNLOCKED; +ReceiveState DPSerial::s_receiveState = NONE; +uint8_t DPSerial::s_expectedPacketId = 1; +bool DPSerial::s_connected = false; +unsigned long DPSerial::s_lastHeartbeatTime = 0; +uint16_t DPSerial::s_unacknowledgedHeartbeats = 0; +std::map + DPSerial::s_receiveHandlers = { + {SYNC_ACK, DPSerial::receiveSyncAck}, + {HEARTBEAT_ACK, DPSerial::receiveHearbeatAck}, + {MOTOR, DPSerial::receiveMotor}, + {PID, DPSerial::receivePID}, + {SPEED, DPSerial::receiveSpeed}, + {CREATE_OBSTACLE, DPSerial::receiveCreateObstacle}, + {ADD_TO_OBSTACLE, DPSerial::receiveAddToObstacle}, + {REMOVE_OBSTACLE, DPSerial::receiveRemoveObstacle}, + {ENABLE_OBSTACLE, DPSerial::receiveEnableObstacle}, + {DISABLE_OBSTACLE, DPSerial::receiveDisableObstacle}, + {CALIBRATE_PANTO, DPSerial::receiveCalibrationRequest}, + {DUMP_HASHTABLE, DPSerial::receiveDumpHashtable}, + {CREATE_PASSABLE_OBSTACLE, DPSerial::receiveCreatePassableObstacle}, + {CREATE_RAIL, DPSerial::receiveCreateRail}, + {FREEZE, DPSerial::receiveFreeze}, + {FREE, DPSerial::receiveFree}, + {SPEED_CONTROL, DPSerial::receiveSpeedControl}}; + +// === private === + +// send helper + +void DPSerial::sendUInt8(uint8_t data) +{ + Serial.write(data); +} + +void DPSerial::sendInt16(int16_t data) +{ + Serial.write(static_cast(data >> 8)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt16(uint16_t data) +{ + sendInt16(reinterpret_cast(data)); +} + +void DPSerial::sendInt32(int32_t data) +{ + Serial.write(static_cast(data >> 24)); + Serial.write(static_cast((data >> 16) & 255)); + Serial.write(static_cast((data >> 8) & 255)); + Serial.write(static_cast(data & 255)); +} + +void DPSerial::sendUInt32(uint32_t data) +{ + sendInt32(reinterpret_cast(data)); +} + +void DPSerial::sendFloat(float data) +{ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + sendInt32(reinterpret_cast(data)); +#pragma GCC diagnostic pop +} + +void DPSerial::sendMessageType(MessageType data) +{ + Serial.write(data); +} + +void DPSerial::sendMagicNumber() +{ + for (auto i = 0; i < c_magicNumberSize; ++i) + { + sendUInt8(c_magicNumber[i]); + } +}; + +void DPSerial::sendHeader(MessageType messageType, uint16_t payloadSize) +{ + sendMessageType(messageType); + sendUInt8(0); // no panto -> fw tracked packages + sendUInt16(payloadSize); +}; + +// send + +void DPSerial::sendSync() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(SYNC, 4); + sendUInt32(c_revision); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendHeartbeat() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(HEARTBEAT, 0); + s_unacknowledgedHeartbeats++; + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferCritical() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_CRITICAL, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendBufferReady() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(BUFFER_READY, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendPacketAck(uint8_t id) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(PACKET_ACK, 1); + sendUInt8(id); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidPacketId(uint8_t expected, uint8_t received) +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_PACKET_ID, 2); + sendUInt8(expected); + sendUInt8(received); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInvalidData() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(INVALID_DATA, 0); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive helper + +uint8_t DPSerial::receiveUInt8() +{ + return static_cast(Serial.read()); +} + +int16_t DPSerial::receiveInt16() +{ + return Serial.read() << 8 | Serial.read(); +} + +uint16_t DPSerial::receiveUInt16() +{ + auto temp = receiveInt16(); + return reinterpret_cast(temp); +} + +int32_t DPSerial::receiveInt32() +{ + return Serial.read() << 24 | Serial.read() << 16 | Serial.read() << 8 | Serial.read(); +} + +uint32_t DPSerial::receiveUInt32() +{ + auto temp = receiveInt32(); + return reinterpret_cast(temp); +} + +float DPSerial::receiveFloat() +{ + auto temp = receiveInt32(); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wstrict-aliasing" + return reinterpret_cast(temp); +#pragma GCC diagnostic pop +} + +MessageType DPSerial::receiveMessageType() +{ + return static_cast(Serial.read()); +} + +void DPSerial::checkBuffer() +{ + const auto available = Serial.available(); + if (available > c_rxBufferCriticalThreshold && !s_rxBufferCritical) + { + s_rxBufferCritical = true; + sendBufferCritical(); + } + if (available < c_rxBufferReadyThreshold && s_rxBufferCritical) + { + s_rxBufferCritical = false; + sendBufferReady(); + } +} + +bool DPSerial::receiveMagicNumber() +{ + int magicNumberProgress = 0; + + bool invalidData = false; + + // as long as enough data is available to find the magic number + while (Serial.available() >= c_magicNumberSize) + { + const uint8_t read = Serial.read(); + const uint8_t expected = c_magicNumber[magicNumberProgress]; + // does next byte fit expected by of magic number? + if (read == expected) + { + // yes - increase index. If check complete, return true. + if (++magicNumberProgress == c_magicNumberSize) + { + s_receiveState = FOUND_MAGIC; + return true; + } + } + else + { + // no - reset search progress + // sendInstantDebugLog( + // "Error: expected %x, read %x. State might by faulty!", + // expected, + // read); + invalidData = true; + magicNumberProgress = 0; + } + } + + if (invalidData) + { + sendInvalidData(); + } + + // ran out of available data before finding complete number - return false + return false; +}; + +bool DPSerial::receiveHeader() +{ + // make sure enough data is available + if (Serial.available() < c_headerSize) + { + return false; + } + + s_header.MessageType = receiveUInt8(); + s_header.PacketId = receiveUInt8(); + s_header.PayloadSize = receiveUInt16(); + s_receiveState = FOUND_HEADER; + return true; +}; + +bool DPSerial::payloadReady() +{ + return Serial.available() >= s_header.PayloadSize; +}; + +// receive + +void DPSerial::receiveSyncAck() +{ + s_connected = true; +}; + +void DPSerial::receiveHearbeatAck() +{ + s_unacknowledgedHeartbeats = 0; +}; + +void DPSerial::receiveMotor() +{ + const auto controlMethod = receiveUInt8(); + const auto pantoIndex = receiveUInt8(); + + const auto target = Vector2D(receiveFloat(), receiveFloat()); + if (!isnan(target.x) && !isnan(target.y)) + { + pantos[pantoIndex].setInTransition(true); + DPSerial::sendInstantDebugLog("In Transition"); + } + pantos[pantoIndex].setRotation(receiveFloat()); + pantos[pantoIndex].setTarget(target, controlMethod == 1); +}; + +void DPSerial::receivePID() +{ + auto motorIndex = receiveUInt8(); + + for (auto i = 0; i < 3; ++i) + { + pidFactor[motorIndex][i] = receiveFloat(); + } +}; + +void DPSerial::receiveSpeed() +{ + auto pantoIndex = receiveUInt8(); //0 or 1 + auto speed = receiveFloat(); + pantos[pantoIndex].setSpeed(speed); +}; + +void DPSerial::receiveCreateObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, false); + } + } +} + +void DPSerial::receiveCreatePassableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createObstacle(id, path, true); + } + } +} + +void DPSerial::receiveCreateRail() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + auto displacement = (double)receiveFloat(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->createRail(id, path, displacement); + } + } +} + +void DPSerial::receiveAddToObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + auto vecCount = (s_header.PayloadSize - 1 - 2) / (4 * 2); + + std::vector path; + path.reserve(vecCount); + + for (auto i = 0; i < vecCount; ++i) + { + path.emplace_back((double)receiveFloat(), (double)receiveFloat()); + } + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->addToObstacle(id, path); + } + } +} + +void DPSerial::receiveRemoveObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->removeObstacle(id); + } + } +} + +void DPSerial::receiveEnableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id); + } + } +} + +void DPSerial::receiveDisableObstacle() +{ + auto pantoIndex = receiveUInt8(); + auto id = receiveUInt16(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->enableObstacle(id, false); + } + } +} + +void DPSerial::receiveFreeze() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + const auto target = pantos[i].getPosition(); + pantos[i].setTarget(target, 0); + pantos[i].setRotation(pantos[i].getRotation()); + pantos[i].setIsFrozen(true); + } + } +} + +void DPSerial::receiveFree() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantos[i].setTarget(Vector2D(NAN, NAN), 0); + pantos[i].setRotation(NAN); + pantos[i].setInTransition(false); + pantos[i].setIsFrozen(false); + } + } +} + +void DPSerial::receiveSpeedControl() +{ + auto tethered = receiveUInt8(); //0 or 1 + auto tetherFactor = receiveFloat(); + auto tetherInnerRadius = receiveFloat(); + auto tetherOuterRadius = receiveFloat(); + auto tetherStrategy = receiveUInt8(); // 0 for MaxSpeed, 1 for Exploration, 2 for Leash + OutOfTetherStrategy strategy; + switch (tetherStrategy) + { + case 0: + strategy = MaxSpeed; + break; + case 1: + strategy = Exploration; + break; + case 2: + strategy = Leash; + break; + default: + break; + } + auto pockEnabled = receiveUInt8(); //0 or 1 + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + pantoPhysics[i].godObject()->setSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); + } +} + +void DPSerial::receiveCalibrationRequest() +{ + DPSerial::sendInstantDebugLog("=== Calibration Request received ==="); + for (auto i = 0; i < pantoCount; ++i) + { + pantos[i].calibratePanto(); + } +} + +void DPSerial::receiveDumpHashtable() +{ + auto pantoIndex = receiveUInt8(); + for (auto i = 0; i < pantoPhysics.size(); ++i) + { + if (pantoIndex == 0xFF || i == pantoIndex) + { + pantoPhysics[i].godObject()->dumpHashtable(); + } + } +} + +void DPSerial::receiveInvalid() +{ + sendQueuedDebugLog( + "Received invalid message: [%02X, %02X, %04X] %", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); +}; + +// === public === + +// setup + +void DPSerial::init() +{ + Serial.flush(); + Serial.begin(c_baudRate); + Serial.setRxBufferSize(c_rxBufferSize); +} + +bool DPSerial::ensureConnection() +{ + if (!s_connected) + { + sendSync(); + // delay to avoid spamming sync messages + delay(10); + return false; + } + + if (s_unacknowledgedHeartbeats > c_maxUnacklowledgedHeartbeats) + { + sendQueuedDebugLog("Disconnected due to too many unacklowledged heartbeats."); + s_unacknowledgedHeartbeats = 0; + s_connected = false; + return false; + } + + if (millis() > s_lastHeartbeatTime + c_heartbeatIntervalMs || s_lastHeartbeatTime == 0) + { + sendHeartbeat(); + s_lastHeartbeatTime = millis(); + } + + return true; +} + +// send + +void DPSerial::sendPosition() +{ + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(POSITION, pantoCount * 5 * 4); // five values per panto, 4 bytes each + + for (auto i = 0; i < pantoCount; ++i) + { + const auto panto = pantos[i]; + const auto pos = panto.getPosition(); + sendFloat(pos.x); + sendFloat(pos.y); + sendFloat(panto.getRotation()); + auto goPos = pantoPhysics[i].godObject()->getPosition(); + sendFloat(goPos.x); + sendFloat(goPos.y); + } + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendInstantDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length + 1, 0, c_debugLogBufferSize); + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write(s_debugLogBuffer, length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendQueuedDebugLog(const char *message, ...) +{ + portENTER_CRITICAL(&s_serialMutex); + va_list args; + va_start(args, message); + uint16_t length = vsnprintf(reinterpret_cast(s_debugLogBuffer), c_debugLogBufferSize, message, args); + va_end(args); + length = constrain(length, 0, c_debugLogBufferSize); + s_debugLogQueue.emplace(reinterpret_cast(s_debugLogBuffer), length); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::sendTransitionEnded(uint8_t panto) +{ + // signal when tweening is over + portENTER_CRITICAL(&s_serialMutex); + sendMagicNumber(); + sendHeader(TRANSITION_ENDED, 1); // 1 byte for the panto index is enough + sendUInt8(panto); + portEXIT_CRITICAL(&s_serialMutex); +}; + +void DPSerial::processDebugLogQueue() +{ + portENTER_CRITICAL(&s_serialMutex); + // quick check to avoid loop if not necessary + if (!s_debugLogQueue.empty()) + { + for (auto i = 0; i < c_processedQueuedMessagesPerFrame; ++i) + { + if (!s_debugLogQueue.empty()) + { + auto &msg = s_debugLogQueue.front(); + auto length = msg.length() + 1; + sendMagicNumber(); + sendHeader(DEBUG_LOG, length); + Serial.write( + reinterpret_cast(msg.c_str()), + length); + s_debugLogQueue.pop(); + } + } + } + portEXIT_CRITICAL(&s_serialMutex); +} + +void DPSerial::sendDebugData() +{ + const auto pos0 = pantos[0].getPosition(); + const auto pos1 = pantos[1].getPosition(); + portENTER_CRITICAL(&s_serialMutex); + sendInstantDebugLog( + "[ang/0] %+08.3f | %+08.3f | %+08.3f [ang/1] %+08.3f | %+08.3f | %+08.3f [pos/0] %+08.3f | %+08.3f | %+08.3f [pos/1] %+08.3f | %+08.3f | %+08.3f", + degrees(pantos[0].getActuationAngle(0)), + degrees(pantos[0].getActuationAngle(1)), + degrees(pantos[0].getActuationAngle(2)), + degrees(pantos[1].getActuationAngle(0)), + degrees(pantos[1].getActuationAngle(1)), + degrees(pantos[1].getActuationAngle(2)), + pos0.x, + pos0.y, + degrees(pantos[0].getRotation()), + pos1.x, + pos1.y, + degrees(pantos[1].getRotation())); + portEXIT_CRITICAL(&s_serialMutex); +}; + +// receive + +void DPSerial::receive() +{ + checkBuffer(); + + if (s_receiveState == NONE && !receiveMagicNumber()) + { + return; + } + + if (s_receiveState == FOUND_MAGIC && !receiveHeader()) + { + return; + } + + if (s_receiveState == FOUND_HEADER && !payloadReady()) + { + return; + } + + if (!s_connected && s_header.MessageType != SYNC_ACK) + { + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + Serial.read(); + } + sendInstantDebugLog( + "Not connected, ignoring [%X %i %i]", + s_header.MessageType, + s_header.PacketId, + s_header.PayloadSize); + return; + } + + s_receiveState = NONE; + + if (s_header.PacketId > 0) + { + if (s_header.PacketId != s_expectedPacketId) + { + sendInvalidPacketId(s_expectedPacketId, s_header.PacketId); + return; + } + + sendPacketAck(s_header.PacketId); + s_expectedPacketId++; + if (s_expectedPacketId == 0) + { + s_expectedPacketId++; + } + } + + auto handler = s_receiveHandlers.find((MessageType)(s_header.MessageType)); + + if (handler == s_receiveHandlers.end()) + { + receiveInvalid(); + } + else + { + handler->second(); + } +}; diff --git a/firmware/haptics/template/firmware/src/utils/vector.cpp b/firmware/haptics/template/firmware/src/utils/vector.cpp new file mode 100644 index 0000000..34826d6 --- /dev/null +++ b/firmware/haptics/template/firmware/src/utils/vector.cpp @@ -0,0 +1,85 @@ +#include "utils/vector.hpp" + +#include + +Vector2D Vector2D::fromPolar(double angle, double length) +{ + return Vector2D(cos(angle) * length, sin(angle) * length); +}; + +double Vector2D::determinant(const Vector2D& first, const Vector2D& second) +{ + return (first.x * second.y) - (first.y * second.x); +}; + +double Vector2D::length() const +{ + return sqrt(x * x + y * y); +}; + +double Vector2D::angle() const +{ + return atan2(y, x); +}; + +Vector2D Vector2D::normalize() const +{ + return Vector2D(x / length(), y / length()); +} + +Vector2D Vector2D::operator+(const Vector2D& other) const +{ + return Vector2D(x + other.x, y + other.y); +}; + +Vector2D Vector2D::operator-(const Vector2D& other) const +{ + return Vector2D(x - other.x, y - other.y); +}; + +double Vector2D::operator*(const Vector2D& other) const +{ + return x * other.x + y * other.y; +}; + +Vector2D Vector2D::operator*(const double scale) const +{ + return Vector2D(x * scale, y * scale); +}; + +bool Vector2D::operator==(const Vector2D &other) const +{ + return x == other.x && y == other.y; +}; + +bool Vector2D::operator!=(const Vector2D &other) const +{ + return x != other.x || y != other.y; +}; + +double Vector2D::distancePointToLineSegment(Vector2D a, Vector2D b) { + // a is the start of the line segment, b is the end + // distance is measured from the point p + Vector2D ab = b - a; + Vector2D ae = *this - a; + Vector2D be = *this - b; + + double ab_ae = ab * ae; + double ab_be = ab * be; + + if (ab_be > 0){ + // Point is above line segment + return be.length(); + } else if (ab_ae < 0) { + // Point is below line segment + return ae.length(); + } else { + // Finding the perpendicular distance + double x1 = ab.x; + double y1 = ab.y; + double x2 = ae.x; + double y2 = ae.y; + double mod = sqrt(x1 * x1 + y1 * y1); + return abs(x1 * y2 - y1 * x2) / mod; + } +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/backtrace/backtrace.sh b/firmware/haptics/template/utils/backtrace/backtrace.sh new file mode 100644 index 0000000..4b75654 --- /dev/null +++ b/firmware/haptics/template/utils/backtrace/backtrace.sh @@ -0,0 +1,26 @@ +#!/bin/bash +# Use this to analyze the ESP32's backtrace in case of a crash. +# Requires gdb to be accessable from bash. +# In order to find line numbers, add -g3 to the PlatformIO build flags. +# The default target is set for running this script from the base framework dir. +# Alternatively, you may pass an path to the elf as the first argument. +# Default usage: ./utils/backtrace/backtrace.sh "0x40085698:0x3ffb5e80 [...]" +# Alternative usage example for running from this dir: ./backtrace.sh ./../../firmware/.pioenvs/esp32dev/firmware.elf "0x40085698:0x3ffb5e80 [...]" + +if [ "$#" -eq 1 ]; then + target=./firmware/.pioenvs/esp32dev/firmware.elf + backtrace=$1 + echo "Using default target $target" +else + target=$1 + backtrace=$2 + echo "Using custom target $1" +fi + +file=$(mktemp) +echo $backtrace \ +| sed -r 's/:0x[[:xdigit:]]{8}\s?/\n/g' \ +| sed '/^[[:space:]]*$/d' \ +| sed 's/^.*$/echo === \0 ===\\n\ninfo symbol \0\ninfo line *\0/g' > $file +gdb -batch -x $file $target +rm $file diff --git a/firmware/haptics/template/utils/protocol/include/protocol/header.hpp b/firmware/haptics/template/utils/protocol/include/protocol/header.hpp new file mode 100644 index 0000000..f6b1d51 --- /dev/null +++ b/firmware/haptics/template/utils/protocol/include/protocol/header.hpp @@ -0,0 +1,14 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +struct Header +{ + uint8_t MessageType = 0; + uint8_t PacketId = 0; + uint16_t PayloadSize = 0; +}; diff --git a/firmware/haptics/template/utils/protocol/include/protocol/messageType.hpp b/firmware/haptics/template/utils/protocol/include/protocol/messageType.hpp new file mode 100644 index 0000000..0e9566e --- /dev/null +++ b/firmware/haptics/template/utils/protocol/include/protocol/messageType.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +enum MessageType +{ + SYNC = 0x00, + HEARTBEAT = 0x01, + BUFFER_CRITICAL = 0x02, + BUFFER_READY = 0x03, + PACKET_ACK = 0x04, + INVALID_PACKET_ID = 0x05, + INVALID_DATA = 0x06, + POSITION = 0x10, + DEBUG_LOG = 0x20, + SYNC_ACK = 0x80, + HEARTBEAT_ACK = 0x81, + MOTOR = 0x90, + PID = 0x91, + SPEED = 0x92, + TRANSITION_ENDED = 0x93, + CREATE_OBSTACLE = 0xA0, + ADD_TO_OBSTACLE = 0xA1, + REMOVE_OBSTACLE = 0xA2, + ENABLE_OBSTACLE = 0xA3, + DISABLE_OBSTACLE = 0xA4, + CALIBRATE_PANTO = 0xA5, + CREATE_PASSABLE_OBSTACLE = 0xA6, + CREATE_RAIL = 0xA7, + FREEZE = 0xA8, + FREE = 0xA9, + SPEED_CONTROL = 0xAA, + DUMP_HASHTABLE = 0xC0, +}; + +extern std::set TrackedMessageTypes; diff --git a/firmware/haptics/template/utils/protocol/include/protocol/protocol.hpp b/firmware/haptics/template/utils/protocol/include/protocol/protocol.hpp new file mode 100644 index 0000000..a2e41c3 --- /dev/null +++ b/firmware/haptics/template/utils/protocol/include/protocol/protocol.hpp @@ -0,0 +1,29 @@ +#pragma once + +#ifdef ARDUINO +#include +#else +#include +#endif + +class DPProtocol +{ +protected: + // revision + static const uint32_t c_revision = 6; + + // connection info + static const uint32_t c_baudRate = 115200; + + // magic number + static const uint8_t c_magicNumber[]; + static const uint8_t c_magicNumberSize = 2; + + // data size + static const uint8_t c_headerSize = 4; + static const uint16_t c_maxPayloadSize = 256; + + // combined maximal size of packet + static const uint16_t c_maxPacketSize = + c_maxPayloadSize + c_magicNumberSize + c_headerSize; +}; diff --git a/firmware/haptics/template/utils/protocol/src/protocol/messageType.cpp b/firmware/haptics/template/utils/protocol/src/protocol/messageType.cpp new file mode 100644 index 0000000..4e57c18 --- /dev/null +++ b/firmware/haptics/template/utils/protocol/src/protocol/messageType.cpp @@ -0,0 +1,10 @@ +#include "protocol/messageType.hpp" + +std::set TrackedMessageTypes = { + CREATE_OBSTACLE, + ADD_TO_OBSTACLE, + REMOVE_OBSTACLE, + ENABLE_OBSTACLE, + DISABLE_OBSTACLE, + CREATE_PASSABLE_OBSTACLE, + CREATE_RAIL}; diff --git a/firmware/haptics/template/utils/protocol/src/protocol/protocol.cpp b/firmware/haptics/template/utils/protocol/src/protocol/protocol.cpp new file mode 100644 index 0000000..6871f88 --- /dev/null +++ b/firmware/haptics/template/utils/protocol/src/protocol/protocol.cpp @@ -0,0 +1,3 @@ +#include "protocol/protocol.hpp" + +const uint8_t DPProtocol::c_magicNumber[] = {0x44, 0x50}; \ No newline at end of file diff --git a/firmware/haptics/template/utils/scripts/generateHardwareConfig.js b/firmware/haptics/template/utils/scripts/generateHardwareConfig.js new file mode 100644 index 0000000..563e783 --- /dev/null +++ b/firmware/haptics/template/utils/scripts/generateHardwareConfig.js @@ -0,0 +1,269 @@ +// just assume the input config file is correct and we don't need this check +/* eslint-disable guard-for-in */ +// also, this is just a helper script and does not need documentation +/* eslint-disable require-jsdoc */ +// the template strings can't ne broken into multiple lines +/* eslint-disable max-len */ +const input = require('../../hardware/'+process.argv[2]+'.json'); +const fs = require('fs'); +const crypto = require('crypto'); +const hash = crypto.createHash('md5').update(JSON.stringify(input)).digest(); +const aggregates = {}; + +function insert(index, categoryName, valueName, value) { + const aggregate = categoryName+'_'+valueName; + if (!aggregates[aggregate]) { + aggregates[aggregate] = []; + } + aggregates[aggregate][index] = value; +} + +let index = 0; let pantoCount = 0; +for (const pantoName in input.pantos) { + ++pantoCount; + const panto = input.pantos[pantoName]; + for (const dofName in panto) { + const dof = panto[dofName]; + for (const categoryName in dof) { + const category = dof[categoryName]; + for (const valueName in category) { + const value = category[valueName]; + insert(index, categoryName, valueName, value); + } + } + ++index; + } +} + +function aggregate(name, valueIfUndefined = 0, map = ((x) => x)) { + let array = aggregates[name]; + if (!array) { + array = []; + } + for (let i = 0; i < index; ++i) { + if (array[i] == undefined) { + array[i] = valueIfUndefined; + } + if (array[i] instanceof Array) { + array[i] = `{${array[i].join(', ')}}`; + } + } + return array.map(map).join(', '); +} + +function count(name) { + const array = aggregates[name]; + if (!array) { + return 0; + } + let count = 0; + for (let i = 0; i < index; ++i) { + if (array[i] != undefined) { + count++; + } + } + return count; +} + +function getRangeForMotor(motor) { + const centerX = motor.linkage.baseX; + const centerY = motor.linkage.baseY; + const range = motor.linkage.innerLength + motor.linkage.outerLength; + return { + minX: centerX - range, + minY: centerY - range, + maxX: centerX + range, + maxY: centerY + range + }; +} + +function getRangeForPanto(panto) { + const left = getRangeForMotor(panto.left); + const right = getRangeForMotor(panto.right); + return { + minX: Math.min(left.minX, right.minX), + minY: Math.min(left.minY, right.minY), + maxX: Math.max(left.maxX, right.maxX), + maxY: Math.max(left.maxY, right.maxY) + }; +} + +function getRange() { + const upper = getRangeForPanto(input.pantos.upper); + const lower = getRangeForPanto(input.pantos.lower); + return { + minX: Math.min(upper.minX, lower.minX), + minY: Math.min(upper.minY, lower.minY), + maxX: Math.max(upper.maxX, lower.maxX), + maxY: Math.max(upper.maxY, lower.maxY) + }; +} + +let range; +if (input.range) { + range = input.range; +} else { + range = getRange(); +} + +// calculate these now because sqrt isn't a constexpr +function getHashtableSettings() { + const rangeX = range.maxX - range.minX; + const rangeY = range.maxY - range.minY; + const maxMemory = input.hashtableMemory / pantoCount; + const sizeofVector = 12; // = sizeof(std::vector) + const maxCells = Math.floor(maxMemory / sizeofVector); + const targetStepSize = Math.sqrt(rangeX * rangeY / maxCells); + const stepsX = Math.floor(rangeX / targetStepSize); + const stepsY = Math.floor(rangeY / targetStepSize); + const stepSizeX = rangeX / stepsX; + const stepSizeY = rangeY / stepsY; + const numCells = stepsX * stepsY; + const usedMemory = numCells * sizeofVector; + + return { + maxMemory, + usedMemory, + maxCells, + numCells, + stepsX, + stepsY, + stepSizeX, + stepSizeY + }; +} + +const hashtable = getHashtableSettings(); + +const headerOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.hpp contains hardware specific data like the pantograph size and the I/O pins. + * It is generated by GenerateHardwareConfig.js using the hardware specifications found in the Hardware dir. + * + * In order to avoid additional checks, unused data is rerouted to invalid pins instead of filtering all calls. + * For the current configuration, this dummy pin is ${input.dummyPin}. Any assignments to this pin will be ignored, all reads from this pin will return 0. + */ + +#pragma once + +#include + +const uint8_t configHash[] = {${Array.from(hash).map((x) => '0x'+('0'+(Number(x).toString(16))).slice(-2).toUpperCase()).join(', ')}}; +const float opMinDist = ${input.opMinDist}, + opMaxDist = ${input.opMaxDist}, + opAngle = ${input.opAngle}; +extern float forceFactor; +const uint8_t pantoCount = ${pantoCount}; +const uint8_t dummyPin = ${input.dummyPin}; +${input.usesSpi ? '#define LINKAGE_ENCODER_USE_SPI' : ''} +const uint32_t numberOfSpiEncoders = ${count('encoder_spiIndex')}; +const float linkageBaseX[] = { + ${aggregate('linkage_baseX')} +}; +const float linkageBaseY[] = { + ${aggregate('linkage_baseY')} +}; +const float linkageInnerLength[] = { + ${aggregate('linkage_innerLength')} +}; +const float linkageOuterLength[] = { + ${aggregate('linkage_outerLength')} +}; +const uint8_t linkageHandleMount[] = { + ${aggregate('linkage_handleMount')} +}; +const float motorPowerLimit[] = { + ${aggregate('motor_powerLimit')} +}; +const float motor_powerLimitForce[] = { + ${aggregate('motor_powerLimitForce')} +}; +extern float pidFactor[${pantoCount*3}][3]; +const float forceP = ${input.forcePidFactor[0]}; +const float forceI = ${input.forcePidFactor[1]}; +const float forceD = ${input.forcePidFactor[2]}; +const float forcePidFactor[2][3] = { + {forceP, forceI, forceD}, {forceP, forceI, forceD} +}; +const uint8_t motorPwmPin[] = { + ${aggregate('motor_pwmPin', input.dummyPin)} +}; +const uint8_t motorPwmPinForwards[] = { + ${aggregate('motor_pwmPinForwards', input.dummyPin)} +}; +const uint8_t motorPwmPinBackwards[] = { + ${aggregate('motor_pwmPinBackwards', input.dummyPin)} +}; +const uint8_t motorDirAPin[] = { + ${aggregate('motor_dirAPin', input.dummyPin)} +}; +const uint8_t motorDirBPin[] = { + ${aggregate('motor_dirBPin', input.dummyPin)} +}; +const bool motorFlipped[] = { + ${aggregate('motor_flipped')} +}; +const uint8_t encoderAPin[] = { + ${aggregate('encoder_aPin', input.dummyPin)} +}; +const uint8_t encoderBPin[] = { + ${aggregate('encoder_bPin', input.dummyPin)} +}; +const uint8_t encoderIndexPin[] = { + ${aggregate('encoder_indexPin', input.dummyPin)} +}; +const uint32_t encoderSteps[] = { + ${aggregate('encoder_steps')} +}; +const uint32_t encoderSpiIndex[] = { + ${aggregate('encoder_spiIndex', 0xffffffff)} +}; +const float encoderFlipped[] = { + ${aggregate('encoder_flipped', false, (x) => x ? -1 : 1)} +}; +const float setupAngle[] = { + ${aggregate('encoder_setup')} +}; +constexpr float rangeMinX = ${range.minX}; +constexpr float rangeMinY = ${range.minY}; +constexpr float rangeMaxX = ${range.maxX}; +constexpr float rangeMaxY = ${range.maxY}; +constexpr uint32_t hashtableMaxMemory = ${hashtable.maxMemory}; +constexpr uint32_t hashtableUsedMemory = ${hashtable.usedMemory}; +constexpr uint32_t hashtableMaxCells = ${hashtable.maxCells}; +constexpr uint32_t hashtableNumCells = ${hashtable.numCells}; +constexpr uint32_t hashtableStepsX = ${hashtable.stepsX}; +constexpr uint32_t hashtableStepsY = ${hashtable.stepsY}; +constexpr double hashtableStepSizeX = ${hashtable.stepSizeX}; +constexpr double hashtableStepSizeY = ${hashtable.stepSizeY}; +const uint32_t obstacleChangesPerFrame = ${input.obstacleChangesPerFrame};`; + +console.log(headerOutput); +const headerDir = 'firmware/include/config/'; +if (!fs.existsSync(headerDir)) { + fs.mkdirSync(headerDir, {recursive: true}); +} +fs.writeFileSync(headerDir + 'config.hpp', headerOutput); + +const sourceOutput = +`/* + * This file is generated by GenerateHardwareConfig.js and ignored in git. Any changes you apply will *not* persist. + * + * config.cpp contains the initial values for the non-const global variables, since defining those in the header leads to linker errors. + */ + +#include "config/config.hpp" + +float forceFactor = ${input.forceFactor}; +float pidFactor[${pantoCount*3}][3] = { + ${aggregate('motor_pidFactor')} +};`; + +console.log(sourceOutput); +const sourceDir = 'firmware/src/config/'; +if (!fs.existsSync(sourceDir)) { + fs.mkdirSync(sourceDir, {recursive: true}); +} +fs.writeFileSync(sourceDir + 'config.cpp', sourceOutput); diff --git a/firmware/haptics/template/utils/scripts/run.js b/firmware/haptics/template/utils/scripts/run.js new file mode 100644 index 0000000..96fae21 --- /dev/null +++ b/firmware/haptics/template/utils/scripts/run.js @@ -0,0 +1,222 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const os = require('os'); +const path = require('path'); +const {exec, remove, color, escape} = require('./tools'); + +function log(message, messageColor) { + console.log(`${messageColor}${message}${color.reset}`); +} + +const buildHandlers = { + 'framework': () => { + return build('voice-command') + & build('serial-plugin') + & build('serial-standalone'); + }, + 'voice-command': () => { + return exec('node', ['./utils/voiceCommand/build/build-release.js']); + }, + 'serial-plugin': () => { + const gypDef = '--cppdefs="NODE_GYP ' + escape(cppDefines.join(' ')) + '"'; + return exec('node-gyp', ['configure', '-C utils/serial', gypDef]) + & exec('node-gyp', ['build', '-C utils/serial']); + }, + 'serial-standalone': () => { + return exec( + cppExec, + cppArgs.concat(cppDefines.map((d) => cppDefinePrefix + d)).concat([ + 'utils/serial/src/standalone/main.cpp', + 'utils/serial/src/standalone/standalone.cpp', + 'utils/serial/src/serial/shared.cpp', + 'utils/serial/src/cppLib/lib.cpp', + 'utils/serial/src/crashAnalyzer/analyze.cpp', + 'utils/serial/src/crashAnalyzer/buffer.cpp', + process.platform == 'win32' ? + 'utils/serial/src/serial/win.cpp' : + 'utils/serial/src/serial/unix.cpp', + 'utils/protocol/src/protocol/protocol.cpp', + '-Iutils/serial/include', + '-Iutils/protocol/include', + '-o utils/serial/serial'])); + }, + 'unity-serial': () =>{ + return unity(); + }, + 'firmware': () => { + return config(process.argv[4]) + & platformio('build'); + } +}; + +function build(target) { + if (target === undefined) { + return build('firmware'); + // 14.5.21: The js framework is deprecated since this + // commit: a045c86fa3754810a9a68b1bc89dcf990d883579 + // To make it work again we have to implement the acknowledgement + // logic, similar to the way it's done in this + // commit: faa30310e884784b5d431ac65c05e3186b9bafab + return build('framework') + & build('firmware'); + } + + if (!buildHandlers.hasOwnProperty(target)) { + log(`Invalid build target ${target}`, color.red); + return false; + } + + log(`Building ${target}`, color.green); + const result = buildHandlers[target](); + if (result) { + log(`Building ${target} successful`, color.green); + } else { + log(`Building ${target} failed`, color.red); + } + return result; +} + +const cleanHandlers = { + 'framework': () => { + return clean('voice-command') + & clean('serial-plugin') + & clean('serial-standalone'); + }, + 'voice-command': () => { + return remove('./utils/voiceCommand/.bin'); + }, + 'serial-plugin': () => { + return remove('./utils/serial/build'); + }, + 'serial-standalone': () => { + log('Clean serial-standalone not implemented yet', color.yellow); + return true; + }, + 'firmware': () => { + return remove('./firmware/src/config/config.cpp') + & remove('./firmware/include/config/config.hpp') + & platformio('clean'); + } +}; + +function clean(target) { + if (target === undefined) { + return clean('framework') + & clean('firmware'); + } + + if (!cleanHandlers.hasOwnProperty(target)) { + log(`Invalid clean target ${target}`, color.red); + return false; + } + + log(`Cleaning ${target}`, color.green); + const result = cleanHandlers[target](); + if (result) { + log(`Cleaning ${target} successful`, color.green); + } else { + log(`Cleaning ${target} failed`, color.red); + } + return result; +} + +function config(target) { + if (target === undefined) { + target = 'fiona'; + } + log(`Generating config ${target}`, color.green); + return exec('node', ['utils/scripts/generateHardwareConfig.js', target]); +} + +function platformio(command) { + if (command == 'build' || command === undefined) { + command = '.'; + } + + + log(`Running platformio ${command}`, color.green); + return exec(platformioExec, ['run', '-d firmware', `-t ${command}`]); +} + +function plotter() { + return exec('http-server', ['utils/plotter/']); +} + +function docs() { + log(`Building docs`, color.green); + return exec('node', ['utils/scripts/docs.js']); +} + +function unity() { + if (process.platform == 'win32') { + return exec('utils\\serial\\unity\\win.bat'); + } else if (process.platform == 'darwin') { + const unityDir = './utils/serial/unity/'; + return exec(unityDir+'mac.sh'); + } else { + return exec('echo "Linux is not supported for building unity framework."'); + } +} + +const handlers = { + 'build': build, + 'clean': clean, + 'config': config, + 'platformio': platformio, + 'plotter': plotter, + 'docs': docs, + 'unity': unity +}; + +const platformioDir = os.homedir() + '/.platformio'; +const xtensaUtilDir = platformioDir + '/packages/toolchain-xtensa32/bin/'; +const addr2linePath = path.join(xtensaUtilDir, 'xtensa-esp32-elf-addr2line'); + +let platformioExec; +let cppExec; +let cppDefinePrefix; +let cppArgs; +const cppDefines = [ + escape('ADDR2LINE_PATH=\"' + addr2linePath + '\"') +]; + +if (process.platform == 'win32') { + platformioExec = path.join(platformioDir, '/penv/Scripts/platformio'); + cppExec = 'cl'; + cppDefinePrefix = '/D'; + cppArgs = ['/Fo:Utils\\Serial\\']; + cppDefines.push('WINDOWS'); +} else { + if (exec('which', ['platformio'])) { + platformioExec = 'platformio'; + } else { + platformioExec = platformioDir + '/penv/bin/platformio'; + } + if (process.platform == 'linux') { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } else { + cppExec = 'g++'; + cppDefinePrefix = '-D'; + cppArgs = ['-std=c++11']; + } +} + +const command = process.argv[2]; +if (!handlers.hasOwnProperty(command)) { + log(`Unknown command ${command}`, color.red); + process.exitCode = 1; + return; +} + +log(`=== Running ${command} ===`, color.green); +const result = handlers[command](process.argv[3]); +if (result) { + log(`=== ${command} successful ===`, color.green); + process.exitCode = 0; +} else { + log(`=== ${command} failed ===`, color.red); + process.exitCode = 1; +} diff --git a/firmware/haptics/template/utils/scripts/tools.js b/firmware/haptics/template/utils/scripts/tools.js new file mode 100644 index 0000000..d125380 --- /dev/null +++ b/firmware/haptics/template/utils/scripts/tools.js @@ -0,0 +1,54 @@ +/* eslint-disable require-jsdoc */ +'use strict'; + +const childProcess = require('child_process'); +const path = require('path'); +const fs = require('fs'); + +const color = { + red: '\x1b[31m', + green: '\x1b[32m', + yellow: '\x1b[33m', + reset: '\x1b[0m' +}; + +function exec(cmd, args) { + return childProcess.spawnSync( + cmd, + args, + { + shell: true, + stdio: ['ignore', process.stdout, process.stderr] + }).status == 0; +}; + +function remove(target) { + if (!fs.existsSync(target)) { + console.log(`Could not find ${target}`, color.yellow); + return true; + } + if (fs.statSync(target).isDirectory()) { + const content = fs.readdirSync(target); + for (const entry in content) { + if (content.hasOwnProperty(entry)) { + remove(path.join(target, content[entry])); + } + } + fs.rmdirSync(target); + } else { + fs.unlinkSync(target); + } + console.log(`Removed ${target}`); + return true; +} + +function escape(string) { + return string.replace(/\\/g, '\\\\').replace(/"/g, '\\\"'); +} + +module.exports = { + exec, + remove, + color, + escape +}; diff --git a/firmware/haptics/template/utils/serial/CMakeLists.txt b/firmware/haptics/template/utils/serial/CMakeLists.txt new file mode 100644 index 0000000..edd3b8a --- /dev/null +++ b/firmware/haptics/template/utils/serial/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.9) + +# project settings + +project(serial CXX) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# sources + +set(serial_source_dir src) +set(protocol_source_dir ../protocol/src/protocol) +set(serial_include_dir include) +set(protocol_include_dir ../protocol/include) +set(base_include_dir include/serial) +set(generated_header_dir ${CMAKE_BINARY_DIR}/generated) + +set(export_header + ${generated_header_dir}/serial_export.hpp) + +set(public_headers + ${serial_include_dir}/libInterface.hpp + ${export_header}) + +set(private_headers + ${serial_include_dir}/serial.hpp + ${serial_include_dir}/packet.hpp + ${serial_include_dir}/crashAnalyzer.hpp + ${protocol_include_dir}/protocol/protocol.hpp + ${protocol_include_dir}/protocol/header.hpp + ${protocol_include_dir}/protocol/messageType.hpp) + +set(shared_sources + ${serial_source_dir}/cppLib/lib.cpp + ${serial_source_dir}/crashAnalyzer/analyze.cpp + ${serial_source_dir}/crashAnalyzer/buffer.cpp + ${serial_source_dir}/serial/shared.cpp + ${serial_source_dir}/serial/packet.cpp + ${protocol_source_dir}/protocol.cpp + ${protocol_source_dir}/messageType.cpp) + +if (WIN32) +# set path vars +set(addr2line "$ENV{USERPROFILE}\\.platformio\\packages\\toolchain-xtensa32\\bin\\xtensa-esp32-elf-addr2line.exe") +set(firmware_rel "..\\..\\firmware\\.pio\\build\\esp32dev\\firmware.elf") +get_filename_component(firmware_abs ${firmware_rel} ABSOLUTE) +# escape backslashes +string(REPLACE "\\" "\\\\" addr2line ${addr2line}) +string(REPLACE "/" "\\\\" firmware_abs ${firmware_abs}) +message(STATUS "addr2line: ${addr2line}") +message(STATUS "firmware (rel): ${firmware_rel}") +message(STATUS "firmware (abs): ${firmware_abs}") +# set defines +add_compile_definitions(WINDOWS) +add_compile_definitions(ADDR2LINE_PATH="${addr2line}") +add_compile_definitions(FIRMWARE_PATH="${firmware_abs}") +set(sources + ${shared_sources} + ${serial_source_dir}/serial/win.cpp) +elseif (UNIX) +set(sources + ${shared_sources} + ${serial_source_dir}/serial/unix.cpp) +else() + message(FATAL_ERROR "System is neither WIN32 nor UNIX.") +endif() + +# lib target + +include_directories(${generated_header_dir}) +include_directories(${serial_include_dir}) +include_directories(${protocol_include_dir}) +#add_compile_definitions(ADDR2LINE_PATH) +add_library(serial SHARED ${public_headers} ${private_headers} ${sources}) + +include(GenerateExportHeader) +generate_export_header(serial + BASE_NAME serial + EXPORT_MACRO_NAME SERIAL_EXPORT + EXPORT_FILE_NAME ${export_header} + STATIC_DEFINE SERIAL_BUILT_AS_STATIC) + +# install settings + +set_target_properties(serial PROPERTIES PUBLIC_HEADER "${public_headers}") + +include(GNUInstallDirs) +install(TARGETS serial + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) diff --git a/firmware/haptics/template/utils/serial/binding.gyp b/firmware/haptics/template/utils/serial/binding.gyp new file mode 100644 index 0000000..17c9986 --- /dev/null +++ b/firmware/haptics/template/utils/serial/binding.gyp @@ -0,0 +1,36 @@ +{ + "variables": { + "cppdefs": "NODE_GYP", + }, + "targets": [{ + "target_name": "serial", + "sources": [ + "./src/node/main.cpp", + "./src/node/setup.cpp", + "./src/node/poll.cpp", + "./src/node/send.cpp", + "./src/node/receiveHelpers.cpp", + "./src/node/sendHelpers.cpp", + "./src/serial/shared.cpp", + "./src/cppLib/lib.cpp", + "./src/crashAnalyzer/buffer.cpp", + "./src/crashAnalyzer/analyze.cpp", + "../protocol/src/protocol/protocol.cpp" + ], + "conditions": [ + ["OS=='win'", { + "sources": ["./src/serial/win.cpp"] + }], + ["OS!='win'", { + "sources": ["./src/serial/unix.cpp"] + }], + ], + "include_dirs": [ + "./include", + "../protocol/include" + ], + "defines": [ + "<@(cppdefs)" + ] + }] +} diff --git a/firmware/haptics/template/utils/serial/include/crashAnalyzer.hpp b/firmware/haptics/template/utils/serial/include/crashAnalyzer.hpp new file mode 100644 index 0000000..dea6f74 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/crashAnalyzer.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include + +#define SAFEMOD(a, b) ((a) % (b) + (b)) % (b) + +class CrashAnalyzer +{ +private: + static const uint16_t c_bufferLength = 1024; + static const uint16_t c_dumpLineWidth = 32; + static uint8_t s_buffer[c_bufferLength]; + static uint16_t s_length; + static uint16_t s_index; + + static void clearBuffer(); + static uint8_t getChar(uint16_t offset); + + static const std::string c_rebootString; + static const std::string c_backtraceString; + + static bool findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t& foundOffset); + static std::vector getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset); + static void addr2line(std::vector addresses); + + static void checkOutput(); +public: + static void push_back(const uint8_t character); +}; diff --git a/firmware/haptics/template/utils/serial/include/libInterface.hpp b/firmware/haptics/template/utils/serial/include/libInterface.hpp new file mode 100644 index 0000000..40b2846 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/libInterface.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include + +#include "serial.hpp" +#include "serial_export.hpp" + +// class stuff + +class CppLib : DPSerial +{ +public: + static uint32_t getRevision(); + static uint64_t open(char* port); + static void setActiveHandle(uint64_t handle); + static void close(); + static void poll(); + static void sendSyncAck(); + static void sendHeartbeatAck(); + static void sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + static void sendSpeed(uint8_t pantoIndex, float speed); + static void createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + static void addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + static void removeObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void enableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void disableObstacle(uint8_t pantoIndex, uint16_t obstacleId); + static void sendFree(uint8_t pantoIndex); + static void sendFreeze(uint8_t pantoIndex); + static void sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + static uint32_t checkSendQueue(uint32_t maxPackets); + static void reset(); +}; + +// handlers + +typedef void (*syncHandler_t)(uint64_t); +extern syncHandler_t syncHandler; +typedef void (*heartbeatHandler_t)(uint64_t); +extern heartbeatHandler_t heartbeatHandler; +typedef void (*positionHandler_t)(uint64_t, double*); +extern positionHandler_t positionHandler; +typedef void (*loggingHandler_t)(char*); +extern loggingHandler_t loggingHandler; +typedef void (*transitionHandler_t)(uint8_t); +extern transitionHandler_t transitionHandler; + +void logString(char* msg); + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +extern "C" +{ + uint32_t SERIAL_EXPORT GetRevision(); + void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler); + void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler); + void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler); + void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler); + void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler); + uint64_t SERIAL_EXPORT Open(char* port); + void SERIAL_EXPORT Close(uint64_t handle); + void SERIAL_EXPORT Poll(uint64_t handle); + void SERIAL_EXPORT SendSyncAck(uint64_t handle); + void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle); + void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation); + void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed); + void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex); + void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement); + void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y); + void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId); + void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled); + uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets); + void SERIAL_EXPORT Reset(); +}; diff --git a/firmware/haptics/template/utils/serial/include/node.hpp b/firmware/haptics/template/utils/serial/include/node.hpp new file mode 100644 index 0000000..5941e9b --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/node.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "serial.hpp" + +#if __has_include("node_api.h") +#include +#elif __has_include("node/node_api.h") +#include +#endif +#define NAPI_CHECK(code) \ + if (code != napi_ok) \ + std::cerr << "NOT OK: " << __FILE__ << ":" << __LINE__ << std::endl; + +// enable for debugging +// #define DEBUG_LOGGING + +class Node : public DPSerial +{ +private: + static napi_value receiveUInt8(napi_env env, uint16_t& offset); + static napi_value receiveInt16(napi_env env, uint16_t& offset); + static napi_value receiveUInt16(napi_env env, uint16_t& offset); + static napi_value receiveInt32(napi_env env, uint16_t& offset); + static napi_value receiveUInt32(napi_env env, uint16_t& offset); + static napi_value receiveFloat(napi_env env, uint16_t& offset); + + static void sendUInt8(napi_env env, napi_value value, uint16_t& offset); + static void sendInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt16(napi_env env, napi_value value, uint16_t& offset); + static void sendInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendUInt32(napi_env env, napi_value value, uint16_t& offset); + static void sendFloat(napi_env env, napi_value value, uint16_t& offset); + +public: + static napi_value open(napi_env env, napi_callback_info info); + static napi_value close(napi_env env, napi_callback_info info); + static napi_value poll(napi_env env, napi_callback_info info); + static napi_value send(napi_env env, napi_callback_info info); +}; diff --git a/firmware/haptics/template/utils/serial/include/packet.hpp b/firmware/haptics/template/utils/serial/include/packet.hpp new file mode 100644 index 0000000..f885582 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/packet.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +class Packet +{ +public: + Header header; + uint8_t payload[256]; + uint8_t payloadIndex = 0; + + Packet(uint8_t messageType, uint16_t payloadSize); + + uint8_t receiveUInt8(); + int16_t receiveInt16(); + uint16_t receiveUInt16(); + int32_t receiveInt32(); + uint32_t receiveUInt32(); + float receiveFloat(); + + void sendUInt8(uint8_t value); + void sendInt16(int16_t value); + void sendUInt16(uint16_t value); + void sendInt32(int32_t value); + void sendUInt32(uint32_t value); + void sendFloat(float value); +}; diff --git a/firmware/haptics/template/utils/serial/include/serial.hpp b/firmware/haptics/template/utils/serial/include/serial.hpp new file mode 100644 index 0000000..6ff8e07 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/serial.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "packet.hpp" + +#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) +#include +#define FILEHANDLE HANDLE +#else +#include +#include +#include +#include +#include +#define FILEHANDLE FILE * +#endif + +enum ReceiveState +{ + NONE = 0, + FOUND_MAGIC = 1, + FOUND_HEADER = 2 +}; + +class DPSerial : public DPProtocol +{ +protected: + static std::string s_path; + static const uint32_t c_packetSize = 0xFF; + static FILEHANDLE s_handle; + static std::thread s_worker; + static bool s_workerRunning; + + static std::queue s_highPrioSendQueue; + static std::queue s_lowPrioSendQueue; + static std::queue s_receiveQueue; + + static uint8_t s_nextTrackedPacketId; + static bool s_haveUnacknowledgedTrackedPacket; + static Packet s_lastTrackedPacket; + static std::chrono::time_point + s_lastTrackedPacketSendTime; + static const std::chrono::milliseconds c_trackedPacketTimeout; + static bool s_pantoReportedInvalidData; + + static bool s_pantoReady; + static uint32_t s_magicReceiveIndex; + static ReceiveState s_receiveState; + static Header s_receiveHeader; + + static void startWorker(); + static void stopWorker(); + static void tearDown(); + static void reset(); + static void update(); + static void processOutput(); + static bool processInput(); + + static bool isTracked(uint8_t t); + static bool checkQueue(std::queue &q); + static void sendPacket(Packet p); + static void sendInstantPacket(Packet p); + static void write(const uint8_t *const data, const uint32_t length); + + static uint32_t getAvailableByteCount(FILEHANDLE s_handle); + static bool readBytesFromSerial(void *target, uint32_t length); + static bool readBytesIfAvailable(void *target, uint32_t length); + static bool readMagicNumber(); + static bool readHeader(); + static bool readPayload(); + +public: + static bool setup(std::string path); +}; diff --git a/firmware/haptics/template/utils/serial/include/serial_export.hpp b/firmware/haptics/template/utils/serial/include/serial_export.hpp new file mode 100644 index 0000000..09738b0 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/serial_export.hpp @@ -0,0 +1,59 @@ + +#ifndef SERIAL_EXPORT_H +#define SERIAL_EXPORT_H + +// figure out the correct attributes +#if defined _WIN32 || defined __CYGWIN__ + #define HELPER_DLL_IMPORT __declspec(dllimport) + #define HELPER_DLL_EXPORT __declspec(dllexport) + #define HELPER_DLL_LOCAL +#else + #if __GNUC__ >= 4 + #define HELPER_DLL_IMPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_EXPORT __attribute__ ((visibility ("default"))) + #define HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden"))) + #else + #define HELPER_DLL_IMPORT + #define HELPER_DLL_EXPORT + #define HELPER_DLL_LOCAL + #endif +#endif + +#ifdef SERIAL_BUILT_AS_STATIC +# define SERIAL_EXPORT +# define SERIAL_NO_EXPORT +#else +# ifndef SERIAL_EXPORT +# ifdef serial_EXPORTS + /* We are building this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# else + /* We are using this library */ +# define SERIAL_EXPORT HELPER_DLL_EXPORT +# endif +# endif + +# ifndef SERIAL_NO_EXPORT +# define SERIAL_NO_EXPORT HELPER_DLL_LOCAL +# endif +#endif + +#ifndef SERIAL_DEPRECATED +# define SERIAL_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef SERIAL_DEPRECATED_EXPORT +# define SERIAL_DEPRECATED_EXPORT SERIAL_EXPORT SERIAL_DEPRECATED +#endif + +#ifndef SERIAL_DEPRECATED_NO_EXPORT +# define SERIAL_DEPRECATED_NO_EXPORT SERIAL_NO_EXPORT SERIAL_DEPRECATED +#endif + +#if 0 /* DEFINE_NO_DEPRECATED */ +# ifndef SERIAL_NO_DEPRECATED +# define SERIAL_NO_DEPRECATED +# endif +#endif + +#endif /* SERIAL_EXPORT_H */ diff --git a/firmware/haptics/template/utils/serial/include/standalone.hpp b/firmware/haptics/template/utils/serial/include/standalone.hpp new file mode 100644 index 0000000..4bf5992 --- /dev/null +++ b/firmware/haptics/template/utils/serial/include/standalone.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include "serial.hpp" + +class Standalone : public DPSerial +{ +public: + static void terminate(int signal); + static void printPacket(); +}; diff --git a/firmware/haptics/template/utils/serial/src/cppLib/lib.cpp b/firmware/haptics/template/utils/serial/src/cppLib/lib.cpp new file mode 100644 index 0000000..446b894 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/cppLib/lib.cpp @@ -0,0 +1,445 @@ +#include "libInterface.hpp" + +#include +#include + +#include "packet.hpp" + +#ifdef _WIN32 +#define FILEPTR void * +#else +#define FILEPTR FILE * +#endif + +// class stuff + +uint32_t CppLib::getRevision() +{ + return c_revision; +} + +uint64_t CppLib::open(char *port) +{ + if (!setup(port)) + { + logString("Open failed"); + return 0; + } + logString("Open successfull"); + return (uint64_t)s_handle; +} + +void CppLib::setActiveHandle(uint64_t handle) +{ + s_handle = (FILEPTR)handle; +} + +void CppLib::close() +{ + tearDown(); +} + +void CppLib::poll() +{ + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + bool receivedTransition = false; + double positionCoords[2 * 5]; + uint8_t pantoIndex; + + while (s_receiveQueue.size() > 0) + { + auto packet = s_receiveQueue.front(); + s_receiveQueue.pop(); + + if (packet.header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + switch (packet.header.MessageType) + { + case SYNC: + { + auto receivedRevision = packet.receiveUInt32(); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::ostringstream oss; + oss << "Revision id not matching. Expected " << c_revision + << ", received " << receivedRevision << "." << std::endl; + logString((char *)oss.str().c_str()); + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (packet.payloadIndex < packet.header.PayloadSize) + { + uint8_t index = packet.payloadIndex / 4; + positionCoords[index] = packet.receiveFloat(); + } + break; + case DEBUG_LOG: + logString((char *)packet.payload); + break; + case TRANSITION_ENDED: + receivedTransition = true; + pantoIndex = packet.receiveUInt8(); + break; + default: + break; + } + } + + if (receivedSync) + { + if (syncHandler == nullptr) + { + logString("Received sync, but handler not set up"); + } + else + { + syncHandler((uint64_t)s_handle); + } + } + + if (receivedHeartbeat) + { + if (heartbeatHandler == nullptr) + { + logString("Received heartbeat, but handler not set up"); + } + else + { + heartbeatHandler((uint64_t)s_handle); + } + } + + if (receivedPosition) + { + if (positionHandler == nullptr) + { + logString("Received position, but handler not set up"); + } + else + { + positionHandler((uint64_t)s_handle, positionCoords); + } + } + + if (receivedTransition) + { + // transition (tweening) ended + if (transitionHandler == nullptr) + { + logString("Received transition ended, but handler not set up"); + } + else + { + transitionHandler(pantoIndex); + } + } +} + +void CppLib::sendSyncAck() +{ + DPSerial::sendInstantPacket(Packet(SYNC_ACK, 0)); +} + +void CppLib::sendHeartbeatAck() +{ + DPSerial::sendInstantPacket(Packet(HEARTBEAT_ACK, 0)); +} + +void CppLib::sendMotor(uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + auto p = Packet(MOTOR, 14); // 1 for control, 1 for index, 3 * 4 for pos + p.sendUInt8(controlMethod); + p.sendUInt8(pantoIndex); + p.sendFloat(positionX); + p.sendFloat(positionY); + p.sendFloat(rotation); + sendPacket(p); +} + +void CppLib::sendSpeed(uint8_t pantoIndex, float speed) +{ + auto p = Packet(SPEED, 5); // 1 for index, 1 * 4 for position + p.sendUInt8(pantoIndex); + p.sendFloat(speed); + sendPacket(p); +} + +void CppLib::sendFree(uint8_t pantoIndex) +{ + auto p = Packet(FREE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendFreeze(uint8_t pantoIndex) +{ + auto p = Packet(FREEZE, 1); + p.sendUInt8(pantoIndex); + sendPacket(p); +} + +void CppLib::sendSpeedControl(uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + auto p = Packet(SPEED_CONTROL, 15); // 1 for index, 4 for tether factor, 4 each for the tether radii, 1 for tether strategy and 1 for pock + p.sendUInt8(tethered); + p.sendFloat(tetherFactor); + p.sendFloat(tetherInnerRadius); + p.sendFloat(tetherOuterRadius); + p.sendUInt8(strategy); + p.sendUInt8(pockEnabled); + sendPacket(p); +} + +void CppLib::createObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createPassableObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(CREATE_PASSABLE_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::createRail(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + auto p = Packet(CREATE_RAIL, 23); // 1 for index, 2 for id, 2 * 2 * 4 for vectors, 4 for displacement + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + p.sendFloat(displacement); + sendPacket(p); +} + +void CppLib::addToObstacle(uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + auto p = Packet(ADD_TO_OBSTACLE, 19); // 1 for index, 2 for id, 2 * 2 * 4 for vectors + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + p.sendFloat(vector1x); + p.sendFloat(vector1y); + p.sendFloat(vector2x); + p.sendFloat(vector2y); + sendPacket(p); +} + +void CppLib::removeObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(REMOVE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::enableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(ENABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::disableObstacle(uint8_t pantoIndex, uint16_t obstacleId) +{ + auto p = Packet(DISABLE_OBSTACLE, 3); // 1 for index, 2 for id + p.sendUInt8(pantoIndex); + p.sendUInt16(obstacleId); + sendPacket(p); +} + +void CppLib::reset() +{ + DPSerial::reset(); +} + +// handlers +syncHandler_t syncHandler; +heartbeatHandler_t heartbeatHandler; +positionHandler_t positionHandler; +loggingHandler_t loggingHandler; +transitionHandler_t transitionHandler; + +void logString(char *msg) +{ + if (loggingHandler != nullptr) + { + loggingHandler(msg); + } +} + +// can't export any member functions, not even static ones +// thus we'll have to add wrappers for everything + +uint32_t GetRevision() +{ + return CppLib::getRevision(); +} + +void SERIAL_EXPORT SetSyncHandler(syncHandler_t handler) +{ + syncHandler = handler; + logString("Sync handler set"); +} + +void SERIAL_EXPORT SetHeartbeatHandler(heartbeatHandler_t handler) +{ + heartbeatHandler = handler; + logString("Heartbeat handler set"); +} + +void SERIAL_EXPORT SetPositionHandler(positionHandler_t handler) +{ + positionHandler = handler; + logString("Position handler set"); +} + +void SERIAL_EXPORT SetLoggingHandler(loggingHandler_t handler) +{ + loggingHandler = handler; + loggingHandler("Logging from plugin is enabled"); +} + +void SERIAL_EXPORT SetTransitionHandler(transitionHandler_t handler) +{ + transitionHandler = handler; + logString("Transition handler set"); +} + +uint64_t SERIAL_EXPORT Open(char *port) +{ + return CppLib::open(port); +} + +void SERIAL_EXPORT Close(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::close(); +} + +void SERIAL_EXPORT Poll(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::poll(); +} + +void SERIAL_EXPORT SendSyncAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSyncAck(); +} + +void SERIAL_EXPORT SendHeartbeatAck(uint64_t handle) +{ + CppLib::setActiveHandle(handle); + CppLib::sendHeartbeatAck(); +} + +void SERIAL_EXPORT SendMotor(uint64_t handle, uint8_t controlMethod, uint8_t pantoIndex, float positionX, float positionY, float rotation) +{ + CppLib::setActiveHandle(handle); + CppLib::sendMotor(controlMethod, pantoIndex, positionX, positionY, rotation); +} + +void SERIAL_EXPORT SendSpeed(uint64_t handle, uint8_t pantoIndex, float speed) +{ + CppLib::setActiveHandle(handle); + CppLib::sendSpeed(pantoIndex, speed); +} + +void SERIAL_EXPORT FreeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFree(pantoIndex); +} + +void SERIAL_EXPORT FreezeMotor(uint64_t handle, uint8_t pantoIndex) +{ + CppLib::setActiveHandle(handle); + CppLib::sendFreeze(pantoIndex); +} + +void SERIAL_EXPORT CreateObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreatePassableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::createPassableObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT CreateRail(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y, float displacement) +{ + CppLib::setActiveHandle(handle); + CppLib::createRail(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y, displacement); +} + +void SERIAL_EXPORT AddToObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId, float vector1x, float vector1y, float vector2x, float vector2y) +{ + CppLib::setActiveHandle(handle); + CppLib::addToObstacle(pantoIndex, obstacleId, vector1x, vector1y, vector2x, vector2y); +} + +void SERIAL_EXPORT RemoveObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::removeObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT EnableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::enableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT DisableObstacle(uint64_t handle, uint8_t pantoIndex, uint16_t obstacleId) +{ + CppLib::setActiveHandle(handle); + CppLib::disableObstacle(pantoIndex, obstacleId); +} + +void SERIAL_EXPORT SetSpeedControl(uint64_t handle, uint8_t tethered, float tetherFactor, float tetherInnerRadius, float tetherOuterRadius, uint8_t strategy, uint8_t pockEnabled) +{ + CppLib::sendSpeedControl(tethered, tetherFactor, tetherInnerRadius, tetherOuterRadius, strategy, pockEnabled); +} + +uint32_t SERIAL_EXPORT CheckQueuedPackets(uint32_t maxPackets) +{ + return 0; +} + +void SERIAL_EXPORT Reset() +{ + CppLib::reset(); +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/src/crashAnalyzer/analyze.cpp b/firmware/haptics/template/utils/serial/src/crashAnalyzer/analyze.cpp new file mode 100644 index 0000000..9569fdb --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/crashAnalyzer/analyze.cpp @@ -0,0 +1,183 @@ +#include "crashAnalyzer.hpp" + +#include +#include +#include +#include +#include +#include +#include "libInterface.hpp" + +#ifdef WINDOWS +#include +#define popen _popen +#define pclose _pclose +#endif + +#ifdef __APPLE__ +#define popen popen +#define pclose pclose +#endif + +const std::string CrashAnalyzer::c_rebootString = "Rebooting..."; +const std::string CrashAnalyzer::c_backtraceString = "Backtrace:"; + +bool CrashAnalyzer::findString( + uint16_t startOffset, + uint16_t endOffset, + const std::string string, + uint16_t &foundOffset) +{ + const auto length = string.length(); + int32_t index = length - 1; + auto offset = startOffset; + + while (index > -1 && offset <= endOffset) + { + if (getChar(offset) == string.at(index)) + { + index--; + } + else + { + index = length - 1; + } + offset++; + } + + if (index == -1) + { + foundOffset = offset; + return true; + } + + return false; +} + +std::vector CrashAnalyzer::getBacktraceAddresses( + uint16_t startOffset, uint16_t endOffset) +{ + std::string data(startOffset - endOffset + 1, 'a'); + for (auto i = startOffset; i >= endOffset; --i) + { + data.at(startOffset - i) = getChar(i); + } + std::regex regex("(0x.{8}):0x.{8}"); + auto it = std::sregex_iterator(data.begin(), data.end(), regex); + auto end = std::sregex_iterator(); + + std::vector result; + while (it != end) + { + result.push_back((*it++).str(1)); + } + + return result; +} + +char *exec(const char *cmd) +{ + loggingHandler((char *)cmd); + + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd, "r"), pclose); + if (!pipe) + { + loggingHandler("Popen failed"); + return "popen() failed!"; + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != NULL) + { + loggingHandler(buffer.data()); + //result += buffer.data(); + } + char *cstr = new char[result.length() + 1]; + strcpy(cstr, result.c_str()); + // do stuff + delete[] cstr; + return cstr; +} + +void CrashAnalyzer::addr2line(std::vector addresses) +{ + std::cout << std::endl + << "===== STACKTRACE BEGIN =====" << std::endl; + + loggingHandler("===== STACKTRACE BEGIN ====="); +#ifdef __APPLE__ +#define ADDR2LINE_PATH "~/.platformio/packages/toolchain-xtensa32/bin/xtensa-esp32-elf-addr2line" +#define FIRMWARE_PATH "/Users/julio/Documents/Uni/5_Master/HCI_Project_Seminar/dualpantoframework/firmware/.pio/build/esp32dev/firmware.elf" +#endif +#ifdef ADDR2LINE_PATH + std::ostringstream command; + command + << ADDR2LINE_PATH + << " -e " << FIRMWARE_PATH + << " -fpCis"; // see https://linux.die.net/man/1/addr2line + for (const auto &address : addresses) + { + command << " " << address; + } + + command << " 2>&1"; + auto result = exec(command.str().c_str()); + + std::ostringstream out; + out << "Stacktrace (most recent call first):" << std::endl + << result; +#else + loggingHandler("Path to addr2line executable not set. Can't analyze stacktrace."); + std::cout + << "Path to addr2line executable not set. Can't analyze stacktrace." + << std::endl; + +#endif + + std::cout << "====== STACKTRACE END ======" << std::endl; +} + +void CrashAnalyzer::checkOutput() +{ + uint16_t rebootOffset; + if (!findString( + 0, + c_rebootString.length(), + c_rebootString, + rebootOffset)) + { + return; + } + + uint16_t backtraceOffset; + if (!findString( + rebootOffset, + s_length, + c_backtraceString, + backtraceOffset)) + { + std::cout << "Reboot detected, but no backtrace found." << std::endl; + return; + } + char out[c_bufferLength + 1]; + std::memcpy(out, s_buffer, c_bufferLength); + out[c_bufferLength] = '\0'; + for (int i = 0; i < c_bufferLength; i++) + { + char x = s_buffer[i]; + if (x < 32 || x > 126) + { + // space + x = 32; + } + out[i] = x; + } + loggingHandler("ERROR!"); + loggingHandler(out); + auto addresses = getBacktraceAddresses( + backtraceOffset - c_backtraceString.length() - 1, + rebootOffset); + + addr2line(addresses); + clearBuffer(); +} diff --git a/firmware/haptics/template/utils/serial/src/crashAnalyzer/buffer.cpp b/firmware/haptics/template/utils/serial/src/crashAnalyzer/buffer.cpp new file mode 100644 index 0000000..8107795 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/crashAnalyzer/buffer.cpp @@ -0,0 +1,28 @@ +#include "crashAnalyzer.hpp" + +#include +#include + +uint8_t CrashAnalyzer::s_buffer[c_bufferLength]; +uint16_t CrashAnalyzer::s_length = 0; +uint16_t CrashAnalyzer::s_index = 0; + +void CrashAnalyzer::clearBuffer() +{ + s_index = 0; + s_length = 0; +} + +uint8_t CrashAnalyzer::getChar(uint16_t offset) +{ + return s_buffer[SAFEMOD((s_index - offset), c_bufferLength)]; +} + +void CrashAnalyzer::push_back(const uint8_t character) +{ + s_buffer[s_index] = character; + s_index = (s_index + 1) % c_bufferLength; + s_length = (s_length >= c_bufferLength) ? c_bufferLength : (s_length + 1); + + checkOutput(); +} diff --git a/firmware/haptics/template/utils/serial/src/node/main.cpp b/firmware/haptics/template/utils/serial/src/node/main.cpp new file mode 100644 index 0000000..fbbcadf --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/main.cpp @@ -0,0 +1,23 @@ +#include "node.hpp" + +#define defFunc(name, ptr) \ + if (napi_create_function(env, NULL, 0, ptr, NULL, &fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to wrap native function"); \ + } \ + if (napi_set_named_property(env, exports, name, fn) != napi_ok) \ + { \ + napi_throw_error(env, NULL, "Unable to populate exports"); \ + } + +napi_value Init(napi_env env, napi_value exports) +{ + napi_value fn; + defFunc("open", Node::open); + defFunc("close", Node::close); + defFunc("poll", Node::poll); + defFunc("send", Node::send); + return exports; +} + +NAPI_MODULE(NODE_GYP_MODULE_NAME, Init) diff --git a/firmware/haptics/template/utils/serial/src/node/poll.cpp b/firmware/haptics/template/utils/serial/src/node/poll.cpp new file mode 100644 index 0000000..fd20886 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/poll.cpp @@ -0,0 +1,121 @@ +#include "node.hpp" + +#include + +napi_value Node::poll(napi_env env, napi_callback_info info) +{ + // argv[0]: handle + // argv[1]: device + // argv[2]: vector constructor + // argv[3]: sync cb + // argv[4]: heartbeat cb + // argv[5]: position cb + // argv[6]: debug log cb + size_t argc = 7; + napi_value argv[7]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + // only keep binary state for packages where only the newest counts + bool receivedSync = false; + bool receivedHeartbeat = false; + bool receivedPosition = false; + double positionCoords[2 * 5]; + + while (getAvailableByteCount(s_handle)) + { + receivePacket(); + + if (s_header.PayloadSize > c_maxPayloadSize) + { + continue; + } + + uint16_t offset = 0; + switch (s_header.MessageType) + { + case SYNC: + { + auto receivedRevision = DPSerial::receiveUInt32(offset); + if (receivedRevision == c_revision) + { + receivedSync = true; + } + else + { + std::cout + << "Received invalid revision id " << receivedRevision + << " (expected " << c_revision << "). Maybe try reset the device?" << std::endl; + } + break; + } + case HEARTBEAT: + receivedHeartbeat = true; + break; + case POSITION: + receivedPosition = true; + while (offset < s_header.PayloadSize) + { + uint8_t index = offset / 4; + positionCoords[index] = DPSerial::receiveFloat(offset); + } + break; + case DEBUG_LOG: + napi_value result; + napi_create_string_utf8(env, reinterpret_cast(s_packetBuffer), s_header.PayloadSize, &result); + napi_call_function(env, argv[1], argv[6], 1, &result, NULL); + break; + default: + break; + } + } + + if(receivedSync) + { + napi_call_function(env, argv[1], argv[3], 0, NULL, NULL); + } + + if(receivedHeartbeat) + { + napi_call_function(env, argv[1], argv[4], 0, NULL, NULL); + } + + if (receivedPosition) + { + napi_value result; + napi_create_array(env, &result); + + const uint32_t posArgc = 3; + const uint32_t goArgc = 2; + for (auto i = 0u; i < 2; ++i) + { + napi_value posArgv[posArgc]; + + for (auto j = 0u; j < posArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + j], &(posArgv[j])); + } + + napi_value vector; + NAPI_CHECK(napi_new_instance(env, argv[2], posArgc, posArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2, vector)); + + napi_value goArgv[goArgc]; + + for (auto j = 0u; j < goArgc; ++j) + { + napi_create_double(env, positionCoords[i * 5 + 3 + j], &(goArgv[j])); + } + + NAPI_CHECK(napi_new_instance(env, argv[2], goArgc, goArgv, &vector)) + NAPI_CHECK(napi_set_element(env, result, i * 2 + 1, vector)); + } + + NAPI_CHECK(napi_call_function(env, argv[1], argv[5], 1, &result, NULL)); + } + + return NULL; +} diff --git a/firmware/haptics/template/utils/serial/src/node/receiveHelpers.cpp b/firmware/haptics/template/utils/serial/src/node/receiveHelpers.cpp new file mode 100644 index 0000000..2450200 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/receiveHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +napi_value Node::receiveUInt8(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt8(offset), &result); + return result; +} + +napi_value Node::receiveInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt16(offset), &result); + return result; +} + +napi_value Node::receiveUInt16(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt16(offset), &result); + return result; +} + +napi_value Node::receiveInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_int32(env, DPSerial::receiveInt32(offset), &result); + return result; +} + +napi_value Node::receiveUInt32(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_uint32(env, DPSerial::receiveUInt32(offset), &result); + return result; +} + +napi_value Node::receiveFloat(napi_env env, uint16_t& offset) +{ + napi_value result; + napi_create_double(env, DPSerial::receiveFloat(offset), &result); + return result; +} diff --git a/firmware/haptics/template/utils/serial/src/node/send.cpp b/firmware/haptics/template/utils/serial/src/node/send.cpp new file mode 100644 index 0000000..90f854e --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/send.cpp @@ -0,0 +1,198 @@ +#include "node.hpp" + +#include + +napi_value Node::send(napi_env env, napi_callback_info info) +{ + size_t argc = 3; + napi_value argv[3]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + + uint32_t messageType; + napi_get_value_uint32(env, argv[1], &messageType); + + s_header.MessageType = static_cast(messageType); + + uint16_t offset = 0; + switch (messageType) + { + case SYNC_ACK: + break; + case HEARTBEAT_ACK: + break; + case MOTOR: + { + napi_value tempNapiValue; + uint32_t tempUInt32; + double tempDouble; + uint32_t index = 0; + + // control method + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // panto index + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + // position + while (index < 5) + { + napi_get_element(env, argv[2], index++, &tempNapiValue); + napi_get_value_double(env, tempNapiValue, &tempDouble); + DPSerial::sendFloat(static_cast(tempDouble), offset); + } + break; + } + case PID: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "motorIndex", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "pid", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_value pidNapiValue; + double pidDouble; + for (auto i = 0; i < 3; ++i) + { + napi_get_element(env, tempNapiValue, i, &pidNapiValue); + napi_get_value_double(env, pidNapiValue, &pidDouble); + DPSerial::sendFloat(static_cast(pidDouble), offset); + } + break; + } + case CREATE_OBSTACLE: + case CREATE_PASSABLE_OBSTACLE: + case ADD_TO_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + break; + } + case CREATE_RAIL: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "posArray", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + uint32_t posArraySize; + napi_get_array_length(env, tempNapiValue, &posArraySize); + napi_value posNapiValue; + double posDouble; + for (auto i = 0; i < posArraySize; ++i) + { + napi_get_element(env, tempNapiValue, i, &posNapiValue); + napi_get_value_double(env, posNapiValue, &posDouble); + DPSerial::sendFloat(static_cast(posDouble), offset); + } + double displacement; + napi_get_value_double(env, tempNapiValue, &displacement); + DPSerial::sendFloat(static_cast(displacement), offset); + break; + } + case REMOVE_OBSTACLE: + case ENABLE_OBSTACLE: + case DISABLE_OBSTACLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + + napi_create_string_utf8(env, "id", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt16(static_cast(tempUInt32), offset); + break; + } + case CALIBRATE_PANTO:{ + break; + } + case DUMP_HASHTABLE: + { + napi_value propertyName; + napi_value tempNapiValue; + uint32_t tempUInt32; + + napi_create_string_utf8(env, "index", NAPI_AUTO_LENGTH, &propertyName); + napi_get_property(env, argv[2], propertyName, &tempNapiValue); + napi_get_value_uint32(env, tempNapiValue, &tempUInt32); + DPSerial::sendUInt8(static_cast(tempUInt32), offset); + break; + } + default: + napi_throw_error(env, NULL, (std::string("Invalid message type") + std::to_string(messageType)).c_str()); + return NULL; + } + + #ifdef DEBUG_LOGGING + std::cout << "Payload size: 0x" << std::setw(2) << offset << std::endl; + #endif + + if(offset > c_maxPayloadSize) + { + std::cout << "Error: payload size of " << offset << " exceeds max payload size (" << c_maxPayloadSize << ")." << std::endl; + return NULL; + } + + s_header.PayloadSize = offset; + sendPacket(); + + #ifdef DEBUG_LOGGING + dumpBuffers(); + #endif + + return NULL; +} diff --git a/firmware/haptics/template/utils/serial/src/node/sendHelpers.cpp b/firmware/haptics/template/utils/serial/src/node/sendHelpers.cpp new file mode 100644 index 0000000..f1b062f --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/sendHelpers.cpp @@ -0,0 +1,43 @@ +#include "node.hpp" + +void Node::sendUInt8(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt8(static_cast(temp), offset); +} + +void Node::sendInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendInt16(static_cast(temp), offset); +} + +void Node::sendUInt16(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt16(static_cast(temp), offset); +} + +void Node::sendInt32(napi_env env, napi_value value, uint16_t& offset) +{ + int32_t temp; + napi_get_value_int32(env, value, &temp); + DPSerial::sendInt32(temp, offset); +} + +void Node::sendUInt32(napi_env env, napi_value value, uint16_t& offset) +{ + uint32_t temp; + napi_get_value_uint32(env, value, &temp); + DPSerial::sendUInt32(temp, offset); +} + +void Node::sendFloat(napi_env env, napi_value value, uint16_t& offset) +{ + double temp; + napi_get_value_double(env, value, &temp); + DPSerial::sendFloat(static_cast(temp), offset); +} diff --git a/firmware/haptics/template/utils/serial/src/node/setup.cpp b/firmware/haptics/template/utils/serial/src/node/setup.cpp new file mode 100644 index 0000000..0c72735 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/node/setup.cpp @@ -0,0 +1,34 @@ +#include "node.hpp" + +napi_value Node::open(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + size_t length; + char buffer[64]; + napi_get_value_string_utf8(env, argv[0], buffer, sizeof(buffer), &length); + if (!setup(buffer)) + { + napi_throw_error(env, NULL, "open failed"); + } + napi_value result; + napi_create_int64(env, reinterpret_cast(s_handle), &result); + return result; +} + +napi_value Node::close(napi_env env, napi_callback_info info) +{ + size_t argc = 1; + napi_value argv[1]; + if (napi_get_cb_info(env, info, &argc, argv, NULL, NULL) != napi_ok) + { + napi_throw_error(env, NULL, "Failed to parse arguments"); + } + napi_get_value_int64(env, argv[0], reinterpret_cast(&s_handle)); + tearDown(); + return NULL; +} diff --git a/firmware/haptics/template/utils/serial/src/serial/packet.cpp b/firmware/haptics/template/utils/serial/src/serial/packet.cpp new file mode 100644 index 0000000..380f276 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/serial/packet.cpp @@ -0,0 +1,86 @@ +#include "packet.hpp" + +Packet::Packet(uint8_t messageType, uint16_t payloadSize) +{ + header.MessageType = messageType; + header.PayloadSize = payloadSize; +} + +uint8_t Packet::receiveUInt8() +{ + return payload[payloadIndex++]; +} + +int16_t Packet::receiveInt16() +{ + uint8_t temp[2]; + for (auto i = 0; i < 2; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 2; + return temp[0] << 8 | temp[1]; +} + +uint16_t Packet::receiveUInt16() +{ + auto temp = receiveInt16(); + return *reinterpret_cast(&temp); +} + +int32_t Packet::receiveInt32() +{ + uint8_t temp[4]; + for (auto i = 0; i < 4; ++i) + { + temp[i] = payload[payloadIndex + i]; + } + payloadIndex += 4; + return temp[0] << 24 | temp[1] << 16 | temp[2] << 8 | temp[3]; +} + +uint32_t Packet::receiveUInt32() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +float Packet::receiveFloat() +{ + auto temp = receiveInt32(); + return *reinterpret_cast(&temp); +} + +void Packet::sendUInt8(uint8_t value) +{ + payload[payloadIndex++] = value; +} + +void Packet::sendInt16(int16_t value) +{ + payload[payloadIndex++] = value >> 8; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt16(uint16_t value) +{ + sendInt16(*reinterpret_cast(&value)); +} + +void Packet::sendInt32(int32_t value) +{ + payload[payloadIndex++] = value >> 24; + payload[payloadIndex++] = (value >> 16) & 255; + payload[payloadIndex++] = (value >> 8) & 255; + payload[payloadIndex++] = value & 255; +} + +void Packet::sendUInt32(uint32_t value) +{ + sendInt32(*reinterpret_cast(&value)); +} + +void Packet::sendFloat(float value) +{ + sendInt32(*reinterpret_cast(&value)); +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/src/serial/shared.cpp b/firmware/haptics/template/utils/serial/src/serial/shared.cpp new file mode 100644 index 0000000..30a72ac --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/serial/shared.cpp @@ -0,0 +1,311 @@ +#include "serial.hpp" + +#include +#include +#include +#include +#include + +#include "crashAnalyzer.hpp" +#include "libInterface.hpp" + +std::string DPSerial::s_path; +FILEHANDLE DPSerial::s_handle; +std::thread DPSerial::s_worker; +bool DPSerial::s_workerRunning = false; + +std::queue DPSerial::s_highPrioSendQueue; +std::queue DPSerial::s_lowPrioSendQueue; +std::queue DPSerial::s_receiveQueue; +uint8_t DPSerial::s_nextTrackedPacketId = 1; +bool DPSerial::s_haveUnacknowledgedTrackedPacket = false; +Packet DPSerial::s_lastTrackedPacket(0, 0); +std::chrono::time_point + DPSerial::s_lastTrackedPacketSendTime; +bool DPSerial::s_pantoReportedInvalidData = false; +const std::chrono::milliseconds DPSerial::c_trackedPacketTimeout(10); + +bool DPSerial::s_pantoReady = true; +uint32_t DPSerial::s_magicReceiveIndex = 0; +ReceiveState DPSerial::s_receiveState = NONE; +Header DPSerial::s_receiveHeader = {0, 0}; + +void DPSerial::startWorker() +{ + reset(); + s_workerRunning = true; + s_worker = std::thread(update); +} + +void DPSerial::stopWorker() +{ + s_workerRunning = false; + s_worker.join(); +} + +void DPSerial::sendInstantPacket(Packet p) +{ + s_highPrioSendQueue.push(p); +} + +void DPSerial::sendPacket(Packet p) +{ + s_lowPrioSendQueue.push(p); +} + +void DPSerial::reset() +{ + std::queue emptyHP; + std::swap(s_highPrioSendQueue, emptyHP); + std::queue emptyLP; + std::swap(s_lowPrioSendQueue, emptyLP); + std::queue emptyRec; + std::swap(s_receiveQueue, emptyRec); + s_nextTrackedPacketId = 1; + s_haveUnacknowledgedTrackedPacket = false; + s_lastTrackedPacketSendTime; + s_pantoReportedInvalidData = false; + s_pantoReady = true; + s_magicReceiveIndex = 0; + s_receiveState = NONE; + s_receiveHeader = {0, 0}; +} + +void DPSerial::update() +{ + while (s_workerRunning) + { + while (processInput()) + ; + processOutput(); + } +} + +bool DPSerial::isTracked(uint8_t t) +{ + auto it = TrackedMessageTypes.find((MessageType)t); + return it != TrackedMessageTypes.end(); +} + +bool DPSerial::checkQueue(std::queue &q) +{ + if (q.empty()) + { + return false; + } + auto tracked = isTracked(q.front().header.MessageType); + return !tracked || !s_haveUnacknowledgedTrackedPacket; +} + +void DPSerial::processOutput() +{ + bool resend = false; + Packet packet(255, 0); + + // send high prio packets (sync/heartbeat) + if (checkQueue(s_highPrioSendQueue)) + { + packet = s_highPrioSendQueue.front(); + s_highPrioSendQueue.pop(); + } + // otherwise, check if panto buffer is critical + else if (!s_pantoReady) + { + return; + } + // check if the last tracked message has to be resent + else if ( + s_haveUnacknowledgedTrackedPacket && + std::chrono::steady_clock::now() - s_lastTrackedPacketSendTime > + c_trackedPacketTimeout) + { + logString("Packet timed out, resending"); + packet = s_lastTrackedPacket; + } + // otherwise, send a low prio packet + else if (checkQueue(s_lowPrioSendQueue)) + { + packet = s_lowPrioSendQueue.front(); + s_lowPrioSendQueue.pop(); + } + // no packets, nothing to do + else + { + return; + } + + if (packet.payloadIndex != packet.header.PayloadSize) + { + logString("INVALID PACKET"); + return; + } + + if (isTracked(packet.header.MessageType)) + { + if (packet.header.PacketId == 0) + { + packet.header.PacketId = s_nextTrackedPacketId++; + if (s_nextTrackedPacketId == 0) + { + s_nextTrackedPacketId++; + } + } + s_haveUnacknowledgedTrackedPacket = true; + s_lastTrackedPacket = packet; + s_lastTrackedPacketSendTime = std::chrono::steady_clock::now(); + } + + uint8_t header[c_headerSize]; + header[0] = packet.header.MessageType; + header[1] = packet.header.PacketId; + header[2] = packet.header.PayloadSize >> 8; + header[3] = packet.header.PayloadSize & 255; + + write(c_magicNumber, c_magicNumberSize); + write(header, c_headerSize); + write(packet.payload, packet.header.PayloadSize); +} + +bool DPSerial::processInput() +{ + switch (s_receiveState) + { + case NONE: + return readMagicNumber(); + case FOUND_MAGIC: + return readHeader(); + case FOUND_HEADER: + return readPayload(); + default: + return false; + } +} + +bool DPSerial::readMagicNumber() +{ + uint8_t received; + while (s_magicReceiveIndex < c_magicNumberSize && + readBytesIfAvailable(&received, 1)) + { + if (received == c_magicNumber[s_magicReceiveIndex]) + { + ++s_magicReceiveIndex; + } + else + { + std::cout << received; + s_magicReceiveIndex = 0; +#ifndef SKIP_ANALYZER + CrashAnalyzer::push_back(received); +#endif + } + } + if (s_magicReceiveIndex != c_magicNumberSize) + { + return false; + }; + s_receiveState = FOUND_MAGIC; + s_magicReceiveIndex = 0; + return true; +} + +bool DPSerial::readHeader() +{ + uint8_t received[c_headerSize]; + if (!readBytesFromSerial(received, c_headerSize)) + { + return false; + } + + s_receiveHeader.MessageType = received[0]; + s_receiveHeader.PacketId = received[1]; + s_receiveHeader.PayloadSize = received[2] << 8 | received[3]; + + // handle no-payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case BUFFER_CRITICAL: + s_pantoReady = false; + s_receiveState = NONE; + logString("Panto buffer critical"); + break; + case BUFFER_READY: + s_pantoReady = true; + s_receiveState = NONE; + logString("Panto buffer ready"); + break; + case INVALID_DATA: + s_receiveState = NONE; + s_pantoReportedInvalidData = true; + logString("Panto received invalid data"); + break; + default: + s_receiveState = FOUND_HEADER; + break; + } + return true; +} + +bool DPSerial::readPayload() +{ + const uint16_t size = s_receiveHeader.PayloadSize; + std::vector received; + received.reserve(size); + if (!readBytesFromSerial(received.data(), size)) + { + return false; + } + + auto packet = Packet(s_receiveHeader.MessageType, size); + memcpy(packet.payload, received.data(), size); + s_receiveState = NONE; + + // handle payload low level messages instantly + switch (s_receiveHeader.MessageType) + { + case PACKET_ACK: + { + auto id = packet.receiveUInt8(); + if (!s_haveUnacknowledgedTrackedPacket) + { + logString("Received unexpected PACKET_ACK"); + } + else if (id != s_lastTrackedPacket.header.PacketId) + { + std::ostringstream oss; + oss << "Received PACKET_ACK for wrong packet. Expected " + << (int)s_lastTrackedPacket.header.PacketId << ", received " + << (int)id << std::endl; + logString((char *)oss.str().c_str()); + } + else + { + s_haveUnacknowledgedTrackedPacket = false; + } + break; + } + case INVALID_PACKET_ID: + { + std::ostringstream oss; + oss << "Panto reports INVALID_PACKET_ID. Expected " + << (int)packet.receiveUInt8() << ", received " + << (int)packet.receiveUInt8() << std::endl; + logString((char *)oss.str().c_str()); + break; + } + default: + s_receiveQueue.push(packet); + break; + } + + return true; +} + +bool DPSerial::readBytesIfAvailable(void *target, uint32_t length) +{ + if (getAvailableByteCount(s_handle) < length) + { + return false; + } + return readBytesFromSerial(target, length); +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/src/serial/unix.cpp b/firmware/haptics/template/utils/serial/src/serial/unix.cpp new file mode 100644 index 0000000..eedd2c4 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/serial/unix.cpp @@ -0,0 +1,77 @@ +#include "serial.hpp" + +#include +#include + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + uint32_t available = 0; + if (ioctl(fileno(s_handle), FIONREAD, &available) < 0) + { + return 0; + } + return available; +} + +void DPSerial::tearDown() +{ + stopWorker(); + fclose(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + const uint32_t result = fread(target, 1, length, s_handle); + const bool valid = result == length; + if (!valid) + { + if (feof(s_handle)) + { + std::cout + << "Read end of file from serial, trying to reconnect." + << std::endl; + tearDown(); + setup(s_path); + } + else if (ferror(s_handle)) + { + perror("Error while reading from serial"); + } + } + return valid; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + ::write(fileno(s_handle), data, length); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + int fd = open(path.c_str(), O_RDWR | O_NOCTTY); + if (fd < 0) + { + return false; + } + struct termios tty; + std::memset(&tty, 0, sizeof(tty)); + if (tcgetattr(fd, &tty) < 0) + { + return false; + } + const speed_t speed = c_baudRate; + cfsetospeed(&tty, speed); + cfsetispeed(&tty, speed); + cfmakeraw(&tty); + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + if (tcsetattr(fd, TCSANOW, &tty) < 0) + { + return false; + } + s_handle = fdopen(fd, "rw"); + + startWorker(); + return true; +} diff --git a/firmware/haptics/template/utils/serial/src/serial/win.cpp b/firmware/haptics/template/utils/serial/src/serial/win.cpp new file mode 100644 index 0000000..51adc05 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/serial/win.cpp @@ -0,0 +1,71 @@ +#include "serial.hpp" + +uint32_t DPSerial::getAvailableByteCount(FILEHANDLE s_handle) +{ + DWORD commerr; + COMSTAT comstat; + if (!ClearCommError(s_handle, &commerr, &comstat)) + { + return 0; + } + return comstat.cbInQue; +} + +void DPSerial::tearDown() +{ + stopWorker(); + CloseHandle(s_handle); +} + +bool DPSerial::readBytesFromSerial(void *target, uint32_t length) +{ + DWORD bytesRead; + ReadFile(s_handle, target, length, &bytesRead, NULL); + return bytesRead == length; +} + +void DPSerial::write(const uint8_t *const data, const uint32_t length) +{ + DWORD bytesWritten = 0; + WriteFile(s_handle, data, length, &bytesWritten, NULL); +} + +bool DPSerial::setup(std::string path) +{ + s_path = path; + s_handle = CreateFileA(path.c_str(), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + if (s_handle == INVALID_HANDLE_VALUE) + { + return false; + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!GetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + dcbSerialParams.BaudRate = c_baudRate; + dcbSerialParams.ByteSize = 8; + dcbSerialParams.StopBits = ONESTOPBIT; + dcbSerialParams.Parity = NOPARITY; + if (!SetCommState(s_handle, &dcbSerialParams)) + { + return false; + } + + COMMTIMEOUTS timeouts = {0}; + timeouts.ReadIntervalTimeout = 50; + timeouts.ReadTotalTimeoutConstant = 50; + timeouts.ReadTotalTimeoutMultiplier = 10; + timeouts.WriteTotalTimeoutConstant = 50; + timeouts.WriteTotalTimeoutMultiplier = 10; + if (!SetCommTimeouts(s_handle, &timeouts) || + !SetCommMask(s_handle, EV_RXCHAR)) + { + return false; + } + + startWorker(); + return true; +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/src/standalone/main.cpp b/firmware/haptics/template/utils/serial/src/standalone/main.cpp new file mode 100644 index 0000000..9e4da35 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/standalone/main.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +#include "standalone.hpp" + +int main(int argc, char** argv) +{ + if (argc != 2) + { + fprintf(stderr, "Expected /dev/serialport\n"); + return -1; + } + if (!Standalone::setup(argv[1])) + { + fprintf(stderr, "Could not open %s\n", argv[1]); + return -2; + } + signal(SIGINT, &(Standalone::terminate)); + + // cin/cout to hex mode + std::cout << std::hex << std::uppercase << std::setfill('0'); + + while (true) + { + Standalone::printPacket(); + } + + return 0; +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/src/standalone/standalone.cpp b/firmware/haptics/template/utils/serial/src/standalone/standalone.cpp new file mode 100644 index 0000000..d177bb4 --- /dev/null +++ b/firmware/haptics/template/utils/serial/src/standalone/standalone.cpp @@ -0,0 +1,27 @@ +#include +#include +#include +#include + +#include "standalone.hpp" + +void Standalone::terminate(int signal) +{ + tearDown(); + exit(0); +} + +void Standalone::printPacket() +{ + receivePacket(); + for (auto i = 0; i < c_headerSize; ++i) + { + std::cout << std::setw(2) << (int)s_headerBuffer[i] << " "; + } + std::cout << "| "; + for (auto i = 0; i < s_header.PayloadSize; ++i) + { + std::cout << std::setw(2) << (int)s_packetBuffer[i] << " "; + } + std::cout << std::endl; +} \ No newline at end of file diff --git a/firmware/haptics/template/utils/serial/unity/Serial.cs b/firmware/haptics/template/utils/serial/unity/Serial.cs new file mode 100644 index 0000000..4936765 --- /dev/null +++ b/firmware/haptics/template/utils/serial/unity/Serial.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using UnityEngine; + +public class Serial : MonoBehaviour { + public delegate void SyncDelegate(ulong handle); + public delegate void HeartbeatDelegate(ulong handle); + public delegate void PositionDelegate(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions); + public delegate void LoggingDelegate(IntPtr msg); + + protected ulong Handle; + + [DllImport("serial")] + private static extern uint GetRevision(); + [DllImport("serial")] + private static extern void SetSyncHandler(SyncDelegate func); + [DllImport("serial")] + private static extern void SetHeartbeatHandler(HeartbeatDelegate func); + [DllImport("serial")] + private static extern void SetPositionHandler(PositionDelegate func); + [DllImport("serial")] + private static extern void SetLoggingHandler(LoggingDelegate func); + [DllImport("serial")] + private static extern ulong Open(IntPtr port); + [DllImport("serial")] + private static extern void Close(ulong handle); + [DllImport("serial")] + private static extern void Poll(ulong handle); + [DllImport("serial")] + private static extern void SendSyncAck(ulong handle); + [DllImport("serial")] + private static extern void SendHeartbeatAck(ulong handle); + + private static void SyncHandler(ulong handle) + { + Debug.Log("Received sync"); + SendSyncAck(handle); + } + + private static void HeartbeatHandler(ulong handle) + { + Debug.Log("Received heartbeat"); + SendHeartbeatAck(handle); + } + + private static void PositionHandler(ulong handle, [MarshalAs(UnmanagedType.LPArray, ArraySubType = UnmanagedType.R8, SizeConst = 10)] double[] positions) + { + Debug.Log("Received positions: (" + positions[0] + "|" + positions[1] + ")"); + //Debug.Log("Received positions"); + } + + private static void LogHandler(IntPtr msg) + { + Debug.Log(Marshal.PtrToStringAnsi(msg)); + } + + private static ulong OpenPort(string port) + { + return Open(Marshal.StringToHGlobalAnsi(port)); + } + + void Start () + { + Debug.Log("Serial protocol revision: " + GetRevision()); + SetLoggingHandler(LogHandler); + SetSyncHandler(SyncHandler); + SetHeartbeatHandler(HeartbeatHandler); + SetPositionHandler(PositionHandler); + // should be discovered automatically + Handle = OpenPort("//.//COM3"); + } + + void Update () + { + Poll(Handle); + } + + void OnDestroy() + { + Close(Handle); + } +} diff --git a/firmware/haptics/template/utils/serial/unity/linux.sh b/firmware/haptics/template/utils/serial/unity/linux.sh new file mode 100755 index 0000000..8d673ed --- /dev/null +++ b/firmware/haptics/template/utils/serial/unity/linux.sh @@ -0,0 +1,3 @@ +#!/bin/sh +cmake .. -G"Unix Makefiles" -S".." -B"../cppLibBuild" -DCMAKE_BUILD_TYPE=Release +cmake --build "../cppLibBuild" --config Release diff --git a/firmware/haptics/template/utils/serial/unity/mac.sh b/firmware/haptics/template/utils/serial/unity/mac.sh new file mode 100755 index 0000000..33b47e9 --- /dev/null +++ b/firmware/haptics/template/utils/serial/unity/mac.sh @@ -0,0 +1,5 @@ +SOURCE_DIR="./utils/serial" +BUILD_DIR="./utils/serial/cppLibBuild" + +cmake $SOURCE_DIR -G"Xcode" -S $SOURCE_DIR -B $BUILD_DIR +cmake --build $BUILD_DIR --config Release diff --git a/firmware/haptics/template/utils/serial/unity/win.bat b/firmware/haptics/template/utils/serial/unity/win.bat new file mode 100755 index 0000000..d18e6aa --- /dev/null +++ b/firmware/haptics/template/utils/serial/unity/win.bat @@ -0,0 +1,5 @@ +SET SOURCE_DIR=utils\serial +SET BUILD_DIR=utils\serial\cppLibBuild + +cmake %SOURCE_DIR% -G"Visual Studio 15 2017 Win64" -S %SOURCE_DIR% -B %BUILD_DIR% +cmake --build %BUILD_DIR% --config RelWithDebInfo \ No newline at end of file diff --git a/firmware/00 template/.gitignore b/firmware/hardware/#example/00 template/.gitignore similarity index 100% rename from firmware/00 template/.gitignore rename to firmware/hardware/#example/00 template/.gitignore diff --git a/firmware/00 template/platformio.ini b/firmware/hardware/#example/00 template/platformio.ini similarity index 100% rename from firmware/00 template/platformio.ini rename to firmware/hardware/#example/00 template/platformio.ini diff --git a/firmware/00 template/src/main.cpp b/firmware/hardware/#example/00 template/src/main.cpp similarity index 100% rename from firmware/00 template/src/main.cpp rename to firmware/hardware/#example/00 template/src/main.cpp diff --git a/firmware/01 hello world/.gitignore b/firmware/hardware/#example/02 echo/.gitignore similarity index 100% rename from firmware/01 hello world/.gitignore rename to firmware/hardware/#example/02 echo/.gitignore diff --git a/firmware/02 echo/platformio.ini b/firmware/hardware/#example/02 echo/platformio.ini similarity index 100% rename from firmware/02 echo/platformio.ini rename to firmware/hardware/#example/02 echo/platformio.ini diff --git a/firmware/02 echo/src/main.cpp b/firmware/hardware/#example/02 echo/src/main.cpp similarity index 100% rename from firmware/02 echo/src/main.cpp rename to firmware/hardware/#example/02 echo/src/main.cpp diff --git a/firmware/02 echo/.gitignore b/firmware/hardware/05 move handles in sync/.gitignore similarity index 100% rename from firmware/02 echo/.gitignore rename to firmware/hardware/05 move handles in sync/.gitignore diff --git a/firmware/05 move handles in sync/platformio.ini b/firmware/hardware/05 move handles in sync/platformio.ini similarity index 100% rename from firmware/05 move handles in sync/platformio.ini rename to firmware/hardware/05 move handles in sync/platformio.ini diff --git a/firmware/05 move handles in sync/src/main.cpp b/firmware/hardware/05 move handles in sync/src/main.cpp similarity index 100% rename from firmware/05 move handles in sync/src/main.cpp rename to firmware/hardware/05 move handles in sync/src/main.cpp diff --git a/firmware/04 encoder read/.gitignore b/firmware/hardware/linkage encoder/.gitignore similarity index 100% rename from firmware/04 encoder read/.gitignore rename to firmware/hardware/linkage encoder/.gitignore diff --git a/firmware/04 encoder read/platformio.ini b/firmware/hardware/linkage encoder/platformio.ini similarity index 100% rename from firmware/04 encoder read/platformio.ini rename to firmware/hardware/linkage encoder/platformio.ini diff --git a/firmware/04 encoder read/src/main.cpp b/firmware/hardware/linkage encoder/src/main.cpp similarity index 80% rename from firmware/04 encoder read/src/main.cpp rename to firmware/hardware/linkage encoder/src/main.cpp index 42f37ae..e63d014 100644 --- a/firmware/04 encoder read/src/main.cpp +++ b/firmware/hardware/linkage encoder/src/main.cpp @@ -26,7 +26,7 @@ uint16_t buf = 0; void setup() { - Serial.begin(9600); // opens serial port, sets data rate to 9600 bps + Serial.begin(115200); // opens serial port, sets data rate to 9600 bps pinMode(13, OUTPUT); pinMode(c_hspiSsPin1, OUTPUT); @@ -64,29 +64,36 @@ void loop(){ delayMicroseconds(1); - if (channel == 0){ - Serial.printf("\ndptest"); - } +// if (channel == 0){ +// Serial.printf("\ndptest"); +// } if(channel == 0) digitalWrite(c_hspiSsPin2, LOW); else if(channel == 1) digitalWrite(c_hspiSsPin1, LOW); + if(channel == 1){ for(auto i = 0; i < m_values.size()/2; ++i) { buf = m_spi.transfer16(c_nop); m_values[i + channel*2] = buf & c_dataMask; - Serial.printf("%d,", m_values[i + channel*2]); + +// Serial.print("Encoder" + String(i)); + Serial.print(m_values[i + channel*2]); + Serial.print(","); +// Serial.printf("%d,", m_values[i + channel*2]); + } + Serial.println(0); } digitalWrite(c_hspiSsPin1, HIGH); digitalWrite(c_hspiSsPin2, HIGH); m_spi.endTransaction(); - if (channel == 1){ - for (int i = 0; i < 2; i++){ - Serial.printf("%d,", endeffectorEncoder[i]->read()); - } - } +// if (channel == 1){ +// for (int i = 0; i < 2; i++){ +// Serial.printf("%d,", endeffectorEncoder[i]->read()); +// } +// } c_i++; channel = c_i % 2; diff --git a/firmware/03 motor/.gitignore b/firmware/hardware/linkage motor/.gitignore similarity index 100% rename from firmware/03 motor/.gitignore rename to firmware/hardware/linkage motor/.gitignore diff --git a/firmware/03 motor/platformio.ini b/firmware/hardware/linkage motor/platformio.ini similarity index 92% rename from firmware/03 motor/platformio.ini rename to firmware/hardware/linkage motor/platformio.ini index a9537c2..a274263 100644 --- a/firmware/03 motor/platformio.ini +++ b/firmware/hardware/linkage motor/platformio.ini @@ -6,7 +6,7 @@ platform = espressif32@2.1.0 framework = arduino board = esp32dev #monitor_speed = 115200 -monitor_speed = 9600 +monitor_speed = 115200 # On OSX you need to use the following USB port # On Windows outcomment this line‚ diff --git a/firmware/03 motor/src/main.cpp b/firmware/hardware/linkage motor/src/main.cpp similarity index 100% rename from firmware/03 motor/src/main.cpp rename to firmware/hardware/linkage motor/src/main.cpp diff --git a/firmware/05 move handles in sync/.gitignore b/firmware/hello world/.gitignore similarity index 100% rename from firmware/05 move handles in sync/.gitignore rename to firmware/hello world/.gitignore diff --git a/firmware/01 hello world/platformio.ini b/firmware/hello world/platformio.ini similarity index 100% rename from firmware/01 hello world/platformio.ini rename to firmware/hello world/platformio.ini diff --git a/firmware/01 hello world/src/main.cpp b/firmware/hello world/src/main.cpp similarity index 100% rename from firmware/01 hello world/src/main.cpp rename to firmware/hello world/src/main.cpp diff --git a/physical_test/mechanical_issue_check_list.md b/physical_test/mechanical_issue_check_list.md new file mode 100644 index 0000000..88858fa --- /dev/null +++ b/physical_test/mechanical_issue_check_list.md @@ -0,0 +1,10 @@ +# Checking Mechanical and some random Issue +First things you needs to check is mechanical issue +(or assembly issue) of dualpanto device. + +0. check dualpanto is connected to PC via microUSB +1. Home the linkages by moving them to the initial position and reset the microcontroller +2. Reconnect the USB cable (power cycling) +3. Check your battery level +4. check motor solder connection +5. check connection between motor and linkages: one set screw should be against the “d-cut” of the shaft \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 4f6ea4d..4456088 100644 --- a/requirements.txt +++ b/requirements.txt @@ -2,4 +2,6 @@ pyserial unittest platformio parameterized -html-testRunner \ No newline at end of file +html-testRunner + +matplotlib \ No newline at end of file diff --git a/resources/dualpanto_switch.jpg b/resources/dualpanto_switch.jpg new file mode 100644 index 0000000..f2adc98 Binary files /dev/null and b/resources/dualpanto_switch.jpg differ diff --git a/resources/images/dualpanto_switch.jpg b/resources/images/dualpanto_switch.jpg deleted file mode 100644 index d12bb44..0000000 Binary files a/resources/images/dualpanto_switch.jpg and /dev/null differ diff --git a/resources/images/pycharm_unittest_1.jpg b/resources/images/pycharm_unittest_1.jpg deleted file mode 100644 index 07e3b31..0000000 Binary files a/resources/images/pycharm_unittest_1.jpg and /dev/null differ diff --git a/test_firmware.py b/test_firmware.py index 2d8263e..62896f9 100644 --- a/test_firmware.py +++ b/test_firmware.py @@ -5,28 +5,89 @@ class Basic(unittest.TestCase): def test_shows_up_as_serial(self): + print("This test requires you to interact with dualpanto device and test it physically using test paper-jig.") + print("Please check./physical_test/line wall/README.md") + print("When you tested, please press Enter key.") self.assertIn(config.COM_PORT, util.serial_ports()) def test_compile_firmware(self): - res = util.compile_firmware('firmware/01 hello world') + res = util.compile_firmware('firmware/hello world') self.assertEqual(res, 0, msg='failed to compile firmware. please first check platformIO config and installation') def test_upload_firmware(self): - res = util.upload_firmware('./firmware/01 hello world') + res = util.upload_firmware('./firmware/hello world') self.assertEqual(res, 0, msg='failed to upload firmware. please first check platformIO config and installation') - class Haptics(unittest.TestCase): - def test_shows_up_as_serial(self): - self.assertIn(config.COM_PORT, util.serial_ports()) def test_line_wall(self): - res = util.upload_firmware("firmware/09 god object/firmware", False) + res = util.upload_firmware("firmware/haptics/line wall/firmware", False) + print("") + print("==========================") + print("===== TEST LINE WALL =====") + print("==========================") + print("") + + print("This test requires you to interact with dualpanto device and test it physically using test paper-jig.") + print("Please check./physical_test/line wall/README.md") + print("When you tested, please press Enter key.") + + print("") + print("==========================") + print("===== TEST LINE WALL =====") + print("==========================") + input() self.assertEqual(res, 0, msg='failed to upload firmware. please first check platformIO config and installation') + def test_force_field(self): + res = util.upload_firmware("firmware/haptics/force field/firmware", False) + print("") + print("==========================") + print("==== TEST FORCE FIELD ====") + print("==========================") + print("") + + print("This test requires you to interact with dualpanto device and test it physically using test paper-jig.") + print("Please check./physical_test/line wall/README.md") + print("When you tested, please press Enter key.") + + print("") + print("==========================") + print("==== TEST FORCE FIELD ====") + print("==========================") + print("") + self.assertEqual(res, 0, msg='failed to upload firmware. please first check platformIO config and installation') + + def test_BIS_week7(self): + res = util.upload_firmware("firmware/haptics/BIS week7/firmware", False) + print("") + print("=========================") + print("======= BIS WEEK7 =======") + print("=========================") + print("") + + print("This test requires you to interact with dualpanto device and test it physically using test paper-jig.") + print("Please check./BIS/") + print("When you tested, please press Enter key.") + + print("") + print("=========================") + print("======= BIS WEEK7 =======") + print("=========================") + print("") + self.assertEqual(res, 0, msg='failed to upload firmware. please first check platformIO config and installation') + + + class Kinematics(unittest.TestCase): def test_forward(self): self.assertIn(config.COM_PORT, util.serial_ports()) def test_inverse(self): - self.assertIn(config.COM_PORT, util.serial_ports()) \ No newline at end of file + self.assertIn(config.COM_PORT, util.serial_ports()) + + +class UploadDualPantoFramework(unittest.TestCase): + def test_upload_dp_firmware(self): + res = util.compile_firmware('firmware/dualpantoframework') + self.assertEqual(res, 0, msg='failed to compile firmware. please first check platformIO config and installation') diff --git a/test_hardware.py b/test_hardware.py index 40e7518..02fe0a3 100644 --- a/test_hardware.py +++ b/test_hardware.py @@ -13,7 +13,7 @@ class Linkage(unittest.TestCase): encoder_pos = [0, 0, 0, 0] continue_serial_connection_flag = True def test_encoder(self): - res = util.upload_firmware('./firmware/04 encoder read') + res = util.upload_firmware('./firmware/hardware/linkage encoder') self.assertEqual(res, 0, msg='failed to upload firmware') time.sleep(1) # move serial connection handling to other thread @@ -163,7 +163,7 @@ def test_sync(self): def handle_serial_connection(self): print("Connecting...") - with serial.Serial(config.COM_PORT, 9600, timeout=1, parity=serial.PARITY_EVEN) as ser: + with serial.Serial(config.COM_PORT, 115200, timeout=1, parity=serial.PARITY_EVEN) as ser: time.sleep(1) self.assertNotEqual(ser.inWaiting(), 0, msg="could not establish serial connection... try restarting the panto") uint_overflow_correction = [0,0,0,0] diff --git a/test_protocol.py b/test_protocol.py index 93d5d31..96542b0 100644 --- a/test_protocol.py +++ b/test_protocol.py @@ -129,7 +129,7 @@ class BaseProtocolTest(unittest.TestCase): def setUp(self): if config.uploading_firmware: - util.upload_firmware("firmware/10 panto firmware/firmware", False) + util.upload_firmware("firmware/dualpantoframework/firmware", False) com = config.COM_PORT self.con = serial.Serial(com, baudrate=115200, timeout=5) diff --git a/utils/util.py b/utils/util.py index 4b9f117..894623c 100644 --- a/utils/util.py +++ b/utils/util.py @@ -4,6 +4,14 @@ import glob import serial +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +import sys, serial, argparse +import numpy as np +from time import sleep +from collections import deque + """ Lists serial port names :raises EnvironmentError: @@ -48,3 +56,50 @@ def upload_firmware(firmware_directory="./firmware/00 template", cleanup=True): if cleanup: res += subprocess.call('pio run --target clean', cwd=firmware_directory, shell=True) return res + + +class AnalogPlot: + # constr + def __init__(self, strPort, maxLen): + # open serial port + self.ser = serial.Serial(strPort, 9600) + + self.ax = deque([0.0] * maxLen) + self.ay = deque([0.0] * maxLen) + self.maxLen = maxLen + + # add to buffer + def addToBuf(self, buf, val): + if len(buf) < self.maxLen: + buf.append(val) + else: + buf.pop() + buf.appendleft(val) + + # add data + def add(self, data): + assert (len(data) == 2) + self.addToBuf(self.ax, data[0]) + self.addToBuf(self.ay, data[1]) + + # update plot + def update(self, frameNum, a0, a1): + try: + line = self.ser.readline() + data = [float(val) for val in line.split()] + # print data + if (len(data) == 2): + self.add(data) + a0.set_data(range(self.maxLen), self.ax) + a1.set_data(range(self.maxLen), self.ay) + except KeyboardInterrupt: + print('exiting') + + return a0, + + # clean up + + def close(self): + # close serial + self.ser.flush() + self.ser.close() \ No newline at end of file