File content as of revision 1:53e96a6c73db:

#include "mbed.h"
#include "Servo.h"
#include "Motor.h"

/*
Soradhmuny Lanh and Michael Capone
ECE 4180 Lab 4 Collision Avoiding Robot demo
*/

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);
    }
}