Code:
#include "asuro.h"
#define false 0
#define true 1
#define SCHWARZ 0
#define WEISS 1
#define LINKS 0
#define RECHTS 1
#define LGRW 540
#define RGRW 530
typedef short bool;
//Gradzahl in Bogenmaß umrechnen
double dtor( double deg )
{
double r = 32*deg/180;// PI*reifenabstand*winkel/180°
return r;
}
// eine bestimmte Strecke mit einem Rad fahren
void FahreEinzeln( double mm , int grenzw , unsigned char dir , short rad );
//eine bestimmte Strecke mit beiden Rädern fahren
void Fahre( double mm , unsigned int lgrenzw , unsigned int rgrenzw , unsigned char dir);
void InitTinte()
{
FahreEinzeln( 5.91 , LGRW , RWD , LINKS );
FahreEinzeln( 5.91 , LGRW , FWD , LINKS );
}
//-----------HAUPTPROGRAMM----------------
int main(void)
{
Init();
InitTinte();
//Gleichseitiges Dreieck mit seitenlänge 10cm (100mm) fahren
Fahre( 100.0 , LGRW , RGRW , FWD );
FahreEinzeln( dtor( 300.0 ) , RGRW , FWD , RECHTS );
Fahre( 100.0 , LGRW , RGRW , FWD );
FahreEinzeln( dtor( 300.0 ) , RGRW , FWD , RECHTS );
Fahre( 100.0 , LGRW , RGRW , FWD );
while(1);
return 0;
}
void Fahre( double mm , unsigned int lgrenzw , unsigned int rgrenzw , unsigned char dir)
{
unsigned int rodo,lodo,rodo_old,lodo_old;
unsigned int lcounter = 0;
unsigned int rcounter = 0;
unsigned int data[2];
unsigned char lspeed = 140;
unsigned char rspeed = 140;
OdometrieData( data );
//Momentaner Wert links
if( data[LINKS] < lgrenzw )
lodo_old = SCHWARZ;
else
lodo_old = WEISS;
//Momentaner Wert rechts
if( data[RECHTS] < rgrenzw )
rodo_old = SCHWARZ;
else
rodo_old = WEISS;
//Losfahren
MotorDir( dir , dir );
MotorSpeed( lspeed , rspeed );
while( 3*lcounter < mm || 3*rcounter < mm )
{
OdometrieData( data );
//links Auswerten
if( data[LINKS] < lgrenzw )
lodo = SCHWARZ;
else
lodo = WEISS;
//rechts Auswerten
if( data[RECHTS] < rgrenzw )
rodo = SCHWARZ;
else
rodo = WEISS;
//Farbwechsel links prüfen
if( lodo != lodo_old )
{
lcounter += 1;
lodo_old = lodo;
}
//Farbwechsel rechts prüfen
if( rodo != rodo_old )
{
rcounter += 1;
rodo_old = rodo;
}
//Ungleichmäßigkeiten ausgleichen
if( lcounter+1 < rcounter )
rspeed = 0;
else if( rcounter+1 < lcounter )
lspeed = 0;
else
{
rspeed = 140;
lspeed = 140;
}
//Falls eine Seite Ziel erreicht, stehenbleiben
if( 3*lcounter >= mm )
lspeed = 0;
if( 3*rcounter >= mm )
rspeed = 0;
MotorSpeed( lspeed , rspeed );
}
MotorSpeed( 0 , 0 );
}
void FahreEinzeln( double mm , int grenzw , unsigned char dir , short rad )
{
unsigned int data[2];
unsigned int counter = 0;
bool odo,odo_old;
OdometrieData(data);
//Momentaner Wert
if( data[rad] < grenzw )
odo_old = SCHWARZ;
else
odo_old = WEISS;
//Richtung und Geschwindigkeit einstellen
if( rad == LINKS )
{
MotorDir( dir , BREAK );
MotorSpeed( 140 , 0 );
}
else
{
MotorDir( BREAK , dir );
MotorSpeed( 0 , 140 );
}
//Strecke fahren
while(3*counter < mm )
{
OdometrieData(data);
//Auswerten
if( data[rad] < grenzw )
odo = SCHWARZ;
else
odo = WEISS;
//Farbwechsel prüfen
if( odo != odo_old )
{
counter += 1;
odo_old = odo;
}
}
//Anhalten
MotorSpeed( 0 , 0 );
MotorDir( BREAK , BREAK );
}
mfg Lukas
Lesezeichen