Mein Asuro kann balancieren!

[waste]

Hallo Asurofreunde!

Ich habe meinen Asuro zum Balancieren gebracht [stolzdieBrustschwelle]. Angeregt durch diesen Thread, bin ich auf diese Idee gekommen. Es war nicht ganz einfach, da der Asuro eigentlich nicht dafür gebaut ist, aber ein simpler Versuch mit der IR-Hinderniserkennung hat mich positiv überrascht, so dass ich es dann richtig versuchte.

Zur Messung des Winkels wird der Liniensensor verwendet. Im Gegensatz zum Linienfolger wird beim Balancieren nicht die Differenz zw. linkem und rechtem Sensor ausgewertet, sondern der Sensor fungiert hier als Abstandssensor und ich addiere beide Sensorwerte, um auch einen höheren Wert zu erhalten. Da in der aufrechten Position die Sensoren einen viel größeren Abstand zum Boden haben, ist das Signal des Sensors relativ klein. Um ein besseres Signal zu erhalten, habe ich deshalb die Front-LED durch eine hellere LED ersetzt. Zusätzlich habe ich den Akku höher gelegt damit die Nase nicht zu hoch kommt und die Sensoren zu weit vom Boden weg sind. Das Höhersetzen des Akkus hat auch noch den Vorteil, dass der Schwerpunkt weiter oben ist, was das Balancieren erleichtert. Außerdem musste ich noch die Versorgung des Liniensensors zusätzlich mit 220µF (parallel zu C5) abblocken, damit sich die Störungen der Motorsteuerung nicht mehr so schlimm auswirken. Mit diesen Änderungen hat es dann geklappt, mein Asuro konnte balancieren.

Die Regelung ist ziemlich kompliziert geworden. Ein PD-Regler stabilisiert den Neigungswinkel. Zusätzlich wird noch mit einem P-Regler die Geschwindigkeit und mit einem I-Regler der Offset des Abstandssensor geregelt, denn der Abstandssensor reagiert auch auf Farbe, Reflektionsfaktor und Umgebungslicht, was die Sache nicht ganz so einfach machte. Eine weitere Erklärung ist auch hier und folgenden Beiträgen zu sehen.

Der Code und ein gezipptes Paket incl. Headerdateien und Hex-File ist angehängt.

Ein Kurzvideo ist hier zu sehen: https://www.roboternetz.de/phpBB2/dl...le&file_id=274

Ein längeres Video ist im GMX MediaCenter hinterlegt, da sieht man noch, wie sich der Asuro sogar selbständig aufrichtet: http://service.gmx.net/mc/z0kENGSIZu...qlEAxROifcu5Mf
Hinweis: Der Link für den Gastzugang zum GMX MediaCenter ist bis zum 19.1.2006 gültig. Ihr müsst dort auf "GMX MediaCenter starten" klicken, dann öffnet sich ein Fenster. Im Fenster könnt ihr dann das Video "BalanceAkt.mov" runter laden oder direkt starten. Zum Download müssen PopUps erlaubt sein.

Viel Erfolg beim selber probieren.
Waste

/*******************************************************************************
* Description: Asuro balanciert
*
* Der Asuro balanciert auf den Hinterrädern.
* Der Liniensensor wird zur Winkelmessung verwendet.
* Damit der Asuro balancieren kann, wurden folgende Umbauten vorgenommen:
* Der Akkupack wurde höher gelegt, damit beim Balancieren die Liniensensoren
* nicht zu weit vom Untergrund entfernt sind.
* Die Spannungsversorgung für den Liniensensor musste zusätzlich abgeblockt werden,
* ein 220µF Elko wurde parallel zu C5 gelötet.
* Für eine höhere Empfindlichkeit wurde eine hellere LED (D11) eingebaut.
*
* Autor: Waste    1.12.05
*****************************************************************************/
#include "asuro.h"
#include <stdlib.h>

int main(void)
{
   int phi, phialt, y, yd, speed, ukorr, drest;
   int x, x1, x2, x3, x4, don, doff, v0;
   float v, u, w;
   unsigned char dir;
   unsigned int lineData[2];

   Init();
   phialt = 0;
   u=0; v=0, v0=0;
   x2=x3=x4=0;
   drest=0;
   w=40;
   	MotorDir(RWD,RWD);						// beschleunigt kurz rückwärts
	MotorSpeed(255,255);					// um den Asuro aufrichten
	Msleep(80);								// zu können
   while(1)
   {
	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]);	
	x1 = don - doff;						// Istwert
	x = (x1+x2+x3+x4)/2;					// Filterung
	phi = w - 14800/(x+100);				// Linearisierung und Vergleich
	x4=x3; x3=x2; x2=x1;
	yd = 500*(phi-phialt);					// D-Anteil berechnen und mit
	yd += drest;							// nicht berücksichtigtem Rest addieren
	if (yd > 350) drest = yd - 350;		// merke Rest
	else if (yd < -350) drest = yd + 350;
	else drest = 0;
	y = 40*phi + yd;						// Ausgang PD-Regler (Winkel)
	v = 0.987*v + 0.00189*u;				// simuliert Antrieb (Geschwindigkeit)
	ukorr = 20*v + v0;						// P-Regler Geschwindigkeit
	if (ukorr > 300) ukorr = 300;			// Begrenzung P-Regler
	if (ukorr < -300) ukorr = -300;
	u = y + ukorr;							// Berechnung Stellgröße
	if (u < 0) {dir = RWD;}
	else {dir = FWD;}
	speed = abs(u)/2 + 80;					// 80 kompensiert Reibung
	if (speed > 255) speed = 255;
	MotorDir(dir,dir);
	MotorSpeed(speed,speed);				// Ausgabe PWM
	w = w + v*0.001;						// Integralregler für Winkeloffset
	if (w > 80) w = 80;					// Begrenzung
	if (w < 20) w = 20;
	phialt = phi;
	if (u > 350) u = 350;					// Begrenzung entsprechend PWM
	if (u < -350) u = -350;
   }
   return 0;
}