Hey guys,
i am curently working on a Arduino based RC Plane. And i want to integrate a Gyro stabilisation with the MPU6050. The Stablisation is just for the plans roll axis. I found a libary to read the angle from the MPU. Also i use the PID_V1 Libary for PID controll. My problem is that the PID Controller only works in one direktion. Only when the Gyro gives me negativ angles.
For testing my PID Setting are p=2, i= 0, d= 0.
The input is the MPU data from -180 to 180.
My Setpoint comes from my controller an is a Value between -40 and 40.
So when i put the joystick on my controller completly to the left the plane should be 40 degrees in the roll Axis.
Sorry for my bad english i hope you guys unterstoond my problem.
This is my code:
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>
#include <PID_v1.h>
#include <Wire.h>
#include <MPU6050_light.h>
#include <EEPROM.h>
// Variablen:
// Funk Ferbindunk
const uint64_t pipeIn = 000322;
const uint64_t pipeOut = 000422;
//PID Regler
double setPointRoll = 0;
double input;
double output;
double Kp = 2, Ki = 0, Kd = 0;
// Datenpackete:
// Datenpacket zur Fernebdinung
struct Back {
bool connection = false;
bool battery = false;
};
// Datenpacket von fernbedingung
struct Signal {
byte x1;
byte y1;
byte x2;
byte y2;
bool gyro;
bool setupMode;
};
//Objekte:
//PID Regler
PID rollPID(&input, &output, &setPointRoll, Kp, Ki, Kd, DIRECT);
//MPU
MPU6050 mpu(Wire);
//Servos
Servo servoRoll;
Servo servoPitch;
Servo servoMotor;
Servo servoYaw;
byte rolldrift;
byte pitchdrift;
byte yawdrift;
// Datenpackete
Back info;
Signal data;
//Funk
RF24 radio(8, 10);
// Setup
void setup() {
// Get drift from Eprom
if (EEPROM.read(2) == 255) {
rolldrift = 30;
} else {
rolldrift = EEPROM.read(2);
}
if (EEPROM.read(3) == 255) {
pitchdrift = 30;
} else {
pitchdrift = EEPROM.read(3);
}
if (EEPROM.read(4) == 255) {
yawdrift = 30;
} else {
yawdrift = EEPROM.read(4);
}
// Set Pins
pinMode(A7, INPUT);
//Seriele Verbinung(Testzwecke)
Serial.begin(9600);
// Set PID
rollPID.SetMode(AUTOMATIC);
rollPID.SetOutputLimits(-90, 90);
rollPID.SetTunings(Kp, Ki, Kd);
// Set Servos
servoRoll.attach(5, 500, 2500);
servoPitch.attach(9, 500, 2500);
servoYaw.attach(6, 500, 2500);
servoMotor.attach(3, 1020, 2000);
// Set Funk
radio.begin();
radio.setAutoAck(false);
radio.openReadingPipe(1, pipeIn);
radio.openWritingPipe(pipeOut);
radio.setDataRate(RF24_250KBPS);
radio.setPALevel(RF24_PA_MAX);
radio.startListening();
//Set MPU
Wire.begin();
mpu.begin();
Serial.println(F("Calculating gyro offset, do not move MPU6050"));
delay(1000);
mpu.calcGyroOffsets();
Serial.println("Done.");
//Setze auf Werkseinstellungen
resetData();
}
unsigned long lastRecvTime = 0;
// Schleife
void loop() {
setupMode();
checkBattery();
recvData();
sendData();
if (data.gyro) {
setServoGyro();
} else {
setServoManuel();
}
}
// Funktionen
void setupMode() {
bool changed = false;
while (data.setupMode) {
changed = true;
recvData();
int x1Change = map(data.x1, 0, 255, -100, 100);
int x2Change = map(data.x2, 0, 255, -100, 100);
int y2Change = map(data.y2, 0, 255, -100, 100);
if (abs(x1Change) > 50) {
if (x1Change < 0) {
x1Change = -1;
} else x1Change = 1;
} else x1Change = 0;
if (abs(x2Change) > 50) {
if (x2Change < 0) {
x2Change = -1;
} else x2Change = 1;
} else x2Change = 0;
if (abs(y2Change) > 50) {
if (y2Change < 0) {
y2Change = -1;
} else y2Change = 1;
} else y2Change = 0;
rolldrift = rolldrift + x2Change;
pitchdrift = pitchdrift + y2Change;
yawdrift = yawdrift + x1Change;
if (rolldrift < 0) rolldrift = 0;
if (rolldrift > 60) rolldrift = 60;
if (pitchdrift < 0) pitchdrift = 0;
if (pitchdrift > 60) pitchdrift = 60;
if (yawdrift < 0) yawdrift = 0;
if (yawdrift > 60) yawdrift = 60;
servoRoll.write(91 + map(rolldrift, 0, 60, -30, 30));
servoPitch.write(91 + map(pitchdrift, 0, 60, -30, 30));
servoYaw.write(91 + map(yawdrift, 0, 60, -30, 30));
delay(1000);
}
if (changed) {
EEPROM.write(2, rolldrift);
EEPROM.write(3, pitchdrift);
EEPROM.write(4, yawdrift);
}
}
void checkBattery() {
float batteryLife = (analogRead(A7) / 1023.0) * 5.0 * 3.0;
if (batteryLife < 11) {
info.battery = true;
}
}
//Setze werte auf Werkseinstellungen
void resetData() {
data.x1 = 128;
data.y1 = 0;
data.x2 = 128;
data.y2 = 128;
data.gyro = true;
data.setupMode = false;
}
// Daten von der fernebdinung bekommen
void recvData() {
while (radio.available()) {
radio.read(&data, sizeof(Signal));
lastRecvTime = millis();
}
}
// Daten zur Fernbeinung Senden
void sendData() {
radio.stopListening();
radio.write(&info, sizeof(Back));
radio.startListening();
delay(5);
}
// Set the Servos with Gyro
void setServoGyro() {
mpu.update();
float angle = mpu.getAngleX();
input = angle;
int controller = map(data.x2, 0, 255, -40, 40);
setPointRoll = controller;
rollPID.Compute();
Serial.println(mpu.getAngleX());
int rollFromPID = output;
servoRoll.write(90 + rollFromPID);
setServo();
}
// Set Servos Manuel
void setServoManuel() {
servoRoll.write(map(data.x2, 0, 255, 30, 150) + map(rolldrift, 0, 60, -30, 30));
setServo();
}
// Set Servos Allgemein
void setServo() {
//Setze Alle servos die immer Manuell gesteurt werden
servoPitch.write(map(data.y2, 0, 255, 30, 150) + map(pitchdrift, 0, 60, -30, 30));
servoYaw.write(map(data.x1, 0, 255, 30, 150) + map(yawdrift, 0, 60, -30, 30));
servoMotor.write(map(data.y1, 0, 255, 0, 180));
if (millis() - lastRecvTime > 3000) {
resetData();
}
}
Stablisation is here:
//PID Regler
double setPointRoll = 0;
double input;
double output;
double Kp = 2, Ki = 0, Kd = 0;
//Objekte:
//PID Regler
PID rollPID(&input, &output, &setPointRoll, Kp, Ki, Kd, DIRECT);
// PID Setup
// Set PID
rollPID.SetMode(AUTOMATIC);
rollPID.SetOutputLimits(-90, 90);
rollPID.SetTunings(Kp, Ki, Kd);
// Set the Servos with Gyro
void setServoGyro() {
mpu.update();
float angle = mpu.getAngleX();
input = angle;
int controller = map(data.x2, 0, 255, -40, 40);
setPointRoll = controller;
rollPID.Compute();
Serial.println(mpu.getAngleX());
int rollFromPID = output;
servoRoll.write(90 + rollFromPID);
setServo();
}