From 1519d1adad7670d6bad9cd39f0b445ae2af73cff Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Thu, 8 May 2025 15:26:33 +0200 Subject: [PATCH] trashed all the servo stuff --- src/drone/src/main.cpp | 103 +++-------------------------------------- 1 file changed, 7 insertions(+), 96 deletions(-) diff --git a/src/drone/src/main.cpp b/src/drone/src/main.cpp index 8d8e68b..093203e 100644 --- a/src/drone/src/main.cpp +++ b/src/drone/src/main.cpp @@ -1,7 +1,6 @@ #include #include #include "SparkFun_BNO080_Arduino_Library.h" -#include #include #include #include @@ -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) - // 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 @@ -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)