ECE 4180 Collision Avoiding Robot demo
Dependencies: Motor Servo mbed
Diff: main.cpp
- Revision:
- 0:8c4965d9689e
- Child:
- 1:53e96a6c73db
diff -r 000000000000 -r 8c4965d9689e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Oct 21 22:18:23 2015 +0000 @@ -0,0 +1,113 @@ +#include "mbed.h" +#include "Servo.h" +#include "Motor.h" + +Servo myservo(p23); +AnalogIn ain(p16); +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); +float L = 1; +float negL = -0.5; +float R = 1; +float speedL = negL; +float speedR = R; +float p = -1; +int n = 0; +int main() { + while(1){ + Servo myservo(p23); + speedL = negL; + speedR = R; + for(p=-1; p<1.0; p += 0.1) { + myservo = p; + if(ain > 0.4) { + myled1 = 1; + myled2 = 1; + myled3 = 1; + myled4 = 1; + speedL = L; + }else if(ain<0.4 && ain > 0.3){ + myled1 = 1; + myled2 = 1; + myled3 = 1; + myled4 = 0; + speedL = L;//check random three spikes + }else if(ain<0.3 && ain > 0.2){ + myled1 = 1; + myled2 = 1; + myled3 = 0; + myled4 = 0; + }else if(ain<0.2 && ain > 0.1){ + myled1 = 1; + myled2 = 0; + myled3 = 0; + myled4 = 0; + }else { + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; + } + wait(0.2); + } + for(p=1.0; p>0.3; p-=.1){ + myservo = p; + wait(0.2); + } + + Motor mR(p21, p30, p29); // pwm, fwd, rev + Motor mL(p22, p28, p27); + n = 0; + while(n<40){ + if(speedL == negL){ + if(ain > 0.4) { + myled1 = 1; + myled2 = 1; + myled3 = 1; + myled4 = 1; + speedL = 0; + speedR = 0; + }else if(ain<0.4 && ain > 0.3){ + myled1 = 1; + myled2 = 1; + myled3 = 1; + myled4 = 0; + speedL = 0;//check random three spikes + speedR = 0; + }else if(ain<0.3 && ain > 0.2){ + myled1 = 1; + myled2 = 1; + myled3 = 0; + myled4 = 0; + }else if(ain<0.2 && ain > 0.1){ + myled1 = 1; + myled2 = 0; + myled3 = 0; + myled4 = 0; + }else { + myled1 = 0; + myled2 = 0; + myled3 = 0; + myled4 = 0; + } + mR.speed(speedR); + mL.speed(speedL); + wait(.1); + n++; + }else{ + mR.speed(speedR); + mL.speed(speedL); + wait(.1); + n = n+8; + } + + } + speedR = 0; + speedL = 0; + mR.speed(speedR); + mL.speed(speedL); + wait(.5); + } +}