Code:
#include "asuro.h"
#include "stdlib.h"
#include "string.h"
unsigned int mittel_links=0;
unsigned int mittel_rechts=0;
unsigned int mittel[2];
unsigned int geschw_l = 200;
unsigned int geschw_r = 200;
unsigned int odo_gL;
unsigned int odo_gR;
unsigned int gbr;
unsigned int gbl;
void verzoegern(int zeit) //Programm um Zeitverzögerung einzubringen
{
int i=0;
for(i=0;i<zeit;i++)
{
Sleep(250);
}
}
void zeile(int zeilen) //Programm um leere Zeilen im HyperTerminal auszugeben
{
unsigned char newline = 10;
unsigned char cR = 13;
int i = 0;
for(i=0;i<zeilen;i++)
{
SerWrite(&newline,1);
SerWrite(&cR,1);
}
}
void countdown(void) //Countdown Programm
{
Init();
zeile(1);
SerWrite(">>>> Countdown <<<<",19);
zeile(2);
SerWrite(">>>> 3 <<<<",19);
zeile(2);
StatusLED(RED);
verzoegern(250);
SerWrite(">>>> 2 <<<<",19);
zeile(2);
StatusLED(YELLOW);
verzoegern(250);
SerWrite(">>>> 1 <<<<",19);
zeile(2);
StatusLED(GREEN);
verzoegern(250);
SerWrite(">>>> Start <<<<",19);
}
void odometrie(void) //Programm zu Kalibrierung der Motoren
{
long int summe_rechts=0;
long int summe_links=0;
unsigned char mittell[10];
unsigned char mittelr[10];
unsigned char flagl=FALSE;
unsigned char flagr=FALSE;
unsigned int mittelwert0=0;
unsigned int mittelwert1=0;
unsigned char fR[10];
unsigned char fL[10];
unsigned char UR[10];
unsigned char UL[10];
unsigned char SL[10];
unsigned char SR[10];
unsigned int data[2];
unsigned int faktorr;
unsigned int faktorl;
unsigned int count=0;
unsigned int i=0;
unsigned int L=0;
unsigned int R=0;
float a=0;
float b=0;
MotorDir(FWD,FWD);
MotorSpeed(200,200);
for(count=0;count<400;count++)
{
verzoegern(3);
OdometrieData(data);
summe_links = summe_links + data[0];
summe_rechts = summe_rechts + data[1];
}
mittelwert0 = summe_links / count;
mittelwert1 = summe_rechts / count;
itoa(mittelwert0, mittell,10);
itoa(mittelwert1, mittelr,10);
zeile(2);
SerWrite("Mittelwert links: ",18);
SerWrite(mittell,strlen(mittell));
zeile(2);
SerWrite("Mittelwert rechts: ",19);
SerWrite(mittelr,strlen(mittelr));
itoa(summe_links, SL,10);
itoa(summe_rechts, SR,10);
zeile(2);
SerWrite("Summe links: ",13);
SerWrite(SL,strlen(SL));
zeile(2);
SerWrite("Summe rechts: ",14);
SerWrite(SR,strlen(SR));
for(i=0;i<count;i++)
{
verzoegern(2);
OdometrieData(data);
if (data[0] < mittelwert0 && flagl == TRUE)
{
L++;
flagl = FALSE;
}
if (data[0] > mittelwert0 && flagl == FALSE)
{
L++;
flagl = TRUE;
}
if (data[1] < mittelwert1 && flagr == TRUE)
{
R++;
flagr = FALSE;
}
if (data[1] > mittelwert1 && flagr == FALSE)
{
R++;
flagr = TRUE;
}
}
if(L<R)
{
a=R/8;
b=L/8;
faktorl=(100/a)*b;
faktorr=100;
}
if(R<L)
{
a=R/8;
b=L/8;
faktorr=(100/b)*a;
faktorl=100;
}
if(R==L)
{
faktorr=faktorl=100;
}
MotorSpeed(0,0);
itoa(faktorl, fL,10);
itoa(faktorr, fR,10);
itoa(L, UL,10);
itoa(R, UR,10);
zeile(2);
SerWrite("Umdrehungen links: ",19);
SerWrite(UL,strlen(UL));
zeile(2);
SerWrite("Umdrehungen rechts: ",20);
SerWrite(UR,strlen(UR));
zeile(2);
SerWrite("Faktor links: ",14);
SerWrite(fL,strlen(fL));
zeile(2);
SerWrite("Faktor rechts: ",15);
SerWrite(fR,strlen(fR));
zeile(4);
odo_gL=(geschw_l*faktorr)/100;
odo_gR=(geschw_r*faktorl)/100;
}
void erkennung(void) //Programm um vordere Diodenwerte einzulesen
{
unsigned int datahell[2];
unsigned int datadunkel[2];
unsigned int temp_h[2];
unsigned int temp_d[2];
unsigned char temp_char0[4];
unsigned char temp_char1[4];
unsigned char temp_mit0[4];
unsigned char temp_mit1[4];
unsigned int i = 2;
unsigned int taster;
StatusLED(OFF);
while (i > 0)
{
taster = PollSwitch();
{
if (taster>0 && taster<33 && i==2)
{
FrontLED(ON);
verzoegern(150);
LineData(temp_h);
datahell[0]=temp_h[0];
datahell[1]=temp_h[1];
StatusLED(RED);
verzoegern(200);
StatusLED(OFF);
verzoegern(200);
FrontLED(OFF);
i=1;
itoa(temp_h[0],temp_char0,10);
itoa(temp_h[1],temp_char1,10);
SerWrite("Asuro: ",7);
zeile(2);
SerWrite("Hell l: ",8);
SerWrite(temp_char0,strlen(temp_char0));
zeile(1);
SerWrite("Hell r: ",8);
SerWrite(temp_char1,strlen(temp_char1));
zeile(2);
}
taster = PollSwitch();
if (taster>0 && taster<33 && i==1)
{
FrontLED(ON);
verzoegern(150);
LineData(temp_d);
datadunkel[0]=temp_d[0];
datadunkel[1]=temp_d[1];
StatusLED(YELLOW);
verzoegern(200);
FrontLED(OFF);
i=0;
StatusLED(GREEN);
itoa(temp_d[0],temp_char0,10);
itoa(temp_d[1],temp_char1,10);
SerWrite("Dunkel l: ",10);
SerWrite(temp_char0,strlen(temp_char0));
zeile(1);
SerWrite("Dunkel r: ",10);
SerWrite(temp_char1,strlen(temp_char1));
zeile(1);
}
}
}
mittel_links=(datahell[0]+datadunkel[0])/2;
mittel_rechts=(datahell[1]+datadunkel[1])/2;
mittel[0]=mittel_links;
mittel[1]=mittel_rechts;
itoa(mittel_links,temp_mit0,10);
itoa(mittel_rechts,temp_mit1,10);
zeile(2);
SerWrite("Mittelwert l: ",14);
SerWrite(temp_mit0,strlen(temp_mit0));
zeile(1);
SerWrite("Mittelwert r: ",14);
SerWrite(temp_mit1,strlen(temp_mit1));
}
void linienfahrt(void) //Programm um einer Linie zu folgen
{
unsigned int data[2];
unsigned int wert_links;
unsigned int wert_rechts;
unsigned int a=0;
unsigned int b=0;
FrontLED(ON);
while(1)
{
LineData(data);
wert_links=data[0];
wert_rechts=data[1];
if(b>20000)
{
a=0;
}
if((wert_links < mittel_links) && (wert_rechts < mittel_rechts))
{
MotorDir(FWD,FWD);
MotorSpeed(odo_gL,odo_gR);
BackLED(OFF,OFF);
a=0;
b=0;
}
if((wert_links > mittel_links) && (wert_rechts > mittel_rechts) && a==0)
{
MotorDir(FWD,FWD);
MotorSpeed(odo_gL,odo_gR);
}
if((wert_links > mittel_links) && (wert_rechts > mittel_rechts) && a==1)
{
MotorDir(FWD,FWD);
gbr=odo_gL*0,4;
gbl=odo_gR;
MotorSpeed(gbr,gbl);
b++;
}
if((wert_links > mittel_links) && (wert_rechts > mittel_rechts) && a==2)
{
MotorDir(FWD,FWD);
gbr=odo_gL;
gbl=odo_gR*0,4;
MotorSpeed(gbr,gbl);
b++;
}
if((wert_links < mittel_links) && (wert_rechts > mittel_rechts))
{
MotorDir(FWD,FWD);
gbr=odo_gL*0,6;
gbl=odo_gR;
MotorSpeed(gbr,gbl);
BackLED(ON,OFF);
a=1;
}
if((wert_links > mittel_links) && (wert_rechts < mittel_rechts))
{
MotorDir(FWD,FWD);
gbr=odo_gL;
gbl=odo_gR*0,6;
MotorSpeed(gbr,gbl);
BackLED(OFF,ON);
a=2;
}
}
}
void gatter(void) //Programm um in einem Kreis zu bleiben
{
unsigned int zwert[2];
while(1)
{
FrontLED(ON);
MotorDir(FWD,FWD);
MotorSpeed(odo_gL,odo_gR);
LineData(zwert);
if(zwert[0] < mittel_links && zwert[1] > mittel_rechts)
{
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(150);
MotorDir(FWD,RWD);
MotorSpeed(odo_gL,odo_gR);
StatusLED(GREEN);
verzoegern(150);
}
if(zwert[1] < mittel_rechts && zwert[0] > mittel_links)
{
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(150);
MotorDir(RWD,FWD);
MotorSpeed(odo_gL,odo_gR);
verzoegern(150);
StatusLED(GREEN);
}
}
}
void tast(void) //Programm um mit Tastern zu fahren
{
unsigned char t1, t2;
Init();
while(1)
{
t1 = PollSwitch();
t2 = PollSwitch();
if(t1 == 0 && t2 == 0) /* keine Taste */
{
MotorDir(FWD,FWD);
MotorSpeed(odo_gL,odo_gR);
StatusLED(GREEN);
FrontLED(ON);
BackLED(OFF,OFF);
}
else if (t1 && t2 && t1 == t2)
{
if(t1 == 1 && t2 == 1) /* Taster 1 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(OFF,ON);
gbr=odo_gL*0,75;
gbl=odo_gR*0;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
if(t1 & 0x02 && t2 & 0x02) /* Taster 2 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(OFF,ON);
gbr=odo_gL*0,75;
gbl=odo_gR*0;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
if(t1 & 0x04 && t2 & 0x04) /* Taster 3 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(OFF,ON);
gbr=odo_gL*0,75;
gbl=odo_gR*0;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
if(t1 & 0x08 && t2 & 0x08) /* Taster 4 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(ON,OFF);
gbr=odo_gL*0;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
if(t1 & 0x16 && t2 & 0x10) /* Taster 5 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(ON,OFF);
gbr=odo_gL*0;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
if(t1 & 0x32 && t2 & 0x20) /* Taster 6 gedrückt? */
{
StatusLED(RED);
FrontLED(OFF);
BackLED(ON,ON);
MotorDir(RWD,RWD);
gbr=odo_gL*0,75;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(333);
BackLED(ON,OFF);
gbr=odo_gL*0;
gbl=odo_gR*0,75;
MotorSpeed(gbr,gbl);
verzoegern(450);
}
}
}
}
void LocalInit(void) //Programm für US
{
TCCR2 = (1 << WGM21) | (1 << CS20);
OCR2 = 0x64;
ADCSRA = 0x00;
ACSR = 0x02;
ADMUX = 0x03;
SFIOR |= (1 << ACME);
DDRD &= ~(1 << 6);
}
void Ping(unsigned char length) //Programm für US
{
count72kHz = 0;
TCCR2 = (1 << WGM21) | (1 << COM20) | (1 << CS20);
while(count72kHz < length)
{
OCR2 = 0x64 + length / 2 - count72kHz;
}
TCCR2 = (1 << WGM21) | (1 << CS20);
OCR2 = 0x64;
}
void ultraschall (void) //Programm um mit US zu fahren
{
int pos, i;
int posmarker;
Init();
LocalInit();
while(TRUE)
{
posmarker = 0;
Ping(20);
for(pos = 0; pos < 100; pos++)
{
Sleep(10);
if((ACSR & (1 << ACI)) != 0)
{
if(posmarker == 0) { posmarker = pos; }
}
ACSR |= (1 << ACI);
}
if(posmarker > 10)
{
StatusLED(GREEN);
MotorDir(FWD, FWD);
MotorSpeed(odo_gL,odo_gR);
}
else
{
StatusLED(RED);
MotorDir(FWD, RWD);
MotorSpeed(odo_gL*0,odo_gR);
for(i = 0; i<100; i++) { Sleep(200); }
}
}
}
int main(void)
{
unsigned int taster_1=0;
unsigned int taster_2=0;
unsigned int abfrage_1=0;
unsigned int abfrage_2=0;
unsigned int x=0;
Init();
zeile(1);
SerWrite(">>>> Willkommen in der unglaublichen Welt des Asuro! <<<<",57);
zeile(5);
SerWrite("Mit welcher Erweiterung soll der Asuro betrieben werden: ",57);
zeile(2);
SerWrite("Druecken sie bitte den entsprechenden Taster:",44);
zeile(2);
SerWrite("1 >>> Ultraschallerweiterung",28);
zeile(1);
SerWrite("2 >>> Lienienerkennung",22);
while(x==0)
{
taster_1=PollSwitch();
abfrage_1=taster_1;
x=taster_1;
taster_1=0;
taster_2=0;
if(x==1) //Ultraschallerweiterung
{
zeile(24);
SerWrite(">>>> Sie moechten den Asuro mit Ultraschallerweiterung betreiben. <<<<",70);
zeile(5);
SerWrite("Programmauswahl:",16);
zeile(2);
SerWrite("Druecken sie bitte den entsprechenden Taster:",44);
zeile(2);
SerWrite("1 >>> Ultraschall-Fahrt",23);
zeile(1);
SerWrite("2 >>> Taster-Fahrt",18);
zeile(1);
taster_2=PollSwitch();
abfrage_2=taster_2;
taster_1=0;
taster_2=0;
if(abfrage_1==1 && abfrage_2==1) //Ultraschall
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Ultraschall-Fahrt gewaehlt! <<<<",59);
zeile(5);
SerWrite(">>>> Kalibrierung der Motoren... <<<<",37);
zeile(5);
odometrie();
SerWrite(">>>> Kalibrierung abgeschlossen <<<<",37);
zeile(5);
verzoegern(100);
countdown();
ultraschall();
}
if(taster_1==1 && taster_2==2) //Taster
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Taster-Fahrt gewaehlt! <<<<",54);
zeile(5);
countdown();
}
}
if(x==2) // Auswahl Linienverfolgung
{
zeile(24);
SerWrite(">>>> Sie moechten den Asuro mit Linienerkennung betreiben. <<<<",63);
zeile(5);
SerWrite("Programmauswahl:",16);
zeile(2);
SerWrite("Druecken sie bitte den entsprechenden Taster:",44);
zeile(2);
SerWrite("1 >>> Selbsttest",16);
zeile(1);
SerWrite("2 >>> Abgrund-Fahrt",19);
zeile(1);
SerWrite("3 >>> Gatter-Fahrt",18);
zeile(1);
SerWrite("4 >>> Linienverfolgungs-Fahrt",29);
zeile(1);
SerWrite("6 >>> Taster-Fahrt",18);
zeile(1);
taster_2=PollSwitch();
abfrage_2=taster_2;
taster_1=0;
taster_2=0;
if(abfrage_1==2 && abfrage_2==1) //Selbsttest
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Sebsttest gewaehlt! <<<<",59);
zeile(5);
countdown();
}
if(abfrage_1==2 && abfrage_2==2) //Abgrund
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Abgrund-Fahrt gewaehlt! <<<<",55);
zeile(5);
countdown();
}
if(abfrage_1==2 && abfrage_2==4) //Gatter
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Gatter-Fahrt gewaehlt! <<<<",55);
zeile(5);
countdown();
}
if(abfrage_1==2 && abfrage_2==8) //Lienienverfolgung
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Lienenverfolgungs-Fahrt gewaehlt! <<<<",65);
zeile(5);
countdown();
}
if(abfrage_1==2 && abfrage_2==32) //Taster
{
zeile(24);
SerWrite(">>>> Sie haben das Programm Taster-Fahrt gewaehlt! <<<<",55);
zeile(5);
countdown();
}
}
}
while(1);
return 0;
}
mfg
Lesezeichen