Smikkelbakje/Drone-FabAcademy-2025
1
0
Fork 0
mirror of https://gitlab.waag.org/make/fablab/interns/2025/sam.git synced 2025-08-03 11:54:58 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

trashed all the servo stuff

Browse Source
This commit is contained in:
Sam Hos
2025-05-08 15:26:33 +02:00
parent cd215c317c
commit 1519d1adad
1 changed files with 7 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
src/drone/src/main.cpp
View File

@@ -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)
// 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)