Asuro: Linienfolger mit PD-Regler

[waste]

Fortsetzung:
Hier ist mein Programm. Es ist soweit kommentiert, dass jeder damit klar kommen sollte.

/*******************************************************************************
*
* 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;
}

Ergänzende Hinweise:
Ich habe den Differenzierbeiwert kd gleich durch dt geteilt, damit ich mit Integer rechnen kann.
kd/dt = 0.225/0.0016 = 140
Mit diesem Wert zittert (schwingt) der Asuro leicht. Als gute Werte haben sich 40 bis 70 herausgestellt. Da fährt der Asuro schön stabil. In der main Routine kann man die Parameter verändern, wenn man nach dem Einschalten des Asuros eine der Tasten drückt bis die Motore anlaufen. Da kann man verschiedene Einstellungen testen.

Das Zittern bei kd=140 liegt vermutlich an dem zu grossen Getriebespiel. Meinen Asuro kann ich etwa 2mm hin und her schwenken ohne dass sich die Motoren mitdrehen. Dadurch ergibt sich eine zusätzliche Totzeit , die das Schwingen erklären könnte. Ganz sicher bin ich mir aber nicht, das müsste man noch genauer untersuchen. Aber mit kleineren kd-Werten funktioniert der Regler wunderbar. Bei Kd=70 ist er sehr schnell und stabil. Bei kd=35 ist er schon deutlich langsamer aber immer noch sehr stabil. Bei noch kleineren Werten sieht man schon leichtes Nachschwingen.

Die Regelabweichung ist in meinem Programm x anstatt Xw. Da der Sollwert 0 ist, habe ich gleich den Istwert x für die Berechnungen genommen. Ansonsten sollten die Variablen mit meinem letzten Beitrag übereinstimmen.

Der I-Anteil wird durch Begrenzen von Isum auch begrenzt. Bei ki=10 ist der Grenzwert 256, das ist praktisch Vollausschlag an den Motoren. Mehr braucht man nicht, es könnte sogar eine niedrigere Begrenzung sinnvoll sein, um bei S-Kurven nicht auszurasten.

Der D-Anteil kann sehr hohe Werte annehmen, die in einem Zyklus gar nicht verwertet werden können. Um dennoch das richtige Verhalten des Reglers zu erreichen, wird der nicht verwertete Rest in der Variable "drest" gespeichert und bei den nächsten Zyklen wieder berücksichtigt.

Das Programm ist auch noch komplett mit hex-File in einer zip-datei angehängt. Probiert es mal aus. Da mein Asuro mit den grösseren Akkus etwas schwerer ist als im Original, können sich bei Euch etwas andere Ergebnisse herausstellen. Ich selber werde auch noch mit den Parametern spielen und einige Freiheitsgrade testen.

Waste