Es geht um zwei Dinge, die mir Probleme bereiteten:
a) die Erfassung der Liniendaten
b) die richtigen Parameter für PID, also Kp, Ki und Kd
Mit folgendem abgeänderten Programm kam ich bisher gut voran:
Allerdings habe ich es nur bei langsamer Geschwindigkeit geschafft, die gesamte Testarena sauber abzufahren. Das Feintuning ist viel schwieriger, als ich ursprünglich dachte. Eine Ochsentour!Code:#include "asuro.h" #include <stdlib.h> #define SPEED 150 #define SPEEDMAX 230 #define SPEEDMIN 30 unsigned char speed, j; int speedLeft,speedRight; unsigned int lineData[2]; int x, xalt, kp, kd, ki, yp, yd, yi, drest=0, y, y2, isum=0, ADOffset; void FollowLine (void) { unsigned char leftDir = FWD, rightDir = FWD; LineData(lineData); x = (lineData[LEFT] - lineData[RIGHT]) - ADOffset; // Regelabweichung yp = x*kp; // P-Anteil isum += x; if (isum > 16000) isum = 16000; //Begrenzung um Überlauf zu vermeiden if (isum < -16000) isum = -16000; yi = isum / 625 * ki; //I-Anteil berechnen yd = (x - xalt) * kd; // D-Anteil yd += drest; // nicht berücksichtigten Rest addieren if (yd > 255) { drest = yd - 255; // Rest } else if (yd < -255) { drest = yd + 255; } else { drest = 0; } if (isum > 15000) BackLED(OFF,ON); // nur zur Diagnostik else if (isum < -15000) BackLED(ON,OFF); else BackLED(OFF,OFF); y = yp + yi + yd; // Gesamtkorrektur y2 = y / 2; // Aufteilung auf beide Motoren xalt = x; // x merken speedLeft = speedRight = speed; MotorDir(FWD,FWD); if ( y > 0) { StatusLED(GREEN); speedLeft = speed + y2; // links beschleunigen if (speedLeft > SPEEDMAX) { speedLeft = SPEEDMAX; // falls Begrenzung y2 = speedLeft - speed; // dann Rest rechts berücksichtigen } y = y - y2; speedRight = speed - y; // rechts abbremsen if (speedRight < SPEEDMIN) { speedRight = SPEEDMIN; } } if ( y < 0) { StatusLED(RED); speedRight = speed - y2; // rechts beschleunigen if (speedRight > SPEEDMAX) { speedRight = SPEEDMAX; // falls Begrenzung y2 = speed - speedRight; // dann Rest links berücksichtigen } y = y - y2; speedLeft = speed + y; // links abbremsen if (speedLeft < SPEEDMIN) { speedLeft = SPEEDMIN; } } leftDir = rightDir = FWD; if (speedLeft < SPEEDMIN + 5) leftDir = BREAK; // richtig bremsen if (speedRight < SPEEDMIN + 5) rightDir = BREAK; MotorDir(leftDir,rightDir); MotorSpeed(abs(speedLeft),abs(speedRight)); } int main(void) { Init(); FrontLED(ON); for (j = 0; j < 255; j++) LineData(lineData); LineData(lineData); ADOffset = lineData[LEFT] - lineData[RIGHT]; // Helligkeitsunterschied links und rechts MotorDir(FWD,FWD); StatusLED(GREEN); speed = SPEED; speedLeft = speedRight = speed; kp = 10; ki = 4; kd = 70; // Regler Parameter kd enthält bereits Division durch dt /* 10(!) 4(!) 70(!) */ /*<=== gute Werte*/ while(1) { FollowLine(); } return 0; }
Lesezeichen