I use the PID Libary from this site, but i think i have problems understanding it. there is an input whith is normally a Sensor Value, a Setpoint which is the value i want the sensorvalue to be and a the output which has impact on the sensor. Moreover there are kp,ki,and kd which representate the behaviour of the PID. If i'm not wrong, the output should be Input-Setpoint if kp=1 and the others are 0. but in my case every thing the output is 0 in Direct mode and 255 in Reverse mode. Do i understand somthing wrong?
double YPR[3];
//-------------------- Flight controll --------------------------------------
double rollPID[3]={0},pitchPID[3]={0},yawPID[3]={0};
double zeroYPR[3]={180};
double wantedYPR[3]={-180};
double trimYPR[3]={0};
double setYPR[3]={0};
double trottle=0;
PID PID_ROLL(&YPR[2], &setYPR[2], &wantedYPR[2], 0.1, 0, 0, DIRECT);
PID PID_PITCH(&YPR[1], &setYPR[1], &wantedYPR[1], pitchPID[0], pitchPID[1], pitchPID[2], DIRECT);
PID PID_YAW(&YPR[0], &setYPR[0], &wantedYPR[0], yawPID[0], yawPID[1], yawPID[2], DIRECT);
void setup()
{
Serial.begin(115200); // Debugging only
if (!driver.init())
Serial.println("init failed");
DMPInit();
Serial.println("Sucessful");
PIDInit();
}
void loop()
{
if (!dmpReady) return;
do
{
if (driver.recv(buf, &buflen)) // Non-blocking
{
switch(buf[0])
{
case CODE_CONTROLL:
wantedYPR[0]+=zeroYPR[0]+map(buf[1],0,255,-MAX_ANGLE_PR,MAX_ANGLE_PR)+trimYPR[0];
for(int i=1;i<=2;i++){
wantedYPR[i]=zeroYPR[i]+map(buf[i+1],0,255,-MAX_ANGLE_PR,MAX_ANGLE_PR)+trimYPR[i];
}
trottle=buf[4];
break;
case CODE_ROLL_PID:
rollPID[0]=(double)buf[1]+((double)buf[2])/100;
rollPID[1]=(double)buf[3]+((double)buf[4])/100;
rollPID[2]=(double)buf[5]+((double)buf[6])/100;
PIDRetune();
break;
case CODE_PITCH_PID:
pitchPID[0]=(double)buf[1]+((double)buf[2])/100;
pitchPID[1]=(double)buf[3]+((double)buf[4])/100;
pitchPID[2]=(double)buf[5]+((double)buf[6])/100;
PIDRetune();
break;
case CODE_YAW_PID:
yawPID[0]=(double)buf[1]+((double)buf[2])/100;
yawPID[1]=(double)buf[3]+((double)buf[4])/100;
yawPID[2]=(double)buf[5]+((double)buf[6])/100;
PIDRetune();
break;
case CODE_CONFIG_ZERO:
for(int i=0;i<=2;i++){
zeroYPR[i]=YPR[i];
}
break;
}
}
}while (!mpuInterrupt && fifoCount < packetSize);
DMPupdate();
Serial.print(PID_ROLL.Compute());
Serial.print(" , ");
Serial.print(YPR[2]);
Serial.print(" , ");
Serial.print(setYPR[2]);
Serial.print(" , ");
Serial.println(PID_ROLL.GetMode());
}
void PIDCompute(){
PID_ROLL.Compute();
PID_PITCH.Compute();
PID_YAW.Compute();
}
void PIDRetune(){
PID_ROLL.SetTunings(rollPID[0], rollPID[1], rollPID[2]);
PID_PITCH.SetTunings(pitchPID[0], pitchPID[1], pitchPID[2]);
PID_YAW.SetTunings(yawPID[0], yawPID[1], yawPID[2]);
}
void PIDInit(){
PID_ROLL.SetSampleTime(10);
PID_PITCH.SetSampleTime(10);
PID_YAW.SetSampleTime(10);
PID_ROLL.SetMode(AUTOMATIC);
PID_PITCH.SetMode(AUTOMATIC);
PID_YAW.SetMode(AUTOMATIC);
}