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:
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;
}
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!
Lesezeichen