Asuro: Linienfolger mit PD-Regler

[pult]

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

Habe diese Umsetzung des Linienfolgers mal ausprobiert, nur irgendwie funzt das alles nicht so richtig. Beim compilen bekomme ich zwar keinen error...

-------- begin --------
avr-gcc --version
avr-gcc (GCC) 3.3.1
Copyright (C) 2003 Free Software Foundation, Inc.
This is free software; see the source for copying conditions.  There is NO
warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

Size before:
test.elf  :
section     size      addr
.text       2568         0
.data          0   8388704
.bss          38   8388704
.noinit        0   8388742
.eeprom        0   8454144
.stab       4416         0
.stabstr    3117         0
Total      10139


Size after:
test.elf  :
section     size      addr
.text       2568         0
.data          0   8388704
.bss          38   8388704
.noinit        0   8388742
.eeprom        0   8454144
.stab       4416         0
.stabstr    3117         0
Total      10139


Errors: none
-------- end --------

> Process Exit Code: 1

...aber beim Starten des Asuro leuchtet die linke Back-LED auf. Soll das so?

[waste]

Hallo pult,

eine leuchtende Backled zeigt eine Begrenzung des I-Anteils an. Das kann bei längerer Kurvenfahrt vorkommen, ist für die Diagnose gedacht.

Vielleicht hast du nicht meine Headerdateien/Libs verwendet. Probier mal mein Hexfile aus, das ist auch hochgeladen. Wenn es da auch nicht funktioniert, dann ist deine Hardware fehlerhaft oder nicht kompatibel, denn bei vielen anderen hat es ja funktioniert.

Waste

[Gladius]

Hi bin noch net so der pro in c++, hab mir gedacht mit helfe der Lichtempfindlichkeit den Asuro sobald ein großer schwarzer Fleck auftaucht zum stehen zubringen geht auch alles soweit ist halt net alles so toll wie das Programm von waste. Daher frag ich was ich bei deinem Prog ändern muss damit er die Motorspeed auf 0 stellt sobald beide Sensoren nur noch schwarz empfangen.

Achja bitte um Hilfe

[waste]

Hallo Gladius!

Nun, ich werde dir jetzt nicht den Code auf dem Silbertablett servieren. Das wäre doch zu einfach und der Lerneffekt gleich Null.

