Hello friends,
I have a problem with my degree project. The thing is that I have to stabilize and control a quadcopter in the Roll angle. For that I´m currently using a MPU6050 sensor, a GY-511 magnetometer, a RF24 and the controller which is an Arduino Nano.
Along with that I have been using several libraries to do the control part; the I2Cdev, LSM303, PIDController, MPU6050_tockn, Wire and AsyncServoLib. For the control part I used 3 different PID´s to every single one of the angles (Roll, Pitch and Yaw).
The main problem is that when I turn on the brushless motors at high speeds the arduino becomes very slow and refresh the data of the sensors after almost 10 seconds in some cases. When the velocities are cero or very slow it goes very fast and keep updating the values every 100 mili seconds.
I have erased all the Serial.print´s that the code has and the issue still.
This is the code, hope anyone can help me with this.
#include <I2Cdev.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <LSM303.h>
#include <PIDController.h>
/// MOTORES ///
#include <AsyncServoLib.h>
AsyncServo motor1;
AsyncServo motor2;
AsyncServo motor3;
AsyncServo motor4;
int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180/3.141592654;
float X,Y,Z;
PIDController pidP;
PIDController pidR;
PIDController pidY;
/// RF ///
#include <SPI.h>
#include <RF24.h>
RF24 radio(7, 8); // CE, CSN
const byte ID[6] ="00001";
LSM303 cmps;
float PID_Pitch, PID_Roll, PID_Yaw, pwm1, pwm2, pwm3, pwm4, errorPitch, previous_error_pitch, errorRoll, errorYaw, previous_error_roll, previous_error_yaw;
/// GANANCIAS ///
//YAW
double kp=0; //3.55
double ki=0; //3
double kd=0;
//PITCH
double kp1=0; //7
double ki1=0; //4
double kd1=0; //4
//ROLL
double kp2=40; //3000
double ki2=0.055; //15
double kd2=.5; //50
float desiredPitch = 0, desiredRoll = 10, desiredYaw = 0, desiredHeight = 0;
float throat1 = 655; // Constantes de Eje Ultrasónico
float throat2 = 657;
float throat3 = 659;
float throat4 = 660;
//float throat1 = 455; // Constantes de Eje Ultrasónico
//float throat2 = 457;
//float throat3 = 459;
//float throat4 = 460;
float timer;
void setup() {
//Serial.begin(9600);
Wire.begin();
cmps.init();
cmps.enableDefault();
cmps.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
cmps.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
/// INICIALIZACIÓN MOTORES ///
// Initialize comunication
Wire.beginTransmission(0x68); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0); // Make reset - place a 0 into the 6B register
delay(1500);
Wire.endTransmission(true);
timer = millis();
motor1.Attach(6);
motor2.Attach(9);
motor4.Attach(5);
motor3.Attach(10);
delay(1000);
motor1.SetOutput(1000,1500,2000);
motor2.SetOutput(1000,1500,2000);
motor4.SetOutput(1000,1500,2000);
motor3.SetOutput(1000,1500,2000);
delay(1000);
motor1.write(0);
motor2.write(0);
motor3.write(0);
motor4.write(0);
delay(1000);
pidP.begin();
pidP.setpoint(desiredPitch);
pidP.tune(kp1, ki1, kd1);
pidP.limit(-500, 700);
pidR.begin();
pidR.setpoint(desiredRoll);
pidR.tune(kp2, ki2, kd2);
pidR.limit(-500, 700);
pidY.begin();
pidY.setpoint(desiredYaw);
pidY.tune(kp, ki, kd);
pidY.limit(-500, 700);
radio.begin();
radio.openReadingPipe(0,ID);
radio.setPALevel(RF24_PA_MIN);
radio.startListening();
}
void loop() {
/* Update all the values */
if (radio.available()){
int interr;
radio.read(&interr,sizeof(interr));
cmps.read();
timePrev = timer; // the previous time is stored before the actual time read
timer = millis(); // actual time read
elapsedTime = (timer - timePrev) / 1000;
//Serial.println(elapsedTime);
float heading = cmps.heading();
Wire.beginTransmission(0x68);
Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(0x68, 6, true);
Acc_rawX=Wire.read()<<8|Wire.read(); //each value needs two registres
Acc_rawY=Wire.read()<<8|Wire.read();
Acc_rawZ=Wire.read()<<8|Wire.read();
//x
Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
//y
Acceleration_angle[1] = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
Wire.beginTransmission(0x68);
Wire.write(0x43); //Gyro data first adress
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true);
Gyr_rawX=Wire.read()<<8|Wire.read(); //Once again we shif and sum
Gyr_rawY=Wire.read()<<8|Wire.read();
//---X---/
Gyro_angle[0] = Gyr_rawX/131.0;
//---Y---/
Gyro_angle[1] = Gyr_rawY/131.0;
//---X axis angle---/
Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
//---Y axis angle---/
Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
Y=Total_angle[0];
X=Total_angle[1];
//Z=round((0.02*yaw)+(0.98*heading));
PID_Roll = pidR.compute(X);
PID_Pitch = pidP.compute(Y);
PID_Yaw = pidY.compute(Z);
pwm4 = round(throat1 - PID_Roll - PID_Pitch + PID_Yaw); //pwm <! 50>!700
pwm3 = round(throat2 - PID_Roll + PID_Pitch - PID_Yaw); //pwm <! 50 >!700
pwm2 = round(throat3 + PID_Roll + PID_Pitch + PID_Yaw); //pwm <! 50>!700
pwm1 = round(throat4 + PID_Roll - PID_Pitch - PID_Yaw); //pwm <! 50 >!70
//
// if (pwm1 <= 100){ //antes 300
// pwm1 = 100;
// }
// if (pwm1 > 600){ // antes 700
// pwm1 = 600;
// }
// if (pwm2 <= 100){
// pwm2 = 100;
// }
// if (pwm2 > 600){
// pwm2 = 600;
// }
// if (pwm3 <= 100){
// pwm3 = 100;
// }
// if (pwm3 > 600){
// pwm3 = 600;
// }
// if (pwm4 <= 100){
// pwm4 = 100;
// }
// if (pwm4 > 600){
// pwm4 = 600;
// }
if (pwm1 <= 300){ //antes 300
pwm1 = 300;
}
if (pwm1 > 700){ // antes 700
pwm1 = 700;
}
if (pwm2 <= 300){
pwm2 = 300;
}
if (pwm2 > 700){
pwm2 = 700;
}
if (pwm3 <= 300){
pwm3 = 300;
}
if (pwm3 > 700){
pwm3 = 700;
}
if (pwm4 <= 300){
pwm4 = 300;
}
if (pwm4 > 700){
pwm4 = 700;1
}
if ( interr == HIGH){
motor1.write(pwm1);
motor2.write(pwm2);
motor3.write(pwm3);
motor4.write(pwm4);
//Serial.print("Angulo Roll ");
//Serial.println(X);
//Serial.println(PID_Roll);
}
if (interr==LOW)
{
motor1.write(0);
motor2.write(0);
motor3.write(0);
motor4.write(0);
//Serial.println(interr);
}
}
}