Hier der Code:
Code:
/*******************************************************************************
* 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);
	MotorSpeed(255,255);					// beschleunigt kurz rückwärts
	Msleep(80);								// um den Asuro aufzurichten

   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;
}
Gruß Waste