diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..13566b8 --- /dev/null +++ b/.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/.idea/dualpanto-testing.iml b/.idea/dualpanto-testing.iml new file mode 100644 index 0000000..cfcaed4 --- /dev/null +++ b/.idea/dualpanto-testing.iml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..fab02fb --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..046cb7c --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/01 debug_panto_heartbeats.py b/01 debug_panto_heartbeats.py new file mode 100644 index 0000000..a4ab0d1 --- /dev/null +++ b/01 debug_panto_heartbeats.py @@ -0,0 +1,27 @@ +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/02 debug_panto.py b/02 debug_panto.py new file mode 100644 index 0000000..06b4c52 --- /dev/null +++ b/02 debug_panto.py @@ -0,0 +1,75 @@ +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/Readme.md b/Readme.md new file mode 100644 index 0000000..3fce896 --- /dev/null +++ b/Readme.md @@ -0,0 +1,36 @@ +# 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 + +## Levels of Testing and Repository structure + +### Hardware + +On a hardware level we need to test that the dualpanto can + +- show up as a serial device +- receive uploaded (mini) firmware +- read encoders +- actuate motors +- move two handles in sync + +### Firmware + +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 + +### Unity + +TODO + + +This project is currenty under developement + +For questions, please reach out to martin.taraz@hpi.de \ No newline at end of file diff --git a/config.py b/config.py new file mode 100644 index 0000000..ff53919 --- /dev/null +++ b/config.py @@ -0,0 +1 @@ +COM_PORT = "/dev/tty.SLAB_USBtoUART" diff --git a/firmware/00 template/.gitignore b/firmware/00 template/.gitignore new file mode 100644 index 0000000..aeaebb2 --- /dev/null +++ b/firmware/00 template/.gitignore @@ -0,0 +1 @@ +.pio \ No newline at end of file diff --git a/firmware/00 template/platformio.ini b/firmware/00 template/platformio.ini new file mode 100644 index 0000000..20538c3 --- /dev/null +++ b/firmware/00 template/platformio.ini @@ -0,0 +1,12 @@ +[platformio] +description = Dual Panto firmware + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +monitor_speed = 115200 + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +# upload_port = /dev/cu.SLAB_USBtoUART \ No newline at end of file diff --git a/firmware/00 template/src/main.cpp b/firmware/00 template/src/main.cpp new file mode 100644 index 0000000..49dabb6 --- /dev/null +++ b/firmware/00 template/src/main.cpp @@ -0,0 +1,26 @@ +#include "Arduino.h" + +#ifndef LED_BUILTIN +#define LED_BUILTIN 13 +#endif + +void setup() +{ + // initialize LED digital pin as an output. + pinMode(LED_BUILTIN, OUTPUT); +} + +void loop() +{ + // turn the LED on (HIGH is the voltage level) + digitalWrite(LED_BUILTIN, HIGH); + + // wait for a second + delay(1000); + + // turn the LED off by making the voltage LOW + digitalWrite(LED_BUILTIN, LOW); + + // wait for a second + delay(1000); +} \ No newline at end of file diff --git a/firmware/01 hello world/.gitignore b/firmware/01 hello world/.gitignore new file mode 100644 index 0000000..aeaebb2 --- /dev/null +++ b/firmware/01 hello world/.gitignore @@ -0,0 +1 @@ +.pio \ No newline at end of file diff --git a/firmware/01 hello world/platformio.ini b/firmware/01 hello world/platformio.ini new file mode 100644 index 0000000..20538c3 --- /dev/null +++ b/firmware/01 hello world/platformio.ini @@ -0,0 +1,12 @@ +[platformio] +description = Dual Panto firmware + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +monitor_speed = 115200 + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +# upload_port = /dev/cu.SLAB_USBtoUART \ No newline at end of file diff --git a/firmware/01 hello world/src/main.cpp b/firmware/01 hello world/src/main.cpp new file mode 100644 index 0000000..49dabb6 --- /dev/null +++ b/firmware/01 hello world/src/main.cpp @@ -0,0 +1,26 @@ +#include "Arduino.h" + +#ifndef LED_BUILTIN +#define LED_BUILTIN 13 +#endif + +void setup() +{ + // initialize LED digital pin as an output. + pinMode(LED_BUILTIN, OUTPUT); +} + +void loop() +{ + // turn the LED on (HIGH is the voltage level) + digitalWrite(LED_BUILTIN, HIGH); + + // wait for a second + delay(1000); + + // turn the LED off by making the voltage LOW + digitalWrite(LED_BUILTIN, LOW); + + // wait for a second + delay(1000); +} \ No newline at end of file diff --git a/firmware/02 echo/.gitignore b/firmware/02 echo/.gitignore new file mode 100644 index 0000000..aeaebb2 --- /dev/null +++ b/firmware/02 echo/.gitignore @@ -0,0 +1 @@ +.pio \ No newline at end of file diff --git a/firmware/02 echo/platformio.ini b/firmware/02 echo/platformio.ini new file mode 100644 index 0000000..20538c3 --- /dev/null +++ b/firmware/02 echo/platformio.ini @@ -0,0 +1,12 @@ +[platformio] +description = Dual Panto firmware + +[env:esp32dev] +platform = espressif32@2.1.0 +framework = arduino +board = esp32dev +monitor_speed = 115200 + +# On OSX you need to use the following USB port +# On Windows outcomment this line‚ +# upload_port = /dev/cu.SLAB_USBtoUART \ No newline at end of file diff --git a/firmware/02 echo/src/main.cpp b/firmware/02 echo/src/main.cpp new file mode 100644 index 0000000..44e0e25 --- /dev/null +++ b/firmware/02 echo/src/main.cpp @@ -0,0 +1,20 @@ +#include "Arduino.h" + +int incomingByte = 0; // for incoming serial data + + +void setup() +{ + Serial.begin(9600); // opens serial port, sets data rate to 9600 bps +} + +void loop() +{ + // send data only when you receive data: + if (Serial.available() > 0) { + // read the incoming byte: + incomingByte = Serial.read(); + // say what you got: + Serial.write(incomingByte); + } +} \ No newline at end of file diff --git a/firmware/03 panto firmware/.gitattributes b/firmware/03 panto firmware/.gitattributes new file mode 100644 index 0000000..dfdb8b7 --- /dev/null +++ b/firmware/03 panto firmware/.gitattributes @@ -0,0 +1 @@ +*.sh text eol=lf diff --git a/firmware/03 panto firmware/.gitignore b/firmware/03 panto firmware/.gitignore new file mode 100644 index 0000000..11b82e0 --- /dev/null +++ b/firmware/03 panto firmware/.gitignore @@ -0,0 +1,69 @@ +utils/serial/* +!utils/serial/include +!utils/serial/src +!utils/serial/binding.gyp +!utils/serial/CMakeLists.txt +!utils/serial/unity +!utils/voiceCommand/build/build-release.js +build/ +node_modules/ +package-lock.json +config.json +persistent.json + +*.obj + +*.exe + +src/osx/voice-command/build +.bin/ + + +# User-specific files +*.DS_Store +*.suo +*.sln.ide/ +*.user +*.userosscache +*.sln.docstates +*.plist +*.xccheckout +!utils/voiceCommand/src/osx/voice-command/voice-command.xcodeproj/project.xcworkspace/xcuserdata/baluubas.xcuserdatad +!utils/voiceCommand/src/osx/voice-command/voice-command.xcodeproj/xcuserdata/baluubas.xcuserdatad +*.xcuserdatad + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +bld/ +[Bb]in/ +[Oo]bj/ + +*.lock + +*.ide + +*.ide-shm + +*.ide-wal + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json +.history +firmware/src/config +firmware/include/config + +legacy/ +.idea +.vscode \ No newline at end of file diff --git a/firmware/03 panto firmware/documentation/protocol/protocol.md b/firmware/03 panto firmware/documentation/protocol/protocol.md new file mode 100644 index 0000000..d74b9bc --- /dev/null +++ b/firmware/03 panto firmware/documentation/protocol/protocol.md @@ -0,0 +1,469 @@ +# Serial Communication Protocol - Revision 6 + +All messages contain a [header](#header) and an optional [payload](#payload). + +## Serial Transmission Configuration + +### Baud Rate + +The transmission uses a baud rate of 115200. + +### Packet Size + +The maximum packet size is 262 bytes, with 6 bytes being used for the header and 256 bytes for the payload. This limit for the payload size was chosen to avoid blocking the transmission for too long. + +## Header + +The header consists of a [magic number](#magic-number), the [message type](#message-type), the [packet ID](#packet-id) and the [payload size](#payload-size). + +### Magic Number + +All messages start with the magic bytes 0x44 0x50 (DP). + +### Message Type + +The next byte specifies the type of the following messages. Values from 0x00 to 0x7F are used for messages from the hardware to the framework, while values from 0x80 to 0xFF are used for messages from the framework to the hardware. + +The available values for messages from the hardware to the framework are: + +- 0x00 to 0x0F - Administration messages + - [0x00 Sync](#0x00-Sync) - The hardware tells the framework that it is a dualpanto devices and wants to connect. + - [0x01 Heartbeat](#0x01-Heartbeat) - Need to be send regularly to avoid being disconnected for inactivity. + - [0x02 Buffer critical](#0x02-Buffer-critical) - Tells the framework to stop sending packets to avoid overflowing the device's receive buffer. + - [0x03 Buffer ready](#0x03-Buffer-ready) - Tells the framework to proceed sending packets. + - [0x04 Packet Ack](#0x04-Packet-Ack) - Acknowledges the successful transmission of a state-changing packet. + - [0x05 Invalid Packet ID](#0x05-Invalid-packet-id) - Informs the framework of an unexpected packet ID. + - [0x06 Invalid Data](#0x06-Invalid-data) - Informs the packet of unexpected bytes in the receive buffer. +- 0x10 to 0x1F - Data messages + - [0x10 Position](#0x10-Position) - This message contains the current positions of the handles, as well as the god objects' positions. +- 0x20 to 0x2F - Auxiliary messages + - [0x20 Debug log](#0x20-Debug-log) - This message contains a user-defined string that is meant as a debug log. + +The available values for messages from the framework to the hardware are: + +- 0x80 to 0x8F - Administration messages + - [0x80 Sync Ack](#0x80-Sync-Ack) - The framework acknowledges the connection. + - [0x81 Heartbeat Ack](#0x81-Heartbeat-Ack) - The framework acknowledges the heartbeat. +- 0x90 to 0xAF - Data messages + - [0x90 Motor](#0x90-Motor) - This message contains a motor movement. + - [0x91 PID values](#0x91-PID-values) - This message contains PID values for one + - [0x92 Speed](#0x92-Speed) - This message specifies the speed to use for tweening positions + - [0x93 Transition ended](#0x93-transition-ended) - DOCUMENTATION MISSING + - [0xA0 Create obstacle](#0xA0-Create-obstacle) - This message specifies an obstacle to be added to one or both handles. Note: The obstacle isn't enabled automatically after creation. An enable message is required to enable it. + - [0xA1 Add to obstacle](#0xA1-Add-to-obstacle) - This message specifies positions to be appended to an obstacle. + - [0xA2 Remove obstacle](#0xA2-Remove-obstacle) - This message specifies an obstacle to remove. + - [0xA3 Enable obstacle](#0xA3-Enable-obstacle) - This message specifies an obstacle to enable. + - [0xA4 Disable obstacle](#0xA4-Disable-obstacle) - This message specifies an obstacle to disable. + - [0xA5 Calibrate panto](#0xA5-Calibrate-panto) - This message specifies a calibration request. + - [0xA6 Create passable obstacle](#0xA6-Create-passable-obstacle) - This message specifies a passable obstacle to be added. + - [0xA7 Create rail](#0xA7-Create-rail) - This message specifies a haptic guide to be added. A haptic guide is also an obstacle and hence also needs to be enabled and can be disabled. A haptic guide also has a displacement area around it that the user needs to pass to overcome it. That way the strength of the guide can be specified. + - [0xA8 Freeze](#0xA8-Freeze) - Freeze the handle + - [0xA9 Free](#0xA9-Free) - Free the handle + - [0xAA Set Speed Control](#0xAA-SpeedControl) - Enable or disable speed control with different configuration options +- 0xC0 to 0xCF - Debug tools + [0xC0 Dump hashtable](#0xC0-Dump-hashtable) - Request a dump of the physics' hashtable. + +### Packet ID + +In order to ensure the firmware state stays in sync with what the framework expects it to be, all state-changing messages must be successfully transmitted. All state-changing packets must include an ID that is different from 0, as well as different from the previous ID. The receiver must acknowledge all packets with a non-zero ID. If the last tracked packet has not been acknowledged by the receiver in a implementation-specified interval, the sender should resend it. + +The packet ID is encoded as a 8 bit unsigned integer. It may be zero only if the message is not state-changing. Otherwise, the ID shall only increase, with the only exception being ID 255 being followed by 1. + +### Payload Size + +The payload size is encoded as a 16 bit unsigned integer. It may be zero if the message doesn't contain a payload. + +Note: The maximum allowed payload size is 256. This is limited by the ESP32's queue length. + +## Payload + +The content of the payload is based on the message type. + +### 0x00 Sync + +The message only contains the protocol revision, encoded as a 32 bit unsigned integer. + +Example message for protocol revision 0: +``` +4450 // magic number +00 // message type: sync +00 // packet ID: not utilized +0004 // payload length: 32 bit integer is 4 bytes long +00000000 // protocol revision 0 +``` + +### 0x01 Heartbeat + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +01 // message type: heartbeat +00 // packet ID: not utilized +0000 // payload length: no payload +``` + +### 0x02 Buffer critical + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +02 // message type: buffer critical +00 // packet ID: not utilized +0000 // payload length: no payload +``` + +### 0x03 Buffer ready + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +03 // message type: buffer ready +00 // packet ID: not utilized +0000 // payload length: no payload +``` + +### 0x04 Packet Ack + +The message contains the packet ID of the acknowledged packet. Note: This message itself is not state-changing and thus has has packet ID of 0. + +Example message: +``` +4450 // magic number +04 // message type: buffer ready +00 // packet ID: not utilized +0001 // payload length: 1 byte for ack'd packet ID +01 // ack'd packet ID +``` + +### 0x05 Invalid packet ID + +The message contains the expected and the received packet ID. + +Example message: +``` +4450 // magic number +05 // message type: invalid packet ID +00 // packet ID: not utilized +0002 // payload length: 1 per included ID +14 // expected packet ID 14 +15 // received packet ID 15 -> a packet got dropped +``` + +### 0x06 Invalid data + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +06 // message type: invalid data +00 // packet ID: not utilized +0000 // payload length: no payload +``` + + +### 0x10 Position + +The message contains - in this order - the x position, the y position and the rotation of a handle, each encoded as a 32 bit float, followed by the x and y position of the god object. This is repeated for each handle. + +Example message for two handles: +``` +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 +``` + +### 0x20 Debug log + +This message contains a custom ASCII-encoded debug string. + +Example message with text "HELP ME!": +``` +4450 // magic number +20 // message type: debug log +00 // packet ID: not utilized +0008 // payload length: string contains 8 bytes +48 // H +45 // E +4C // L +50 // P +20 // [space] +4D // M +45 // E +21 // ! +``` + +### 0x80 Sync Ack + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +80 // message type: sync acknowledgement +00 // packet ID: not utilized +0000 // payload length: no payload +``` + +### 0x81 Heartbeat Ack + +This message type does not require a payload. + +Example message: +``` +4450 // magic number +81 // message type: heartbeat acknowledgement +00 // packet ID: not utilized +0000 // payload length: no payload +``` + +### 0x90 Motor + +This message contains the control method and pantograph index, both encoded as a 8 bit unsigned integer, followed by the target position data (x, y, rotation), each encoded as a 32 bit float. + +Available control method values: + +- 0x00 Position +- 0x01 Force rendering + +Example message for setting the it handle position: +``` +4450 // magic number +90 // message type: motor +00 // packet ID: not utilized +000E // payload length: 1 byte for control method, 1 for index, 3*4 for target position +00 // control method: 0x00 for position mode +01 // panto index: 0x01 for it handle +FFFFFFFF // target x position +FFFFFFFF // target y position +FFFFFFFF // target rotation +``` + +### 0x91 PID values + +This message contains the motor index encoded as an 8 bit unsigned integer, followed by the P, I and D values, each encoded as 32 bit float. + +The motor indices are counted as follows: + +- 0x00 first pantograph, left motor +- 0x01 first pantograph, right motor +- 0x02 first pantograph, rotation motor +- 0x03 second pantograph, left motor +- ... + +Example message for tuning the second pantograph's rotation motor: +``` +4450 // magic number +91 // message type: PID values +00 // packet ID: not utilized +000D // payload length: 1 byte for index, 3*4 for values +05 // motor index: 0x05 for second pantograph, rotation motor +FFFFFFFF // P value +FFFFFFFF // I value +FFFFFFFF // D value +``` + + +### 0x92 Speed + +This message contains the pantograph index encoded as an 8 bit unsigned integer, followed by the speed value encoded as 32 bit float. + +Example message for setting the speed on both handles: +``` +4450 // magic number +92 // message type: PID values +00 // packet ID: not utilized +0005 // payload length: 1 byte for index, 4 for speed value +FF // pantograph index - both handles +FFFFFFFF // speed +``` + +### 0x93 Transition ended + +DOCUMENTATION MISSING + +### 0xA0 Create obstacle + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, the obstacle ID, encoded as a 16 bit unsigned integer, and multiple 2D vectors, each encoded as a pair of 32 bit floats. + +Setting the pantograph index to 0xFF creates the obstacle for both handles. + +Example message for adding an obstacle to both handles: +``` +4450 // magic number +A0 // message type: Create obstacle +01 // packet ID +0013 // payload length: 1 byte for index, 2 for ID, 4*4 for values +FF // pantograph index - both handles +0023 // obstacle ID +FFFFFFFF // first vector, x +FFFFFFFF // first vector, y +FFFFFFFF // second vector, x +FFFFFFFF // second vector, y +``` + +### 0xA1 Add to obstacle + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, the obstacle ID, encoded as a 16 bit unsigned integer, and multiple 2D vectors, each encoded as a pair of 32 bit floats. + +Setting the pantograph index to 0xFF adds the positions to the obstacle for both handles. + +Example message for adding positions to an obstacle for both handles: +``` +4450 // magic number +A1 // message type: Add to obstacle +01 // packet ID +0013 // payload length: 1 byte for index, 2 for ID, 4*4 for values +FF // pantograph index - both handles +0023 // obstacle ID +FFFFFFFF // first additional vector, x +FFFFFFFF // first additional vector, y +FFFFFFFF // second additional vector, x +FFFFFFFF // second additional vector, y +``` + +### 0xA2 Remove obstacle + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, and the obstacle ID, encoded as a 16 bit unsigned integer. + +Setting the pantograph index to 0xFF removes the obstacle for both handles. + +Example message for removing an obstacle from both handles: +``` +4450 // magic number +A2 // message type: Remove obstacle +01 // packet ID +0003 // payload length: 1 byte for index, 2 for ID +FF // pantograph index - both handles +0023 // obstacle ID +``` + +### 0xA3 Enable obstacle + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, and the obstacle ID, encoded as a 16 bit unsigned integer. + +Setting the pantograph index to 0xFF enables the obstacle for both handles. + +Example message for enabling an obstacle for both handles: +``` +4450 // magic number +A3 // message type: Enable obstacle +01 // packet ID +0003 // payload length: 1 byte for index, 2 for ID +FF // pantograph index - both handles +0023 // obstacle ID +``` + +### 0xA4 Disable obstacle + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, and the obstacle ID, encoded as a 16 bit unsigned integer. + +Setting the pantograph index to 0xFF disables the obstacle for both handles. + +Example message for disables an obstacle for both handles: +``` +4450 // magic number +A4 // message type: Disable obstacle +01 // packet ID +0003 // payload length: 1 byte for index, 2 for ID +FF // pantograph index - both handles +0023 // obstacle ID +``` + +### 0xA5 Calibrate panto + +Example message for sending calibration request: +``` +4450 // magic number +A5 // message type: Calibrate panto +00 // packet ID: not utilized +0000 // payload length: 0 +``` + +### 0xA6 Create passable obstacle + +Works the same way as 'create obstacle' just that the obstacle will be overcomeable + +### 0xA7 Create rail + +Add a haptic rail (guide) + +This message contains the pantograph index, encoded as an 8 bit unsigned integer, the obstacle ID, encoded as a 16 bit unsigned integer, and 2 2D vectors, both encoded as a pair of 32 bit floats. + +Setting the pantograph index to 0xFF creates the obstacle for both handles. + +Example message for adding an obstacle to both handles: +``` +4450 // magic number +A7 // message type: Create rail +01 // packet ID +0013 // payload length: 1 byte for index, 2 for ID, 4*4 for values +FF // pantograph index - both handles +0023 // obstacle ID +FFFFFFFF // first vector, x +FFFFFFFF // first vector, y +FFFFFFFF // second vector, x +FFFFFFFF // second vector, y +``` + +### 0xA8 Freeze + +DOCUMENTATION MISSING + +### 0xA9 Free + +DOCUMENTATION MISSING + +### 0xAA Set speed control + +Enable / disable the speed control (tethering). + +Details about the speed control configuration can be found here: https://www.dropbox.com/scl/fi/uljoe140fet2b53bjhr4y/DualPanto-Speed-Control.pptx?dl=0&rlkey=6k77wrfnb3oaxg186489tpinj + +Example message for setting the global speed control: +``` +4450 // magic number +AA // message type: set speed contol +00 // packet ID: not utilized +0015 // payload length: 1 byte for enable/disable, 4 for tether factor, 2x4 for the tether radius, 1 for the tether strategy when the handle moves out of the outer tether radius, 1 for pock enabled/disabled +01 // tether enabled +0000000000000000 // tether factor (value between 0 and 1; examples: 0.005, 0.01, 0.05) +0000000000000000 // inner radius (value between 0 and 3) +0000000000000000 // outer radius (value between 0 and 10) +01 // tether strategy (one out of 0,1,2) +01 // "pock" enabled +``` + + +### 0xC0 Dump hashtable + +This message contains the pantograph index, encoded as an 8 bit unsigned integer. + +Setting the pantograph index to 0xFF dumps the hashtable for both handles. + +Example message for dumping the hashtable for both handles: +``` +4450 // magic number +C0 // message type: Dump hashtable +00 // packet ID: not utilized +0001 // payload length: 1 byte for index +FF // pantograph index - both handles +``` + diff --git a/firmware/03 panto firmware/firmware/.gitignore b/firmware/03 panto firmware/firmware/.gitignore new file mode 100644 index 0000000..a1efe9f --- /dev/null +++ b/firmware/03 panto firmware/firmware/.gitignore @@ -0,0 +1,6 @@ +config.hpp +config.cpp +.vscode +.pio +.pioenvs +.piolibdeps diff --git a/firmware/03 panto firmware/firmware/include/hardware/angleAccessor.hpp b/firmware/03 panto firmware/firmware/include/hardware/angleAccessor.hpp new file mode 100644 index 0000000..fc7e636 --- /dev/null +++ b/firmware/03 panto firmware/firmware/include/hardware/angleAccessor.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function AngleAccessor; diff --git a/firmware/03 panto firmware/firmware/include/hardware/panto.hpp b/firmware/03 panto firmware/firmware/include/hardware/panto.hpp new file mode 100644 index 0000000..143d308 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/hardware/spiCommands.hpp b/firmware/03 panto firmware/firmware/include/hardware/spiCommands.hpp new file mode 100644 index 0000000..4f80dbe --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/hardware/spiEncoder.hpp b/firmware/03 panto firmware/firmware/include/hardware/spiEncoder.hpp new file mode 100644 index 0000000..21e9575 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/hardware/spiEncoderChain.hpp b/firmware/03 panto firmware/firmware/include/hardware/spiEncoderChain.hpp new file mode 100644 index 0000000..0b5bb86 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/hardware/spiPacket.hpp b/firmware/03 panto firmware/firmware/include/hardware/spiPacket.hpp new file mode 100644 index 0000000..73750d6 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/ioMain.hpp b/firmware/03 panto firmware/firmware/include/ioMain.hpp new file mode 100644 index 0000000..1641622 --- /dev/null +++ b/firmware/03 panto firmware/firmware/include/ioMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void ioSetup(); +void ioLoop(); diff --git a/firmware/03 panto firmware/firmware/include/physics/annotatedEdge.hpp b/firmware/03 panto firmware/firmware/include/physics/annotatedEdge.hpp new file mode 100644 index 0000000..d2c8370 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/collider.hpp b/firmware/03 panto firmware/firmware/include/physics/collider.hpp new file mode 100644 index 0000000..5d78845 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/edge.hpp b/firmware/03 panto firmware/firmware/include/physics/edge.hpp new file mode 100644 index 0000000..35b00ff --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/godObject.hpp b/firmware/03 panto firmware/firmware/include/physics/godObject.hpp new file mode 100644 index 0000000..5c4e091 --- /dev/null +++ b/firmware/03 panto firmware/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 = true; + double m_tetherFactor = 0.01; + Vector2D m_lastErrorTether; + double m_tetherInnerRadius = 1; + double m_tetherOuterRadius = 3; + 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 = true; + + 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/03 panto firmware/firmware/include/physics/godObjectAction.hpp b/firmware/03 panto firmware/firmware/include/physics/godObjectAction.hpp new file mode 100644 index 0000000..97c9c5d --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/godObjectActionData.hpp b/firmware/03 panto firmware/firmware/include/physics/godObjectActionData.hpp new file mode 100644 index 0000000..1a23cac --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/godObjectActionType.hpp b/firmware/03 panto firmware/firmware/include/physics/godObjectActionType.hpp new file mode 100644 index 0000000..9b31a78 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/hashtable.hpp b/firmware/03 panto firmware/firmware/include/physics/hashtable.hpp new file mode 100644 index 0000000..2a272d0 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/indexedEdge.hpp b/firmware/03 panto firmware/firmware/include/physics/indexedEdge.hpp new file mode 100644 index 0000000..4b20339 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/obstacle.hpp b/firmware/03 panto firmware/firmware/include/physics/obstacle.hpp new file mode 100644 index 0000000..1f67687 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/pantoPhysics.hpp b/firmware/03 panto firmware/firmware/include/physics/pantoPhysics.hpp new file mode 100644 index 0000000..8b5bded --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physics/rail.hpp b/firmware/03 panto firmware/firmware/include/physics/rail.hpp new file mode 100644 index 0000000..00f2631 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/physicsMain.hpp b/firmware/03 panto firmware/firmware/include/physicsMain.hpp new file mode 100644 index 0000000..e98e807 --- /dev/null +++ b/firmware/03 panto firmware/firmware/include/physicsMain.hpp @@ -0,0 +1,4 @@ +#pragma once + +void physicsSetup(); +void physicsLoop(); diff --git a/firmware/03 panto firmware/firmware/include/tasks/task.hpp b/firmware/03 panto firmware/firmware/include/tasks/task.hpp new file mode 100644 index 0000000..ec2acf9 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/tasks/taskFunction.hpp b/firmware/03 panto firmware/firmware/include/tasks/taskFunction.hpp new file mode 100644 index 0000000..e3fb3dc --- /dev/null +++ b/firmware/03 panto firmware/firmware/include/tasks/taskFunction.hpp @@ -0,0 +1,3 @@ +#pragma once + +typedef void (*TaskFunction)(); diff --git a/firmware/03 panto firmware/firmware/include/tasks/taskRegistry.hpp b/firmware/03 panto firmware/firmware/include/tasks/taskRegistry.hpp new file mode 100644 index 0000000..8bcf229 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/assert.hpp b/firmware/03 panto firmware/firmware/include/utils/assert.hpp new file mode 100644 index 0000000..70f3d82 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/crashDump.hpp b/firmware/03 panto firmware/firmware/include/utils/crashDump.hpp new file mode 100644 index 0000000..d526096 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/framerateLimiter.hpp b/firmware/03 panto firmware/firmware/include/utils/framerateLimiter.hpp new file mode 100644 index 0000000..94d43b2 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/performanceMonitor.hpp b/firmware/03 panto firmware/firmware/include/utils/performanceMonitor.hpp new file mode 100644 index 0000000..eff8a4f --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/receiveHandler.hpp b/firmware/03 panto firmware/firmware/include/utils/receiveHandler.hpp new file mode 100644 index 0000000..3b33431 --- /dev/null +++ b/firmware/03 panto firmware/firmware/include/utils/receiveHandler.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +typedef std::function ReceiveHandler; diff --git a/firmware/03 panto firmware/firmware/include/utils/receiveState.hpp b/firmware/03 panto firmware/firmware/include/utils/receiveState.hpp new file mode 100644 index 0000000..abb521f --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/serial.hpp b/firmware/03 panto firmware/firmware/include/utils/serial.hpp new file mode 100644 index 0000000..accc846 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/utils.hpp b/firmware/03 panto firmware/firmware/include/utils/utils.hpp new file mode 100644 index 0000000..1eaca9e --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/include/utils/vector.hpp b/firmware/03 panto firmware/firmware/include/utils/vector.hpp new file mode 100644 index 0000000..02bf456 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/platformio.ini b/firmware/03 panto firmware/firmware/platformio.ini new file mode 100644 index 0000000..1acb909 --- /dev/null +++ b/firmware/03 panto firmware/firmware/platformio.ini @@ -0,0 +1,36 @@ +[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 + #-DENABLE_CRASHDUMP + #-DENABLE_PERFMON + # use this while debugging + # -Og -ggdb +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 \ No newline at end of file diff --git a/firmware/03 panto firmware/firmware/src/hardware/panto.cpp b/firmware/03 panto firmware/firmware/src/hardware/panto.cpp new file mode 100644 index 0000000..063b680 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/hardware/spiCommands.cpp b/firmware/03 panto firmware/firmware/src/hardware/spiCommands.cpp new file mode 100644 index 0000000..b2093cd --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/hardware/spiEncoder.cpp b/firmware/03 panto firmware/firmware/src/hardware/spiEncoder.cpp new file mode 100644 index 0000000..6a73ea7 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/hardware/spiEncoderChain.cpp b/firmware/03 panto firmware/firmware/src/hardware/spiEncoderChain.cpp new file mode 100644 index 0000000..9278324 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/hardware/spiPacket.cpp b/firmware/03 panto firmware/firmware/src/hardware/spiPacket.cpp new file mode 100644 index 0000000..c4a5080 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/ioMain.cpp b/firmware/03 panto firmware/firmware/src/ioMain.cpp new file mode 100644 index 0000000..f5fc191 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/main.cpp b/firmware/03 panto firmware/firmware/src/main.cpp new file mode 100644 index 0000000..e007de8 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/annotatedEdge.cpp b/firmware/03 panto firmware/firmware/src/physics/annotatedEdge.cpp new file mode 100644 index 0000000..8072350 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/collider.cpp b/firmware/03 panto firmware/firmware/src/physics/collider.cpp new file mode 100644 index 0000000..ae51266 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/godObject.cpp b/firmware/03 panto firmware/firmware/src/physics/godObject.cpp new file mode 100644 index 0000000..9e72077 --- /dev/null +++ b/firmware/03 panto firmware/firmware/src/physics/godObject.cpp @@ -0,0 +1,474 @@ +#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_FREE_VAL, 0}, 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; + } + + //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; +} \ No newline at end of file diff --git a/firmware/03 panto firmware/firmware/src/physics/hashtable.cpp b/firmware/03 panto firmware/firmware/src/physics/hashtable.cpp new file mode 100644 index 0000000..0a2e4bc --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/indexedEdge.cpp b/firmware/03 panto firmware/firmware/src/physics/indexedEdge.cpp new file mode 100644 index 0000000..c1f5f09 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/obstacle.cpp b/firmware/03 panto firmware/firmware/src/physics/obstacle.cpp new file mode 100644 index 0000000..9ef21a4 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/pantoPhysics.cpp b/firmware/03 panto firmware/firmware/src/physics/pantoPhysics.cpp new file mode 100644 index 0000000..2f27f4d --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physics/rail.cpp b/firmware/03 panto firmware/firmware/src/physics/rail.cpp new file mode 100644 index 0000000..09613d3 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/physicsMain.cpp b/firmware/03 panto firmware/firmware/src/physicsMain.cpp new file mode 100644 index 0000000..2eedae2 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/tasks/task.cpp b/firmware/03 panto firmware/firmware/src/tasks/task.cpp new file mode 100644 index 0000000..36e28ad --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/tasks/taskRegistry.cpp b/firmware/03 panto firmware/firmware/src/tasks/taskRegistry.cpp new file mode 100644 index 0000000..4414b0a --- /dev/null +++ b/firmware/03 panto firmware/firmware/src/tasks/taskRegistry.cpp @@ -0,0 +1,3 @@ +#include "tasks/taskRegistry.hpp" + +TaskRegistry Tasks; diff --git a/firmware/03 panto firmware/firmware/src/utils/crashDump.cpp b/firmware/03 panto firmware/firmware/src/utils/crashDump.cpp new file mode 100644 index 0000000..26ab153 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/utils/framerateLimiter.cpp b/firmware/03 panto firmware/firmware/src/utils/framerateLimiter.cpp new file mode 100644 index 0000000..050c2ee --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/utils/performanceMonitor.cpp b/firmware/03 panto firmware/firmware/src/utils/performanceMonitor.cpp new file mode 100644 index 0000000..050e401 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/firmware/src/utils/serial.cpp b/firmware/03 panto firmware/firmware/src/utils/serial.cpp new file mode 100644 index 0000000..6386c4f --- /dev/null +++ b/firmware/03 panto firmware/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_FREE_VAL, 0}; +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/03 panto firmware/firmware/src/utils/vector.cpp b/firmware/03 panto firmware/firmware/src/utils/vector.cpp new file mode 100644 index 0000000..34826d6 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/hardware/BP1.json b/firmware/03 panto firmware/hardware/BP1.json new file mode 100644 index 0000000..ee0e0ac --- /dev/null +++ b/firmware/03 panto firmware/hardware/BP1.json @@ -0,0 +1,128 @@ +{ + "firmware": "Arduino", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 0, + "pantos": { + "upper": { + "left": { + "linkage": { + "baseX": -24, + "baseY": 0, + "innerLength": 98, + "outerLength": 128 + }, + "motor": { + "pwmPin": 9, + "dirAPin": 26, + "flipped": true, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 29, + "bPin": 28, + "steps": 5540, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "baseX": 24, + "baseY": 0, + "innerLength": 98, + "outerLength": 128 + }, + "motor": { + "pwmPin": 8, + "dirAPin": 22, + "flipped": true, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 25, + "bPin": 24, + "steps": 5540, + "setup": 0.0 + } + }, + "handle": { + "motor": { + "pwmPin": 5, + "dirAPin": 38, + "flipped": true, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 41, + "bPin": 40, + "steps": 512, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "baseX": -24, + "baseY": 0, + "innerLength": 98, + "outerLength": 128 + }, + "motor": { + "pwmPin": 6, + "dirAPin": 34, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 36, + "bPin": 37, + "steps": 5540, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "baseX": 24, + "baseY": 0, + "innerLength": 98, + "outerLength": 128 + }, + "motor": { + "pwmPin": 7, + "dirAPin": 30, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 32, + "bPin": 33, + "steps": 5540, + "setup": 0.0 + } + }, + "handle": { + "motor": { + "pwmPin": 4, + "dirAPin": 42, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 45, + "bPin": 44, + "steps": 15360, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/LP1.json b/firmware/03 panto firmware/hardware/LP1.json new file mode 100644 index 0000000..c72d799 --- /dev/null +++ b/firmware/03 panto firmware/hardware/LP1.json @@ -0,0 +1,116 @@ +{ + "firmware": "Arduino", + "baseDist": 43, + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "powerLimit": 0.2, + "pidFactor": [5.0, 0.0, 0.0], + "dummyPin": 0, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 72, + "outerLength": 86 + }, + "motor": { + "pwmPin": 8, + "dirAPin": 10, + "dirBPin": 9, + "flipped": false + }, + "encoder": { + "aPin": 50, + "bPin": 52, + "steps": 15360, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "innerLength": 57, + "outerLength": 86 + }, + "motor": { + "pwmPin": 13, + "dirAPin": 11, + "dirBPin": 12, + "flipped": false + }, + "encoder": { + "aPin": 22, + "bPin": 24, + "steps": 15360, + "setup": 0.0 + } + }, + "handle": { + "motor": { + "pwmPin": 3, + "dirAPin": 36, + "dirBPin": 34, + "flipped": true + }, + "encoder": { + "aPin": 30, + "bPin": 32, + "steps": 15360, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 57, + "outerLength": 86 + }, + "motor": { + "pwmPin": 2, + "dirAPin": 1, + "dirBPin": 0, + "flipped": false + }, + "encoder": { + "aPin": 48, + "bPin": 46, + "steps": 15360, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "innerLength": 72, + "outerLength": 86 + }, + "motor": { + "pwmPin": 7, + "dirAPin": 23, + "dirBPin": 25, + "flipped": false + }, + "encoder": { + "aPin": 28, + "bPin": 26, + "steps": 15360, + "setup": 0.0 + } + }, + "handle": { + "motor": { + "pwmPin": 4, + "dirAPin": 38, + "dirBPin": 40, + "flipped": false + }, + "encoder": { + "aPin": 44, + "bPin": 42, + "steps": 15360, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/LP_PCB.json b/firmware/03 panto firmware/hardware/LP_PCB.json new file mode 100644 index 0000000..1c922c3 --- /dev/null +++ b/firmware/03 panto firmware/hardware/LP_PCB.json @@ -0,0 +1,141 @@ +{ + "firmware": "Arduino", + "baseDist": 21.5, + "opMinDist": 15, + "opMaxDist": 120, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 0, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 66, + "outerLength": 79, + "baseX": -10, + "baseY": 0 + }, + "motor": { + "pwmPin": 2, + "dirAPin": 25, + "dirBPin": 23, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 48, + "bPin": 46, + "steps": 15360, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "innerLength": 49, + "outerLength": 79, + "baseX": 30, + "baseY": 0 + }, + "motor": { + "pwmPin": 8, + "dirAPin": 10, + "dirBPin": 9, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 52, + "bPin": 50, + "steps": 15360, + "setup": -0.05 + } + }, + "handle": { + "linkage": { + "handleMount": 1 + }, + "motor": { + "pwmPin": 3, + "dirAPin": 34, + "dirBPin": 36, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.6, 0.0, 0.01] + }, + "encoder": { + "aPin": 32, + "bPin": 30, + "steps": 136, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 49, + "outerLength": 79, + "baseX": -30, + "baseY": 0 + }, + "motor": { + "pwmPin": 7, + "dirAPin": 5, + "dirBPin": 6, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 26, + "bPin": 28, + "steps": 15360, + "setup": -0.45 + } + }, + "right": { + "linkage": { + "innerLength": 66, + "outerLength": 79, + "baseX": 10, + "baseY": 0 + }, + "motor": { + "pwmPin": 13, + "dirAPin": 11, + "dirBPin": 12, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 22, + "bPin": 24, + "steps": 15360, + "setup": 0.0 + } + }, + "handle": { + "linkage": { + "handleMount": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 38, + "dirBPin": 40, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.6, 0.0, 0.01] + }, + "encoder": { + "aPin": 42, + "bPin": 44, + "steps": 600, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/LP_PCB_ESP32.json b/firmware/03 panto firmware/hardware/LP_PCB_ESP32.json new file mode 100644 index 0000000..3950a9b --- /dev/null +++ b/firmware/03 panto firmware/hardware/LP_PCB_ESP32.json @@ -0,0 +1,132 @@ +{ + "firmware": "ESP32", + "baseDist": 21.5, + "opMinDist": 15, + "opMaxDist": 120, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "forcePidFactor": [0.375, 0, 0.01], + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 66, + "outerLength": 79, + "baseX": -10, + "baseY": 0 + }, + "motor": { + "pwmPin": 15, + "dirAPin": 2, + "dirBPin": 0, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 4, + "bPin": 16, + "steps": 15360, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "innerLength": 49, + "outerLength": 79, + "baseX": 30, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 5, + "dirBPin": 18, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 19, + "bPin": 21, + "steps": 15360, + "setup": -0.05 + } + }, + "handle": { + "linkage": { + "handleMount": 1 + }, + "motor": { + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.6, 0.0, 0.01] + }, + "encoder": { + "steps": 136, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 49, + "outerLength": 79, + "baseX": -30, + "baseY": 0 + }, + "motor": { + "pwmPin": 22, + "dirAPin": 23, + "dirBPin": 13, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 12, + "bPin": 14, + "steps": 15360, + "setup": -0.45 + } + }, + "right": { + "linkage": { + "innerLength": 66, + "outerLength": 79, + "baseX": 10, + "baseY": 0 + }, + "motor": { + "pwmPin": 27, + "dirAPin": 26, + "dirBPin": 25, + "flipped": false, + "powerLimit": 0.4, + "pidFactor": [1.0, 0.0, 0.01] + }, + "encoder": { + "aPin": 33, + "bPin": 32, + "steps": 15360, + "setup": 0.0 + } + }, + "handle": { + "linkage": { + "handleMount": 0 + }, + "motor": { + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.6, 0.0, 0.01] + }, + "encoder": { + "steps": 600, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/angie.json b/firmware/03 panto firmware/hardware/angie.json new file mode 100644 index 0000000..847ba6e --- /dev/null +++ b/firmware/03 panto firmware/hardware/angie.json @@ -0,0 +1,122 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "usesSpi": true, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -45, + "baseY": 0 + }, + "motor": { + "pwmPin": 21, + "dirAPin": 22, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "steps": 16384, + "setup": -0.44, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 45, + "baseY": 0 + }, + "motor": { + "pwmPin": 18, + "dirAPin": 19, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "steps": 16384, + "setup": -0.06, + "spiIndex": 2 + } + }, + "handle": { + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "dirBPin": 33, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.3, 0.0, 30.0] + }, + "encoder": { + "steps": 500, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -45, + "baseY": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 16, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": 0.44, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 45, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 5, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": 0.06, + "spiIndex": 1 + } + }, + "handle": { + "motor": { + "flipped": false, + "powerLimit": 0.7, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "steps": 13820, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/barbie.json b/firmware/03 panto firmware/hardware/barbie.json new file mode 100644 index 0000000..78cbab2 --- /dev/null +++ b/firmware/03 panto firmware/hardware/barbie.json @@ -0,0 +1,138 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "usesSpi": true, + "forcePidFactor": [0.375, 0, 0.8], + "hashtableMemory": 200000, + "obstacleChangesPerFrame": 1, + "range": { + "minX": -180, + "minY": -205, + "maxX": 180, + "maxY": 5 + }, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -19.5, + "baseY": 0 + }, + "motor": { + "pwmPin":21, + "dirAPin": 22, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "setup": 0.44, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 18, + "dirAPin": 19, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "setup": 0.06, + "spiIndex": 2 + } + }, + "handle": { + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "dirBPin": 33, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.3, 0.0, 30.0] + }, + "encoder": { + "aPin": 35, + "bPin": 34, + "steps": 512, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 16, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.44, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 5, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.06, + "spiIndex": 1 + } + }, + "handle": { + "motor": { + "pwmPin": 23, + "dirAPin": 26, + "dirBPin": 27, + "flipped": false, + "powerLimit": 0.7, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 15360, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/chantalle.json b/firmware/03 panto firmware/hardware/chantalle.json new file mode 100644 index 0000000..4ba290a --- /dev/null +++ b/firmware/03 panto firmware/hardware/chantalle.json @@ -0,0 +1,140 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "usesSpi": true, + "forcePidFactor": [0.375, 0, 0.8], + "hashtableMemory": 200000, + "obstacleChangesPerFrame": 1, + "range": { + "minX": -175, + "minY": -205, + "maxX": 175, + "maxY": 5 + }, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -19.5, + "baseY": 0 + }, + "motor": { + "pwmPin":21, + "dirAPin": 22, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.44, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 18, + "dirAPin": 19, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.06, + "spiIndex": 2 + } + }, + "handle": { + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "dirBPin": 33, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.3, 0.0, 30.0] + }, + "encoder": { + "aPin": 35, + "bPin": 34, + "steps": 512, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": -19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 16, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.44, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 80, + "outerLength": 120, + "baseX": 19.5, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 5, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.06, + "spiIndex": 1 + } + }, + "handle": { + "motor": { + "pwmPin": 23, + "dirAPin": 26, + "dirBPin": 27, + "flipped": false, + "powerLimit": 0.7, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 15360, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/doerte.json b/firmware/03 panto firmware/hardware/doerte.json new file mode 100644 index 0000000..a9785a2 --- /dev/null +++ b/firmware/03 panto firmware/hardware/doerte.json @@ -0,0 +1,140 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "usesSpi": true, + "forcePidFactor": [0.375, 0, 0], + "hashtableMemory": 100000, + "obstacleChangesPerFrame": 1, + "range": { + "minX": -175, + "minY": -205, + "maxX": 175, + "maxY": 5 + }, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 84.5, + "outerLength": 114, + "baseX": -22, + "baseY": 0 + }, + "motor": { + "pwmPin":21, + "dirAPin": 22, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.4806, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 84.5, + "outerLength": 114, + "baseX": 22, + "baseY": 0 + }, + "motor": { + "pwmPin": 18, + "dirAPin": 19, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.0194, + "spiIndex": 2 + } + }, + "handle": { + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "dirBPin": 33, + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.3, 0.0, 30.0] + }, + "encoder": { + "aPin": 35, + "bPin": 34, + "steps": 512, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 84.5, + "outerLength": 114, + "baseX": -22, + "baseY": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 16, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.4806, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 84.5, + "outerLength": 114, + "baseX": 22, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 5, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [7, 0.0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.0194, + "spiIndex": 1 + } + }, + "handle": { + "motor": { + "pwmPin": 23, + "dirAPin": 26, + "dirBPin": 27, + "flipped": false, + "powerLimit": 0.7, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 15360, + "setup": 0.0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/ember.json b/firmware/03 panto firmware/hardware/ember.json new file mode 100644 index 0000000..62cc4bb --- /dev/null +++ b/firmware/03 panto firmware/hardware/ember.json @@ -0,0 +1,152 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.0085, + "dummyPin": 40, + "usesSpi": true, + "forcePidFactor": [0.375, 0, 0], + "hashtableMemory": 200000, + "obstacleChangesPerFrame": 1, + "range": { + "minX": -180, + "minY": -205, + "maxX": 180, + "maxY": 5 + }, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 95.07, + "outerLength": 119.79, + "baseX": -23.7, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 16, + "pwmPinBackwards": 4, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": false, + "setup": -0.5, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 57.71, + "outerLength": 119.79, + "baseX": 61.1, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 2, + "pwmPinBackwards": 17, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": false, + "setup": 0.001, + "spiIndex": 2 + } + }, + "handle": { + "linkage": { + "handleMount": 1 + }, + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "flipped": true, + "powerLimit": 0.2, + "powerLimitForce": 0.2, + "pidFactor": [0.5, 0, 30] + }, + "encoder": { + "aPin": 35, + "bPin": 34, + "steps": 136, + "flipped": true, + "setup": 0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 57.71, + "outerLength": 119.79, + "baseX": -61.1, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 22, + "pwmPinBackwards": 21, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.5, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 95.07, + "outerLength": 119.79, + "baseX": 23.7, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 19, + "pwmPinBackwards": 18, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": 0.001, + "spiIndex": 1 + } + }, + "handle": { + "linkage": { + "handleMount": 0 + }, + "motor": { + "pwmPin": 23, + "dirAPin": 26, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.2, + "pidFactor": [0.5, 0, 30] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 136, + "flipped": false, + "setup": 0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/fiona.json b/firmware/03 panto firmware/hardware/fiona.json new file mode 100644 index 0000000..ffda9f6 --- /dev/null +++ b/firmware/03 panto firmware/hardware/fiona.json @@ -0,0 +1,152 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.0085, + "dummyPin": 40, + "usesSpi": true, + "forcePidFactor": [0.375, 0, 0], + "hashtableMemory": 200000, + "obstacleChangesPerFrame": 1, + "range": { + "minX": -180, + "minY": -205, + "maxX": 180, + "maxY": 5 + }, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 95.07, + "outerLength": 119.79, + "baseX": -23.7, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 16, + "pwmPinBackwards": 4, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": false, + "setup": -0.5, + "spiIndex": 3 + } + }, + "right": { + "linkage": { + "innerLength": 57.71, + "outerLength": 119.79, + "baseX": 61.1, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 2, + "pwmPinBackwards": 17, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": false, + "setup": 0.001, + "spiIndex": 2 + } + }, + "handle": { + "linkage": { + "handleMount": 1 + }, + "motor": { + "pwmPin": 25, + "dirAPin": 32, + "flipped": true, + "powerLimit": 0.2, + "powerLimitForce": 0.2, + "pidFactor": [0.5, 0, 30] + }, + "encoder": { + "aPin": 35, + "bPin": 34, + "steps": 260, + "flipped": false, + "setup": 0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 57.71, + "outerLength": 119.79, + "baseX": -61.1, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 22, + "pwmPinBackwards": 21, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": -0.5, + "spiIndex": 0 + } + }, + "right": { + "linkage": { + "innerLength": 95.07, + "outerLength": 119.79, + "baseX": 23.7, + "baseY": 0 + }, + "motor": { + "pwmPinForwards": 19, + "pwmPinBackwards": 18, + "flipped": false, + "powerLimit": 0.2, + "powerLimitForce": 0.6, + "pidFactor": [6, 0, 600] + }, + "encoder": { + "steps": 16384, + "flipped": true, + "setup": 0.001, + "spiIndex": 1 + } + }, + "handle": { + "linkage": { + "handleMount": 0 + }, + "motor": { + "pwmPin": 23, + "dirAPin": 26, + "flipped": true, + "powerLimit": 0.2, + "powerLimitForce": 0.2, + "pidFactor": [0.5, 0, 30] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 260, + "flipped": true, + "setup": 0 + } + } + } + } +} diff --git a/firmware/03 panto firmware/hardware/mab_opt2k4_dir_esp32.json b/firmware/03 panto firmware/hardware/mab_opt2k4_dir_esp32.json new file mode 100644 index 0000000..5f37f51 --- /dev/null +++ b/firmware/03 panto firmware/hardware/mab_opt2k4_dir_esp32.json @@ -0,0 +1,127 @@ +{ + "firmware": "ESP32", + "opMinDist": 65, + "opMaxDist": 210, + "opAngle": 2.2, + "forceFactor": 0.01, + "dummyPin": 40, + "pantos": { + "upper": { + "left": { + "linkage": { + "innerLength": 100, + "outerLength": 125, + "baseX": -51, + "baseY": 0 + }, + "motor": { + "pwmPin": 4, + "dirAPin": 5, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "aPin": 12, + "bPin": 13, + "steps": 2400, + "setup": -0.375 + } + }, + "right": { + "linkage": { + "innerLength": 100, + "outerLength": 125, + "baseX": 51, + "baseY": 0 + }, + "motor": { + "pwmPin": 17, + "dirAPin": 16, + "flipped": true, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "aPin": 14, + "bPin": 15, + "steps": 2400, + "setup": -0.125 + } + }, + "handle": { + "motor": { + "flipped": false, + "powerLimit": 0.2, + "pidFactor": [0.3, 0.0, 30.0] + }, + "encoder": { + "aPin": 34, + "bPin": 35, + "steps": 500, + "setup": 0.0 + } + } + }, + "lower": { + "left": { + "linkage": { + "innerLength": 98, + "outerLength": 128, + "baseX": -24, + "baseY": 0 + }, + "motor": { + "pwmPin": 18, + "dirAPin": 19, + "flipped": false, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "aPin": 32, + "bPin": 27, + "steps": 20000, + "setup": -0.5 + } + }, + "right": { + "linkage": { + "innerLength": 98, + "outerLength": 128, + "baseX": 24, + "baseY": 0 + }, + "motor": { + "pwmPin": 21, + "dirAPin": 22, + "flipped": false, + "powerLimit": 1.0, + "pidFactor": [0.99, 0.0, 6.5] + }, + "encoder": { + "aPin": 2, + "bPin": 33, + "steps": 20000, + "setup": 0.0 + } + }, + "handle": { + "motor": { + "pwmPin": 23, + "dirAPin": 25, + "dirBPin": 26, + "flipped": false, + "powerLimit": 0.7, + "pidFactor": [5.0, 0.0, 0.0] + }, + "encoder": { + "aPin": 36, + "bPin": 39, + "steps": 13820, + "setup": 0.0 + } + } + } + } +} \ No newline at end of file diff --git a/firmware/03 panto firmware/package.json b/firmware/03 panto firmware/package.json new file mode 100644 index 0000000..71ac162 --- /dev/null +++ b/firmware/03 panto firmware/package.json @@ -0,0 +1,111 @@ +{ + "name": "dualpantoframework", + "version": "0.3.0", + "description": "dev : Oliver Schneider, Jotaro Shigeyama, Alexander Meißner, Nico Böckhoff, Jonas Bounama, Lukas Wagner, Daniel-Amadeus Johannes Glöckner", + "main": "lib/dualpantoframework.js", + "scripts": { + "install": "npm run build && npm run setup-commit-hook", + "build": "npm run script build", + "clean": "npm run script clean", + "platformio": "npm run script platformio", + "config": "npm run script config", + "plotter": "npm run script plotter", + "docs": "npm run script docs", + "unity": "npm run script unity", + "script": "node ./utils/scripts/run.js", + "svgToJs": "node ./utils/svgConverter/svgToJs.js", + "setup-commit-hook": "if-env NODE_ENV=production && echo \" production mode \" || node ./utils/scripts/createJsCommitHook.js" + }, + "repository": { + "type": "git", + "url": "https://github.com/HassoPlattnerInstituteHCI/dualpantoframework.git" + }, + "author": "jotaro shigeyama", + "license": "ISC", + "gypfile": true, + "bugs": { + "url": "https://github.com/HassoPlattnerInstituteHCI/dualpantoframework/issues" + }, + "homepage": "https://github.com/HassoPlattnerInstituteHCI/dualpantoframework#readme", + "dependencies": { + "@tweenjs/tween.js": "^17.2.0", + "co": "^4.6.0", + "http-server": "^0.12.1", + "if-env": "^1.0.4", + "keypress": "^0.2.1", + "node-gyp": "^3.6.2", + "node-pid-controller": "^1.0.1", + "open": "^6.3.0", + "play-sound": "^1.1.2", + "polygon-offset": "^0.3.1", + "say": "^0.16.0", + "serialport": "^9.0.7", + "usb": "^1.6.0", + "websocket": "^1.0.26", + "xml2js": "^0.4.19" + }, + "devDependencies": { + "documentation": "^11.0.1", + "eslint": "^5.15.1", + "eslint-config-google": "^0.12.0", + "eslint-plugin-jsdoc": "^7.0.2" + }, + "eslintConfig": { + "env": { + "browser": true, + "node": true, + "es6": true + }, + "plugins": [ + "jsdoc" + ], + "extends": "./node_modules/eslint-config-google/index.js", + "rules": { + "comma-dangle": [ + "error", + "never" + ], + "linebreak-style": [ + "off", + "unix" + ], + "valid-jsdoc": [ + "off" + ], + "jsdoc/check-alignment": "error", + "jsdoc/check-indentation": "error", + "jsdoc/check-param-names": "error", + "jsdoc/check-syntax": "error", + "jsdoc/check-tag-names": "error", + "jsdoc/check-types": "error", + "jsdoc/implements-on-classes": "error", + "jsdoc/match-description": "error", + "jsdoc/newline-after-description": "error", + "jsdoc/no-undefined-types": "error", + "jsdoc/require-description": [ + "error", + { + "contexts": "ClassDeclaration" + } + ], + "jsdoc/require-description-complete-sentence": "error", + "jsdoc/require-hyphen-before-param-description": "error", + "jsdoc/require-param": "error", + "jsdoc/require-param-description": "error", + "jsdoc/require-param-name": "error", + "jsdoc/require-param-type": "error", + "jsdoc/require-returns": "error", + "jsdoc/require-returns-check": "error", + "jsdoc/require-returns-description": "error", + "jsdoc/require-returns-type": "error", + "jsdoc/valid-types": "error" + }, + "settings": { + "jsdoc": { + "tagNamePreference": { + "returns": "return" + } + } + } + } +} diff --git a/firmware/03 panto firmware/readme.md b/firmware/03 panto firmware/readme.md new file mode 100644 index 0000000..8c96d57 --- /dev/null +++ b/firmware/03 panto firmware/readme.md @@ -0,0 +1,64 @@ +# Dualpanto Firmware +Welcome to the `dualpantoframework` repository. +This repository contains the JS framework to generate hardware config, firmware and some useful tools to debug and test the pantograph. + +## For BIS students +Currently this repo is mainly used for Firmware and Kinematics lecutre of Bulding Interactive System (BIS) class. Please read `Pre-Installation` and `Firmware Uploading` sections. If you have any question, please ask to your TA. + +# Pre-Installation + +## Install the ESP32 driver + +- [Download](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers?tab=downloads) the installer for your OS-Version. +- Run the installer. + +## Setup C++ environment + +- Install node (v12.22.1) and npm (v6.14.12) + +### macOS + - Go to Appstore and install Xcode + - Run `xcode-select –install` to install the compilers + - Run `sudo xcode-select -s /Applications/Xcode.app/Contents/Developer` + + ### windows + - Install Visual Studio 2019 or 2017 + - Select at least the workload “Desktopentwicklung mit C++” + + +## Install Platformio +Please follow the respective instruction. +- [VS code user](https://docs.platformio.org/en/latest/integration/ide/vscode.html) +- [CLion user](https://www.jetbrains.com/help/clion/platformio.html) + +# Firmware Uploading + +## Generate hardware config via npm +`npm run script config` + +This converts each version's hardware config file written in json into .hpp and .cpp file in firmware. + +## Upload Firmware +Please follow the respective instruction. +- [VS code user](https://docs.platformio.org/en/latest/integration/ide/vscode.html) +- [CLion user](https://www.jetbrains.com/help/clion/platformio.html) + +## Troubleshooting + +OSX: if you want to upload a new firmware version to a device you have to make sure the `upload_port` of platformio is set correctly in the firmware/platformio.ini file like this: +`upload_port = /dev/cu.SLAB_USBtoUART` + +# Development +[![CircleCI](https://circleci.com/gh/HassoPlattnerInstituteHCI/dualpantoframework.svg?style=svg&circle-token=32b766f8a9d2c9a0c612d215322a6dab4aec813d)](https://circleci.com/gh/HassoPlattnerInstituteHCI/dualpantoframework) + +version 0.3 + +dev : Jonas Bounama, Lukas Wagner, Daniel-Amadeus Johannes Glöckner, Julius Rudolph, Oliver Schneider, Jotaro Shigeyama, Alexander Meißner, Nico Böckhoff, **Shohei Katakura (active developer)** + +firstname.lastname@hpi.de + +## Documentation +- `Transmission Protocol`: The communication protocol between the framework and the device is specified [here](documentation/protocol/protocol.md). +- `setup`: Instuctions on how to set up the framework for different operating systems. + + diff --git a/firmware/03 panto firmware/utils/backtrace/backtrace.sh b/firmware/03 panto firmware/utils/backtrace/backtrace.sh new file mode 100644 index 0000000..4b75654 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/protocol/include/protocol/header.hpp b/firmware/03 panto firmware/utils/protocol/include/protocol/header.hpp new file mode 100644 index 0000000..f6b1d51 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/protocol/include/protocol/messageType.hpp b/firmware/03 panto firmware/utils/protocol/include/protocol/messageType.hpp new file mode 100644 index 0000000..0e9566e --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/protocol/include/protocol/protocol.hpp b/firmware/03 panto firmware/utils/protocol/include/protocol/protocol.hpp new file mode 100644 index 0000000..a2e41c3 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/protocol/src/protocol/messageType.cpp b/firmware/03 panto firmware/utils/protocol/src/protocol/messageType.cpp new file mode 100644 index 0000000..4e57c18 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/protocol/src/protocol/protocol.cpp b/firmware/03 panto firmware/utils/protocol/src/protocol/protocol.cpp new file mode 100644 index 0000000..6871f88 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/scripts/generateHardwareConfig.js b/firmware/03 panto firmware/utils/scripts/generateHardwareConfig.js new file mode 100644 index 0000000..563e783 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/scripts/run.js b/firmware/03 panto firmware/utils/scripts/run.js new file mode 100644 index 0000000..96fae21 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/scripts/tools.js b/firmware/03 panto firmware/utils/scripts/tools.js new file mode 100644 index 0000000..d125380 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/CMakeLists.txt b/firmware/03 panto firmware/utils/serial/CMakeLists.txt new file mode 100644 index 0000000..edd3b8a --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/binding.gyp b/firmware/03 panto firmware/utils/serial/binding.gyp new file mode 100644 index 0000000..17c9986 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/crashAnalyzer.hpp b/firmware/03 panto firmware/utils/serial/include/crashAnalyzer.hpp new file mode 100644 index 0000000..dea6f74 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/libInterface.hpp b/firmware/03 panto firmware/utils/serial/include/libInterface.hpp new file mode 100644 index 0000000..40b2846 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/node.hpp b/firmware/03 panto firmware/utils/serial/include/node.hpp new file mode 100644 index 0000000..5941e9b --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/packet.hpp b/firmware/03 panto firmware/utils/serial/include/packet.hpp new file mode 100644 index 0000000..f885582 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/serial.hpp b/firmware/03 panto firmware/utils/serial/include/serial.hpp new file mode 100644 index 0000000..6ff8e07 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/serial_export.hpp b/firmware/03 panto firmware/utils/serial/include/serial_export.hpp new file mode 100644 index 0000000..09738b0 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/include/standalone.hpp b/firmware/03 panto firmware/utils/serial/include/standalone.hpp new file mode 100644 index 0000000..4bf5992 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/cppLib/lib.cpp b/firmware/03 panto firmware/utils/serial/src/cppLib/lib.cpp new file mode 100644 index 0000000..446b894 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/crashAnalyzer/analyze.cpp b/firmware/03 panto firmware/utils/serial/src/crashAnalyzer/analyze.cpp new file mode 100644 index 0000000..9569fdb --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/crashAnalyzer/buffer.cpp b/firmware/03 panto firmware/utils/serial/src/crashAnalyzer/buffer.cpp new file mode 100644 index 0000000..8107795 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/main.cpp b/firmware/03 panto firmware/utils/serial/src/node/main.cpp new file mode 100644 index 0000000..fbbcadf --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/poll.cpp b/firmware/03 panto firmware/utils/serial/src/node/poll.cpp new file mode 100644 index 0000000..fd20886 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/receiveHelpers.cpp b/firmware/03 panto firmware/utils/serial/src/node/receiveHelpers.cpp new file mode 100644 index 0000000..2450200 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/send.cpp b/firmware/03 panto firmware/utils/serial/src/node/send.cpp new file mode 100644 index 0000000..90f854e --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/sendHelpers.cpp b/firmware/03 panto firmware/utils/serial/src/node/sendHelpers.cpp new file mode 100644 index 0000000..f1b062f --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/node/setup.cpp b/firmware/03 panto firmware/utils/serial/src/node/setup.cpp new file mode 100644 index 0000000..0c72735 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/serial/packet.cpp b/firmware/03 panto firmware/utils/serial/src/serial/packet.cpp new file mode 100644 index 0000000..380f276 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/serial/shared.cpp b/firmware/03 panto firmware/utils/serial/src/serial/shared.cpp new file mode 100644 index 0000000..30a72ac --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/serial/unix.cpp b/firmware/03 panto firmware/utils/serial/src/serial/unix.cpp new file mode 100644 index 0000000..eedd2c4 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/serial/win.cpp b/firmware/03 panto firmware/utils/serial/src/serial/win.cpp new file mode 100644 index 0000000..51adc05 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/standalone/main.cpp b/firmware/03 panto firmware/utils/serial/src/standalone/main.cpp new file mode 100644 index 0000000..9e4da35 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/src/standalone/standalone.cpp b/firmware/03 panto firmware/utils/serial/src/standalone/standalone.cpp new file mode 100644 index 0000000..d177bb4 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/unity/Serial.cs b/firmware/03 panto firmware/utils/serial/unity/Serial.cs new file mode 100644 index 0000000..4936765 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/unity/linux.sh b/firmware/03 panto firmware/utils/serial/unity/linux.sh new file mode 100755 index 0000000..8d673ed --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/unity/mac.sh b/firmware/03 panto firmware/utils/serial/unity/mac.sh new file mode 100755 index 0000000..33b47e9 --- /dev/null +++ b/firmware/03 panto firmware/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/03 panto firmware/utils/serial/unity/win.bat b/firmware/03 panto firmware/utils/serial/unity/win.bat new file mode 100755 index 0000000..d18e6aa --- /dev/null +++ b/firmware/03 panto firmware/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/requirements.txt b/requirements.txt new file mode 100644 index 0000000..c927485 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +pyserial +unittest +platformio \ No newline at end of file diff --git a/test_hardware.py b/test_hardware.py new file mode 100644 index 0000000..5c491d5 --- /dev/null +++ b/test_hardware.py @@ -0,0 +1,39 @@ +import time +import unittest + +import serial + +import util +import config + + +class HardwareTest(unittest.TestCase): + def test_shows_up_as_serial(self): + self.assertIn(config.COM_PORT, util.serial_ports()) + + def test_compile_firmware(self): + res = util.compile_firmware('firmware/01 hello world') + self.assertEqual(res, 0) + + def test_upload_firmware(self): + res = util.upload_firmware('./firmware/01 hello world') + self.assertEqual(res, 0) + + def test_upload_firmware_check_serial(self): + res = util.upload_firmware('./firmware/02 echo') + self.assertEqual(res, 0, msg='failed to upload firmware') + time.sleep(1) + with serial.Serial(config.COM_PORT, 9600, timeout=1) as ser: + message = b'x' + ser.write(message) + res = ser.read(1) + self.assertEqual(res, message) + + + def test_motor(self): + # TODO + ... + + def test_encoder(self): + # TODO + ... diff --git a/test_protocol.py b/test_protocol.py new file mode 100644 index 0000000..7454cec --- /dev/null +++ b/test_protocol.py @@ -0,0 +1,86 @@ +import logging +import time +import unittest +import util +import serial +import binascii + + +logging.basicConfig(level=logging.DEBUG) + +class ProtocolTest(unittest.TestCase): + con = None + + def setUp(self): + util.upload_firmware("firmware/03 panto firmware/firmware") + + com = "/dev/cu.usbserial-0001" + + self.con = serial.Serial(com, baudrate=115200, timeout=5) + + def test_timeout_10min(self): + timeout = 5 * 60 + start_time = time.time() + while start_time + timeout > time.time(): + header = self.con.read(size=2) + + msg_type = self.con.read(size=1) + + package_id = self.con.read(size=1) + + message_size = self.con.read(size=2) + + body = self.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") + self.con.write(bytes.fromhex("445080000000")) + elif msg_type == bytes.fromhex("01"): + logging.info("receive heartbeat") + self.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)) + self.assertTrue(False) + print("done") + + def tearDown(self): + self.con.close() + diff --git a/util.py b/util.py new file mode 100644 index 0000000..b669b52 --- /dev/null +++ b/util.py @@ -0,0 +1,48 @@ +import subprocess + +import sys +import glob +import serial + +""" Lists serial port names + + :raises EnvironmentError: + On unsupported or unknown platforms + :returns: + A list of the serial ports available on the system +""" + + +def serial_ports(): + if sys.platform.startswith('win'): + ports = ['COM%s' % (i + 1) for i in range(256)] + elif sys.platform.startswith('linux') or sys.platform.startswith('cygwin'): + # this excludes your current terminal "/dev/tty" + ports = glob.glob('/dev/tty[A-Za-z]*') + elif sys.platform.startswith('darwin'): + ports = glob.glob('/dev/tty.*') + else: + raise EnvironmentError('Unsupported platform') + + result = [] + for port in ports: + try: + s = serial.Serial(port) + s.close() + result.append(port) + except (OSError, serial.SerialException): + pass + return result + + +def compile_firmware(firmware_directory="./firmware/00 template"): + res = subprocess.call('pio run', cwd=firmware_directory, shell=True) + res += subprocess.call('pio run --target clean', cwd=firmware_directory, shell=True) + return res + + +def upload_firmware(firmware_directory="./firmware/00 template"): + res = subprocess.call('pio run', cwd=firmware_directory, shell=True) + res += subprocess.call('pio run --target upload', cwd=firmware_directory, shell=True) + res += subprocess.call('pio run --target clean', cwd=firmware_directory, shell=True) + return res