trashed all the servo stuff

src/drone/src/main.cpp

@@ -1,7 +1,6 @@
 #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>
@@ -245,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;
 
 //========================================================================================================================//
 
@@ -324,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;
@@ -365,7 +349,6 @@ void printMagData();
 void printRollPitchYaw();
 void printPIDoutput();
 void printMotorCommands();
-void printServoCommands();
 void printLoopRate();
 float invSqrt(float x);
 void controlMixer();
@@ -389,12 +372,7 @@ void setup()
   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);
 
@@ -477,10 +446,9 @@ void loop()
   // 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)
+  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)
+  // 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)
   // printLoopRate();      //Prints the time between loops in microseconds (expected: microseconds between loop iterations)
   // printDebugInfo(); //PWM 1 and 5 + Armed condition
 
@@ -508,13 +476,7 @@ 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
@@ -1352,22 +1314,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()
@@ -1562,13 +1509,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)
@@ -1671,14 +1612,6 @@ 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;
   }
 }
 
@@ -1927,28 +1860,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)