Code:
#include "asuro.h"
//M1.R Mission 1
int i; //zähler
unsigned int data[2]; //Speicher bereitstellen für fototrans
int t1; //taster
int t2; //taster
int vl; //ausgangsgeschwindigkeit links
int vr; //ausgangsgeschwindigkeit rechts
int vl_max; //max geschwindigkeit links
int vr_max; //max geschwindigkeit rechts
int dif; //differenz
int vzl; //zufall links
int vzr; //zufall rechts
int zuf; //zufall
int l; //links
int r; //rechts
int ms; //wert msleep
int vl; //ausgangsgeschwindigkeit links
int vr; //ausgangsgeschwindigkeit rechts
// funktionen
//----------------------------------------------------------------------------------------
//zufall
uint8_t zufall()
{
static uint16_t startwert=0x0AA;
uint16_t temp;
uint8_t n;
for(n=1;n<8;n++)
{
temp = startwert;
startwert=startwert << 1;
temp ^= startwert;
if ( ( temp & 0x4000 ) == 0x4000 )
{
startwert |= 1;
}
}
OdometrieData(data); // aktuelle Helligkeitswerte der
startwert^= data[0];
return (startwert);
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//sprechen
void sprechen(void)
{
MotorDir(FWD,FWD);
zuf = zufall();
if (zuf < 32)
{StatusLED(OFF);
MotorSpeed(0,0);}
else if ((zuf >= 32) && (zuf < 64))
{StatusLED(RED);
MotorSpeed(10,10);}
else if ((zuf >= 64) && (zuf < 128))
{StatusLED(YELLOW);
MotorSpeed(20,20);}
else if ((zuf >= 128) && (zuf < 256))
{StatusLED(GREEN);
MotorSpeed(30,30);}
Msleep(160);
zuf = zufall();
if (zuf < 32)
{BackLED(OFF,OFF);
MotorSpeed(40,40);}
else if ((zuf >= 32) && (zuf < 64))
{BackLED(ON,OFF);
MotorSpeed(50,50);}
else if ((zuf >= 64) && (zuf < 128))
{BackLED(OFF,ON);
MotorSpeed(60,60);}
else if ((zuf >= 128) && (zuf < 256))
{BackLED(ON,ON);
MotorSpeed(70,70);}
Msleep(80);
StatusLED(OFF);
BackLED(OFF,OFF);
MotorSpeed(0,0);
Msleep(20);
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//ampel
void ampel(void)
{
zufall();
StatusLED(RED);
Msleep(500);
zufall();
StatusLED(YELLOW);
Msleep(500);
zufall();
StatusLED(GREEN);
Msleep(500);
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//beschleunigen vorwärts oder rückwärts allgemein von Ausgangsgeschwindigkeit bis max.geschwindigkeit
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
void a_allg(void)
{
while (vl < vl_max || vr < vr_max)
{
vl = vl + l;
vr = vr + r;
MotorSpeed(vl,vr);
Msleep(ms);
}
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//verlangsamen vorwärts oder rückwärts allgemein bis 80 Endgeschwindigkeit
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//l links minus
//r rechts minus
//ms Dauer pro Durchlauf
//definieren!
void b_allg(void)
{
while (vl > 80 || vr > 80)
{
vl = vl - l;
vr = vr - r;
MotorSpeed(vl,vr);
Msleep(ms);
}
MotorSpeed(0,0);
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//wenden
void wenden (void)
{
//BackLEDs an - rückwärts beschleunigen
StatusLED(RED);
MotorDir(RWD,RWD);
zuf = zufall();
BackLED(ON,ON);
vl_max = 255 - (zuf/16);
vr_max = 255 - (zuf/16);
zuf = zufall();
BackLED(ON,ON);
zuf = zuf/4;
zuf = zuf - 32;
vl = 100 + zuf;
zuf = zufall();
BackLED(ON,ON);
zuf = zuf/4;
zuf = zuf - 32;
vr = 100 - zuf;
l=1;
r=1;
ms = 8;
a_allg();
zuf = zufall();
BackLED(ON,ON);
Msleep(250 + zuf*2);
BackLED(OFF,OFF); //LEDs aus und wieder vorwärts beschleunigen
StatusLED(YELLOW);
MotorDir(FWD,FWD);
zuf = zufall();
vr_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//kollision
// persönliche taster-werte von M1.R: 1 : 1
// 2 : 2
// 3 : 4
// 4 : 7 oder 8
// 5 : 15 oder 16
// 6 : 31 oder 32
void kollision(void)
{
//wenn rechte taster nach links fahren
if //wenn rechts folgende schalter gedrückt sind
( t1 == 1 && t2 == 1 //rechter Taster1: linkes rad rückwärts - rechtes rad vorwärtss
)
{
// ein bischen nach links abbiegen
//MotorDir(xxx,xxx)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(RED);
MotorDir(RWD,FWD); //links rückwärts - rechts vorwärts
zuf = zufall();
BackLED(OFF,ON); //rechte LED an
vr_max = 166 - (zuf/16);
vl = 130;
vr = 80;
l=1;
r=1;
ms = 3;
a_allg();
zuf = zufall();
BackLED(OFF,ON); //rechte LED an
Msleep(220 + zuf/32);
BackLED(OFF,OFF); //LEDs aus und wieder vorwärts beschleunigen
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(YELLOW);
MotorDir(FWD,FWD);
vr = vl;
zuf = zufall();
vl_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
else if //wenn rechts folgende schalter gedrückt sind
( (t1 == 2 && t2 == 2) //rechter Taster2
|| (t1 == 4 && t2 == 4) //rechter Taster3
|| (t1 == 3 && t2 == 3) //1und2
|| (t1 == 5 && t2 == 5) //1und3
|| (t1 == 6 && t2 == 6) //2und3
|| (t1 == 21 && t2 == 21) //2,3 und 5
|| (t1 == 22 && t2 == 22) //2,3 und 5
)
{
// nach links abbiegen
//MotorDir(xxx,xxx)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(RED);
MotorDir(RWD,FWD); //links rückwärts - rechts vorwärts
zuf = zufall();
BackLED(OFF,ON); //rechte LED an
vr_max = 166 - (zuf/16);
vl = 130;
vr = 130;
l=1;
r=1;
ms = 3;
a_allg();
zuf = zufall();
BackLED(OFF,ON); //rechte LED an
Msleep(220 + zuf/32);
BackLED(OFF,OFF); //LEDs aus und wieder vorwärts beschleunigen
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(YELLOW);
MotorDir(FWD,FWD);
zuf = zufall();
vl_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
//linke taster nach rechts fahren
else if //wenn links folgende Taster gedrückt sind
( t1 == 31 && t2 == 31 //ganz links - linker Taster 6
)
{
// ein bischen nach rechts abbiegen
//MotorDir(xxx,xxx)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(RED);
MotorDir(FWD,RWD); //rechts rückwärts - links vorwärts
zuf = zufall();
BackLED(ON,OFF); //linke LED an
vl_max = 166 - (zuf/16);
vr = 130;
vl = 80;
l=1;
r=1;
ms = 3;
a_allg();
zuf = zufall();
BackLED(ON,OFF); //linke LED an
Msleep(220 + zuf/32);
BackLED(OFF,OFF); //LEDs aus und wieder vorwärts beschleunigen
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(YELLOW);
MotorDir(FWD,FWD);
vl = vr;
zuf = zufall();
vl_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
else if //wenn links folgende Taster gedrückt sind
(
(t1 == 7 && t2 == 7) //linker Taster 4
|| (t1 == 8 && t2 == 8) //linker Taster 4
|| (t1 == 15 && t2 == 15) //linker Taster 5
|| (t1 == 16 && t2 == 16) //linker Taster 5
|| (t1 == 22 && t2 == 22) //4und5
|| (t1 == 23 && t2 == 23) //4und5
|| (t1 == 24 && t2 == 24) //4und5
|| (t1 == 38 && t2 == 38) //4und6
|| (t1 == 39 && t2 == 39) //4und6
|| (t1 == 40 && t2 == 40) //4und6
|| (t1 == 46 && t2 == 46) //5und6
|| (t1 == 47 && t2 == 47) //5und6
|| (t1 == 48 && t2 == 48) //5und6
|| (t1 == 24 && t2 == 24) //2,4,5
|| (t1 == 25 && t2 == 25) //2,4,5
|| (t1 == 26 && t2 == 26) //2,4,5
)
{
//nach rechts abbiegen
//MotorDir(xxx,xxx)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(RED);
MotorDir(FWD,RWD); //rechts rückwärts - links vorwärts
zuf = zufall();
BackLED(ON,OFF); //linke LED an
vr_max = 166 - (zuf/16);
vr = 130;
vl = 130;
l=1;
r=1;
ms = 3;
a_allg();
BackLED(ON,OFF); //linke LED an
zuf = zufall();
Msleep(220 + zuf/32);
BackLED(OFF,OFF); //LEDs aus und wieder vorwärts beschleunigen
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
StatusLED(YELLOW);
MotorDir(FWD,FWD);
zuf = zufall();
vr_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
//sonst irgendwelche taster
else
{
wenden();
}
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
//beschleunigung vorwärts zufall bis max
void a_vorw_z(void)
{
vl = 120;
vr = 120;
t1 = PollSwitch(); //taster prüfen
t2 = PollSwitch(); //taster prüfen
while ((vl < 180 && vr < 180) && (t1 == 0 && t2 == 0))
{
//zufallszahl für linke geschwindigkeit ermitteln
vzl = zufall();
vzl = (vzl + 1) / 32;
//zufallszahl für rechte geschwindigkeit
vzr = zufall();
vzr = (vzr + 1) / 32;
vl = vl + vzl; //linke geschwindigkeit
vr = vr + vzr; //rechte geschwindigkeit
MotorDir(FWD,FWD); //fahr vorwärts
MotorSpeed(vl,vr);
zuf = zufall();
Msleep(50 + zuf / 8);
t1 = PollSwitch(); //taster prüfen
t2 = PollSwitch(); //taster prüfen
}
kollision();
}
//----------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------
//ab und zu umschauen
void umschauen(void)
{
{
StatusLED(YELLOW);
BackLED(ON,ON);
//verlangsamen vorwärts oder rückwärts allgemein bis 80 Endgeschwindigkeit
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//l links minus
//r rechts minus
//ms Dauer pro Durchlauf
//definieren!
MotorDir(FWD,FWD);
l = 1;
r = 1;
ms = 4;
b_allg();
MotorSpeed(0,0);
Msleep(200);
BackLED(OFF,OFF);
for (i = 1; i <= 2; ++i)
{
StatusLED(YELLOW);
MotorDir(RWD,FWD);
zuf = zufall();
vl = 80 + zuf/4;
zuf = zufall();
vr = 80 + zuf/4;
MotorSpeed(vl,vr);
zuf = zufall();
Msleep(zuf*3);
zuf = zufall();
vl = 80 + zuf/4;
zuf = zufall();
vr = 80 + zuf/4;
StatusLED(GREEN);
MotorDir(FWD,RWD);
MotorSpeed(vl,vr);
zuf = zufall();
Msleep(zuf*3);
}
//LEDs aus und wieder vorwärts beschleunigen
//MotorDir(FWD oder RWD, FWD oder RWD)
//vl Ausgangsgeschwindigkeit links
//vr Ausgangsgeschwindigkeit rechts
//vl_max geschwindigkeit links oder
//vr_max geschwindigkeit rechts
//l links plus
//r rechts plus
//ms Dauer pro Durchlauf
//definieren!
MotorDir(FWD,FWD);
zuf = zufall();
vr_max = 255 - (zuf/4);
l=1;
r=1;
ms = 20;
a_allg();
}
}
//----------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------
int main(void)
{
Init();
StatusLED(OFF);
BackLED(OFF,OFF);
Msleep(1000);
StatusLED(RED);
BackLED(ON,ON);
MotorDir(FWD,FWD);
MotorSpeed(0,0);
Msleep(1000);
StatusLED(OFF);
BackLED(OFF,OFF);
MotorSpeed(50,50);
Msleep(200);
for (i = 1; i <= 80; ++i)
{
sprechen();
}
StatusLED(OFF);
BackLED(OFF,OFF);
Msleep(1000);
ampel();
a_vorw_z();
while (1) //hauptschleife
{
t1 = PollSwitch(); //taster prüfen
t2 = PollSwitch(); //taster prüfen
//StatusLED(GREEN);
//wenn kein Taster gedrückt ist fahr zufällig
if(t1 == 0 && t2 == 0) //wenn kein taster gedrückt ist
{
//geschwindigkeit links
//zufallszahl für linke geschwindigkeit ermitteln
vzl = zufall();
//vzl = vzl / 32; //zahlen zwischen 0 und 7
//vzl = vzl + 1; //8 zahlen zwischen 1 und 8
//vzl = vzl * 4; // 8 zahlen zwischen 4 und 32 in 4erschritten
//vzl = vzl - 18; //zahlen zwischen -14 und +14 in 4erschritten
vzl = vzl / 32; //zahlen zwischen 0 und 7
vzl = vzl * 2; // 8 zahlen zwischen 0 und 14 in 2erschritten
vzl = vzl - 7; //zahlen zwischen -7 und +7 in 2erschritten
vl = vl + vzl;
if(vl < 105) //minimale geschwindigkeit
{
vl = 118;
}
else if(vl > 255) //maximale geschwindigkeit
{
vl = 242;
}
//geschwindigkeit rechts
//zufallszahl für rechte geschwindigkeit
vzr = zufall();
vzr = vzr / 32; //zahlen zwischen 0 und 7
vzr = vzr * 4;
vzr = vzr - 7;
vr = vr + vzr;
if(vr < 105) //minimale geschwindigkeit
{
vr = 118;
}
else if(vr > 255) //maximale geschwindigkeit
{
vr = 242;
}
MotorDir(FWD,FWD); //fahr vorwärts
MotorSpeed(vl ,vr); // mit geschwindigkeit
zuf = zufall();
//Differenzabfrage
dif = vl - vr; //differenz zw linker u rechter geschwindigkeit
if((dif >= -10) && (dif <= 10)) //wenn (fast) geradeaus - langes msleep
{
StatusLED(GREEN);
Msleep(100 + zuf);
StatusLED(OFF);
}
else if(((dif < -10) && (dif > -60))
|| ((dif > 10) && (dif < 60))) //wenn leichte kurve - kurzes msleep
{
StatusLED(YELLOW);
Msleep(10 + zuf / 2);
StatusLED(OFF);
}
else if((dif < -60) || (dif > 60)) //wenn starke kurve - ganz kurzes msleep
{
StatusLED(RED);
Msleep(zuf / 4);
StatusLED(OFF);
}
zuf = zufall();
if (zuf < 4)
{
wenden();
}
if (zuf > 251)
{
umschauen();
}
}
else if (t1 > 0 && t2 > 0)
{
kollision(); // kollision
}
}
while(1);
return(0);
}
Zitieren
Lesezeichen