main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "Motor.h"
00004 
00005 /*
00006 Soradhmuny Lanh and Michael Capone
00007 ECE 4180 Lab 4 Collision Avoiding Robot demo
00008 */
00009 
00010 Servo myservo(p23);
00011 AnalogIn   ain(p16);
00012 DigitalOut myled1(LED1);
00013 DigitalOut myled2(LED2);
00014 DigitalOut myled3(LED3);
00015 DigitalOut myled4(LED4);
00016 float L = 1;
00017 float negL = -0.5;
00018 float R = 1;
00019 float speedL = negL;
00020 float speedR = R;
00021 float p = -1;
00022 int n = 0;
00023 int main() {
00024     while(1){ 
00025         Servo myservo(p23);
00026         speedL = negL;
00027         speedR = R;
00028         for(p=-1; p<1.0; p += 0.1) {
00029             myservo = p;
00030             if(ain > 0.4) {
00031                 myled1 = 1;
00032                 myled2 = 1;
00033                 myled3 = 1;
00034                 myled4 = 1;
00035                 speedL = L;
00036             }else if(ain<0.4 && ain > 0.3){
00037                 myled1 = 1;
00038                 myled2 = 1;
00039                 myled3 = 1;
00040                 myled4 = 0;
00041                 speedL = L;//check random three spikes
00042             }else if(ain<0.3 && ain > 0.2){
00043                 myled1 = 1;
00044                 myled2 = 1;
00045                 myled3 = 0;
00046                 myled4 = 0;
00047             }else if(ain<0.2 && ain > 0.1){
00048                 myled1 = 1;
00049                 myled2 = 0;
00050                 myled3 = 0;
00051                 myled4 = 0;
00052             }else {
00053                 myled1 = 0;
00054                 myled2 = 0;
00055                 myled3 = 0;
00056                 myled4 = 0;
00057             }
00058             wait(0.2);
00059         } 
00060         for(p=1.0; p>0.3; p-=.1){
00061             myservo = p;
00062             wait(0.2);
00063         }
00064         
00065         Motor mR(p21, p30, p29); // pwm, fwd, rev
00066         Motor mL(p22, p28, p27);
00067         n = 0;
00068         while(n<40){
00069             if(speedL == negL){
00070                 if(ain > 0.4) {
00071                     myled1 = 1;
00072                     myled2 = 1;
00073                     myled3 = 1;
00074                     myled4 = 1;
00075                     speedL = 0;
00076                     speedR = 0;
00077                 }else if(ain<0.4 && ain > 0.3){
00078                     myled1 = 1;
00079                     myled2 = 1;
00080                     myled3 = 1;
00081                     myled4 = 0;
00082                     speedL = 0;//check random three spikes
00083                     speedR = 0;
00084                 }else if(ain<0.3 && ain > 0.2){
00085                     myled1 = 1;
00086                     myled2 = 1;
00087                     myled3 = 0;
00088                     myled4 = 0;
00089                 }else if(ain<0.2 && ain > 0.1){
00090                     myled1 = 1;
00091                     myled2 = 0;
00092                     myled3 = 0;
00093                     myled4 = 0;
00094                 }else {
00095                     myled1 = 0;
00096                     myled2 = 0;
00097                     myled3 = 0;
00098                     myled4 = 0;
00099                 }
00100                 mR.speed(speedR); 
00101                 mL.speed(speedL);
00102                 wait(.1);
00103                 n++;
00104             }else{
00105                 mR.speed(speedR); 
00106                 mL.speed(speedL);
00107                 wait(.1);
00108                 n = n+8;
00109             }
00110             
00111         }
00112         speedR = 0;
00113         speedL = 0;
00114         mR.speed(speedR);
00115         mL.speed(speedL);
00116         wait(.5);
00117     }
00118 }