Also hier erstmal der Code:
Code:
#include "asuro.h" // Schwellwert Hell Dunkel
#define TRIGGERLEVEL 540
#define HYSTERESIS 10
#define LOW 0
#define HIGH 1
int main (void){
unsigned int data[2];
signed int status [2]={0,0};
signed int difference=0;
unsigned int distance=0;
signed int speed;
Init();
while(1){
switch(distance){
case 0:MotorDir(FWD,FWD);speed=255;break;
case 150:speed=200; break;
case 160:speed=150;break;
case 172:MotorDir(BREAK,BREAK);break;
}
OdometrieData(data);
if((status[0]==LOW)&&(data[0]>TRIGGERLEVEL+HYSTERESIS)){
status[0]=HIGH;
difference++;
distance++;
}
if((status[0]==HIGH)&&(data[0]<TRIGGERLEVEL-HYSTERESIS)){
status[0]=LOW;
difference++;
distance++;
}
if((status[1]==LOW)&&(data[1]>TRIGGERLEVEL+HYSTERESIS)){
status[1]=HIGH;
difference--;
}
if((status[1]==HIGH)&&(data[1]<TRIGGERLEVEL-HYSTERESIS)){
status[1]=LOW;
difference--;
}
if (difference<-speed)difference=-speed;
if (difference>speed)difference=speed;
StatusLED(status[0]+status[1]*2);
if (difference>0)MotorSpeed(speed-difference,speed);
else MotorSpeed(speed,speed+difference);
}
return 0;
}
Lesezeichen