ECE 4180 Collision Avoiding Robot demo
Dependencies: Motor Servo mbed
main.cpp@1:53e96a6c73db, 2015-10-21 (annotated)
- Committer:
- Soradhmuny
- Date:
- Wed Oct 21 22:29:07 2015 +0000
- Revision:
- 1:53e96a6c73db
- Parent:
- 0:8c4965d9689e
Final Lab 4 4180 program
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Soradhmuny | 0:8c4965d9689e | 1 | #include "mbed.h" |
Soradhmuny | 0:8c4965d9689e | 2 | #include "Servo.h" |
Soradhmuny | 0:8c4965d9689e | 3 | #include "Motor.h" |
Soradhmuny | 0:8c4965d9689e | 4 | |
Soradhmuny | 1:53e96a6c73db | 5 | /* |
Soradhmuny | 1:53e96a6c73db | 6 | Soradhmuny Lanh and Michael Capone |
Soradhmuny | 1:53e96a6c73db | 7 | ECE 4180 Lab 4 Collision Avoiding Robot demo |
Soradhmuny | 1:53e96a6c73db | 8 | */ |
Soradhmuny | 1:53e96a6c73db | 9 | |
Soradhmuny | 0:8c4965d9689e | 10 | Servo myservo(p23); |
Soradhmuny | 0:8c4965d9689e | 11 | AnalogIn ain(p16); |
Soradhmuny | 0:8c4965d9689e | 12 | DigitalOut myled1(LED1); |
Soradhmuny | 0:8c4965d9689e | 13 | DigitalOut myled2(LED2); |
Soradhmuny | 0:8c4965d9689e | 14 | DigitalOut myled3(LED3); |
Soradhmuny | 0:8c4965d9689e | 15 | DigitalOut myled4(LED4); |
Soradhmuny | 0:8c4965d9689e | 16 | float L = 1; |
Soradhmuny | 0:8c4965d9689e | 17 | float negL = -0.5; |
Soradhmuny | 0:8c4965d9689e | 18 | float R = 1; |
Soradhmuny | 0:8c4965d9689e | 19 | float speedL = negL; |
Soradhmuny | 0:8c4965d9689e | 20 | float speedR = R; |
Soradhmuny | 0:8c4965d9689e | 21 | float p = -1; |
Soradhmuny | 0:8c4965d9689e | 22 | int n = 0; |
Soradhmuny | 0:8c4965d9689e | 23 | int main() { |
Soradhmuny | 0:8c4965d9689e | 24 | while(1){ |
Soradhmuny | 0:8c4965d9689e | 25 | Servo myservo(p23); |
Soradhmuny | 0:8c4965d9689e | 26 | speedL = negL; |
Soradhmuny | 0:8c4965d9689e | 27 | speedR = R; |
Soradhmuny | 0:8c4965d9689e | 28 | for(p=-1; p<1.0; p += 0.1) { |
Soradhmuny | 0:8c4965d9689e | 29 | myservo = p; |
Soradhmuny | 0:8c4965d9689e | 30 | if(ain > 0.4) { |
Soradhmuny | 0:8c4965d9689e | 31 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 32 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 33 | myled3 = 1; |
Soradhmuny | 0:8c4965d9689e | 34 | myled4 = 1; |
Soradhmuny | 0:8c4965d9689e | 35 | speedL = L; |
Soradhmuny | 0:8c4965d9689e | 36 | }else if(ain<0.4 && ain > 0.3){ |
Soradhmuny | 0:8c4965d9689e | 37 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 38 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 39 | myled3 = 1; |
Soradhmuny | 0:8c4965d9689e | 40 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 41 | speedL = L;//check random three spikes |
Soradhmuny | 0:8c4965d9689e | 42 | }else if(ain<0.3 && ain > 0.2){ |
Soradhmuny | 0:8c4965d9689e | 43 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 44 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 45 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 46 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 47 | }else if(ain<0.2 && ain > 0.1){ |
Soradhmuny | 0:8c4965d9689e | 48 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 49 | myled2 = 0; |
Soradhmuny | 0:8c4965d9689e | 50 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 51 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 52 | }else { |
Soradhmuny | 0:8c4965d9689e | 53 | myled1 = 0; |
Soradhmuny | 0:8c4965d9689e | 54 | myled2 = 0; |
Soradhmuny | 0:8c4965d9689e | 55 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 56 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 57 | } |
Soradhmuny | 0:8c4965d9689e | 58 | wait(0.2); |
Soradhmuny | 0:8c4965d9689e | 59 | } |
Soradhmuny | 0:8c4965d9689e | 60 | for(p=1.0; p>0.3; p-=.1){ |
Soradhmuny | 0:8c4965d9689e | 61 | myservo = p; |
Soradhmuny | 0:8c4965d9689e | 62 | wait(0.2); |
Soradhmuny | 0:8c4965d9689e | 63 | } |
Soradhmuny | 0:8c4965d9689e | 64 | |
Soradhmuny | 0:8c4965d9689e | 65 | Motor mR(p21, p30, p29); // pwm, fwd, rev |
Soradhmuny | 0:8c4965d9689e | 66 | Motor mL(p22, p28, p27); |
Soradhmuny | 0:8c4965d9689e | 67 | n = 0; |
Soradhmuny | 0:8c4965d9689e | 68 | while(n<40){ |
Soradhmuny | 0:8c4965d9689e | 69 | if(speedL == negL){ |
Soradhmuny | 0:8c4965d9689e | 70 | if(ain > 0.4) { |
Soradhmuny | 0:8c4965d9689e | 71 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 72 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 73 | myled3 = 1; |
Soradhmuny | 0:8c4965d9689e | 74 | myled4 = 1; |
Soradhmuny | 0:8c4965d9689e | 75 | speedL = 0; |
Soradhmuny | 0:8c4965d9689e | 76 | speedR = 0; |
Soradhmuny | 0:8c4965d9689e | 77 | }else if(ain<0.4 && ain > 0.3){ |
Soradhmuny | 0:8c4965d9689e | 78 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 79 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 80 | myled3 = 1; |
Soradhmuny | 0:8c4965d9689e | 81 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 82 | speedL = 0;//check random three spikes |
Soradhmuny | 0:8c4965d9689e | 83 | speedR = 0; |
Soradhmuny | 0:8c4965d9689e | 84 | }else if(ain<0.3 && ain > 0.2){ |
Soradhmuny | 0:8c4965d9689e | 85 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 86 | myled2 = 1; |
Soradhmuny | 0:8c4965d9689e | 87 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 88 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 89 | }else if(ain<0.2 && ain > 0.1){ |
Soradhmuny | 0:8c4965d9689e | 90 | myled1 = 1; |
Soradhmuny | 0:8c4965d9689e | 91 | myled2 = 0; |
Soradhmuny | 0:8c4965d9689e | 92 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 93 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 94 | }else { |
Soradhmuny | 0:8c4965d9689e | 95 | myled1 = 0; |
Soradhmuny | 0:8c4965d9689e | 96 | myled2 = 0; |
Soradhmuny | 0:8c4965d9689e | 97 | myled3 = 0; |
Soradhmuny | 0:8c4965d9689e | 98 | myled4 = 0; |
Soradhmuny | 0:8c4965d9689e | 99 | } |
Soradhmuny | 0:8c4965d9689e | 100 | mR.speed(speedR); |
Soradhmuny | 0:8c4965d9689e | 101 | mL.speed(speedL); |
Soradhmuny | 0:8c4965d9689e | 102 | wait(.1); |
Soradhmuny | 0:8c4965d9689e | 103 | n++; |
Soradhmuny | 0:8c4965d9689e | 104 | }else{ |
Soradhmuny | 0:8c4965d9689e | 105 | mR.speed(speedR); |
Soradhmuny | 0:8c4965d9689e | 106 | mL.speed(speedL); |
Soradhmuny | 0:8c4965d9689e | 107 | wait(.1); |
Soradhmuny | 0:8c4965d9689e | 108 | n = n+8; |
Soradhmuny | 0:8c4965d9689e | 109 | } |
Soradhmuny | 0:8c4965d9689e | 110 | |
Soradhmuny | 0:8c4965d9689e | 111 | } |
Soradhmuny | 0:8c4965d9689e | 112 | speedR = 0; |
Soradhmuny | 0:8c4965d9689e | 113 | speedL = 0; |
Soradhmuny | 0:8c4965d9689e | 114 | mR.speed(speedR); |
Soradhmuny | 0:8c4965d9689e | 115 | mL.speed(speedL); |
Soradhmuny | 0:8c4965d9689e | 116 | wait(.5); |
Soradhmuny | 0:8c4965d9689e | 117 | } |
Soradhmuny | 0:8c4965d9689e | 118 | } |