main.cpp

00001 #include "mbed.h"
00002 #include "hcsr04.h"
00003 #include "rtos.h"
00004 #include <string>
00005 
00006 #include "DriveCommand.h"
00007 #include "queue.h"
00008 
00009 #define ULTRASONIC_TRIG_PIN p8
00010 #define ULTRASONIC_ECHO_PIN p9
00011 #define MOTOR_A_PIN1        p23
00012 #define MOTOR_A_PIN2        p24
00013 #define MOTOR_B_PIN1        p21 
00014 #define MOTOR_B_PIN2        p22
00015 #define BLUETOOTH_RX        p27
00016 #define BLUETOOTH_TX        p28
00017 #define SERVO_PWM_PIN       p25
00018 
00019 #define BT_CHECK_DELAY_MS 50
00020 
00021 #define ULTRASONIC_CHECK_DELAY_MS 50
00022 #define ULTRASONIC_EMERGENCY_DELAY_MS 20
00023 #define AUTODRIVE_CHECK_DELAY_MS 100
00024 
00025 
00026 
00027 //------DEBUG OPTIONS ----------
00028 //#define DEBUG_BT_COMMANDS
00029 #define DEBUG_ULTRASONICE
00030 
00031 //------------------------------
00032 
00033 PwmOut motorA1(MOTOR_A_PIN2); //left motor
00034 PwmOut motorA2(MOTOR_A_PIN1); //left motor
00035 PwmOut motorB1(MOTOR_B_PIN1); //right motor
00036 PwmOut motorB2(MOTOR_B_PIN2); //right motor
00037 
00038 PwmOut servo(SERVO_PWM_PIN);
00039 Serial bt(BLUETOOTH_TX,BLUETOOTH_RX);
00040 Serial pc(USBTX,USBRX);
00041 
00042 HCSR04  usensor(p8,p9);
00043 float lastDistance = 0; // save the last ultrasonic val
00044 bool moveForward_flag = false; // determined when vehicle moving forward - using to stop and avoid obascales
00045 bool turnRight_Flag = false;
00046 bool turnLeft_Flag = false;
00047 bool obstacleAlert = false;
00048 float vehicleSpeed = 1;
00049 int rotateRightMillis  = 650;
00050 int rotateLeftMillis = 650;
00051 float turnSpeed = 0.3;
00052 int obstacleDistance = 30;
00053 
00054 Timeout stopper;
00055 Queue autoDriverQueue(sizeof(DriveCommand),10);
00056 
00057 
00058 //string globalString =" ";
00059 char lastCommandChar ='x';
00060 char incomingMsg[1024];
00061 int incomingMsg_iter = 0;
00062 bool succesReadFlag = false;
00063 bool autoDriveFlag = false;
00064 bool autoDriveRun = false;
00065 int autoDriveCounter = 0;
00066 bool autoDrivePartOk = true; // check if every part of auto drive is whitout obstacles
00067 
00068 //Prototype
00069 void Drive_Stop();
00070 void AutoDrive_Stop();
00071 void ExecuteNextCommand();
00072 void Rotate_Left();
00073 void MotorRight_Run(float speedPercent,bool forwardDir);
00074 void MotorLeft_Run(float speedPercent,bool forwardDir);
00075 void Drive_Force_Stop();
00076 int Get_Min_Distance();
00077 void Servo_Look_Left();
00078 void Servo_Look_Right();
00079 void Servo_Look_Center();
00080 
00081 
00082 void FlagController(char dir)
00083 {
00084     switch(dir)
00085     {
00086         case 'f':
00087         moveForward_flag = true; 
00088         turnRight_Flag = false;
00089         turnLeft_Flag = false;
00090         break;
00091         case 'l':
00092         moveForward_flag = false; 
00093         turnRight_Flag = false;
00094         turnLeft_Flag = true;
00095         break; 
00096         case 'r':
00097         moveForward_flag = false; 
00098         turnRight_Flag = true;
00099         turnLeft_Flag = false;
00100         break;    
00101         case 'b':
00102         moveForward_flag = false; 
00103         turnRight_Flag = false;
00104         turnLeft_Flag = false;
00105         break;     
00106     }
00107 }
00108 
00109 void StopAfter(float sec)
00110 {
00111    //  printf("StopAfter Got %f\n",sec);
00112      stopper.attach(&AutoDrive_Stop,sec);  
00113 }
00114 
00115 float GetDistance()
00116 {
00117     float sum = 0;
00118     
00119     for(int i=i ; i< 7 ; i++)
00120     {
00121          sum += usensor.get_dist_cm();      
00122     }
00123     
00124     sum /= 7.0;   
00125     return sum;    
00126     }
00127 
00128 
00129 
00130 void Reset_IncomingMsg()
00131 {
00132     for(int i=0 ; i<1024 ; i++)
00133         incomingMsg[i] = '*'; 
00134     incomingMsg_iter  =0;    
00135 }
00136 
00137 string Get_last_Msg()
00138 {
00139     int i=0;
00140     string temp = "";
00141     while(incomingMsg[i] != '~' && incomingMsg[i] != '*')
00142     {
00143         temp+=incomingMsg[i++];         
00144     }
00145        
00146        if(incomingMsg[i] == '~')
00147        {
00148             succesReadFlag = true;  
00149             Reset_IncomingMsg();  
00150         }
00151             
00152        else
00153        {
00154            succesReadFlag = false;       
00155         }                         
00156         return temp;
00157 }
00158 
00159 float map(float x, float in_min, float in_max, float out_min, float out_max)
00160 {
00161     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00162 }
00163 
00164 
00165  void dist(int distance)
00166 {
00167     //put code here to happen when the distance is changed
00168     printf("Distance changed to %dmm\r\n", distance);
00169 }
00170 
00171 
00172 void MotorLeft_Run(float speedPercent,bool forwardDir)
00173 {
00174     if(forwardDir)
00175     {
00176       motorA1.write(0);
00177       motorA2.write(speedPercent);      
00178     }
00179     else
00180     {
00181       motorA1.write(speedPercent);
00182       motorA2.write(0);          
00183     } 
00184 }
00185 
00186 void MotorLeft_Stop()
00187 {
00188     motorA1.write(0);
00189     motorA2.write(0); 
00190 }
00191 
00192 void MotorRight_Run(float speedPercent,bool forwardDir)
00193 {
00194     if(forwardDir)
00195     {
00196       motorB1.write(0);
00197       motorB2.write(speedPercent);      
00198     }
00199     else
00200     {
00201       motorB1.write(speedPercent);
00202       motorB2.write(0);          
00203     } 
00204 }
00205 
00206 void MotorRight_Stop()
00207 {
00208     motorB1.write(0);
00209     motorB2.write(0); 
00210 }
00211 
00212 void Drive_Forward(float speedPercent)
00213 {
00214     FlagController('f');
00215     MotorLeft_Run(speedPercent,true);
00216     MotorRight_Run(speedPercent,true) ;  
00217 }
00218 
00219 void Drive_Left(float leftPercent)
00220 {
00221     FlagController('f');
00222     MotorRight_Run(leftPercent,true);
00223     MotorLeft_Run(1-leftPercent,true) ; // left motor reversed
00224 }
00225 
00226 void Reverse_Left(float leftPercent)
00227 {
00228     MotorRight_Run(leftPercent,false);
00229     MotorLeft_Run(1-leftPercent,false) ; // left motor reversed
00230 }
00231 
00232 void Turn_Left(float leftPercent)
00233 {
00234     FlagController('l');
00235     Servo_Look_Left();
00236     wait_ms(100);
00237     MotorRight_Run(leftPercent,true);
00238     MotorLeft_Run(leftPercent,false) ; // left motor reversed
00239 }
00240 
00241 void Drive_Right(float rightPercent)
00242 {
00243       MotorLeft_Run(rightPercent,true) ;
00244       MotorRight_Run(1-rightPercent,true); // right motor reversed
00245 }
00246 
00247 void Reverse_Right(float rightPercent)
00248 {
00249       MotorLeft_Run(rightPercent,false) ;
00250       MotorRight_Run(1-rightPercent,false); // right motor reversed
00251 }
00252 
00253 void Turn_Right(float rightPercent)
00254 {
00255       FlagController('r');
00256       Servo_Look_Right();
00257       wait_ms(200);
00258       MotorLeft_Run(rightPercent,true) ;
00259       MotorRight_Run(rightPercent,false); // right motor reversed
00260 }
00261 
00262 void Drive_Backwards(float speedPercent)
00263 {
00264     FlagController('b');
00265     MotorLeft_Run(speedPercent,false);
00266     MotorRight_Run(speedPercent,false) ; 
00267     moveForward_flag = false; 
00268 }
00269 
00270 
00271 void Drive_Stop()
00272 {
00273     FlagController('s');
00274     MotorLeft_Stop();
00275     MotorRight_Stop();
00276     moveForward_flag = false;
00277     autoDriveRun = false;
00278 }
00279 void Drive_Force_Stop()
00280 {
00281     MotorRight_Run(turnSpeed,false);
00282     MotorLeft_Run(turnSpeed,false) ;
00283     wait_ms(500);
00284     MotorLeft_Stop();
00285     MotorRight_Stop();
00286     moveForward_flag = false;
00287     autoDriveRun = false;
00288     
00289 }
00290 
00291 
00292 void AutoDrive_Stop()
00293 {
00294 //    if(autoDriveFlag) // stop only if autoDrive mode is running
00295 //    {
00296         MotorLeft_Stop();
00297         MotorRight_Stop();
00298         moveForward_flag = false;        
00299 //    }
00300     autoDriveRun = false;  
00301 }
00302 
00303 
00304 void Rotate_Left()
00305 {
00306     MotorRight_Run(turnSpeed,true);
00307     MotorLeft_Run(turnSpeed,false) ; // left motor reversed
00308     wait_ms(rotateLeftMillis);
00309     Drive_Stop(); 
00310 }
00311 
00312 void Rotate_Right()
00313 {
00314     MotorRight_Run(turnSpeed,false);
00315     MotorLeft_Run(turnSpeed,true) ; // left motor reversed
00316     wait_ms(rotateRightMillis);
00317     Drive_Stop(); 
00318 }
00319 
00320 
00321 void ServoMove(int pos)
00322 {
00323     if(pos < 0 || pos >180)
00324         return;
00325     float outVal = map(pos,0,180,1000,2000);
00326     servo.pulsewidth_us(outVal);   
00327 }
00328 
00329 
00330 int Get_Min_Distance()
00331 {
00332     int min;
00333     
00334     Servo_Look_Left();
00335     wait_ms(250);
00336     usensor.start();
00337     int left = usensor.get_dist_cm();
00338         min=left;
00339         
00340     if(min <  obstacleDistance )
00341     {
00342         autoDrivePartOk = false;
00343         return min;   
00344      }
00345        
00346     Servo_Look_Right();
00347     wait_ms(250);
00348     usensor.start();
00349     int right = usensor.get_dist_cm();
00350         if(right<min)
00351         min=right;
00352         
00353       if(min <  obstacleDistance ) 
00354       {
00355           autoDrivePartOk = false;
00356           return min;  
00357           
00358       }
00359 
00360     Servo_Look_Center();
00361     wait_ms(250);
00362     usensor.start();
00363     int center = usensor.get_dist_cm();
00364         if(center<min)
00365         min=center;
00366    
00367      if(min <  obstacleDistance )
00368      {
00369         autoDrivePartOk = false;
00370         return min;   
00371       }  
00372       //  printf("Min Distance %d\n",min);
00373     
00374     return min;  
00375 } 
00376 
00377   
00378 
00379 void Servo_Look_Left()
00380 {
00381     servo.pulsewidth_us(2000);   
00382 }
00383 
00384 void Servo_Look_Right()
00385 {
00386     servo.pulsewidth_us(1000);   
00387 }
00388 
00389 void Servo_Look_Center()
00390 {
00391     servo.pulsewidth_us(1500);   
00392 }
00393 
00394 void DataReceived_ISR(){
00395    //  char globalChar = LPC_UART2->RBR;
00396    //  lastCommandChar = LPC_UART2->RBR;
00397      incomingMsg[incomingMsg_iter++] = LPC_UART2->RBR ;
00398      
00399     // globalString += globalChar; 
00400 }
00401 
00402 void DataReceived_PCISR(){
00403    //  char globalChar = LPC_UART2->RBR;
00404    //  lastCommandChar = LPC_UART2->RBR;
00405      incomingMsg[incomingMsg_iter++] = LPC_UART0->RBR ;
00406      
00407     // globalString += globalChar; 
00408 }
00409 
00410 
00411 void Servo_NoNo(int count)
00412 {
00413     for(int i=0 ; i< count ; i++) {
00414         Servo_Look_Left();
00415         wait(0.2);
00416         Servo_Look_Right();
00417         wait(0.2);
00418     } 
00419    Servo_Look_Center();
00420 }
00421 
00422 float getCommandValue(string cmd){
00423     string cmdValue;   
00424     std::size_t sharpPosition = cmd.find("#");  
00425     cmdValue = cmd.substr(sharpPosition+1);
00426  //   printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
00427     return atof(cmdValue.c_str());
00428 }
00429 
00430 
00431 
00432 
00433 void BTCommands(string cmd)
00434 {
00435    #ifdef DEBUG_BT_COMMANDS
00436    printf("Incoming BT command : %s\n",cmd);  // - Print the incoming command
00437    #endif
00438    if (cmd.find("forward#")!= string::npos)
00439    {
00440        float time = getCommandValue(cmd);
00441        DriveCommand* dc;
00442        dc = (DriveCommand*)malloc(sizeof(DriveCommand));
00443        dc->SetTime(time);
00444        dc->SetDir('f');
00445        autoDriverQueue.Put(dc);
00446    }
00447    else if(cmd.find("backwards#")!= string::npos)
00448    {
00449        float time = getCommandValue(cmd);
00450        DriveCommand* dc;
00451        dc = (DriveCommand*)malloc(sizeof(DriveCommand));
00452        dc->SetTime(time);
00453        dc->SetDir('b');
00454        autoDriverQueue.Put(dc);
00455    }
00456     else if(cmd.find("obstacle#")!= string::npos)
00457    {
00458        obstacleDistance = getCommandValue(cmd);
00459       
00460    }  
00461    
00462    else if (cmd.find("rotleftms#")!= string::npos)
00463    {
00464        rotateLeftMillis = getCommandValue(cmd);
00465        Rotate_Left();  // todo remove after testing
00466        
00467    } 
00468     else if (cmd.find("rotrightms#")!= string::npos)
00469    {
00470        rotateRightMillis = getCommandValue(cmd);
00471        Rotate_Right(); // todo remove after testing
00472    } 
00473     else if (cmd.find("turnspeed#")!= string::npos)
00474    {
00475        turnSpeed = getCommandValue(cmd);
00476       
00477    }    
00478    else if(cmd.find("rotleft")!= string::npos)
00479    {    
00480        Rotate_Left();
00481    }
00482    else if(cmd.find("rotright")!= string::npos)
00483    {    
00484        Rotate_Right();
00485    }  
00486    else if(cmd.find("left#")!= string::npos)
00487    {
00488        float time = getCommandValue(cmd);
00489        DriveCommand* dc;
00490        dc = (DriveCommand*)malloc(sizeof(DriveCommand));
00491        dc->SetTime(time);
00492        dc->SetDir('l');
00493        autoDriverQueue.Put(dc);
00494    }  
00495          else if(cmd.find("right#")!= string::npos)
00496    {
00497        float time = getCommandValue(cmd);
00498        DriveCommand* dc;
00499        dc = (DriveCommand*)malloc(sizeof(DriveCommand));
00500        dc->SetTime(time);
00501        dc->SetDir('r');
00502        autoDriverQueue.Put(dc);
00503    }   
00504    else if (cmd.find("autodrive")!= string::npos)
00505    {
00506        autoDriveFlag = true;
00507 
00508    } 
00509    else if (cmd.find("forward")!= string::npos)
00510    {
00511       if(lastDistance < obstacleDistance)
00512       {
00513         Servo_NoNo(2);
00514         return;
00515        } 
00516        Drive_Forward(vehicleSpeed);
00517    }   
00518    else if (cmd.find("stop")!= string::npos)
00519    {
00520        Drive_Stop();
00521    } 
00522    else if (cmd.find("backwards")!= string::npos)
00523    {
00524        Drive_Backwards(vehicleSpeed);
00525    }
00526    else if (cmd.find("tleft")!= string::npos)
00527    {
00528        Turn_Left(vehicleSpeed);
00529    }
00530    else if (cmd.find("tright")!= string::npos)
00531    {
00532        Turn_Right(vehicleSpeed);
00533    }
00534    else if (cmd.find("dright")!= string::npos)
00535    {
00536        Drive_Right(vehicleSpeed);
00537    }
00538    else if (cmd.find("dleft")!= string::npos)
00539    {
00540        Drive_Left(vehicleSpeed);
00541    }
00542       else if (cmd.find("rleft")!= string::npos)
00543    {
00544        Reverse_Left(vehicleSpeed);
00545    }
00546       else if (cmd.find("rright")!= string::npos)
00547    {
00548        Reverse_Right(vehicleSpeed);
00549    }
00550     else if (cmd.find("speed1")!= string::npos)
00551    {
00552        vehicleSpeed = 0.25; 
00553        
00554    }
00555       else if (cmd.find("speed2")!= string::npos)
00556    {
00557       vehicleSpeed = 0.50; 
00558        
00559    }
00560        else if (cmd.find("speed3")!= string::npos)
00561    {
00562       vehicleSpeed = 0.75; 
00563        
00564    }
00565        else if (cmd.find("speed4")!= string::npos)
00566    {
00567       vehicleSpeed = 1; 
00568        
00569    }  
00570    else
00571    {
00572        Drive_Stop();
00573     }
00574     
00575 }
00576 
00577 void BTCommands2(char cmd)
00578 {
00579    #ifdef DEBUG_BT_COMMANDS
00580    printf("Incoming BT command : %c\n",cmd);  // - Print the incoming command
00581    #endif
00582 
00583     switch(cmd)
00584     {
00585         case 'F' : Drive_Forward(1);    break;
00586         case 'B' : Drive_Backwards(1);    break;
00587         case 'S' : Drive_Stop();        break;
00588         case 'L' : Drive_Left(1);       break;
00589         case 'R' : Drive_Right(1);      break;  
00590 }
00591 }
00592 
00593 
00594 void init()
00595 {
00596     bt.baud(115200);
00597     bt.attach(&DataReceived_ISR,Serial::RxIrq);  
00598     pc.attach(&DataReceived_PCISR,Serial::RxIrq);  
00599     Reset_IncomingMsg();
00600 }
00601 
00602 
00603 void UltraSonic_Thread_Func(void const *args)
00604 {
00605     while(1)
00606     {
00607       
00608     //  lastDistance = GetDistance();
00609     usensor.start(); 
00610     lastDistance = usensor.get_dist_cm();
00611       
00612       if(moveForward_flag)
00613       { 
00614         lastDistance = Get_Min_Distance();    
00615       }
00616 
00617       
00618       #ifdef DEBUG_ULTRASONICE
00619       pc.printf("Distance : %f\n",lastDistance );
00620       #endif
00621       
00622         if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
00623             autoDrivePartOk = false;
00624             Drive_Force_Stop();
00625             moveForward_flag = false; 
00626             Servo_NoNo(1);
00627         }
00628         else if((turnRight_Flag || turnLeft_Flag) && lastDistance < obstacleDistance  ) { // todo : minimize code here
00629             autoDrivePartOk = false;
00630             Drive_Stop();
00631             
00632            // moveForward_flag = false;
00633         } 
00634 
00635         
00636       Thread::wait(ULTRASONIC_CHECK_DELAY_MS);     
00637     }  
00638 }
00639 
00640 void UltraSonic_Thread_Emergency(void const *args)
00641 {
00642     while(1)
00643     {
00644     usensor.start(); 
00645     lastDistance = usensor.get_dist_cm();
00646     
00647       #ifdef DEBUG_ULTRASONICE
00648       pc.printf("Distance : %f\n",lastDistance );
00649       #endif
00650       
00651         if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
00652             Drive_Force_Stop();
00653             moveForward_flag = false;
00654             Servo_NoNo(1);
00655         }     
00656       Thread::wait(ULTRASONIC_EMERGENCY_DELAY_MS);     
00657     }  
00658 }
00659 
00660 
00661 
00662 void ExecuteNextCommand()
00663 {
00664     DriveCommand* dc = (DriveCommand*)malloc(sizeof(DriveCommand)) ;
00665   //  printf("Before get from queue\n");
00666     autoDriverQueue.Get(dc);
00667   //  printf("after get from queue\n");
00668     float time = dc->GetTime();
00669     char dir = dc->GetDir();
00670   //  printf("AutoDrive Runing at Dir:%c, Time:%d\n",dir,time);
00671     autoDriveRun = true;
00672     autoDrivePartOk = true;
00673     switch(dir)
00674     {
00675         case 'f': 
00676         Drive_Forward(vehicleSpeed);
00677         break;
00678         case 'b':
00679         Drive_Backwards(vehicleSpeed);
00680         break;
00681         case 'l':
00682         Turn_Left(turnSpeed);
00683         wait_ms(rotateLeftMillis);
00684         Drive_Forward(vehicleSpeed);
00685         break;
00686         case 'r':
00687         Turn_Right(turnSpeed);
00688         wait_ms(rotateRightMillis);      
00689         Drive_Forward(vehicleSpeed);
00690         break;        
00691     } 
00692     
00693     StopAfter(time);
00694     free(dc);
00695     
00696     while(autoDriveRun)
00697         wait_ms(50);
00698         
00699     if(autoDrivePartOk)
00700         bt.printf("ok%d~",autoDriveCounter);
00701     else
00702         bt.printf("fail%d~",autoDriveCounter);    
00703  
00704     autoDriveCounter++;
00705 
00706     
00707 }
00708 
00709 
00710 
00711 
00712 
00713 void AutoDrive_Thread_Func(void const *args)
00714 {
00715         while(true) { 
00716         
00717         // todo add check for autoDriveFlag
00718          if(autoDriveFlag && autoDriverQueue.GetNumberOfItems() >0 )
00719          {
00720             // printf("Autodrive Commands  before commit\n");
00721             if(!autoDriveRun) //check that there is no running command right now
00722                 ExecuteNextCommand(); 
00723             // printf("Autodrive Commands  after commit\n");
00724              
00725          }
00726          else
00727          {
00728             autoDriveFlag = false;
00729             autoDriveCounter =0;            
00730          }
00731             
00732         //printf("Checking for Autodrive Commands\n");
00733         Thread::wait(AUTODRIVE_CHECK_DELAY_MS);
00734     }
00735 }
00736 
00737 
00738 
00739 
00740 void BT_Thread_Func(void const *args)
00741 {
00742 //    while(true) { 
00743 //        if(globalString.length() > 0 )
00744 //        {
00745 //            BTCommands(globalString);
00746 //            globalString = "";
00747 //        } 
00748 //        Thread::wait(100);
00749 //    }
00750 //    
00751         while(true) { 
00752 //        if(lastCommandChar != 'x' )
00753 //        {
00754 //            BTCommands2(lastCommandChar);
00755 //            lastCommandChar = 'x';
00756 //        }
00757         
00758          string cmd = Get_last_Msg();
00759          if(succesReadFlag)
00760          {
00761              
00762              BTCommands(cmd);
00763              succesReadFlag = false;
00764          }        
00765         
00766         Thread::wait(BT_CHECK_DELAY_MS);
00767     }
00768 }
00769 
00770 
00771 int main()
00772 {
00773  //   mu.startUpdates();//start mesuring the distance
00774     
00775     init();
00776     printf("Hello\n");
00777     Thread btThread(BT_Thread_Func); 
00778     btThread.set_priority(osPriorityNormal);
00779     Thread ultraSonicThread(UltraSonic_Thread_Func);
00780     ultraSonicThread.set_priority(osPriorityNormal);
00781     Thread ultraSonicEmergencyThread(UltraSonic_Thread_Emergency);
00782     ultraSonicEmergencyThread.set_priority(osPriorityNormal); 
00783     Thread AutoDriveThread(AutoDrive_Thread_Func);
00784     AutoDriveThread.set_priority(osPriorityNormal);
00785     
00786     
00787     
00788          
00789    Servo_Look_Left();
00790    wait(2);   
00791    Servo_Look_Right();
00792    wait(2);    
00793    Servo_Look_Center();
00794    
00795       
00796  //  Drive_Forward(0.5);
00797    
00798  //  StopAfter(3.0);
00799     
00800        
00801   //     autoDriverQueue.Put(&DriveCommand('f',120)); 
00802                  
00803     while(1)
00804     {
00805     //   mu._checkDistance();   
00806         //Do something else here
00807    //     mu.checkDistance();     //call checkDistance() as much as possible, as this is where  //the class checks if dist needs to be called.
00808     }
00809        
00810 }
00811 
00812 
00813