Code:
//----------------------------------------------------------------------------------
//Projekt: HAUS VOM NIKOLAUS
//----------------------------------------------------------------------------------
#include "asuro.h"
#define TRIGGERLEVEL 680
#define HYSTERESIS 30
#define LOW 0
#define HIGH 1
//----------------------------------------------------------------------------------
//Variablen deklarieren
//----------------------------------------------------------------------------------
unsigned int data [2];
signed int status [2]={0,0};
unsigned int speed=255;
//Startwert für halbe Motorgeschwindigkeit
signed int difference=0;
unsigned int distance=0;
int i;
int zaehler=0;
//----------------------------------------------------------------------------------
//Unterprogramm Odometrie
//----------------------------------------------------------------------------------
void Odometrie (void)
{
//Helligkeitswerte der Lichtschranken auslesen
OdometrieData(data);
//Wechsel linker Sensor von niedrig auf hoch?
if((status[0]==LOW)&&(data[0]>TRIGGERLEVEL+HYSTERESIS)) {
status[0]=HIGH;
difference++;
distance++; }
//Wechsel linker Sensor von hoch auf niedrig?
if((status[0]==HIGH)&&(data[0]<TRIGGERLEVEL-HYSTERESIS)) {
status[0]=LOW;
difference++;
distance++; }
//Wechsel rechter Sensor von niedrig auf hoch?
if((status[1]==LOW)&&(data[1]>TRIGGERLEVEL+HYSTERESIS)) {
status[1]=HIGH;
difference--; }
//Wechsel rechter Sensor von hoch auf niedrig?
if((status[1]==HIGH)&&(data[1]<TRIGGERLEVEL-HYSTERESIS)) {
status[1]=LOW;
difference--; }
//zur Sicherheit: verhindern, dass der Differenzzähler den erlaubten
//Wertebereich verlässt
if(difference<-155) difference=-155;
if(difference>155) difference=155;
//StatusLED noch entsprechend der erkannten Segmente aufleuchten
//lassen, grün für links, rot für rechts
StatusLED(status[0]+status[1]*2);
//Zähldifferenz passemd auf die Motoren verteilen
if(difference>0) MotorSpeed(155-difference,155);
else MotorSpeed(155,155+difference);
}
//----------------------------------------------------------------------------------
//Unterprogramm Haus (Quadrat, Kantenlänge 29cm)
//----------------------------------------------------------------------------------
void Haus (void)
{
//Schleife (wird 4mal ausgeführt), da 4 Seiten
while (zaehler<4)
{
switch(distance)
{
//Hausseite
case 0: MotorDir(FWD,FWD);
MotorSpeed(speed,speed);
break;
case 110: speed=200;
break;
case 120: speed=150;
break;
//Drehung nach links, 90°
case 145: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(RWD,FWD);
speed=255;
distance=146;
break;
case 176: speed=200;
break;
case 182: speed=150;
break;
//stehen bleiben und kurz warten
case 186: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,FWD);
speed=255;
//zurück zum Anfang
distance=0;
//Schleifenzähler
zaehler++;
break;
}
Odometrie();
}
//stehen bleiben, nachdem ASURO ein Quadrat gefahren ist
MotorDir(BREAK,BREAK);
}
//--------------------------------------------------------------------------------
//Unterprogramm Dach1 (Diagonale 1, 41cm; Dach Seite 1, 19cm)
//--------------------------------------------------------------------------------
void Dach1 (void)
{
zaehler=0;
while(zaehler<1)
{
switch(distance)
{
//Drehung nach links, 45°
case 0: MotorDir(RWD,FWD);
MotorSpeed(speed,speed);
distance=1;
break;
case 10: speed=200;
break;
case 15: speed=150;
break;
//Diagonale 1
case 21: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,FWD);
MotorSpeed(speed,speed);
break;
case 190: speed=200;
break;
case 200: speed=150;
break;
//Drehung nach rechts, 95°
case 227: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,RWD);
speed=255;
distance=228;
break;
case 248: speed=200;
break;
case 258: speed=150;
break;
//Dach Seite 1, linke Seite
case 268: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,FWD);
speed=255;
distance=269;
break;
case 340: speed=200;
break;
case 350: speed=150;
break;
case 363: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
zaehler++;
break;
}
Odometrie();
}
MotorDir(BREAK,BREAK);
}
//---------------------------------------------------------------------------------
//Unterprogramm Dach2 (Diagonale 2, 41cm; Dach Seite 2, 19cm)
//---------------------------------------------------------------------------------
void Dach2 (void)
{
zaehler=0;
while(zaehler<1)
{
switch(distance)
{
//Drehung nach rechts, 80°
case 0: MotorDir(RWD,FWD);
MotorSpeed(speed,speed);
distance=1;
break;
case 25: speed=200;
break;
case 30: speed=150;
break;
//Dach Seite 2, rechte Seite
case 37: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,FWD);
MotorSpeed(speed,speed);
break;
case 110: speed=200;
break;
case 120: speed=150;
break;
//Drehung nach rechts, 95°
case 131: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,RWD);
speed=255;
distance=132;
break;
case 150: speed=200;
break;
case 160: speed=150;
break;
//Diagonale 2
case 171: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
MotorDir(FWD,FWD);
speed=255;
distance=172;
break;
case 340: speed=200;
break;
case 350: speed=150;
break;
case 377: MotorDir(BREAK,BREAK);
for (i=0; i<100; i++)
{
Sleep(255);
}
zaehler++;
break;
}
Odometrie();
}
MotorDir(BREAK,BREAK);
}
//-----------------------------------------------------------------------------------
//Hauptprogramm
//-----------------------------------------------------------------------------------
int main (void)
{
Init();
while(zaehler<1)
{
BackLED(OFF,OFF);
StatusLED(OFF);
FrontLED(ON);
Haus();
FrontLED(OFF);
StatusLED(RED);
//MotorDir(FWD,FWD);
Dach1();
StatusLED(GREEN);
//MotorDir(FWD,FWD);
Dach2();
BackLED(ON,ON);
zaehler++;
}
return 0;
}
denke allerdings, dass dieses programm genau das gleiche macht wie meines von oben, vielleicht habe ih ja au nen denkfehler drin.
Zitieren
Lesezeichen