File content as of revision 12:7a9903be8988:

#include "mbed.h"
#include "Servo.h"
#include "rtos.h"
#include "ultrasonic.h"


/// Objects 
DigitalOut led1(LED1); // Used For testsing
DigitalOut led2(LED2); // Used For tesitng 
Serial blue(p28,p27); // Bluetooh
Servo myservo(p21); // Motor 
Serial pc(USBTX,USBRX); // PC
Thread t1;
Thread t2;
Thread t3;

Mutex off;
bool close=false;  

void bluetooth(){
char bnum = 0;
while(1) {
  //     off.lock();
  Thread::wait(50);
        if (blue.getc() == '!') {
            if (blue.getc()=='B') {
                bnum = blue.getc();
                pc.printf("%s",bnum);
                // Up Arrow
                
                if ((bnum == '5') && (myservo <= 1.0)) {
                    myservo = myservo + 0.05;
                     pc.printf("\n Servo up\n ");
                    
                }

                // Down Arrow
                else if ((bnum == '6') && (myservo >= 0.54)) {
                    myservo = myservo - 0.05;
                    pc.printf("\n Servo down\n ");
                }

                // Left Arrow

                else if (bnum == '7') {
                   myservo=0.00;
                   pc.printf("\n brake \n ");
                   
                   /* while (myservo > 0.0) {
                        if (myservo < 0.05)
                            myservo = 0.0;
                        else
                            myservo = myservo - 0.05; // Brake
                             pc.printf("brekae");
                    }  */

                }

                // Right Arrow

                else if (bnum == '8') {
                    //servo.lock();
                    myservo = 0.5; // Neutral
                    pc.printf("\n netrual");
                    //servo.unlock();

                }
            }
        }
//                        off.unlock();
    }

   
    }// ends bluetooh



 void dist(int distance)
{
    //put code here to execute when the distance has changed
    pc.printf("Distance %d mm\r\n", distance);
    if(distance<300){
        close=true;
     
        }
}
 
ultrasonic mu(p6, p7, .1, 1, &dist);

void stop(){
    
pc.printf("\n STOP MOTOR");
//off.lock();   
myservo=0.0;        
Thread::wait(2000);
//off.unlock();

   
}//ends stop



int main() {


    myservo = 0.0;
    wait(0.5); //detects signal
//Required ESC Calibration/Arming sequence  
//sends longest and shortest PWM pulse to learn and arm at power on
    myservo = 1.0; //send longest PWM
    wait(5);
    myservo = 0.0; //send shortest PWM
    wait(5);
//ESC now operational using standard servo PWM signals
    myservo = 0.5;
    wait(2);

pc.printf("\n SETUP \n ");
t1.start(bluetooth);
t2.start(stop);

mu.startUpdates();
  while (true) {
  
  if(close){
        pc.printf("\n Thread realsed \n ");
        t2.start(stop);
        Thread::wait(1000);
        close=false;
        
        }
   mu.checkDistance(); 
    
    }//ends While 

}// ends Main