replace how data get's received and passed on the rest of the code

docs/Assignments/extra_research/FR-1-traces.md

@@ -7,6 +7,25 @@ For the experiment, I used FR-1 that has a thickness of 35µm. On there I will m
 
 In this experiment I will measure the temperature of the traces with a multimeter and a thermal laser. I will also watch the board closely for any damage. Since it is FR-1 I expect it to shows signs of damage under heat
 
+## Some background information
+
+### Resistance
+When you decrease a PCB trace width, the resistance of the trace increases.
+Imagine a trace like a water pipe. Whenever you make it smaller the water needs more pressure to flow with the same amount of water.
+The same analogy goes for electricity. So for longer traces this is also the case that the amount of resistance increases with length.
+Whenever the resistance increases different things happen when voltage travels trough them. A higher resistance causes a voltage drop.
+This can be explained using Ohm's law 
+
+ $$V = I \cdot R$$
+
+The Voltage drop is Amperage $$I$$ times the Resistance $$R$$. So if you have a higher resistance or a lower Amperage you will get a voltage drop.
+Another side effect of having a higher resistance is that the resistor or trace will heat up more easily.
+
+### Capacitance
+The capacitance of a PCB trace is the amount of energy it can hold. It works like a capacitor it stores a small amount of energy.
+The capacitance is determined by the amount of copper is in the trace. So length, width and the thickness. When working with low frequency signals the capacitance is not super important.
+But when working with higher speed frequencies the capacitance can actually mess up the signal integrity.
+
 ## Results
 
 the power. So I grabbed an old robot with 4 stepper motors attached but I could not get them to draw more than 30 watts. So that is why the maximum in this test is 30 watts. I also had a hard time finding a way to measure the temperature of the traces because we only had a heat gun here and the laser was not the place it was actually measuring. So 0,4 mm is not measured properly.

docs/Assignments/week_12_machine_building/machine_building.md

@@ -559,4 +559,8 @@ I double, triple checked every connection and grabbed new wires in case that was
 ![alt text](image-13.jpg)
 
 After going to Henk and reading the datasheet he said I needed to use 5 volts instead of 3.3 volts
