Also noch mal zum mitschreiben:
Mein Problem ist, dass der Quadrocopter nicht in der Waage bleiben will, sondern immer Schwingneigungen auftreten.
Nun könnte es am PID liegen, an der Programmierung oder an den Sensoren.
Als Sesnor habe ich die MPU-6050 und benutze den Arduino Mega als Steuerung.
Für den PID habe ich schon ettliche Werte ausprobiert sei es P, I oder D, es treten immer Schwingneigungen auf.
Falls es am Code liegen sollte. Hier der Code:
#include <PIDCont.h>
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>
int arm = 15;
String incomingString;
PIDCont PIDroll, PIDpitch;
int Throttle = 42;
#define ROLL_PID_KP 0.5 //0.09
#define ROLL_PID_KI 0 //.2 //0.09
#define ROLL_PID_KD 0 //.01
#define ROLL_PID_MIN -10.0
#define ROLL_PID_MAX 10.0
Servo motors[4];
#define FRONTL 0
#define FRONTR 1
#define BACKL 2
#define BACKR 3
/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
void setup(){
Serial.begin(9600);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
// atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2? and then from radians to degrees
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
timer = micros();
// Kp, Ki, Kd Lval Hval
PIDroll.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
PIDpitch.ChangeParameters(ROLL_PID_KP,ROLL_PID_KI,ROLL_PID_KD,ROLL_PID_MIN,ROLL_PID_MAX);
motors[FRONTL].attach(2);
motors[FRONTR].attach(3);
motors[BACKL].attach(4);
motors[BACKR].attach(5);
setSpeed(FRONTL, arm);
setSpeed(FRONTR, arm);
setSpeed(BACKL, arm);
setSpeed(BACKR, arm);
delay(3000);
}
void loop(){
// If there is incoming value
if(Serial.available() > 0){
// read the value
char ch = Serial.read();
if (ch != 10){
incomingString += ch;
}
else{
// Convert the string to an integer
int valin = incomingString.toInt();
if(valin >=100){
valin = 100;}
if(valin <=30){
valin = 30;}
Serial.println(valin);
// Reset the value of the incomingString
Throttle = valin;
incomingString = "";
}
}
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
double gyroXrate = (double)gyroX / 131.0;
double gyroYrate = -((double)gyroY / 131.0);
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000);
timer = micros();
//Serial.print(kalAngleX);Serial.print(" ");Serial.println(kalAngleY);
kalAngleY = (179.2 - kalAngleY);
kalAngleX = (179.2 - kalAngleX);
int PIDroll_val= (int)PIDroll.Compute( (float)kalAngleY);
int PIDpitch_val= 0; //(int)PIDpitch.Compute((float)kalAngleX);
// yyy xxx
int m0_val=(Throttle+( PIDroll_val -PIDpitch_val)-8);
int m1_val=(Throttle+( -PIDroll_val +PIDpitch_val));
int m2_val=(Throttle+( -PIDroll_val -PIDpitch_val));
int m3_val=(Throttle+( PIDroll_val +PIDpitch_val));
setSpeed(FRONTL, m0_val);
setSpeed(BACKR, m1_val);
setSpeed(FRONTR, m2_val);
setSpeed(BACKL, m3_val);
//Serial.print(m0_val);Serial.print(" ");Serial.print(PIDroll_val);Serial.print(PIDpitch_val);Serial.print(" ");Serial.println(m1_val);
}
void setSpeed(int motor, int speed)
{
int fakeAngle = map(speed, 0, 100, 0, 180);
motors[motor].write(fakeAngle);
}
Gruß Moritz 