Feature/uwb firmware (#48)

* uwb firmware updates

- added lunabot_embedded readme

* Added UWB code to teensy
    - Added new class in interfaces
            - Followed pattern of encoder class
            - Sourced from uwb_sensor_test.ino
    - Used class in robot.cpp in new namespace
            - Followed pattern of other namespaces
            - No read command; unecessary
    - Added new interrupt timer in teensy_main
            - To run code from interfaces

* added compilation and flashing docs, in progress

* added Serial8 to UWB interface

* squashed feature/uwb_firmware

* uwb works on robot, made range 5m

---------

Co-authored-by: WoozyDragon <[email protected]>

lunabot_embedded/firmware/netzer_encoder_test/netzer_encoder_test.ino

@@ -1,7 +1,7 @@
 #include <Arduino.h>
 #include <SPI.h>
 
-#define CLOCK_SPEED 1'000'000u // 2.5 MHz SSI Clock
+#define CLOCK_SPEED 1'000'000u // 1 MHz SSI Clock
 #define SCK_PIN 13             // SSI CLK line
 
 #define SEL0_PIN 8  // SEL0 line
@@ -22,94 +22,93 @@ void calculateAndPrintPosition(uint32_t &encoderData);
 void selectEnc(uint8_t id);
 
 void setup() {
-    SerialUSB.begin(9600);
-    SPI.begin(); // SPI.begin() will initialize the SPI port, as well as CLK pin
-    pinMode(SCK_PIN, OUTPUT); // pinMode() will initialize the CLK pin as output
-                              // (SPI port can't use it now!)
-    pinMode(SEL0_PIN,
-            OUTPUT); // pinMode() will initialize the CLK pin as output
-    pinMode(SEL1_PIN,
-            OUTPUT); // pinMode() will initialize the CLK pin as output
-    pinMode(SEL2_PIN,
-            OUTPUT); // pinMode() will initialize the CLK pin as output
-    selectEnc(0);
-    digitalWriteFast(
-        SCK_PIN,
-        HIGH); // Set CLK line HIGH (to meet the requirements of SSI interface)
-
-    while (!SerialUSB) {
-    }
-    SerialUSB.println("Started!");
+  SerialUSB.begin(9600);
+  SPI.begin(); // SPI.begin() will initialize the SPI port, as well as CLK pin
+  pinMode(SCK_PIN, OUTPUT); // pinMode() will initialize the CLK pin as output
+                            // (SPI port can't use it now!)
+  pinMode(SEL0_PIN,
+          OUTPUT); // pinMode() will initialize the CLK pin as output
+  pinMode(SEL1_PIN,
+          OUTPUT); // pinMode() will initialize the CLK pin as output
+  pinMode(SEL2_PIN,
+          OUTPUT); // pinMode() will initialize the CLK pin as output
+  selectEnc(0);
+  digitalWriteFast(
+      SCK_PIN,
+      HIGH); // Set CLK line HIGH (to meet the requirements of SSI interface)
+
+  while (!SerialUSB) {
+  }
+  SerialUSB.println("Started!");
 }
 
 void loop() {
-    makeTransfer();
-    uint32_t encoderData = decodeEncoderFrame();
-    calculateAndPrintPosition(encoderData);
+  makeTransfer();
+  uint32_t encoderData = decodeEncoderFrame();
+  calculateAndPrintPosition(encoderData);
 
-    delay(50);
+  delay(50);
 }
 
 void selectEnc(uint8_t id) {
-    uint8_t sel0 = id & 1;
-    uint8_t sel1 = id >> 1 & 1;
-    uint8_t sel2 = id >> 2 & 1;
-    digitalWrite(SEL0_PIN, sel0);
-    digitalWrite(SEL1_PIN, sel1);
-    digitalWrite(SEL2_PIN, sel2);
+  uint8_t sel0 = id & 1;
+  uint8_t sel1 = id >> 1 & 1;
+  uint8_t sel2 = id >> 2 & 1;
+  digitalWrite(SEL0_PIN, sel0);
+  digitalWrite(SEL1_PIN, sel1);
+  digitalWrite(SEL2_PIN, sel2);
 }
 
 void makeTransfer() {
-    digitalWriteFast(SCK_PIN,
-                     LOW); // Set CLK line LOW (to inform encoder -> latch data)
+  digitalWriteFast(SCK_PIN,
+                   LOW); // Set CLK line LOW (to inform encoder -> latch data)
 
-    // Before in setup() the pinMode() change the CLK pin function to output,
-    // now we have to enable usage of this pin by SPI port with calling
-    // SPI.begin():
-    SPI.begin();
+  // Before in setup() the pinMode() change the CLK pin function to output,
+  // now we have to enable usage of this pin by SPI port with calling
+  // SPI.begin():
+  SPI.begin();
 
-    // Or use this one below - a bit faster but SPI port and SCK pin dependent
-    // option. I belive there is a more elegant and more scalable solution,
-    // maybe even supported by hardware for this purpose? - if anyone can point
-    // me it out I would be grateful:
+  // Or use this one below - a bit faster but SPI port and SCK pin dependent
+  // option. I belive there is a more elegant and more scalable solution,
+  // maybe even supported by hardware for this purpose? - if anyone can point
+  // me it out I would be grateful:
 
-    SPI.beginTransaction(settingsA); // We use transactional API
+  SPI.beginTransaction(settingsA); // We use transactional API
 
-    for (int i = 0; i < 2; i++) {
-        encoderBuf[i] =
-            SPI.transfer(0xAA); // Transfer anything and read data back
-    }
+  for (int i = 0; i < 2; i++) {
+    encoderBuf[i] = SPI.transfer(0xAA); // Transfer anything and read data back
+  }
 
-    SPI.endTransaction(); // We use transactional API
+  SPI.endTransaction(); // We use transactional API
 
-    pinMode(SCK_PIN, OUTPUT); // A while before we set CLK pin to be used by SPI
-                              // port, now we have to change it manually...
-    digitalWrite(SCK_PIN, HIGH); // ... back to idle HIGH
+  pinMode(SCK_PIN, OUTPUT); // A while before we set CLK pin to be used by SPI
+                            // port, now we have to change it manually...
+  digitalWrite(SCK_PIN, HIGH); // ... back to idle HIGH
 
-    delayMicroseconds(
-        27); // Running above 500kHz perform Delay First Clock function
+  delayMicroseconds(
+      27); // Running above 500kHz perform Delay First Clock function
 }
 
 uint32_t decodeEncoderFrame() {
-    uint32_t data =
-        static_cast<uint32_t>(encoderBuf[0] << 8) |
-        static_cast<uint32_t>(encoderBuf[1]); // here transfer8 was made
+  uint32_t data =
+      static_cast<uint32_t>(encoderBuf[0] << 8) |
+      static_cast<uint32_t>(encoderBuf[1]); // here transfer8 was made
 
-    // Shift one bit right - (MSB of position was placed on at MSB of uint32_t,
-    // so make it right and shift to make MSB position at 30'th bit):
-    // data = data >> 1;
+  // Shift one bit right - (MSB of position was placed on at MSB of uint32_t,
+  // so make it right and shift to make MSB position at 30'th bit):
+  // data = data >> 1;
 
-    return data;
+  return data;
 }
 
 void calculateAndPrintPosition(uint32_t &encoderData) {
-    // encoderPosition is placed in front (starting from MSB of uint32_t) so
-    // shift it for 11 bits to align position data right
-    uint32_t encoderPosition = encoderData;
+  // encoderPosition is placed in front (starting from MSB of uint32_t) so
+  // shift it for 11 bits to align position data right
+  uint32_t encoderPosition = encoderData;
 
-    float angularAbsolutePosition = static_cast<float>(encoderPosition) /
-                                    static_cast<float>(1 << 16) * 360.0F;
+  float angularAbsolutePosition = static_cast<float>(encoderPosition) /
+                                  static_cast<float>(1 << 16) * 360.0F;
 
-    SerialUSB.println("Encoder Positon = " + String(encoderPosition) +
-                      " Angle = " + String(angularAbsolutePosition, 4));
+  SerialUSB.println("Encoder Positon = " + String(encoderPosition) +
+                    " Angle = " + String(angularAbsolutePosition, 4));
 }

lunabot_embedded/firmware/setup_flash_tools.sh

@@ -0,0 +1,11 @@
+#!/bin/sh
+
+## Installs compilation tools
+# get arduino-cli executable (replace with Linux_x86 if necessary)
+wget https://downloads.arduino.cc/arduino-cli/arduino-cli_latest_Linux_ARM64.tar.gz
+tar -xvf arduino-cli_latest_Linux_ARM64.tar.gz
+./arduino-cli config init
+python3 update_board_manager.py 
+./arduino-cli core install teensy:avr
+
+## Installs teensy flasher

lunabot_embedded/firmware/teensy_main/lib/lunabot_drivers/RobotMsgs.pb.h

@@ -22,6 +22,10 @@ typedef struct _RobotState {
     float dep_ang;
     float lead_screw_ang;
     float act_ang;
+    float dep_weight;
+    float uwb_dist_0;
+    float uwb_dist_1;
+    float uwb_dist_2;
 } RobotState;
 
 typedef struct _RobotEffort {
@@ -39,11 +43,11 @@ extern "C" {
 
 /* Initializer values for message structs */
 #define RobotState_init_default                                                \
-    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
+    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
 #define RobotEffort_init_default                                               \
     { 0, 0, 0, 0, 0, 0 }
 #define RobotState_init_zero                                                   \
-    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
+    { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
 #define RobotEffort_init_zero                                                  \
     { 0, 0, 0, 0, 0, 0 }
 
@@ -59,6 +63,10 @@ extern "C" {
 #define RobotState_dep_ang_tag 9
 #define RobotState_lead_screw_ang_tag 10
 #define RobotState_act_ang_tag 11
+#define RobotState_dep_weight_tag 12
+#define RobotState_uwb_dist_0_tag 13
+#define RobotState_uwb_dist_1_tag 14
+#define RobotState_uwb_dist_2_tag 15
 #define RobotEffort_lead_screw_tag 1
 #define RobotEffort_lin_act_tag 2
 #define RobotEffort_left_drive_tag 3
@@ -78,7 +86,11 @@ extern "C" {
     X(a, STATIC, SINGULAR, FLOAT, drive_right_ang, 8)                          \
     X(a, STATIC, SINGULAR, FLOAT, dep_ang, 9)                                  \
     X(a, STATIC, SINGULAR, FLOAT, lead_screw_ang, 10)                          \
-    X(a, STATIC, SINGULAR, FLOAT, act_ang, 11)
+    X(a, STATIC, SINGULAR, FLOAT, act_ang, 11)                                 \
+    X(a, STATIC, SINGULAR, FLOAT, dep_weight, 12)                              \
+    X(a, STATIC, SINGULAR, FLOAT, uwb_dist_0, 13)                              \
+    X(a, STATIC, SINGULAR, FLOAT, uwb_dist_1, 14)                              \
+    X(a, STATIC, SINGULAR, FLOAT, uwb_dist_2, 15)
 #define RobotState_CALLBACK NULL
 #define RobotState_DEFAULT NULL
 
@@ -101,7 +113,7 @@ extern const pb_msgdesc_t RobotEffort_msg;
 
 /* Maximum encoded size of messages (where known) */
 #define RobotEffort_size 36
-#define RobotState_size 61
+#define RobotState_size 81
 
 #ifdef __cplusplus
 } /* extern "C" */

lunabot_embedded/firmware/teensy_main/lib/lunabot_drivers/RobotMsgs.proto

@@ -14,7 +14,10 @@ message RobotState {
     float lead_screw_ang = 10;
     float act_ang = 11;
 
-    float dep_weight = 12;
+    float dep_weight = 12; 
+    float uwb_dist_0 = 13;
+    float uwb_dist_1 = 14;
+    float uwb_dist_2 = 15;
 
 }