Made by Maor and Shai

Dependencies:   DriveCommand HCSR04 Queue mbed-rtos mbed

Fork of Nucleo_UltrasonicHelloWorld by EJ Teb

Files at this revision

API Documentation at this revision

Comitter:
Maor_T
Date:
Mon Jul 04 15:28:11 2016 +0000
Parent:
1:4a5586eb1765
Commit message:
SmartRobotCar;

Changed in this revision

DriveCommand.lib Show annotated file Show diff for this revision Revisions of this file
HCSR04.lib Show annotated file Show diff for this revision Revisions of this file
HC_SR04_Ultrasonic_Library.lib Show diff for this revision Revisions of this file
Queue.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 4a5586eb1765 -r 9a4938a0475b DriveCommand.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DriveCommand.lib	Mon Jul 04 15:28:11 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/Maor_T/code/DriveCommand/#9b6dc5b1dc33
diff -r 4a5586eb1765 -r 9a4938a0475b HCSR04.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Mon Jul 04 15:28:11 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/prabhuvd/code/HCSR04/#71da0dbf4400
diff -r 4a5586eb1765 -r 9a4938a0475b HC_SR04_Ultrasonic_Library.lib
--- a/HC_SR04_Ultrasonic_Library.lib	Thu Dec 04 08:04:55 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
diff -r 4a5586eb1765 -r 9a4938a0475b Queue.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Queue.lib	Mon Jul 04 15:28:11 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/Maor_T/code/Queue/#4770d82994dd
diff -r 4a5586eb1765 -r 9a4938a0475b main.cpp
--- a/main.cpp	Thu Dec 04 08:04:55 2014 +0000
+++ b/main.cpp	Mon Jul 04 15:28:11 2016 +0000
@@ -1,5 +1,166 @@
 #include "mbed.h"
