Da ich mich ja auch mit dem Thema befasse kann ich dir diese Seite mal empfehlen Improving the Beginner’s PID – Introduction « Project Blog du kannst auch die PID lib vom Arduino nehmen.
Hier ein Beispiel:
#include <PIDCont.h>
#include <Wire.h>
#include "Kalman.h"
#include <Servo.h>
int arm = 15;
String incomingString;
PIDCont PIDroll, PIDpitch;
int Throttle = 30;
#define ROLL_PID_KP 0.08 //0.09
#define ROLL_PID_KI 0 //.2 //0.09
#define ROLL_PID_KD 0.002
#define ROLL_PID_MIN -15.0
#define ROLL_PID_MAX 15.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();
kalAngleY = (179.2 - kalAngleY);
kalAngleX = (179.2 - kalAngleX);
int PIDroll_val= (int)PIDroll.Compute( (float)kalAngleY);
int PIDpitch_val= (int)PIDpitch.Compute((float)kalAngleX);
// yyy xxx
int m0_val=(Throttle+( PIDroll_val -PIDpitch_val));
int m1_val=(7+Throttle+( -PIDroll_val +PIDpitch_val));
int m2_val=(2+Throttle+( -PIDroll_val -PIDpitch_val));
int m3_val=(2+Throttle+( PIDroll_val +PIDpitch_val));
setSpeed(FRONTL, m0_val);
setSpeed(BACKR, m1_val);
setSpeed(FRONTR, m2_val);
setSpeed(BACKL, m3_val);
}
void setSpeed(int motor, int speed)
{
int fakeAngle = map(speed, 0, 100, 0, 180);
motors[motor].write(fakeAngle);
}
Mit dem Kalman-Filter werden die Rohdaten vom Accelerometer und Gyro zusammengefasst und ausgebessert, da ja auch das Acceleromter abdriftet je länger es arbeitet und der Gyro nicht Schockresistent ist.