Bluetooth Motor Control Code

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

/// Objects
DigitalIn pb(p22);
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;
    myservo=0.5;
    Thread::wait(2000);
    while(1) {

        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') {
                    pc.printf("\n brake \n ");

                    while (myservo > 0.0) {
                         if (myservo < 0.05)
                             myservo = 0.0;
                         else
                             myservo = myservo - 0.005; // Brake
                              pc.printf("brekae");
                     }  

                }

                // Right Arrow

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


                }
            }
        }
    }


}// 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<400) {
        close=true;

    }
}

ultrasonic mu(p6, p7, .1, 1, &dist);

void stop()
{


 while (myservo > 0.0) {
                         if (myservo < 0.05)
                             myservo = 0.0;
                         else
                             myservo = myservo - 0.008; // Brake
                              pc.printf("brake");
                     }  





}//ends stop

int main()
{
    pb.mode(PullUp);
    pc.printf("Program Begins ");
    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(8);
    myservo = 0.0; //send shortest PWM
    wait(8);
//ESC now operational using standard servo PWM signals
    pc.printf("Before");


    pc.printf("\n SETUP \n ");

    t1.start(bluetooth);

    mu.startUpdates();

    while (true) {

        if(close) {
            pc.printf("\n Thread released \n ");
            t2.start(stop);
            Thread::wait(1000);
            close=false;
        }

        mu.checkDistance();

    }//ends While

}// ends Main