-When I changed that connection it worked.
+When I changed that connection it worked.
+
+## The result of the robot
+
+![https://fabacademy.org/2025/labs/waag/img/FINAL_XIAO_DONE.mp4](https://fabacademy.org/2025/labs/waag/img/FINAL_XIAO_DONE.mp4)

docs/Assignments/week_13_molding_and_casting/molding_and_casting.md

@@ -80,6 +80,16 @@ This was my second attempt which has gone a lot better.
 ![alt text](image-17.jpg)
 This is the `Mold Star 16 Fast`
 
+I also made 2 other moulds for the group assignment so we knew what every material was.
+
+Dragon Skin
+* translucent
+* super flexible
+//todo:add images
+
+SORTA-Clear
+* translucent
+* A bit more flexible
 
 ## Making our own moulds
 To create our own moulds we first need to create a block where we can cut in.
@@ -137,6 +147,10 @@ We also needed to change this in Vcarve otherwise the toolpaths won't 't get cal
 
 To mill a wax block it first needs to be fastened. We did that using 4 pieces of wood.
 
+Another thing is the placement of the milling bit job home.
+![alt text](image-29.jpg)
+Because the milling bit is dead centre on the corner in the software it should also be placed half on the corner and half off. When working with super tight tolerances and smaller things it is super important to place the job home perfectly. I learned that the hard way by almost cutting into the wood.
+
 ### Small accident
 ![alt text](image-24.jpg)
 While fastening the wood push it into the wax block. Otherwise it may get loose during milling and this can happen.
@@ -163,6 +177,3 @@ Here you can see the part that broke off during the milling accident. Then I use
 
 For next time: I should've poured slower and more gentle because there are a lot of air bubbles and be more careful when milling and better inspect the block and if its mounted well. 
 
-Another thing is the placement of the milling bit job home.
-![alt text](image-29.jpg)
-Because the milling bit is dead centre on the corner in the software it should also be placed half on the corner and half off. When working with super tight tolerances and smaller things it is super important to place the job home perfectly. I learned that the hard way by almost cutting into the wood.

docs/final_project/final_project.md

@@ -166,15 +166,212 @@ namespace Controller {
 
 ESPC3 --> ESPC6 Thread 1 : Send data to flight controller using ESPNOW
 ```
+Now that I am in week 15 this has changed. The change is on the drone are going to be 2 mcu's. One for the matrixes and one for the flight controller. So they are both separate systems and can't interfere with each other.
 
-I've already gotten the motors to spin in [week 10](../Assignments/week_10_output_devices/output_devices.md) using the script in there. Now I also need to modify the driver so my ESC's can properly understand it's instructions.
+![alt text](image-30.jpg)
+
+I've already gotten the motors to spin in [week 10](../Assignments/week_10_output_devices/output_devices.md) using the script in there. Now I also need to modify the original VTOL driver so my ESC's can properly understand it's instructions.
+
+There's one specific function I need to change.
+??? Old code
+    ```cpp
+    void commandMotors()
+    {
+    // DESCRIPTION: Send pulses to motor pins, oneshot125 protocol
+    /*
+    * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being
+    * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future.
+    */
+    int wentLow = 0;
+    int pulseStart, timer;
+    int flagM1 = 0;
+    int flagM2 = 0;
+    int flagM3 = 0;
+    int flagM4 = 0;
+    int flagM5 = 0;
+    int flagM6 = 0;
+
+    // Write all motor pins high
+    digitalWrite(m1Pin, HIGH);
+    digitalWrite(m2Pin, HIGH);
+    digitalWrite(m3Pin, HIGH);
+    digitalWrite(m4Pin, HIGH);
+    digitalWrite(m5Pin, HIGH);
+    digitalWrite(m6Pin, HIGH);
+    pulseStart = micros();
+
+    // Write each motor pin low as correct pulse length is reached
+    while (wentLow < 6)
+    { // Keep going until final (6th) pulse is finished, then done
+        timer = micros();
+        if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0))
+        {
+        digitalWrite(m1Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM1 = 1;
+        }
+        if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0))
+        {
+        digitalWrite(m2Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM2 = 1;
+        }
+        if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0))
+        {
+        digitalWrite(m3Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM3 = 1;
+        }
+        if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0))
+        {
+        digitalWrite(m4Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM4 = 1;
+        }
+        if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0))
+        {
+        digitalWrite(m5Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM5 = 1;
+        }
+        if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0))
+        {
+        digitalWrite(m6Pin, LOW);
+        wentLow = wentLow + 1;
+        flagM6 = 1;
+        }
+    }
+    }
+    ```
+What this function does is generate PWM values in software which is super inefficient. The esp32C6 according to it's datasheet supports 6 seperate PWM clocks in hardware.
+![alt text](image-2pc-4.jpg)
+So I wanna use these to reduce the overhead and so the loop doesn't have to run 20 times a second but faster making the drone respond faster. To use these I need to use this library.
+Right link: [https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html](https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html)
+
+When first looking at the documentation I got super confused why nothing was working but that was because I was looking at the documentation for the wrong framework. I was looking at the ESP-IDF framework instead of the Arduino framework.
+Wrong link: [https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html](https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/peripherals/ledc.html)
+
+So I really needed 2 functions of the library.
+* To set pwm channel, pin and resolution (ledcAttach)
+* To set the PWM frequency (ledcWrite)
+
+![alt text](image-2pc-5.jpg)
+![alt text](image-2pc-6.jpg)
+
+## Generative design
+Online I saw a lot of organic designs that where created by generative design.
+![alt text](image-12.jpg)
+Credits: [formlabs](https://formlabs.com/eu/blog/generative-design/)
+I wanted to do this myself for the drone arms. So I started messing around in the generative design tab of my drone arm model.
+
+![alt text](image-13.jpg)
+I've already designed the base of the arm. What generative design does is either add or remove material based on parameters you give it.
+I've already prepared my model by created some parts as seperate bodies. So I can preserve them during the generation process.
+When creating a Study you need to assign which parts to groups.
+
+![alt text](image-14.jpg)
+* The preserve Geometry group where loads get applied
+* Obstacle Geometry gets avoided and no loads get placed on there.
+* Obstacle offset increases the size of a body by an x amount of mm.
+* Starting shape is the group that is mainly going to get modified
+* Unsigned geometry are bodies that are not assigned yet
+* symmetry planes are a way to define a plane where both sides need to be mirrored (symmetrical)
+
+So after assigning everything this is my result.
+
+![alt text](image-15.jpg)
+
+After that you can open the Objectives tab to see what you wanna do and set more limits.
+![alt text](image-16.jpg)
+
+The only things I understand from this menu is the Objectives and the Displacement. The objective is straight foward and the Displacement is the amount of millimeters it is allowed to fluctuate from the original design in said direction.
+
+Now it's time to add the loads on the part. So the amount weight and force the part needs to be able to handle.
+For that there's a keybind `L`
+![alt text](image-17.jpg)
+After you've pressed that you can click a face and add force to it. In my instance it needs to withstand at least 2 kg in all directions.
+The formula for Kg to Newton is. `Earths gravity * Kg = Newton`. So 9.81 * 2 = 19.62N.
+
+After adding all of that there are 2 steps left over.
+Adding the place where it is locked in place and materials we wanna study.
+
+Over in the Manufacturing tab we can select materials.
+![alt text](image-18.jpg)
+I've added PET since there isn't PLA in the library.
+![alt text](image-19.jpg)
+
+Also make sure you have a constraint.
+![alt text](image-20.jpg)
+This can be added by pressing `C`.
+Now we can run the case!
+
+When running a study I first recommend running the pre-check.
+![alt text](image-21.jpg)
+In my case I am not milling it and the part that's hidden is the motor itself so that's fine.
+
+So now I am ready for generation.
+![alt text](image-22.jpg)
+Once I press that button this will pop up and I can press Generate 1 Study. 
+This does cost money If you don't have unlimited cloud tokens. With my education license I do have unlimited.
+
+![alt text](image-23.jpg)
+Once it starts generating this will pop up. These are the finished results. The generation process can take about an hour.
+
+## Full redesign
+I started a full re-design from the ground up because the old design was too cluttered and it didn't scale properly anymore parametrically. I first started off with the first sketch from the original design.
+
+![alt text](image-pc.jpg)
+
+From there I extruded the body symmetrically so they are always the exact same size on both sides.
+![alt text](image-pc-1.jpg)
+
+I made a sketch on the side of the drone making place for the LED matrix and all its cables
+
+![alt text](image-pc-2.jpg)
+
+After that I used the mirror tool to mirror the features over to the other side using the axes as the tool since they are dead centre. I did the same for the holes for the wiring
+
+![alt text](image-pc-3.jpg)
+After that I dragged the motor arm design into the designing window and jointed it to the body of the robot.
+
+![alt text](image-pc-4.jpg)
+Then that I jointed the arm to the robot in the corner.
+![alt text](image-pc-5.jpg)
+Now I needed to make the same holes through the case itself in the same location. So I choose edit in place from the menu.
+
+![alt text](image-pc-6.jpg)
+
+From this tab I can edit the body with all the references in place. So I could use the holes in the drone arm as reference for my cuts for in the drone body.
+![alt text](image-pc-7.jpg)
 
 
+## Burnout!
+One of my motors during testing burned out completely when it was attached to the printed arm. I think the cause for this is overheating due to a propellor that's too big.
 
-https://grabcad.com/library/readytosky-rs2205-1
+![alt text](image-27.jpg)
 
+These motors where originally made for 5 inch propellors instead of 8 inch so that's kinda a big difference.
+
+![alt text](image-28.jpg)
+
+Another thing that also could be the issue is that my ESC was surging above 40 Amps and the motors could only take up to 40 amps.
+![alt text](image-29.jpg)
+
+I think the main reason was heat and the coating on the motors evaporating causing a chain reaction.
+
+## TODO
+* [ ] Matrix panel subsystem
+* [x] Rewrite PWM esc control system in driver
+* [ ] Does it fly?
+* [x] Power distribution system (for matrixes and mcu's)
+* [x] Test physical controller
 
 ## BOM (bill of materials)
 | item  | price | link     |
 | :---- | ----- | -------- |
-| item1 | 999   | link.com |
+| item1 | 999   | link.com |
+
+## Files
+
+* [Handheld controller pcb](Drone%20controller.zip)
+* [Drone motor 3D file](https://grabcad.com/library/readytosky-rs2205-1)

mkdocs.yml

@@ -41,3 +41,12 @@ markdown_extensions:
 
 
 
+extra_javascript:
+  - javascripts/katex.js
+  - https://unpkg.com/katex@0/dist/katex.min.js
+  - https://unpkg.com/katex@0/dist/contrib/auto-render.min.js
+
+extra_css:
+  - https://unpkg.com/katex@0/dist/katex.min.css
+
+

src/controller/drone controller/platformio.ini

@@ -12,8 +12,10 @@
 platform = espressif32
 board = lolin_c3_mini
 framework = arduino
+lib_deps = adafruit/Adafruit SSD1306@^2.5.13
 
 [env:Xiao_c3]
 board = seeed_xiao_esp32c3
 framework = arduino
 platform = espressif32
+lib_deps = adafruit/Adafruit SSD1306@^2.5.13

src/controller/drone controller/src/main.cpp

@@ -2,19 +2,12 @@
 #include <WiFi.h>
 #include <esp_now.h>
 
-// declarations
-int normalizePot(int pin, int minValue);
-int mapPot(int normalizedValue);
-int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
-void espNow();
-void readAllMultiPlexer();
-void espNowLoop();
-
 const int MAXPWMVALUE = 1000;
 const int MINPWMVALUE = 2000;
 const uint8_t broadcastAddress[] = {0x8C, 0xBF, 0xEA, 0xCC, 0x8E, 0x5C};
 
-// Define the struct that will be sent
+//=====================================================================================//
+// Struct declarations
 typedef struct struct_message
 {
   int PWMCH1;
@@ -26,35 +19,53 @@ struct_message JoystickData; // declare the struct as JoystickData
 
 esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class
 
+struct hardJoystickValues
+{
+  int LXU; // Left joystick X axis up
+  int LXD; // Left joystick X axis down
+  int RXU; // Right joystick X axis up
+  int RXD; // Right joystick X axis down
+}; 
+
+//=====================================================================================//
+// declarations
+int normalizePot(int pin, int minValue);
+int mapPot(int normalizedValue);
+int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
+void espNow();
+void readAllMultiPlexer();
+void espNowLoop();
+void printPWMValues();
+int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
+hardJoystickValues GUIParser();
+void MUXSetup();
+
+
+
 void setup()
 {
   espNow();
+  MUXSetup(); // Setup the multiplexer
   Serial.begin(9600);
 
-  pinMode(D3, OUTPUT);
-  pinMode(D6, OUTPUT);
-  pinMode(D7, OUTPUT);
-  pinMode(D8, OUTPUT);
-  pinMode(D9, OUTPUT);
-  pinMode(D0, INPUT);
 }
 
 void loop()
 {
+  //Debugging
+  // readAllMultiPlexer();
+  // printPWMValues();
+
+
   // Set values to send
   JoystickData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0)); //Right joystick Y
   JoystickData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // Right joystick X
   JoystickData.PWMCH3 = mapPot(analogReadMultiPlexer(0, 0, 0, 1, A0)); // left joystick Y
   JoystickData.PWMCH4 = mapPot(analogReadMultiPlexer(1, 0, 0, 1, A0)); // left joystick X
-  Serial.print("PWMCH1: ");
-  Serial.println(JoystickData.PWMCH1);
-  Serial.print(" PWMCH2: ");
-  Serial.println(JoystickData.PWMCH2);
-  Serial.print(" PWMCH3: ");
-  Serial.println(JoystickData.PWMCH3);  
-  Serial.print(" PWMCH4: ");
-  Serial.println(JoystickData.PWMCH4);
+  bool buttonRight = abs(analogReadMultiPlexer(0, 0, 1, 0, A0)/4095); // right button
+  bool buttonLeft = abs(analogReadMultiPlexer(1, 0, 1, 0, A0)/4095); // left button
 
+  GUIParser();
   espNowLoop();
   delay(10); // delay to avoid hammering the radio and to save power/heat
 }
@@ -89,6 +100,16 @@ int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD
   digitalWrite(D8, addressD);
   return analogRead(pin);
 }
+//For some reason the digitalRead function does not work with the multiplexer. Maybe because the Resistance is too high. Even though analogRead returns 4095
+int digitalReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
+{
+  digitalWrite(D3, LOW);
+  digitalWrite(D6, addressA);
+  digitalWrite(D7, addressB);
+  digitalWrite(D9, addressC);
+  digitalWrite(D8, addressD);
+  return digitalRead(pin);
+}
 
 void espNow()
 { // Set device as a Wi-Fi Station
@@ -111,6 +132,71 @@ void espNow()
   }
 }
 
+void espNowLoop(){
+  // Send message via ESP-NOW
+  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData));
+  if (result == ESP_OK)
+  {
+    // Serial.println("Sent with success");
+  }
+  else
+  {
+    // Serial.println("Error sending the data");
+  }
+}
+
+void MUXSetup()
+{
+  pinMode(D3, OUTPUT); // MUX enable
+  pinMode(D6, OUTPUT); // MUX address A
+  pinMode(D7, OUTPUT); // MUX address B
+  pinMode(D9, OUTPUT); // MUX address C
+  pinMode(D8, OUTPUT); // MUX address D
+  pinMode(A0, INPUT);  // MUX input
+}
+// Function to parse joystick data to hard values
+//If joystick is Up then the value is 1 of var A
+//If joystick is Down then the value is 1 of var B
+//If joystick is in the middle A and B are 0
+hardJoystickValues GUIParser(){
+
+    // Define joystick offsets (calibrated resting values)
+    const int offsetPWMCH1 = 1090; // Resting value for PWMCH1 (right joystick Y-axis)
+    const int offsetPWMCH2 = 1072; // Resting value for PWMCH2 (right joystick X-axis)
+    const int offsetPWMCH3 = 1043; // Resting value for PWMCH3 (left joystick X-axis)
+    const int offsetPWMCH4 = 1476; // Resting value for PWMCH4 (left joystick Y-axis)
+  
+    // Define deadzone threshold
+    const int deadzone = 120;
+  
+    // Adjust joystick values by subtracting offsets
+    int adjustedPWMCH1 = JoystickData.PWMCH1 - offsetPWMCH1;
+    int adjustedPWMCH2 = JoystickData.PWMCH2 - offsetPWMCH2;
+    int adjustedPWMCH3 = JoystickData.PWMCH3 - offsetPWMCH3;
+    int adjustedPWMCH4 = JoystickData.PWMCH4 - offsetPWMCH4;
+  
+    // Apply deadzone
+    if (abs(adjustedPWMCH1) < deadzone) adjustedPWMCH1 = 0; //abs to avoid negatives
+    if (abs(adjustedPWMCH2) < deadzone) adjustedPWMCH2 = 0;
+    if (abs(adjustedPWMCH3) < deadzone) adjustedPWMCH3 = 0;
+    if (abs(adjustedPWMCH4) < deadzone) adjustedPWMCH4 = 0;
+
+    // Map joystick values to hard values
+    int LXU = 0; // Left joystick X axis up
+    int LXD = 0; // Left joystick X axis down
+    if (adjustedPWMCH1 > 0) {
+        LXU = 1; // Joystick is up
+    } else if (adjustedPWMCH1 < 0) {
+        LXD = 1; // Joystick is down
+    }
+    return {LXU, LXD, 0, 0}; // Return the values as a struct
+}
+
+/////////////////////////////////////////////
+//           Debugging Functions           //
+/////////////////////////////////////////////
+
+
 void readAllMultiPlexer()
 {
   // we counting in binary
@@ -148,15 +234,15 @@ void readAllMultiPlexer()
   Serial.println(analogReadMultiPlexer(1, 1, 1, 1, A0));
 }
 
-void espNowLoop(){
-  // Send message via ESP-NOW
-  esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *)&JoystickData, sizeof(JoystickData));
-  if (result == ESP_OK)
-  {
-    // Serial.println("Sent with success");
-  }
-  else
-  {
-    // Serial.println("Error sending the data");
-  }
+
+void printPWMValues()
+{
+  Serial.print("PWMCH1: ");
+  Serial.print(JoystickData.PWMCH1);
+  Serial.print(" PWMCH2: ");
+  Serial.print(JoystickData.PWMCH2);
+  Serial.print(" PWMCH3: ");
+  Serial.print(JoystickData.PWMCH3);
+  Serial.print(" PWMCH4: ");
+  Serial.println(JoystickData.PWMCH4);
 }

src/drone/src/main.cpp

@@ -1,9 +1,9 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include "SparkFun_BNO080_Arduino_Library.h"
-#include <ESP32Servo.h>
 #include <esp_now.h>
 #include <WiFi.h>
+#include <driver/ledc.h>
 // Arduino/Teensy Flight Controller - dRehmFlight
 // Author: Nicholas Rehm
 // Project Start: 1/6/2020
@@ -157,6 +157,12 @@ BNO080 myIMU;
 #define ACCEL_SCALE_FACTOR 1.0 // BNO085 gives values already in g's when converted in getIMUdata
 #endif
 
+#define LEDC_FREQ 500             // PWM frequency in Hz for OneShot125 (can be higher)
+#define LEDC_RESOLUTION 13        // 13-bit resolution gives values from 0-8191
+#define LEDC_TIMER LEDC_TIMER_0   // Use timer 0
+#define LEDC_MODE LEDC_HIGH_SPEED_MODE  // Use high-speed mode
+
+
 //========================================================================================================================//
 //                                               USER-SPECIFIED VARIABLES                                                 //
 //========================================================================================================================//
@@ -238,21 +244,7 @@ const int m3Pin = D2;
 const int m4Pin = D3;
 const int m5Pin = D7;
 const int m6Pin = D8;
-// PWM servo or ESC outputs:
-const int servo1Pin = 6;
-const int servo2Pin = 7;
-const int servo3Pin = 8;
-const int servo4Pin = 9;
-const int servo5Pin = 10;
-const int servo6Pin = 11;
-const int servo7Pin = 12;
-Servo servo1; // Create servo objects to control a servo or ESC with PWM
-Servo servo2;
-Servo servo3;
-Servo servo4;
-Servo servo5;
-Servo servo6;
-Servo servo7;
+
 
 //========================================================================================================================//
 
@@ -317,7 +309,6 @@ float error_yaw, error_yaw_prev, integral_yaw, integral_yaw_prev, derivative_yaw
 float m1_command_scaled, m2_command_scaled, m3_command_scaled, m4_command_scaled, m5_command_scaled, m6_command_scaled;
 int m1_command_PWM, m2_command_PWM, m3_command_PWM, m4_command_PWM, m5_command_PWM, m6_command_PWM;
 float s1_command_scaled, s2_command_scaled, s3_command_scaled, s4_command_scaled, s5_command_scaled, s6_command_scaled, s7_command_scaled;
-int s1_command_PWM, s2_command_PWM, s3_command_PWM, s4_command_PWM, s5_command_PWM, s6_command_PWM, s7_command_PWM;
 
 // Flight status
 bool armedFly = false;
@@ -358,14 +349,13 @@ void printMagData();
 void printRollPitchYaw();
 void printPIDoutput();
 void printMotorCommands();
-void printServoCommands();
 void printLoopRate();
 float invSqrt(float x);
 void controlMixer();
 void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
 void ESPNowSetup();
 void printDebugInfo();
-
+void setupMotorPWM();
 //========================================================================================================================//
 //                                                      VOID SETUP                                                        //
 //========================================================================================================================//
@@ -379,22 +369,10 @@ void setup()
   Serial.println("Initiating IMU");
   IMUinit();
 
-  Serial.println("Initiating pins");
-  // Initialize all pins
-  // pinMode(13, OUTPUT); // Pin 13 LED blinker on board, do not modify
-  pinMode(m1Pin, OUTPUT);
-  pinMode(m2Pin, OUTPUT);
-  pinMode(m3Pin, OUTPUT);
-  pinMode(m4Pin, OUTPUT);
-  pinMode(m5Pin, OUTPUT);
-  pinMode(m6Pin, OUTPUT);
+  Serial.println("Initiating ESC");
+  setupMotorPWM();
   Serial.println("Attach servos");
-  servo1.attach(servo1Pin, 900, 2100); // Pin, min PWM value, max PWM value
-  servo2.attach(servo2Pin, 900, 2100);
-  servo3.attach(servo3Pin, 900, 2100);
-  servo4.attach(servo4Pin, 900, 2100);
-  servo5.attach(servo5Pin, 900, 2100);
-  servo6.attach(servo6Pin, 900, 2100);
+
   // servo7.attach(servo7Pin, 900, 2100);
 
   // Set built in LED to turn on to signal startup
@@ -425,15 +403,6 @@ void setup()
   // Get IMU error to zero accelerometer and gyro readings, assuming vehicle is level when powered up
   // calculate_IMU_error(); //Calibration parameters printed to serial monitor. Paste these in the user specified variables section, then comment this out forever.
 
-  // Arm servo channels
-  Serial.println("Arming servos");
-  servo1.write(0); // Command servo angle from 0-180 degrees (1000 to 2000 PWM)
-  servo2.write(0); // Set these to 90 for servos if you do not want them to briefly max out on startup
-  servo3.write(0); // Keep these at 0 if you are using servo outputs for motors
-  servo4.write(0);
-  servo5.write(0);
-  servo6.write(0);
-  servo7.write(0);
 
   delay(5);
 
@@ -473,14 +442,13 @@ void loop()
 
   // Print data at 100hz (uncomment one at a time for troubleshooting) - SELECT ONE:
   // printRadioData();     //Prints radio pwm values (expected: 1000 to 2000)
-  // printDesiredState();  //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
+  printDesiredState();  //Prints desired vehicle state commanded in either degrees or deg/sec (expected: +/- maxAXIS for roll, pitch, yaw; 0 to 1 for throttle)
   // printGyroData();      //Prints filtered gyro data direct from IMU (expected: ~ -250 to 250, 0 at rest)
   // printAccelData(); // Prints filtered accelerometer data direct from IMU (expected: ~ -2 to 2; x,y 0 when level, z 1 when level)
   // printMagData();       //Prints filtered magnetometer data direct from IMU (expected: ~ -300 to 300)
   // printRollPitchYaw();  //Prints roll, pitch, and yaw angles in degrees from Madgwick filter (expected: degrees, 0 when level)
   // printPIDoutput();     //Prints computed stabilized PID variables from controller and desired setpoint (expected: ~ -1 to 1)
-  printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
-  // printServoCommands(); //Prints the values being written to the servos (expected: 0 to 180)
+  // printMotorCommands(); //Prints the values being written to the motors (expected: 120 to 250)
   // printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
   // printDebugInfo(); //PWM 1 and 5 + Armed condition
 
@@ -489,7 +457,6 @@ void loop()
 
   // Get vehicle state
   getIMUdata();                                                              // Pulls raw gyro, accelerometer, and magnetometer data from IMU and LP filters to remove noise
-  Madgwick(GyroX, -GyroY, -GyroZ, -AccX, AccY, AccZ, MagY, -MagX, MagZ, dt); // Updates roll_IMU, pitch_IMU, and yaw_IMU angle estimates (degrees)
 
   // Compute desired state
   getDesState(); // Convert raw commands to normalized values based on saturated control limits
@@ -508,20 +475,11 @@ void loop()
 
   // Command actuators
   commandMotors();              // Sends command pulses to each motor pin using OneShot125 protocol
-  servo1.write(s1_command_PWM); // Writes PWM value to servo object
-  servo2.write(s2_command_PWM);
-  servo3.write(s3_command_PWM);
-  servo4.write(s4_command_PWM);
-  servo5.write(s5_command_PWM);
-  servo6.write(s6_command_PWM);
-  servo7.write(s7_command_PWM);
+
 
   // Get vehicle commands for next loop iteration
   getCommands(); // Pulls current available radio commands
   failSafe();    // Prevent failures in event of bad receiver connection, defaults to failsafe values assigned in setup
-
-  // Regulate loop rate
-  loopRate(2000); // Do not exceed 2000Hz, all filter parameters tuned to 2000Hz by default
 }
 
 //========================================================================================================================//
@@ -637,6 +595,8 @@ void IMUinit()
   myIMU.enableAccelerometer(50);
   delay(100);
   myIMU.enableMagnetometer(50);
+  delay(100);
+  myIMU.enableGyroIntegratedRotationVector(50); 
   delay(500);
 
 #endif
@@ -660,79 +620,63 @@ void getIMUdata()
   mpu9250.getMotion9(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ, &MgX, &MgY, &MgZ);
   #elif defined USE_BNO085_I2C
   // Keep waiting until data becomes available
-  while (!myIMU.dataAvailable())
-  {
-    delay(1); // Small delay to avoid hammering the I2C bus too hard
+  // Wait for data with timeout
+  unsigned long startTime = millis();
+  while (!myIMU.dataAvailable()) {
+    delay(1);
+    if (millis() - startTime > 500) {
+      Serial.println("BNO085 timeout");
+      return;
+    }
   }
-  // All the error checking is done here because the BNO085 handles all the sensor fusion internally
-  // BNO magnetometer
-  MgX = myIMU.getMagX();
-  MgY = myIMU.getMagY();
-  MgZ = myIMU.getMagZ();
-
-  // convert radians to degrees
-  GyX = myIMU.getGyroX() * 180 / PI;
-  GyY = myIMU.getGyroY() * 180 / PI;
-  GyZ = myIMU.getGyroZ() * 180 / PI;
-
-  // Acceleration
-  // Convert acceleration m/s² to g (9.81 m/s² = 1g)
-  AcX = myIMU.getAccelX() / 9.80665; // m/s² to g
-  AcY = myIMU.getAccelY() / 9.80665; // m/s² to g
-  AcZ = myIMU.getAccelZ() / 9.80665; // m/s² to g
-
-  AccX = AcX;
-  AccY = AcY;
-  AccZ = AcZ;
-
-  // Correct the outputs with the calculated error values
-  AccX = AccX - AccErrorX;
-  AccY = AccY - AccErrorY;
-  AccZ = AccZ - AccErrorZ;
   
-  // LP filter accelerometer data
-  AccX = (1.0 - B_accel) * AccX_prev + B_accel * AccX;
-  AccY = (1.0 - B_accel) * AccY_prev + B_accel * AccY;
-  AccZ = (1.0 - B_accel) * AccZ_prev + B_accel * AccZ;
-  AccX_prev = AccX;
-  AccY_prev = AccY;
-  AccZ_prev = AccZ;
 
-  // Process gyro data
-  GyroX = GyX;
-  GyroY = GyY;
-  GyroZ = GyZ;
+    // Get quaternion directly from BNO085
+    q0 = myIMU.getQuatReal();  // w component
+    q1 = myIMU.getQuatI();     // x component
+    q2 = myIMU.getQuatJ();     // y component
+    q3 = myIMU.getQuatK();     // z component
+    
+    // Calculate Euler angles from quaternion
+    roll_IMU = atan2(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2)) * 57.29577951f;
+    pitch_IMU = asin(2.0f * (q0*q2 - q3*q1)) * 57.29577951f;
+    yaw_IMU = atan2(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3)) * 57.29577951f;
+
   
-  // Correct with gyro error values
+  // Get raw sensor data for PID controller
+  GyroX = myIMU.getGyroX() * 57.29577951f; // Convert rad/s to deg/s
+  GyroY = myIMU.getGyroY() * 57.29577951f;
+  GyroZ = myIMU.getGyroZ() * 57.29577951f;
+  
+  // Apply error corrections and filtering
   GyroX = GyroX - GyroErrorX;
   GyroY = GyroY - GyroErrorY;
   GyroZ = GyroZ - GyroErrorZ;
   
-  // LP filter gyro data
-  GyroX = (1.0 - B_gyro) * GyroX_prev + B_gyro * GyroX;
-  GyroY = (1.0 - B_gyro) * GyroY_prev + B_gyro * GyroY;
-  GyroZ = (1.0 - B_gyro) * GyroZ_prev + B_gyro * GyroZ;
+  GyroX = (1.0f - B_gyro) * GyroX_prev + B_gyro * GyroX;
+  GyroY = (1.0f - B_gyro) * GyroY_prev + B_gyro * GyroY;
+  GyroZ = (1.0f - B_gyro) * GyroZ_prev + B_gyro * GyroZ;
   GyroX_prev = GyroX;
   GyroY_prev = GyroY;
   GyroZ_prev = GyroZ;
+  
+  // Get accelerometer data (still useful for PID control)
+  AccX = myIMU.getAccelX() / 9.80665f; // Convert m/s² to g
+  AccY = myIMU.getAccelY() / 9.80665f;
+  AccZ = myIMU.getAccelZ() / 9.80665f;
+  
+  // Apply corrections and filtering
+  AccX = AccX - AccErrorX;
+  AccY = AccY - AccErrorY;
+  AccZ = AccZ - AccErrorZ;
+  
+  AccX = (1.0f - B_accel) * AccX_prev + B_accel * AccX;
+  AccY = (1.0f - B_accel) * AccY_prev + B_accel * AccY;
+  AccZ = (1.0f - B_accel) * AccZ_prev + B_accel * AccZ;
+  AccX_prev = AccX;
+  AccY_prev = AccY;
+  AccZ_prev = AccZ;
 
-  // Process mag data
-  MagX = MgX / 6.0; // uT
-  MagY = MgY / 6.0;
-  MagZ = MgZ / 6.0;
-
-  // Correct the outputs with the calculated error values
-  MagX = (MagX - MagErrorX) * MagScaleX;
-  MagY = (MagY - MagErrorY) * MagScaleY;
-  MagZ = (MagZ - MagErrorZ) * MagScaleZ;
-
-  // LP filter magnetometer data
-  MagX = (1.0 - B_mag) * MagX_prev + B_mag * MagX;
-  MagY = (1.0 - B_mag) * MagY_prev + B_mag * MagY;
-  MagZ = (1.0 - B_mag) * MagZ_prev + B_mag * MagZ;
-  MagX_prev = MagX;
-  MagY_prev = MagY;
-  MagZ_prev = MagZ;
 
   // Return to avoid executing code for other IMUs
   return;
@@ -833,6 +777,8 @@ void calculate_IMU_error()
     AcY = myIMU.getAccelY() / 9.80665; // m/s² to g
     AcZ = myIMU.getAccelZ() / 9.80665; // m/s² to g
 
+
+
 #endif
 
     AccX = AcX / ACCEL_SCALE_FACTOR;
@@ -1352,22 +1298,7 @@ void scaleCommands()
   m5_command_PWM = constrain(m5_command_PWM, 125, 250);
   m6_command_PWM = constrain(m6_command_PWM, 125, 250);
 
-  // Scaled to 0-180 for servo library
-  s1_command_PWM = s1_command_scaled * 180;
-  s2_command_PWM = s2_command_scaled * 180;
-  s3_command_PWM = s3_command_scaled * 180;
-  s4_command_PWM = s4_command_scaled * 180;
-  s5_command_PWM = s5_command_scaled * 180;
-  s6_command_PWM = s6_command_scaled * 180;
-  s7_command_PWM = s7_command_scaled * 180;
-  // Constrain commands to servos within servo library bounds
-  s1_command_PWM = constrain(s1_command_PWM, 0, 180);
-  s2_command_PWM = constrain(s2_command_PWM, 0, 180);
-  s3_command_PWM = constrain(s3_command_PWM, 0, 180);
-  s4_command_PWM = constrain(s4_command_PWM, 0, 180);
-  s5_command_PWM = constrain(s5_command_PWM, 0, 180);
-  s6_command_PWM = constrain(s6_command_PWM, 0, 180);
-  s7_command_PWM = constrain(s7_command_PWM, 0, 180);
+
 }
 
 void getCommands()
@@ -1487,72 +1418,24 @@ void failSafe()
   }
 }
 
-void commandMotors()
-{
-  // DESCRIPTION: Send pulses to motor pins, oneshot125 protocol
-  /*
-   * My crude implimentation of OneShot125 protocol which sends 125 - 250us pulses to the ESCs (mXPin). The pulselengths being
-   * sent are mX_command_PWM, computed in scaleCommands(). This may be replaced by something more efficient in the future.
-   */
-  int wentLow = 0;
-  int pulseStart, timer;
-  int flagM1 = 0;
-  int flagM2 = 0;
-  int flagM3 = 0;
-  int flagM4 = 0;
-  int flagM5 = 0;
-  int flagM6 = 0;
+void commandMotors() {
+  //https://docs.espressif.com/projects/arduino-esp32/en/latest/api/ledc.html
 
-  // Write all motor pins high
-  digitalWrite(m1Pin, HIGH);
-  digitalWrite(m2Pin, HIGH);
-  digitalWrite(m3Pin, HIGH);
-  digitalWrite(m4Pin, HIGH);
-  digitalWrite(m5Pin, HIGH);
-  digitalWrite(m6Pin, HIGH);
-  pulseStart = micros();
-
-  // Write each motor pin low as correct pulse length is reached
-  while (wentLow < 6)
-  { // Keep going until final (6th) pulse is finished, then done
-    timer = micros();
-    if ((m1_command_PWM <= timer - pulseStart) && (flagM1 == 0))
-    {
-      digitalWrite(m1Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM1 = 1;
-    }
-    if ((m2_command_PWM <= timer - pulseStart) && (flagM2 == 0))
-    {
-      digitalWrite(m2Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM2 = 1;
-    }
-    if ((m3_command_PWM <= timer - pulseStart) && (flagM3 == 0))
-    {
-      digitalWrite(m3Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM3 = 1;
-    }
-    if ((m4_command_PWM <= timer - pulseStart) && (flagM4 == 0))
-    {
-      digitalWrite(m4Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM4 = 1;
-    }
-    if ((m5_command_PWM <= timer - pulseStart) && (flagM5 == 0))
-    {
-      digitalWrite(m5Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM5 = 1;
-    }
-    if ((m6_command_PWM <= timer - pulseStart) && (flagM6 == 0))
-    {
-      digitalWrite(m6Pin, LOW);
-      wentLow = wentLow + 1;
-      flagM6 = 1;
-    }
-  }
+  // Convert OneShot125 pulse values (125-250μs) to duty cycle values (1100-2000) for my ESC's
+  uint32_t m1_duty = map(m1_command_PWM, 125, 250, 1100, 2000);
+  uint32_t m2_duty = map(m2_command_PWM, 125, 250, 1100, 2000);
+  uint32_t m3_duty = map(m3_command_PWM, 125, 250, 1100, 2000);
+  uint32_t m4_duty = map(m4_command_PWM, 125, 250, 1100, 2000);
+  uint32_t m5_duty = map(m5_command_PWM, 125, 250, 1100, 2000);
+  uint32_t m6_duty = map(m6_command_PWM, 125, 250, 1100, 2000);
+  
+  // Write the duty cycle values directly to each pin
+  ledcWrite(m1Pin, m1_duty);
+  ledcWrite(m2Pin, m2_duty);
+  ledcWrite(m3Pin, m3_duty);
+  ledcWrite(m4Pin, m4_duty);
+  ledcWrite(m5Pin, m5_duty);
+  ledcWrite(m6Pin, m6_duty);
 }
 
 void armMotors()
@@ -1610,13 +1493,7 @@ void calibrateESCs()
 
     // throttleCut(); //Directly sets motor commands to low based on state of ch5
 
-    servo1.write(s1_command_PWM);
-    servo2.write(s2_command_PWM);
-    servo3.write(s3_command_PWM);
-    servo4.write(s4_command_PWM);
-    servo5.write(s5_command_PWM);
-    servo6.write(s6_command_PWM);
-    servo7.write(s7_command_PWM);
+
     commandMotors(); // Sends command pulses to each motor pin using OneShot125 protocol
 
     // printRadioData(); //Radio pwm values (expected: 1000 to 2000)
@@ -1719,17 +1596,20 @@ void throttleCut()
     m5_command_PWM = 120;
     m6_command_PWM = 120;
 
-    // Uncomment if using servo PWM variables to control motor ESCs
-    // s1_command_PWM = 0;
-    // s2_command_PWM = 0;
-    // s3_command_PWM = 0;
-    // s4_command_PWM = 0;
-    // s5_command_PWM = 0;
-    // s6_command_PWM = 0;
-    // s7_command_PWM = 0;
   }
 }
 
+// Add this function to initialize LEDC for each motor
+void setupMotorPWM() {
+  // Configure LEDC pins with frequency and resolution, channels selected automatically
+  ledcAttach(m1Pin, LEDC_FREQ, LEDC_RESOLUTION); // Channel selected automatically
+  ledcAttach(m2Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m3Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m4Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m5Pin, LEDC_FREQ, LEDC_RESOLUTION);
+  ledcAttach(m6Pin, LEDC_FREQ, LEDC_RESOLUTION);
+}
+
 void calibrateMagnetometer()
 {
 #if defined USE_MPU9250_SPI
@@ -1964,28 +1844,6 @@ void printMotorCommands()
   }
 }
 
-void printServoCommands()
-{
-  if (current_time - print_counter > 10000)
-  {
-    print_counter = micros();
-    Serial.print(F("s1_command:"));
-    Serial.print(s1_command_PWM);
-    Serial.print(F(" s2_command:"));
-    Serial.print(s2_command_PWM);
-    Serial.print(F(" s3_command:"));
-    Serial.print(s3_command_PWM);
-    Serial.print(F(" s4_command:"));
-    Serial.print(s4_command_PWM);
-    Serial.print(F(" s5_command:"));
-    Serial.print(s5_command_PWM);
-    Serial.print(F(" s6_command:"));
-    Serial.print(s6_command_PWM);
-    Serial.print(F(" s7_command:"));
-    Serial.println(s7_command_PWM);
-  }
-}
-
 void printLoopRate()
 {
   if (current_time - print_counter > 10000)