Hi!
I am building a drone with theese parts:
- MPU9250
- Arduino Pro Mini 3.3v
- NRF24 with Antenna
- 20A UBEC ESC
- Proporive 900KV motors
- 3s 5000mAh 11.1V battery
Everything seems to work "okey" but the problem is the PID, I have read a lot on how to setup a PID so I think mine looks pretty ok, but I cannot manage to propperly calibrate it, every page Ive looked up on how to calibrate it it says that you should set the Derivative first till it goes crazy, then back 25% at least till it runs smooth, then you should gain the Prospect (gain) with +0.2 every step. But here is where it goes crazy, it just goes back and forward all crazy. Here is my code:
#include <MPU9250.h>
#include <Servo.h>
#include <nRF24L01.h>
#include <SPI.h>
#include <RF24.h>
#include <printf.h>
#define CE_PIN 9
#define CSN_PIN 10
// Create Radio and define pins
RF24 radio(CE_PIN, CSN_PIN);
// Create Radio channels
const uint64_t pAddress = 0xB00B1E5000LL;
// Create a boolean for checkConnectionLost()
bool lostConnection = false;
//Radio recived array
int recivedDataArray[4];
// Create timer
float elapsedTime, time, timePrev;
long loop_timer;
float previousMessageMillis;
Servo motor_LF;
Servo motor_RF;
Servo motor_LB;
Servo motor_RB;
MPU9250 mpu;
int input_throttle = 1000;
int input_yaw = 0;
int input_pitch = 0;
int input_roll = 0;
/* Roll PID */
float roll_PID, pid_throttle_L_F, pid_throttle_L_B, pid_throttle_R_F, pid_throttle_R_B, roll_error, roll_previous_error;
float roll_pid_p = 0;
float roll_pid_i = 0;
float roll_pid_d = 0;
/* Roll PID Constants */
double roll_kp = 1.2;
double roll_ki = 0;
double roll_kd = 30;
float roll_desired_angle = 0;
/* Pitch PID */
float pitch_PID, pitch_error, pitch_previous_error;
float pitch_pid_p = 0;
float pitch_pid_i = 0;
float pitch_pid_d = 0;
/* Pitch PID Constants */
double pitch_kp = roll_kp;
double pitch_ki = roll_ki;
double pitch_kd = roll_kd;
float pitch_desired_angle = 0;
void imuSetup() {
if (!mpu.setup(0x68)) { // change to your own address
while (1) {
Serial.println("MPU connection failed. Please check your connection with `connection_check` example.");
delay(5000);
}
}
}
void setup() {
Serial.begin(115200);
while (!Serial);
printf_begin();
imuSetup();
if (!radio.begin()) {
Serial.println(F("radio hardware is not responding!!"));
while (1) {}
}
radio.setPALevel(RF24_PA_MAX);
radio.openReadingPipe(1, pAddress);
radio.printDetails();
radio.startListening();
delay(1000);
motor_LF.attach(5);
motor_RF.attach(6);
motor_LB.attach(7);
motor_RB.attach(8);
motor_LF.writeMicroseconds(1000);
motor_RF.writeMicroseconds(1000);
motor_LB.writeMicroseconds(1000);
motor_RB.writeMicroseconds(1000);
}
void loop() {
if (radio.available()) {
radio.read( &recivedDataArray, sizeof(recivedDataArray) );
//Serial.println("Recieved array:");
previousMessageMillis = millis();
for (byte i = 0; i < 4; i++) {
//Serial.println(recivedDataArray[i]);
}
input_throttle = recivedDataArray[0];
input_yaw = recivedDataArray[1];
pitch_desired_angle = -recivedDataArray[2];
roll_desired_angle = recivedDataArray[3];
//Serial.println();
}
if (input_throttle > 1600) {
input_throttle = 1600;
}
PIDControl();
/* Start Timer */
timePrev = time;
time = millis();
elapsedTime = (time - timePrev) / 1000;
if (millis() - previousMessageMillis >= 400) {
Serial.println("Connection Lost- Reseting PID");
PID_reset();
motor_LF.writeMicroseconds(1000);
motor_RF.writeMicroseconds(1000);
motor_LB.writeMicroseconds(1000);
motor_RB.writeMicroseconds(1000);
} else {
motor_LF.writeMicroseconds(pid_throttle_L_F);
motor_RF.writeMicroseconds(pid_throttle_R_F);
motor_LB.writeMicroseconds(pid_throttle_L_B);
motor_RB.writeMicroseconds(pid_throttle_R_B);
}
//print_roll_pitch_yaw();
print_pid();
}
void print_roll_pitch_yaw() {
if (mpu.update()) {
Serial.print("Yaw, Pitch, Roll: ");
Serial.print(mpu.getYaw(), 2);
Serial.print(", ");
Serial.print(mpu.getPitch() + 20, 2);
Serial.print(", ");
Serial.println(mpu.getRoll() + 44, 2);
}
}
void print_pid() {
/*
Serial.print("ROLL: ");
Serial.println(roll_PID);
Serial.print("PITCH: ");
Serial.println(pitch_PID);
*/
Serial.print("LF: ");
Serial.println(pid_throttle_L_F);
Serial.print("RF: ");
Serial.println(pid_throttle_R_F);
Serial.print("LB: ");
Serial.println(pid_throttle_L_B);
Serial.print("RB: ");
Serial.println(pid_throttle_R_B);
}
void PIDControl() {
if (input_throttle > 1000) {
if (mpu.update()) {
/* PID */
roll_error = roll_desired_angle + mpu.getRoll() + 44;
pitch_error = pitch_desired_angle + mpu.getPitch() + 20;
/* Prospect */
roll_pid_p = roll_kp * roll_error;
pitch_pid_p = pitch_kp * pitch_error;
/* Integral */
if (-3 < roll_error < 3)
{
roll_pid_i = roll_pid_i + (roll_ki * roll_error);
}
if (-3 < pitch_error < 3)
{
pitch_pid_i = pitch_pid_i + (pitch_ki * pitch_error);
}
/* Derivate OLD */
roll_pid_d = roll_kd * (roll_error - roll_previous_error);
pitch_pid_d = pitch_kd * (pitch_error - pitch_previous_error);
/* PID summery */
roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
/* Regulate PID output for ESCs */
if (roll_PID < -300) {
roll_PID = -300;
}
if (roll_PID > 300) {
roll_PID = 300;
}
if (pitch_PID < -300) {
pitch_PID = -300;
}
if (pitch_PID > 300) {
pitch_PID = 300;
}
/* Set the throttle PID for each motor */
pid_throttle_L_F = input_throttle + roll_PID + pitch_PID;
pid_throttle_R_F = input_throttle - roll_PID + pitch_PID;
pid_throttle_L_B = input_throttle + roll_PID - pitch_PID;
pid_throttle_R_B = input_throttle - roll_PID - pitch_PID;
/*
Serial.print("ROLL: ");
Serial.println(roll_PID);
Serial.print("PITCH: ");
Serial.println(pitch_PID);
*/
/* Regulate throttle for ESCs */
//Right front
if (pid_throttle_R_F < 1100)
{
pid_throttle_R_F = 1100;
}
if (pid_throttle_R_F > 2000)
{
pid_throttle_R_F = 2000;
}
//Left front
if (pid_throttle_L_F < 1100)
{
pid_throttle_L_F = 1100;
}
if (pid_throttle_L_F > 2000)
{
pid_throttle_L_F = 2000;
}
//Right back
if (pid_throttle_R_B < 1100)
{
pid_throttle_R_B = 1100;
}
if (pid_throttle_R_B > 2000)
{
pid_throttle_R_B = 2000;
}
//Left back
if (pid_throttle_L_B < 1100)
{
pid_throttle_L_B = 1100;
}
if (pid_throttle_L_B > 2000)
{
pid_throttle_L_B = 2000;
}
/* Save Previous Error */
roll_previous_error = roll_error;
pitch_previous_error = pitch_error;
}
}
else {
PID_reset();
}
}
void PID_reset() {
pid_throttle_L_F = 1000;
pid_throttle_L_B = 1000;
pid_throttle_R_F = 1000;
pid_throttle_R_B = 1000;
roll_error = 0;
pitch_error = 0;
roll_previous_error = 0;
pitch_previous_error = 0;
}
An here is my schematic (not beautiful 
)
If anyone has any tips or tricks it would be awesome! Thank you so much beforehand!
Best regards Max