Soradhmuny Lanh / Mbed 2 deprecated 4180Lab4SoradhmunyAndMichael

ECE 4180 Collision Avoiding Robot demo

Dependencies:   Motor Servo mbed

main.cpp@0:8c4965d9689e, 2015-10-21 (annotated)

Committer:
Soradhmuny
Date:
Wed Oct 21 22:18:23 2015 +0000
Revision:
0:8c4965d9689e
Child:
1:53e96a6c73db
Final Program for Lab 4;

Who changed what in which revision?

UserRevisionLine numberNew 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 0:8c4965d9689e 5 Servo myservo(p23);
Soradhmuny 0:8c4965d9689e 6 AnalogIn ain(p16);
Soradhmuny 0:8c4965d9689e 7 DigitalOut myled1(LED1);
Soradhmuny 0:8c4965d9689e 8 DigitalOut myled2(LED2);
Soradhmuny 0:8c4965d9689e 9 DigitalOut myled3(LED3);
Soradhmuny 0:8c4965d9689e 10 DigitalOut myled4(LED4);
Soradhmuny 0:8c4965d9689e 11 float L = 1;
Soradhmuny 0:8c4965d9689e 12 float negL = -0.5;
Soradhmuny 0:8c4965d9689e 13 float R = 1;
Soradhmuny 0:8c4965d9689e 14 float speedL = negL;
Soradhmuny 0:8c4965d9689e 15 float speedR = R;
Soradhmuny 0:8c4965d9689e 16 float p = -1;
Soradhmuny 0:8c4965d9689e 17 int n = 0;
Soradhmuny 0:8c4965d9689e 18 int main() {
Soradhmuny 0:8c4965d9689e 19 while(1){
Soradhmuny 0:8c4965d9689e 20 Servo myservo(p23);
Soradhmuny 0:8c4965d9689e 21 speedL = negL;
Soradhmuny 0:8c4965d9689e 22 speedR = R;
Soradhmuny 0:8c4965d9689e 23 for(p=-1; p<1.0; p += 0.1) {
Soradhmuny 0:8c4965d9689e 24 myservo = p;
Soradhmuny 0:8c4965d9689e 25 if(ain > 0.4) {
Soradhmuny 0:8c4965d9689e 26 myled1 = 1;
Soradhmuny 0:8c4965d9689e 27 myled2 = 1;
Soradhmuny 0:8c4965d9689e 28 myled3 = 1;
Soradhmuny 0:8c4965d9689e 29 myled4 = 1;
Soradhmuny 0:8c4965d9689e 30 speedL = L;
Soradhmuny 0:8c4965d9689e 31 }else if(ain<0.4 && ain > 0.3){
Soradhmuny 0:8c4965d9689e 32 myled1 = 1;
Soradhmuny 0:8c4965d9689e 33 myled2 = 1;
Soradhmuny 0:8c4965d9689e 34 myled3 = 1;
Soradhmuny 0:8c4965d9689e 35 myled4 = 0;
Soradhmuny 0:8c4965d9689e 36 speedL = L;//check random three spikes
Soradhmuny 0:8c4965d9689e 37 }else if(ain<0.3 && ain > 0.2){
Soradhmuny 0:8c4965d9689e 38 myled1 = 1;
Soradhmuny 0:8c4965d9689e 39 myled2 = 1;
Soradhmuny 0:8c4965d9689e 40 myled3 = 0;
Soradhmuny 0:8c4965d9689e 41 myled4 = 0;
Soradhmuny 0:8c4965d9689e 42 }else if(ain<0.2 && ain > 0.1){
Soradhmuny 0:8c4965d9689e 43 myled1 = 1;
Soradhmuny 0:8c4965d9689e 44 myled2 = 0;
Soradhmuny 0:8c4965d9689e 45 myled3 = 0;
Soradhmuny 0:8c4965d9689e 46 myled4 = 0;
Soradhmuny 0:8c4965d9689e 47 }else {
Soradhmuny 0:8c4965d9689e 48 myled1 = 0;
Soradhmuny 0:8c4965d9689e 49 myled2 = 0;
Soradhmuny 0:8c4965d9689e 50 myled3 = 0;
Soradhmuny 0:8c4965d9689e 51 myled4 = 0;
Soradhmuny 0:8c4965d9689e 52 }
Soradhmuny 0:8c4965d9689e 53 wait(0.2);
Soradhmuny 0:8c4965d9689e 54 }
Soradhmuny 0:8c4965d9689e 55 for(p=1.0; p>0.3; p-=.1){
Soradhmuny 0:8c4965d9689e 56 myservo = p;
Soradhmuny 0:8c4965d9689e 57 wait(0.2);
Soradhmuny 0:8c4965d9689e 58 }
Soradhmuny 0:8c4965d9689e 59
Soradhmuny 0:8c4965d9689e 60 Motor mR(p21, p30, p29); // pwm, fwd, rev
Soradhmuny 0:8c4965d9689e 61 Motor mL(p22, p28, p27);
Soradhmuny 0:8c4965d9689e 62 n = 0;
Soradhmuny 0:8c4965d9689e 63 while(n<40){
Soradhmuny 0:8c4965d9689e 64 if(speedL == negL){
Soradhmuny 0:8c4965d9689e 65 if(ain > 0.4) {
Soradhmuny 0:8c4965d9689e 66 myled1 = 1;
Soradhmuny 0:8c4965d9689e 67 myled2 = 1;
Soradhmuny 0:8c4965d9689e 68 myled3 = 1;
Soradhmuny 0:8c4965d9689e 69 myled4 = 1;
Soradhmuny 0:8c4965d9689e 70 speedL = 0;
Soradhmuny 0:8c4965d9689e 71 speedR = 0;
Soradhmuny 0:8c4965d9689e 72 }else if(ain<0.4 && ain > 0.3){
Soradhmuny 0:8c4965d9689e 73 myled1 = 1;
Soradhmuny 0:8c4965d9689e 74 myled2 = 1;
Soradhmuny 0:8c4965d9689e 75 myled3 = 1;
Soradhmuny 0:8c4965d9689e 76 myled4 = 0;
Soradhmuny 0:8c4965d9689e 77 speedL = 0;//check random three spikes
Soradhmuny 0:8c4965d9689e 78 speedR = 0;
Soradhmuny 0:8c4965d9689e 79 }else if(ain<0.3 && ain > 0.2){
Soradhmuny 0:8c4965d9689e 80 myled1 = 1;
Soradhmuny 0:8c4965d9689e 81 myled2 = 1;
Soradhmuny 0:8c4965d9689e 82 myled3 = 0;
Soradhmuny 0:8c4965d9689e 83 myled4 = 0;
Soradhmuny 0:8c4965d9689e 84 }else if(ain<0.2 && ain > 0.1){
Soradhmuny 0:8c4965d9689e 85 myled1 = 1;
Soradhmuny 0:8c4965d9689e 86 myled2 = 0;
Soradhmuny 0:8c4965d9689e 87 myled3 = 0;
Soradhmuny 0:8c4965d9689e 88 myled4 = 0;
Soradhmuny 0:8c4965d9689e 89 }else {
Soradhmuny 0:8c4965d9689e 90 myled1 = 0;
Soradhmuny 0:8c4965d9689e 91 myled2 = 0;
Soradhmuny 0:8c4965d9689e 92 myled3 = 0;
Soradhmuny 0:8c4965d9689e 93 myled4 = 0;
Soradhmuny 0:8c4965d9689e 94 }
Soradhmuny 0:8c4965d9689e 95 mR.speed(speedR);
Soradhmuny 0:8c4965d9689e 96 mL.speed(speedL);
Soradhmuny 0:8c4965d9689e 97 wait(.1);
Soradhmuny 0:8c4965d9689e 98 n++;
Soradhmuny 0:8c4965d9689e 99 }else{
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 = n+8;
Soradhmuny 0:8c4965d9689e 104 }
Soradhmuny 0:8c4965d9689e 105
Soradhmuny 0:8c4965d9689e 106 }
Soradhmuny 0:8c4965d9689e 107 speedR = 0;
Soradhmuny 0:8c4965d9689e 108 speedL = 0;
Soradhmuny 0:8c4965d9689e 109 mR.speed(speedR);
Soradhmuny 0:8c4965d9689e 110 mL.speed(speedL);
Soradhmuny 0:8c4965d9689e 111 wait(.5);
Soradhmuny 0:8c4965d9689e 112 }
Soradhmuny 0:8c4965d9689e 113 }