Mathias Riis
/
UltrasonicCasper
Porn
Fork of UltrasonicCasper by
Diff: main.cpp
- Revision:
- 3:bebb73b82a17
- Parent:
- 2:39179781a18c
--- a/main.cpp Fri Apr 10 23:54:59 2015 +0000 +++ b/main.cpp Sat Apr 11 08:10:49 2015 +0000 @@ -23,42 +23,74 @@ } +//SELVKØRENDE I MØRKET! +void selfDrive(){ + + int dLEFT = sensorLEFT.distance(CM); + int dRIGHT = sensorRIGHT.distance(CM); + + // Writes the left and right distance variable + pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT)); + pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT)); + + + //Drej til venstre for at rette op + if(dRIGHT > 60 && dLEFT > 60 || BT.getc()=='p'){ + control = 0; + } + + else if(dRIGHT > 35){ + leftForward = 0.75; + leftBackward = 0.25; + rightForward = 0.25; + rightBackward = 0.75; + } + + else if(dLEFT > 35){ + leftForward = 0.25; + leftBackward = 0.75; + rightForward = 0.75; + rightBackward = 0.25; + } + + else if(dRIGHT > 50){ + leftForward = 0.1; + leftBackward = 0.9; + rightForward = 0.9; + rightBackward = 0.1; + } + + else if(dLEFT > 50){ + leftForward = 0.1; + leftBackward = 0.9; + rightForward = 0.9; + rightBackward = 0.1; + + else{ + leftForward = 1; + leftBackward = 0; + rightForward = 1; + rightBackward = 0; + } +} // The main() function int main() { + // The loop() function while(1) { - //left and right distance variables - int dLEFT = sensorLEFT.distance(CM); - int dRIGHT = sensorRIGHT.distance(CM); + //left and right distance variables + selfControl(dRIGHT, dLEFT); - // Writes the left and right distance variable - pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT)); - pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT)); // Delay wait(0.5); - - //LED control - if(dRIGHT<=10) { - led = 0; - led2 = 1; - } - - if(dRIGHT>=40) { - led2 = 0; - led = 1; - } - if(dRIGHT>=11 && dRIGHT<=39) { - led2 = 0; - led=0; - } } } + -