Hello, I recently created my own Arduino drone and remote, and I am having problems with stabilizing it for takeoff. I tried using the PID formula in correspondence with the MPU6050 library by Jeff Rowberg, and my drone takes off in a similar, reckless fashion as it did without it. I haven't implemented any of my remote's pitch, yaw, and roll controls yet as I'm first trying to get the thing to take off with only a throttle input from the remote.
Is there anything wrong with my code that is preventing from my drone from properly stabilizing itself, and if so what can I do to fix it?
#include <RF24.h>
#include <nRF24L01.h>
#include <Servo.h>
#include <MPU6050.h>
#include <Wire.h>
#include <SPI.h>
struct dataPackage
{
byte throttleYValue;
byte throttleState;
byte yawXValue;
byte rollXValue;
byte pitchYValue;
byte throttlePeakValue;
byte headingValue;
};
dataPackage data;
void resetData()
{
data.throttleYValue = 0;
data.yawXValue = 121;
data.throttleState = false;
data.rollXValue = 124;
data.pitchYValue = 127;
data.throttlePeakValue = 0;
data.headingValue = 0;
}
unsigned long lastReceiveTime = 0;
unsigned long currentTime = 0;
RF24 radio(8, 10); // CE, CSN
MPU6050 mpu;
Servo motor1, motor2, motor3, motor4;
// PID Constants
float Kp = 1.5;
float Ki = 0.2;
float Kd = 0.1;
// Target angles
float targetRoll = 0.0;
float targetPitch = 0.0;
// PID Variables
float previousRollError = 0.0;
float previousPitchError = 0.0;
float integralRollError = 0.0;
float integralPitchError = 0.0;
void setup() {
motor1.attach(5, 1000, 2000);
motor2.attach(3, 1000, 2000);
motor3.attach(9, 1000, 2000);
motor4.attach(6, 1000, 2000);
const byte address[6] = "00001";
radio.begin();
radio.openReadingPipe(0, address);
radio.setAutoAck(false);
radio.setDataRate(RF24_250KBPS);
radio.setPALevel(RF24_PA_MAX);
radio.startListening();
Wire.begin();
mpu.initialize();
calibrateMPU6050();
delay(1000);
}
void loop() {
currentTime = millis();
if (currentTime - lastReceiveTime > 1000)
resetData();
if (radio.available())
{
radio.read(&data, sizeof(dataPackage));
/*Serial.println("1|X" + String(data.yawXValue) + ":Y" + String(data.throttleYValue) + ":P" + String(data.throttlePeakValue));
Serial.println("2|X" + String(data.rollXValue) + ":Y" + String(data.pitchYValue) + ":H" + String(data.headingValue));
Serial.println("throttleState:" + String(data.throttleState) + '\n');*/
lastReceiveTime = millis();
}
if (data.throttleState == true)
{
if(data.throttleYValue <= 124){
motor1.writeMicroseconds(1065);
motor2.writeMicroseconds(1065);
motor3.writeMicroseconds(1065);
motor4.writeMicroseconds(1065);
}
else{
float roll = getRoll();
float pitch = getPitch();
float rollError = targetRoll - roll;
float pitchError = targetPitch - pitch;
float rollCorrection = calculatePID(rollError, previousRollError, integralRollError);
float pitchCorrection = calculatePID(pitchError, previousPitchError, integralPitchError);
int throttleMax = map(data.throttlePeakValue, 0, 255, 1500, 2000);
int desiredSpeed = map(data.throttleYValue, 124, 255, 1000, throttleMax);
// Calculate motor speeds
int motorSpeed1 = desiredSpeed + rollCorrection + pitchCorrection;
int motorSpeed2 = desiredSpeed - rollCorrection + pitchCorrection;
int motorSpeed3 = desiredSpeed - rollCorrection - pitchCorrection;
int motorSpeed4 = desiredSpeed + rollCorrection - pitchCorrection;
// Set motor speeds
motor1.writeMicroseconds(motorSpeed1);
motor2.writeMicroseconds(motorSpeed2);
motor3.writeMicroseconds(motorSpeed3);
motor4.writeMicroseconds(motorSpeed4);
// Update PID variables
previousRollError = rollError;
previousPitchError = pitchError;
integralRollError += rollError;
integralPitchError += pitchError;
}
}
else{
motor1.writeMicroseconds(0);
motor2.writeMicroseconds(0);
motor3.writeMicroseconds(0);
motor4.writeMicroseconds(0);
}
}
void calibrateMPU6050() {
int calibrateIterations = 2000;
float rollTotal = 0.0;
float pitchTotal = 0.0;
for (int i = 0; i < calibrateIterations; i++) {
float roll = getRoll();
float pitch = getPitch();
rollTotal += roll;
pitchTotal += pitch;
delay(3);
}
targetRoll = rollTotal / calibrateIterations;
targetPitch = pitchTotal / calibrateIterations;
}
float getRoll() {
int16_t accX = mpu.getAccelerationX();
int16_t accY = mpu.getAccelerationY();
int16_t accZ = mpu.getAccelerationZ();
float roll = atan2(accY, accZ) * RAD_TO_DEG;
return roll;
}
float getPitch() {
int16_t accX = mpu.getAccelerationX();
int16_t accY = mpu.getAccelerationY();
int16_t accZ = mpu.getAccelerationZ();
float pitch = atan2(accX, accZ) * RAD_TO_DEG;
return pitch;
}
float calculatePID(float error, float previousError, float integralError) {
float proportionalTerm = Kp * error;
float derivativeTerm = Kd * (error - previousError);
float integralTerm = Ki * integralError;
float output = proportionalTerm + derivativeTerm + integralTerm;
return output;
}
Thanks for taking the time to read my question, and I appreciate any feedback and responses.