Hier ist mein Programm. Es ist soweit kommentiert, dass jeder damit klar kommen sollte.
Code:
/*******************************************************************************
*
* Description: Asuro Linienverfolgung mit PID-Regler
* Version 1: Korrektur auf beide Motoren verteilt
* Autor: Waste 26.8.05
*
*****************************************************************************/
#include "asuro.h"
#include <stdlib.h>
unsigned char speed;
int speedLeft,speedRight;
unsigned int lineData[2];
int x, xalt, don, doff, kp, kd, ki, yp, yd, yi, drest, y, y2, isum;
void FollowLine (void)
{
unsigned char leftDir = FWD, rightDir = FWD;
FrontLED(OFF);
LineData(lineData); // Messung mit LED OFF
doff = (lineData[0] - lineData[1]); // zur Kompensation des Umgebungslicht
FrontLED(ON);
LineData(lineData); // Messung mit LED ON
don = (lineData[0] - lineData[1]);
x = don - doff; // Regelabweichung
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 berechnen und mit
yd += drest; // nicht berücksichtigtem Rest addieren
if (yd > 255) drest = yd - 255; // merke 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);
yp = x*kp; // P-Anteil berechnen
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) { // nach rechts
StatusLED(GREEN);
speedLeft = speed + y2; // links beschleunigen
if (speedLeft > 255) {
speedLeft = 255; // falls Begrenzung
y2 = speedLeft - speed; // dann Rest rechts berücksichtigen
}
y = y - y2;
speedRight = speed - y; // rechts abbremsen
if (speedRight < 0) {
speedRight = 0;
}
}
if ( y < 0) { // nach links
StatusLED(RED);
speedRight = speed - y2; // rechts beschleunigen
if (speedRight > 255) {
speedRight = 255; // falls Begrenzung
y2 = speed - speedRight; // dann Rest links berücksichtigen
}
y = y - y2;
speedLeft = speed + y; // links abbremsen
if (speedLeft < 0) {
speedLeft = 0;
}
}
leftDir = rightDir = FWD;
if (speedLeft < 20) leftDir = BREAK; // richtig bremsen
if (speedRight < 20) rightDir = BREAK;
MotorDir(leftDir,rightDir);
MotorSpeed(abs(speedLeft),abs(speedRight));
}
int main(void)
{
unsigned char sw;
Init();
MotorDir(FWD,FWD);
StatusLED(GREEN);
speed = 150;
kp = 3; ki = 10; kd = 70; // Regler Parameter kd enthält bereits Division durch dt
sw = PollSwitch();
if (sw & 0x01)
{ki=20;}
if (sw & 0x02)
{speed = 200;}
if (sw & 0x04)
speed = 100;
if (sw & 0x08)
kd = 35;
if (sw & 0x10)
kd = 70;
if (sw & 0x20)
kd = 140;
FrontLED(ON);
LineData(lineData);
speedLeft = speedRight = speed;
while(1){
FollowLine();
}
return 0;
}
Zitat von waste
Zitieren
Lesezeichen