Aber hier die Vorgehensweise:
Zuerst musst du rausfinden, welche Werte der Liniensensor bei Schwarz liefert. Da ich in meinem Programm das Umgebungslicht kompensiere, musst du also die Werte mit eingeschalteter FrontLED und mit ausgeschalteter FrontLED messen und voneinander abziehen und das jeweils für links und rechts. Wenn nun der typische Wert für schwarz etwa 20 ist, dann mach in mein Programm eine zusätzliche Abfrage auf < 30. Das sieht in etwa so aus:
if ((Wertlinks < 30) && (Wertrechts < 30) dann Motor aus und Ausstieg

Den typischen Wert von 20 für schwarz habe ich jetzt nur angenommen. Den richtigen Wert musst du selbst aus deinem Asuro mit dem Hyperterminal auslesen. Wie das geht findest du hier im Forum, einfach die Suchfunktion benutzen.

Wenn dein Programm fertig ist, dann poste es hier. Es interessiert bestimmt auch andere.

Gruß Waste

[Gladius]

Habs jetzt den ganzen Tag versucht die Abfrage in dein Programm reinzubekommen , aber bis jetzt hab ich wohl die richtige Stelle nicht gefunden oder bekamm irgendwelche komischen Fehler beim compilieren. Ich
frag vielleicht kann mir ja noch einer Helfen die richtigen Werte ausgelesen das hab ich geschafft war ja net so ganz schwer nur die richtige Funktion in deine Funktionen einzubauen schaff ich leider nicht hab das Manual das bei lag schon 10x durchgelesen...

[waste]

Ich würde die Abfrage im Hauptprogramm (main) nach FollowLine() durchführen. Da die Variable lineData[] überschrieben wird, braucht man eine Zwischenspeicherung in FollowLine(). Ich würde da wegen der Übersicht gleich 4 globale Variablen dafür spendieren, z.B.:
Loff = lineData[0]
Roff = lineData[1]
...
Lon = lineData[0]
Ron = lineData[1]

Im Hauptprogramm dann die Differenz Lon - Loff zur Kompensation des Umgebungslichts bilden. Das Selbe nochmal für rechts und dann die Abfrage starten. So sollte es eigentlich funktionieren.

Waste

[pult]

@waste

Ich glaube, das Problem ist bei mir auf den Antrieb zurückzuführen. Irgendwie dreht sich das linke Rad langsamer als das rechte, egal welches (auch vorher halbwegs funktionierende) linienfolger-prog ich benutze. Schade eigentlich, da muss ich wohl nochmal ran, obwohl ich sagen muss, das die Übersetzung der Motordrehzahl auf die Achsen beim Asuro sehr sehr unglücklich gelöst wurde.

[vanharmelen]

Hallo Waste,

Obschon es schon lange her ist, will ich dir doch gratulieren mit deine „Linienfolger mit PID Regler fur die Asuro“. A fantastic job!!!!
Weil meine Kenntnisse auf diesem Gebiet sehr klein ist habe ich ein paar Frage:
1. Clockwise fahrt mein Asuro (ziemlich) flot um die Kurven auf eine schwarze Linie (auf weisse Untergrund). Anticlockwise verliert er gerne (bei gleiche parameters: speed, Kp.,Ki, Kd) bei zu enge Rechtskurven die Linie und fangt an im Kreis zu drehen.
Hat das zu tun mit das Einlesen der Sensorsignale (an der linken Linienfolge_LED ist es heller (oder Dunkler ?) als an der rechten ??
2. Ich habe versucht selbst ein Antwort zu finden , aber is ist mir nicht gelungen :
Warum bekomme ich auf eine weisse Linie (auf schwarze Untergrund) kein Resultat. ?

Mit freundliche Grusse Rien vamn Harmelen (NL)

[waste]

1. Clockwise fahrt mein Asuro (ziemlich) flot um die Kurven auf eine schwarze Linie (auf weisse Untergrund). Anticlockwise verliert er gerne (bei gleiche parameters: speed, Kp.,Ki, Kd) bei zu enge Rechtskurven die Linie und fangt an im Kreis zu drehen.
Hat das zu tun mit das Einlesen der Sensorsignale (an der linken Linienfolge_LED ist es heller (oder Dunkler ?) als an der rechten ??

Das kann an einem unsymmetrischen Liniensensor als auch an unterschiedlichen Motoren oder Getrieben liegen. Auf der Seite, wo weniger Regelverstärkung vorhanden ist, fliegt er eher raus. Ich habe im Programm nichts vorgesehen, um die Linie wieder zu finden, falls der Asuro einmal die Linie verloren hat.
Seit mein Asuro einmal vom Tisch gefallen ist, habe ich auch das Problem dass er bei voller Geschwindigkeit in engen Kurven auf einer Seite nicht immer folgen kann. Er fährt auch ohne Regelung nicht mehr gerade aus.
Grundsätzlich kann die Regelung nicht die physikalischen Gesetze ausser Kraft setzen. Ein Auto kann auch nicht mit 100km/h eine enge Kurve durchfahren.

2. Ich habe versucht selbst ein Antwort zu finden , aber is ist mir nicht gelungen :
Warum bekomme ich auf eine weisse Linie (auf schwarze Untergrund) kein Resultat. ?

Bei weisser Linie auf schwarzem Untergrund ist die Polarität des Liniensensor umgekehrt. Damit dreht sich auch der Regelsinn um. Der Asuro wird also von der weissen Linie wegregeln.

Gruß Waste

[vanharmelen]

Hallo Waste

Danke fur deine schnelle Antwort.
Ich habe in deine Programm doff resp don geandert in:
doff = Lineright - lineleft
don = Lineright – lineleft
und … es ist zum Staunen...... mein Asuro lauft flot auf einer weisser Linie (auf schwarzem Untergrund)

Is dass wass Sie meinen mit: Bei weisser Linie auf schwarzem Untergrund ist die Polarität des Liniensensor umgekehrt ?

Mit freundliche Gruss : Rien van harmelen (NL)