Code:
Stand 14.Dezember 2006 00:16 Uhr
Version 1.3
--------------------------------------------
Asuroprojekt: Der Weg durch das Labyrinth
Asuroname: Odysseus I
Gruppe: S. Gessulat, M. SchŸmann, J. Uecker
--------------------------------------------
EinrŸckung nach Horstman
Unterfunktionen alphabetisch geordnet
--------------------------------------------
Aufgrund von Compilerproblemen werden an
vielen Stellen while-Schleifen statt
for-Schleifen verwendet
--------------------------------------------
Aufgrund eines Lštfehlers sind die Dreh-
richtungen der Motoren invertiert, fŸr
VorwŠrtsbetrieb ist MotorDir(RWD) zu setzen,
fŸr RŸckwŠrtsbetrieb MotorDir(FWD)
--------------------------------------------
Versionshistory:
1.0:
- Alle notwendigen Funktionen sind
implementiert, sind getestet und
funktionieren
1.1:
- Linienverfolgung wurde verbessert (ist
jetzt sowohl schneller, als auch genauer
-> kleine Toleranz eingefŸhrt)
- Kommentare wurden Ÿberarbeitet
1.2:
- Ausgabefunktion wurde entschlankt und
durch effizienteren Code verbessert
(ca. 10 pages weniger)
- Kommentare Ÿberarbeitet und vereinheitlicht
- Komplettes Programm durchkommentiert
1.3:
- PrŠsentationsreif
---------------------------------------------
Funktionen im Überblick:
Name ------------ Ÿbergebene Parameter -- Status ---- Zustand ------- Testzustand
+ Ausgabe *feld komplett funktioniert ist getestet
+ Blink void komplett funktioniert ist getestet
+ BodenTest &motorl,&motorr komplett funktioniert ist getestet
+ Dreh90 richtung(LEFT/RIGHT) komplett funktioniert ist getestet
+ Fahren richtung(FWD/RWD/BREAK) komplett funktioniert ist getestet
+ Koltest void komplett funktioniert ist getestet
+ LichtTest &schwarz,&weiss komplett funktioniert ist getestet
+ LinieAusrichten richtung(LEFT/RIGHT) komplett funktioniert ist getestet
+ LinieFinden void komplett funktioniert ist getestet
+ LinieVerfolgen distanz(int) komplett funktioniert ist getestet
- LinieLaenge - verworfen ungenau verworfen
- LinieVerfolgen distanz, Richtung verworfen nicht mšglich technisch nicht Umsetzbar
- Notfall - - - verworfen Zeitmangel
- Steht - - - verworfen Zeitmangel
---------------------------------------------
To-Do List:
- Quellcode optimieren
- Quellcode sŠubern
- Kommentare Ÿberarbeiten
---------------------------------------------
*/
//--- Funktionsdeklarationen ----------------
void Ausgabe( void );
void Blink( void );
void BodenTest( int *motorl, int *motorr );
void Dreh90(unsigned char richtung);
void Fahren( unsigned char richtung);
void KolTest( void );
void LichtTest( int *schwarz, int *weiss );
void LinieAusrichten( unsigned char richtung );
int LinieFinden( void );
int LinieVerfolgen( int distanz);
//------------------------------------------
//--- Globale Variablen --------------------
int motorr = 110; // Variablen fŸr die Motorwerte - mšglichst kleine Werte
int motorl = 110; // Werden nochmal angepasst durch Bodentest();
int schwarz = 100; // Variablen fŸr die Lichtwerte
int weiss = 923; // Werden nochmal angepasst durch Lichtmessen();
int lageplan[4][3];
//------------------------------------------
int main( void )
{
//--- Initialisierung ----------------------
Init(); // Initialisieren
int x,y; // vorher anlegen, sonst kommt es zu Compilerfehlern
for( x=0; x<4; x++ ) // Array mit Nullen vollschreiben
for( y=0; y<3; y++ )
lageplan[x][y]= 0;
lageplan[3][2]=1;
BodenTest(&motorl, &motorr); // Werte fŸr motorr und motorl anpassen
LichtTest(&schwarz, &weiss); // Werte fŸr schwarz und weiss anpassen
//------------------------------------------
/*
Asuro zeigt durch Blinken an, dass die
Messungen abgeschlossen sind und Wartet
auf Startbefehl (Tasterdruck)
*/
//--- Beginn der Hauptroutine --------------
int i=2; // fŸr Operationen mit den Reihen (0 bis 2 = 3)
int j; // fŸr Operationen mit den Spalten
Msleep(500); // Kurz warten vor dem Start
Fahren(RWD); // losfahren
Msleep(2000); // nur kurz bis ins Labyrinth
Encoder_Init(); // Encoder resetten fŸr die Turnfunktion
Encoder_Set(0,0); // auf 0 setzen
Turn(30,110); // halblinks einschlagen um die Linie besser zu finden
Fahren(RWD); // Weiterfahren
LinieFinden(); // Liniefinden
// Reihenschleife
while(i>=0) // sind alle 3 Linien schon durchlaufen? Wenn nicht
{ if((2!=i)&&(lageplan[i+1][0])) // PrŸfen, dass er nicht am Ende ist UND ob er ganz links ist
{ LinieAusrichten(RIGHT); // Wenn ja, dann nach rechts ausrichten
LinieVerfolgen(300); // kurz fahren um gerade zu stehen
Dreh90(LEFT); // Wieder senkrecht zur Linie
Fahren(FWD); // ZurŸcksetzen
LinieFinden(); // bis zur Linie
}
LinieAusrichten(LEFT); // nach Links ausrichten
LinieVerfolgen(1500); // bis zum linken Ende der Linie fahren
Dreh90(RIGHT); // nach rechts drehen
// Spaltenschleife
j=0; // in jeder Reihe auf nullsetzen j steht fŸr die Spalten
while(j<3) // Alle drei Spalten durchlaufen
{ Msleep(500); // kurz warten
Fahren(RWD); // Geradeausfahren
if(LinieFinden()) // kucken ob eine Linie oder eine Wand kommt ( Linie = 1)
{ lageplan[i][j]=1; // Wenn Linie kommt, ist dort ein Durchgang, merken
j=42; // aus der Spalten-Schleife in die nŠchste Reihe
}else // keine Liniegefunden -> Wand
{ Fahren(FWD); // zurŸcksetzen
Msleep(100); // kurz warten
LinieFinden(); // ZurŸckfahren bis zur Linie
LinieAusrichten(RIGHT); // Nach rechts ausrichten
LinieVerfolgen(350); // In die nŠchste Spalte fahren
Dreh90(LEFT); // zur Wand (oder Durchgang) hin ausrichten
j++; // Spaltencounter erhšhen, und die Schleife nochmal durchlaufen
}
}
i--;
}
Blink();
//------------------------------------------
/*
Asuro zeigt durch Blinken an, dass er den
Schatz gefunden hat. Er wartet auf Taster-
druck und gibt dann den Weg aus.
*/
Ausgabe();
while(1){}; // Sicher ist sicher!
}
//--- Definition der Unterfunktionen ---------------------------------------------------------------------
//--- Ausgabe ------------------------------
// Ausgabe berechnet den kŸrzesten Weg aus
// lageplan, und sendet die Daten an den
// EmpfŠnger
void Ausgabe( void )
{
KolTest(); // Wartet auf Tastendruck
//--- Daten ausgeben -----------------------
SerWrite(" ------------\r\n",16); // Rahmen oben
int i, j; // vorher deklarieren
for( i=0; i<3; i++) // fŸr alle drei Reihen
{ SerWrite(" | ",3); // Rahmen rechts
for( j=0; j<3; j++ ) // fŸr alle drei Spalten
{ PrintInt(lageplan[i][j]); // Daten ausgeben ( 0 = Wand, 1 = Durchgang)
if (j != 2) SerWrite( " - ",3); // Platzhalter setzen (nur 2)
}
SerWrite(" |\r\n",5); // Rahmen rechts
}
SerWrite(" ------------\r\n",16); // Rahmen unten
SerWrite(" \r\n",3); // Eine Leerzeile zur Übersicht
//--- Grafische Ausgabe der Koordinaten ----
//--------- 1. Zeile ------------------------
j=0; // Schleifenparameter rŸcksetzen
while(j<3)
{ if(lageplan[0][j]) // Wenn Ziel an Stelle J
SerWrite(" Ziel ",6); // Ziel schreiben
else // sonst
SerWrite(" ",6); // Leerzeichen
j++; // Schleifenparameter erhšhen
}
SerWrite("\r\n",2); // Sprung zur nŠchsten Zeile
//--------- 2. bis 7. Zeile -----------------
i=0; // Šußeren Schleifenparameter rŸcksetzen
while(i<3)
{ j=0; // inneren Schleifenparameter rŸcksetzen
//--------- 2. / 4. / 6. Zeile---------------
while(j<3)
{ if(lageplan[i][j]) // Wenn Tor an dieser Stelle
SerWrite("+ . ",6); // Tor zeichnen
else // Sonst
SerWrite("+-----",6); // Wand zeichnen
j++; // Inneren Schleifenparameter erhšhen
}
SerWrite("+\r\n",3); // Letztes WandstŸck zeichnen und zur nŠchsten Zeile
//--------- 3. / 5. / 7. Zeile---------------
SerWrite("|",1); // WandstŸck zeichnen
if((lageplan[i][1] & lageplan[i+1][0]) || (lageplan[i][0] & lageplan[i+1][1]))
SerWrite(" ....... ",17);// Weg von der Mitte nach links oder umgekehrt
else
if((lageplan[i][2] & lageplan[i+1][1]) || (lageplan[i][1] & lageplan[i+1][2]))
SerWrite(" ....... ",17); // Weg von der Mitte nach rechts oder umgekehrt
else
if((lageplan[i][0] & lageplan[i+1][2]) || (lageplan[i][2] & lageplan[i+1][0]))
SerWrite(" ............. ",17); // Weg von links nach rechts oder umgekehrt
else
{ j=0; // inneren Schleifenparameter rŸcksetzen
while(j<3)
{ if(lageplan[i][j] & lageplan[i+1][j])// Wenn Zwei Tore Ÿbereinander an Stelle j
SerWrite(" . ",5); // Spur zeichnen
else // Sonst
SerWrite(" ",5); // Leerzeichen
if(j<2) // Zweimal
SerWrite(" ",1); // FŸllzeichen
j++; // inneren Schleifenparameter erhšhen
}
}
SerWrite("|\r\n",3); // WandstŸck zeichnen und zur nŠchsten Zeile
i++; // Äußeren Schleifenparameter erhšhen
}
//--------- 8. + 9. Zeile -------------------
SerWrite("+-----+-----+ . +\r\n",21); // Erste Wand sieht immer gleich aus
SerWrite(" Start\r\n",20); // Start ist auch immer an der gleichen Stelle
}
//------------------------------------------
//--- Blink --------------------------------
// Blinkt um wichtige Stationen im Programm
// anzuzeigen
void Blink( void )
{
int i = 0; // Schleifenparameter anlegen
while(i<3) // Es soll 3 mal geblinkt werden.
{ BackLED( ON, OFF ); // Die BackLEDs wechseln sich im 200-Milisekundentakt ab.
Msleep(200);
BackLED( OFF, ON );
Msleep(200);
i++;
}
BackLED( OFF, OFF ); // fertig geblinkt - BackLEDs aus.
}
//------------------------------------------
//--- BodenTest ----------------------------
// Ermittelt (anhand der Encoder) Motorwerte
// zum geradeausfahren
void BodenTest( int *motorl, int *motorr ) // Call by Reference Zugriff
{
int rechts=0; // Variablen fŸr die Encoderwerte initialisieren
int links=0; // werden in der While-Schleife fŸr die if Abfragen benštigt
int dif=0; // als Differenz von links und rechts
int da = 42; // fŸr D. Adams - honoris causae
while( 42 == da )
{ MotorDir(RWD,RWD); // Motor auf vorwŠrts stellen
StatusLED(RED); // StatusLED rot
if(rechts < links) // PrŸft ob das rechte Rad weniger gefahren ist als das Linke
*motorr+=1; // Wenn ja: Motorwert fŸr rechts erhšhen
else if(rechts > links) // PrŸft ob das rechte Rad mehr gefahren ist als das Linke
*motorl+=1; // Wenn ja: Motorwert fŸr links erhšhen
Encoder_Init(); // Encoder neu starten
Encoder_Set(0,0); // und resetten
MotorSpeed(*motorl,*motorr); // Motorwerte einsetzen und damit...
Msleep(1000); // 1 Sekunden fahren
MotorSpeed(0,0); // Stoppen
rechts = encoder[RIGHT]; // Encoder rechts auslesen
links = encoder[LEFT]; // Encoder links auslesen
dif = rechts - links; // Differenz wird berechnet
if(0 == dif) // Wenn beide RŠder gleichweit gefahren sind
da=0; // -> aus der While springen
Msleep(300); // Kurz warten um durchdrehende RŠder zu verhindern
StatusLED(GREEN); // GrŸnes Licht fŸr den RŸckwertsgang
MotorDir(FWD,FWD); // Motor auf rŸckwŠrts stellen
MotorSpeed(*motorl,*motorr); // Die aktuellen Motorwerte benutzen
Msleep(1075); // die gleiche Strecke zurŸckzufahren (dauert etwas lŠnger)
MotorSpeed(0,0); // Stoppen
Msleep(300); // Kurz warten um Hochstarts zu verhindern
}
Blink(); // Signalisiert, dass er Fertig ist
KolTest(); // Wartet auf nŠchsten Schritt
}
//------------------------------------------
//-- Dreh90 --------------------------------
// Dreht sich um 90 Grad in die Ÿbergebene
// Richtung
void Dreh90(unsigned char richtung)
{
Encoder_Init(); // Encoder resetten
Encoder_Set(0,0); // auf 0 setzen
if( LEFT==richtung ) // richtung prŸfen
Turn(79,100); // nach links drehen
else
Turn(-79,100); // nach rechts drehen
}
//------------------------------------------
//--- Fahren -------------------------------
// FŠhrt geradeaus in die Ÿbergebene
// Richtung. Benutzt dazu die ermittelten
// Motorwerte motorl und motorr
void Fahren( unsigned char richtung)// FŠhrt vorwŠrts/rŸckwerts - MotorstŠrke unabhŠngig skalierbar
{
MotorDir(richtung,richtung); // Richtung einstellen
MotorSpeed( motorl, motorr ); // Motorspeed einstellen
}
//------------------------------------------
//--- KolTest ------------------------------
// PrŸft alle 50 Milisekunden ob die
// Schalter gedrŸckt sind und springt dann
// wieder zurŸck in die †bergeordnete
// Funktion
void KolTest( void )
{
switched=0; // switched ist die Variable des RŸckgabewertes der Sensoren
StartSwitch(); // Startet die Sensorenabfrage
while(!switched) // Wartet solange bis ein Tastendruck kommt.
{ Msleep(50); // Hier wird StartSwitch() bzw. den Sensoren etwas Zeit gegeben
}
}
//------------------------------------------
//--- LichtTest ----------------------------
// Ermittelt Werte fŸr schwarz und weiss
// Damit diese an die aktuellen Licht-
// verhŠltnisse angepasst sind
void LichtTest( int *schwarz, int *weiss ) // Call by Refernce Zugriff
{
// Auf Schattenwurf des ASURO achten!
unsigned int data[2]; // Array fŸr Lichtwerte anlegen
FrontLED(ON); // FrontLED fŸr den Messvorgang anstellen
StatusLED(GREEN); // Dem Benutzer anzeigen, dass der Asuro startbereit ist
Msleep(300);
KolTest(); // Auf Benutzer warten
StatusLED(RED);
int i=0; // Vorher anlegen sonst kommt es zu Compilerfehlern
while(i<5) // Misst 5 mal die Werte fŸr Schwarz
{ LineData(data); // um besonders gute Messwerte zu erhalten
Msleep(50);
*schwarz = *schwarz+( (data[0]+data[1]) /2);
i++;
}
*schwarz = (*schwarz/5);
StatusLED(GREEN); // Dem Benutzer anzeigen, dass der Asuro startbereit ist
Msleep(1000);
KolTest(); // Auf Benutzer warten
StatusLED(RED);
i=0; // Schleifenparameter rŸcksetzen
while(i<5) // Misst 5 mal die Werte fŸr Weiss
{ LineData(data); // um besonders gute Messwerte zu erhalten
Msleep(50);
*weiss = *weiss+( (data[0]+data[1]) /2);
i++;
}
*weiss = (*weiss/5);
StatusLED(GREEN); // Test erfolgreich
*schwarz=*schwarz+40; // Toleranz von 40 (empirisch ermittelt)
*weiss=*weiss-200; // Toleranz von 200 (empirisch ermittelt)
FrontLED(OFF); // Licht aus -> Strom sparen
Blink(); // Signalisiert, dass er fertig ist
KolTest(); // Wartet auf nŠchsten Schritt
}
//------------------------------------------
//--- LinieAusrichten ----------------------
// Richtet sich in eine bestimmte Richtung
// auf der Linie aus, wenn er vorher
// Senkrecht zu ihr gestanden hat.
// Im Gegensatz zu Turn sucht er sich
// wircklich die Linie
void LinieAusrichten( unsigned char richtung )
{
MotorDir(RWD,RWD); // Stellt Motorrichtung ein, sicher ist sicher
MotorSpeed(motorl,motorr); // FŠhrt ein StŸck nach vorne
Msleep(150); // FŸr 150ms
if ( LEFT == richtung ) // Wenn er sich nach Links ausrichten soll
MotorSpeed(0, motorr); // Dreht er sich nach links
else if ( RIGHT == richtung ) // Wenn er sich nach rechts ausrichten soll
MotorSpeed(motorl,0); // dreht er sich nach rechts
Msleep(250); // Warten damit LinieFinden nicht sofort losgeht
LinieFinden(); // Dreht sich Solange bis er die Linie wiedergefunden hat
}
//------------------------------------------
//--- LinieFinden --------------------------
// BehŠlt solange den vorherigen Zustand
// bis er entweder eine Linie oder eine Wand
// findet und gibt einen dementsprechenden
// RŸckgabewert
// Linie = 1
// Wand = 0
int LinieFinden( void )
{
FrontLED(ON); // Licht an, damit was gemessen werden kann
StatusLED(RED); // StatusLED rot
unsigned int data[2]; // Array anlegen fŸr die Liniensensorwerte
switched=0; // switched, Variable des RŸckgabewertes der Sensoren
StartSwitch(); // Startet die Sensorenabfrage
while(!switched) // Wartet solange bis ein Tastendruck kommt.
{ LineData(data); // holt sich die Liniensensorwerte
if( schwarz > data[0] || schwarz > data[1]) // Vergleicht beide Werte mit Schwarzwert
{ MotorSpeed(0,0); // Stopp
StatusLED(GREEN);
FrontLED(OFF);
return 1; // und Sprung aus der Schleife 1 zurŸck
}
}
MotorSpeed(0,0); // Wand -> Stopp
StatusLED(GREEN); // StatusLED grŸn
FrontLED(OFF); // Licht aus -> Strom sparen
return 0; // ZurŸckgeben, dass er gegen eine Wand gefahren ist
}
//------------------------------------------
//--- LinieVerfolgen -----------------------
// Verfolgt eine Linie Ÿber eine angegebene
// Distanz, oder bricht ab, wenn die Linie
// endet oder, er sie verloren hat und gibt
// entsprechende Werte zurŸck
// Linie verloren = 0
// Linie nicht verloren = 1
// RŸckgabewerte werden im Hauptprogramm
// nicht mehr ausgewertet, diente zum Testen
int LinieVerfolgen( int distanz)
{
int count = 0; // Einen Counter mit 0 initialisieren.
unsigned int data[2]; // Array fŸr Liniensensorwerte anlegen
FrontLED(ON); // Licht vorne an, damit etwas gemessen werden kann
MotorDir(RWD,RWD); // Motor vorwŠrts stellen. Sicher ist Sicher
Encoder_Init(); // Encoder wird neu gestartet
Encoder_Set(0,0); // und geresettet
while(count < distanz) // Abbruchbedingung Distanz zuende
{ count++; // Counter zŠhlt hoch
LineData(data); // Lichtwerte werden gemessen
if(data[0] > weiss && data[1] > weiss) // Hier wird ŸberprŸft, ob Linie verloren
{ MotorSpeed(0,0); // Falls das der Fall ist, Stop
return 1; // und Fehlermeldung ans Hauptprogramm
}
if(data[0]>data[1]&&(data[0]-data[1])>10) // Links ist es heller als rechts? Toleranz 10
{ MotorSpeed(motorl,0); // nach rechts steuern
Msleep(5); // ein wenig Zeit geben
}else
if(data[0]<data[1]&&(data[1]-data[0])>10) // rechts ist es heller als links? Toleranz 10
{ MotorSpeed(0,motorr); // nach links steuern
Msleep(5); // ein wenig Zeit geben
}else // sonst fŠhrt er gerade auf der Linie
{ MotorSpeed(motorl,motorr); // Motorenwerte auf "gerade fahren"
Msleep(5);
}
}
MotorSpeed(0,0); // Distanz ist erreicht. Stop
StatusLED(GREEN); // StatusLED GrŸn
FrontLED(OFF); // Licht aus -> Strom sparen
return 0;
}
Berechtigungen
Zitieren
Lesezeichen