-#include "ultrasonic.h"
+#include "hcsr04.h"
+#include "rtos.h"
+#include <string>
+
+#include "DriveCommand.h"
+#include "queue.h"
+
+#define ULTRASONIC_TRIG_PIN p8
+#define ULTRASONIC_ECHO_PIN p9
+#define MOTOR_A_PIN1        p23
+#define MOTOR_A_PIN2        p24
+#define MOTOR_B_PIN1        p21 
+#define MOTOR_B_PIN2        p22
+#define BLUETOOTH_RX        p27
+#define BLUETOOTH_TX        p28
+#define SERVO_PWM_PIN       p25
+
+#define BT_CHECK_DELAY_MS 50
+
+#define ULTRASONIC_CHECK_DELAY_MS 50
+#define ULTRASONIC_EMERGENCY_DELAY_MS 20
+#define AUTODRIVE_CHECK_DELAY_MS 100
+
+
+
+//------DEBUG OPTIONS ----------
+//#define DEBUG_BT_COMMANDS
+#define DEBUG_ULTRASONICE
+
+//------------------------------
+
+PwmOut motorA1(MOTOR_A_PIN2); //left motor
+PwmOut motorA2(MOTOR_A_PIN1); //left motor
+PwmOut motorB1(MOTOR_B_PIN1); //right motor
+PwmOut motorB2(MOTOR_B_PIN2); //right motor
+
+PwmOut servo(SERVO_PWM_PIN);
+Serial bt(BLUETOOTH_TX,BLUETOOTH_RX);
+Serial pc(USBTX,USBRX);
+
+HCSR04  usensor(p8,p9);
+float lastDistance = 0; // save the last ultrasonic val
+bool moveForward_flag = false; // determined when vehicle moving forward - using to stop and avoid obascales
+bool turnRight_Flag = false;
+bool turnLeft_Flag = false;
+bool obstacleAlert = false;
+float vehicleSpeed = 1;
+int rotateRightMillis  = 650;
+int rotateLeftMillis = 650;
+float turnSpeed = 0.3;
+int obstacleDistance = 30;
+
+Timeout stopper;
+Queue autoDriverQueue(sizeof(DriveCommand),10);
+
+
+//string globalString =" ";
+char lastCommandChar ='x';
+char incomingMsg[1024];
+int incomingMsg_iter = 0;
+bool succesReadFlag = false;
+bool autoDriveFlag = false;
+bool autoDriveRun = false;
+int autoDriveCounter = 0;
+bool autoDrivePartOk = true; // check if every part of auto drive is whitout obstacles
+
+//Prototype
+void Drive_Stop();
+void AutoDrive_Stop();
+void ExecuteNextCommand();
+void Rotate_Left();
+void MotorRight_Run(float speedPercent,bool forwardDir);
+void MotorLeft_Run(float speedPercent,bool forwardDir);
+void Drive_Force_Stop();
+int Get_Min_Distance();
+void Servo_Look_Left();
+void Servo_Look_Right();
+void Servo_Look_Center();
+
+
+void FlagController(char dir)
+{
+    switch(dir)
+    {
+        case 'f':
+        moveForward_flag = true; 
+        turnRight_Flag = false;
+        turnLeft_Flag = false;
+        break;
+        case 'l':
+        moveForward_flag = false; 
+        turnRight_Flag = false;
+        turnLeft_Flag = true;
+        break; 
+        case 'r':
+        moveForward_flag = false; 
+        turnRight_Flag = true;
+        turnLeft_Flag = false;
+        break;    
+        case 'b':
+        moveForward_flag = false; 
+        turnRight_Flag = false;
+        turnLeft_Flag = false;
+        break;     
+    }
+}
+
+void StopAfter(float sec)
+{
+   //  printf("StopAfter Got %f\n",sec);
+     stopper.attach(&AutoDrive_Stop,sec);  
+}
+
+float GetDistance()
+{
+    float sum = 0;
+    
+    for(int i=i ; i< 7 ; i++)
+    {
+         sum += usensor.get_dist_cm();      
+    }
+    
+    sum /= 7.0;   
+    return sum;    
+    }
+
+
+
+void Reset_IncomingMsg()
+{
+    for(int i=0 ; i<1024 ; i++)
+        incomingMsg[i] = '*'; 
+    incomingMsg_iter  =0;    
+}
+
+string Get_last_Msg()
+{
+    int i=0;
+    string temp = "";
+    while(incomingMsg[i] != '~' && incomingMsg[i] != '*')
+    {
+        temp+=incomingMsg[i++];         
+    }
+       
+       if(incomingMsg[i] == '~')
+       {
+            succesReadFlag = true;  
+            Reset_IncomingMsg();  
+        }
+            
+       else
+       {
+           succesReadFlag = false;       
+        }                         
+        return temp;
+}
+
+float map(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
 
  void dist(int distance)
 {
@@ -7,17 +168,646 @@
     printf("Distance changed to %dmm\r\n", distance);
 }
 
-ultrasonic mu(D8, D9, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
-                                        //have updates every .1 seconds and a timeout after 1
-                                        //second, and call dist when the distance changes
+
+void MotorLeft_Run(float speedPercent,bool forwardDir)
+{
+    if(forwardDir)
+    {
+      motorA1.write(0);
+      motorA2.write(speedPercent);      
+    }
+    else
+    {
+      motorA1.write(speedPercent);
+      motorA2.write(0);          
+    } 
+}
+
+void MotorLeft_Stop()
+{
+    motorA1.write(0);
+    motorA2.write(0); 
+}
+
+void MotorRight_Run(float speedPercent,bool forwardDir)
+{
+    if(forwardDir)
+    {
+      motorB1.write(0);
+      motorB2.write(speedPercent);      
+    }
+    else
+    {
+      motorB1.write(speedPercent);
+      motorB2.write(0);          
+    } 
+}
+
+void MotorRight_Stop()
+{
+    motorB1.write(0);
+    motorB2.write(0); 
+}
+
+void Drive_Forward(float speedPercent)
+{
+    FlagController('f');
+    MotorLeft_Run(speedPercent,true);
+    MotorRight_Run(speedPercent,true) ;  
+}
+
+void Drive_Left(float leftPercent)
+{
+    FlagController('f');
+    MotorRight_Run(leftPercent,true);
+    MotorLeft_Run(1-leftPercent,true) ; // left motor reversed
+}
+
+void Reverse_Left(float leftPercent)
+{
+    MotorRight_Run(leftPercent,false);
+    MotorLeft_Run(1-leftPercent,false) ; // left motor reversed
+}
+
+void Turn_Left(float leftPercent)
+{
+    FlagController('l');
+    Servo_Look_Left();
+    wait_ms(100);
+    MotorRight_Run(leftPercent,true);
+    MotorLeft_Run(leftPercent,false) ; // left motor reversed
+}
+
+void Drive_Right(float rightPercent)
+{
+      MotorLeft_Run(rightPercent,true) ;
+      MotorRight_Run(1-rightPercent,true); // right motor reversed
+}
+
+void Reverse_Right(float rightPercent)
+{
+      MotorLeft_Run(rightPercent,false) ;
+      MotorRight_Run(1-rightPercent,false); // right motor reversed
+}
+
+void Turn_Right(float rightPercent)
+{
+      FlagController('r');
+      Servo_Look_Right();
+      wait_ms(200);
+      MotorLeft_Run(rightPercent,true) ;
+      MotorRight_Run(rightPercent,false); // right motor reversed
+}
+
+void Drive_Backwards(float speedPercent)
+{
+    FlagController('b');
+    MotorLeft_Run(speedPercent,false);
+    MotorRight_Run(speedPercent,false) ; 
+    moveForward_flag = false; 
+}
+
+
+void Drive_Stop()
+{
+    FlagController('s');
+    MotorLeft_Stop();
+    MotorRight_Stop();
+    moveForward_flag = false;
+    autoDriveRun = false;
+}
+void Drive_Force_Stop()
+{
+    MotorRight_Run(turnSpeed,false);
+    MotorLeft_Run(turnSpeed,false) ;
+    wait_ms(500);
+    MotorLeft_Stop();
+    MotorRight_Stop();
+    moveForward_flag = false;
+    autoDriveRun = false;
+    
+}
+
+
+void AutoDrive_Stop()
+{
+//    if(autoDriveFlag) // stop only if autoDrive mode is running
+//    {
+        MotorLeft_Stop();
+        MotorRight_Stop();
+        moveForward_flag = false;        
+//    }
+    autoDriveRun = false;  
+}
+
+
+void Rotate_Left()
+{
+    MotorRight_Run(turnSpeed,true);
+    MotorLeft_Run(turnSpeed,false) ; // left motor reversed
+    wait_ms(rotateLeftMillis);
+    Drive_Stop(); 
+}
+
+void Rotate_Right()
+{
+    MotorRight_Run(turnSpeed,false);
+    MotorLeft_Run(turnSpeed,true) ; // left motor reversed
+    wait_ms(rotateRightMillis);
+    Drive_Stop(); 
+}
+
+
+void ServoMove(int pos)
+{
+    if(pos < 0 || pos >180)
+        return;
+    float outVal = map(pos,0,180,1000,2000);
+    servo.pulsewidth_us(outVal);   
+}
+
+
+int Get_Min_Distance()
+{
+    int min;
+    
+    Servo_Look_Left();
+    wait_ms(250);
+    usensor.start();
+    int left = usensor.get_dist_cm();
+        min=left;
+        
+    if(min <  obstacleDistance )
+    {
+        autoDrivePartOk = false;
+        return min;   
+     }
+       
+    Servo_Look_Right();
+    wait_ms(250);
+    usensor.start();
+    int right = usensor.get_dist_cm();
+        if(right<min)
+        min=right;
+        
+      if(min <  obstacleDistance ) 
+      {
+          autoDrivePartOk = false;
+          return min;  
+          
+      }
+
+    Servo_Look_Center();
+    wait_ms(250);
+    usensor.start();
+    int center = usensor.get_dist_cm();
+        if(center<min)
+        min=center;
+   
+     if(min <  obstacleDistance )
+     {
+        autoDrivePartOk = false;
+        return min;   
+      }  
+      //  printf("Min Distance %d\n",min);
+    
+    return min;  
+} 
+
+  
+
+void Servo_Look_Left()
+{
+    servo.pulsewidth_us(2000);   
+}
+
+void Servo_Look_Right()
+{
+    servo.pulsewidth_us(1000);   
+}
+
+void Servo_Look_Center()
+{
+    servo.pulsewidth_us(1500);   
+}
+
+void DataReceived_ISR(){
+   //  char globalChar = LPC_UART2->RBR;
+   //  lastCommandChar = LPC_UART2->RBR;
+     incomingMsg[incomingMsg_iter++] = LPC_UART2->RBR ;
+     
+    // globalString += globalChar; 
+}
+
+void DataReceived_PCISR(){
+   //  char globalChar = LPC_UART2->RBR;
+   //  lastCommandChar = LPC_UART2->RBR;
+     incomingMsg[incomingMsg_iter++] = LPC_UART0->RBR ;
+     
+    // globalString += globalChar; 
+}
+
+
+void Servo_NoNo(int count)
+{
+    for(int i=0 ; i< count ; i++) {
+        Servo_Look_Left();
+        wait(0.2);
+        Servo_Look_Right();
+        wait(0.2);
+    } 
+   Servo_Look_Center();
+}
+
+float getCommandValue(string cmd){
+    string cmdValue;   
+    std::size_t sharpPosition = cmd.find("#");  
+    cmdValue = cmd.substr(sharpPosition+1);
+ //   printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
+    return atof(cmdValue.c_str());
+}
+
+
+
+
+void BTCommands(string cmd)
+{
+   #ifdef DEBUG_BT_COMMANDS
+   printf("Incoming BT command : %s\n",cmd);  // - Print the incoming command
+   #endif
+   if (cmd.find("forward#")!= string::npos)
+   {
+       float time = getCommandValue(cmd);
+       DriveCommand* dc;
+       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
+       dc->SetTime(time);
+       dc->SetDir('f');
+       autoDriverQueue.Put(dc);
+   }
+   else if(cmd.find("backwards#")!= string::npos)
+   {
+       float time = getCommandValue(cmd);
+       DriveCommand* dc;
+       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
+       dc->SetTime(time);
+       dc->SetDir('b');
+       autoDriverQueue.Put(dc);
+   }
+    else if(cmd.find("obstacle#")!= string::npos)
+   {
+       obstacleDistance = getCommandValue(cmd);
+      
+   }  
+   
+   else if (cmd.find("rotleftms#")!= string::npos)
+   {
+       rotateLeftMillis = getCommandValue(cmd);
+       Rotate_Left();  // todo remove after testing
+       
+   } 
+    else if (cmd.find("rotrightms#")!= string::npos)
+   {
+       rotateRightMillis = getCommandValue(cmd);
+       Rotate_Right(); // todo remove after testing
+   } 
+    else if (cmd.find("turnspeed#")!= string::npos)
+   {
+       turnSpeed = getCommandValue(cmd);
+      
+   }    
+   else if(cmd.find("rotleft")!= string::npos)
+   {    
+       Rotate_Left();
+   }
+   else if(cmd.find("rotright")!= string::npos)
+   {    
+       Rotate_Right();
+   }  
+   else if(cmd.find("left#")!= string::npos)
+   {
+       float time = getCommandValue(cmd);
+       DriveCommand* dc;
+       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
+       dc->SetTime(time);
+       dc->SetDir('l');
+       autoDriverQueue.Put(dc);
+   }  
+         else if(cmd.find("right#")!= string::npos)
+   {
+       float time = getCommandValue(cmd);
+       DriveCommand* dc;
+       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
+       dc->SetTime(time);
+       dc->SetDir('r');
+       autoDriverQueue.Put(dc);
+   }   
+   else if (cmd.find("autodrive")!= string::npos)
+   {
+       autoDriveFlag = true;
+
+   } 
+   else if (cmd.find("forward")!= string::npos)
+   {
+      if(lastDistance < obstacleDistance)
+      {
+        Servo_NoNo(2);
+        return;
+       } 
+       Drive_Forward(vehicleSpeed);
+   }   
+   else if (cmd.find("stop")!= string::npos)
+   {
+       Drive_Stop();
+   } 
+   else if (cmd.find("backwards")!= string::npos)
+   {
+       Drive_Backwards(vehicleSpeed);
+   }
+   else if (cmd.find("tleft")!= string::npos)
+   {
+       Turn_Left(vehicleSpeed);
+   }
+   else if (cmd.find("tright")!= string::npos)
+   {
+       Turn_Right(vehicleSpeed);
+   }
+   else if (cmd.find("dright")!= string::npos)
+   {
+       Drive_Right(vehicleSpeed);
+   }
+   else if (cmd.find("dleft")!= string::npos)
+   {
+       Drive_Left(vehicleSpeed);
+   }
+      else if (cmd.find("rleft")!= string::npos)
+   {
+       Reverse_Left(vehicleSpeed);
+   }
+      else if (cmd.find("rright")!= string::npos)
+   {
+       Reverse_Right(vehicleSpeed);
+   }
+    else if (cmd.find("speed1")!= string::npos)
+   {
+       vehicleSpeed = 0.25; 
+       
+   }
+      else if (cmd.find("speed2")!= string::npos)
+   {
+      vehicleSpeed = 0.50; 
+       
+   }
+       else if (cmd.find("speed3")!= string::npos)
+   {
+      vehicleSpeed = 0.75; 
+       
+   }
+       else if (cmd.find("speed4")!= string::npos)
+   {
+      vehicleSpeed = 1; 
+       
+   }  
+   else
+   {
+       Drive_Stop();
+    }
+    
+}
+
+void BTCommands2(char cmd)
+{
+   #ifdef DEBUG_BT_COMMANDS
+   printf("Incoming BT command : %c\n",cmd);  // - Print the incoming command
+   #endif
+
+    switch(cmd)
+    {
+        case 'F' : Drive_Forward(1);    break;
+        case 'B' : Drive_Backwards(1);    break;
+        case 'S' : Drive_Stop();        break;
+        case 'L' : Drive_Left(1);       break;
+        case 'R' : Drive_Right(1);      break;  
+}
+}
+
+
+void init()
+{
+    bt.baud(115200);
+    bt.attach(&DataReceived_ISR,Serial::RxIrq);  
+    pc.attach(&DataReceived_PCISR,Serial::RxIrq);  
+    Reset_IncomingMsg();
+}
+
+
+void UltraSonic_Thread_Func(void const *args)
+{
+    while(1)
+    {
+      
+    //  lastDistance = GetDistance();
+    usensor.start(); 
+    lastDistance = usensor.get_dist_cm();
+      
+      if(moveForward_flag)
+      { 
+        lastDistance = Get_Min_Distance();    
+      }
+
+      
+      #ifdef DEBUG_ULTRASONICE
+      pc.printf("Distance : %f\n",lastDistance );
+      #endif
+      
+        if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
+            autoDrivePartOk = false;
+            Drive_Force_Stop();
+            moveForward_flag = false; 
+            Servo_NoNo(1);
+        }
+        else if((turnRight_Flag || turnLeft_Flag) && lastDistance < obstacleDistance  ) { // todo : minimize code here
+            autoDrivePartOk = false;
+            Drive_Stop();
+            
+           // moveForward_flag = false;
+        } 
+
+        
+      Thread::wait(ULTRASONIC_CHECK_DELAY_MS);     
+    }  
+}
+
+void UltraSonic_Thread_Emergency(void const *args)
+{
+    while(1)
+    {
+    usensor.start(); 
+    lastDistance = usensor.get_dist_cm();
+    
+      #ifdef DEBUG_ULTRASONICE
+      pc.printf("Distance : %f\n",lastDistance );
+      #endif
+      
+        if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
+            Drive_Force_Stop();
+            moveForward_flag = false;
+            Servo_NoNo(1);
+        }     
+      Thread::wait(ULTRASONIC_EMERGENCY_DELAY_MS);     
+    }  
+}
+
+
+
+void ExecuteNextCommand()
+{
+    DriveCommand* dc = (DriveCommand*)malloc(sizeof(DriveCommand)) ;
+  //  printf("Before get from queue\n");
+    autoDriverQueue.Get(dc);
+  //  printf("after get from queue\n");
+    float time = dc->GetTime();
+    char dir = dc->GetDir();
+  //  printf("AutoDrive Runing at Dir:%c, Time:%d\n",dir,time);
+    autoDriveRun = true;
+    autoDrivePartOk = true;
+    switch(dir)
+    {
+        case 'f': 
+        Drive_Forward(vehicleSpeed);
+        break;
+        case 'b':
+        Drive_Backwards(vehicleSpeed);
+        break;
+        case 'l':
+        Turn_Left(turnSpeed);
+        wait_ms(rotateLeftMillis);
+        Drive_Forward(vehicleSpeed);
+        break;
+        case 'r':
+        Turn_Right(turnSpeed);
+        wait_ms(rotateRightMillis);      
+        Drive_Forward(vehicleSpeed);
+        break;        
+    } 
+    
+    StopAfter(time);
+    free(dc);
+    
+    while(autoDriveRun)
+        wait_ms(50);
+        
+    if(autoDrivePartOk)
+        bt.printf("ok%d~",autoDriveCounter);
+    else
+        bt.printf("fail%d~",autoDriveCounter);    
+ 
+    autoDriveCounter++;
+
+    
+}
+
+
+
+
+
+void AutoDrive_Thread_Func(void const *args)
+{
+        while(true) { 
+        
+        // todo add check for autoDriveFlag
+         if(autoDriveFlag && autoDriverQueue.GetNumberOfItems() >0 )
+         {
+            // printf("Autodrive Commands  before commit\n");
+            if(!autoDriveRun) //check that there is no running command right now
+                ExecuteNextCommand(); 
+            // printf("Autodrive Commands  after commit\n");
+             
+         }
+         else
+         {
+            autoDriveFlag = false;
+            autoDriveCounter =0;            
+         }
+            
+        //printf("Checking for Autodrive Commands\n");
+        Thread::wait(AUTODRIVE_CHECK_DELAY_MS);
+    }
+}
+
+
+
+
+void BT_Thread_Func(void const *args)
+{
+//    while(true) { 
+//        if(globalString.length() > 0 )
+//        {
+//            BTCommands(globalString);
+//            globalString = "";
+//        } 
+//        Thread::wait(100);
+//    }
+//    
+        while(true) { 
+//        if(lastCommandChar != 'x' )
+//        {
+//            BTCommands2(lastCommandChar);
+//            lastCommandChar = 'x';
+//        }
+        
+         string cmd = Get_last_Msg();
+         if(succesReadFlag)
+         {
+             
+             BTCommands(cmd);
+             succesReadFlag = false;
+         }        
+        
+        Thread::wait(BT_CHECK_DELAY_MS);
+    }
+}
+
 
 int main()
 {
-    mu.startUpdates();//start mesuring the distance
+ //   mu.startUpdates();//start mesuring the distance
+    
+    init();
+    printf("Hello\n");
+    Thread btThread(BT_Thread_Func); 
+    btThread.set_priority(osPriorityNormal);
+    Thread ultraSonicThread(UltraSonic_Thread_Func);
+    ultraSonicThread.set_priority(osPriorityNormal);
+    Thread ultraSonicEmergencyThread(UltraSonic_Thread_Emergency);
+    ultraSonicEmergencyThread.set_priority(osPriorityNormal); 
+    Thread AutoDriveThread(AutoDrive_Thread_Func);
+    AutoDriveThread.set_priority(osPriorityNormal);
+    
+    
+    
+         
+   Servo_Look_Left();
+   wait(2);   
+   Servo_Look_Right();
+   wait(2);    
+   Servo_Look_Center();
+   
+      
+ //  Drive_Forward(0.5);
+   
+ //  StopAfter(3.0);
+    
+       
+  //     autoDriverQueue.Put(&DriveCommand('f',120)); 
+                 
     while(1)
     {
+    //   mu._checkDistance();   
         //Do something else here
-        mu.checkDistance();     //call checkDistance() as much as possible, as this is where
-                                //the class checks if dist needs to be called.
+   //     mu.checkDistance();     //call checkDistance() as much as possible, as this is where  //the class checks if dist needs to be called.
     }
+       
 }
+
+
+
diff -r 4a5586eb1765 -r 9a4938a0475b mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Mon Jul 04 15:28:11 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/Maor_T/code/mbed-rtos/#78d11dd53a40
diff -r 4a5586eb1765 -r 9a4938a0475b mbed.bld
--- a/mbed.bld	Thu Dec 04 08:04:55 2014 +0000
+++ b/mbed.bld	Mon Jul 04 15:28:11 2016 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34
\ No newline at end of file