Buggy.cpp

00001 /***********************************************************************
00002 This file contains the different function calls that can be used to control
00003 the buggy and read and control the ultrasonic sensor on the buggy
00004 *************************************************************************/
00005 /************
00006 Includes
00007 **************/
00008 #include "Buggy.h"
00009 #include <stdio.h>
00010 
00011 /******************
00012 Definition of constants
00013 ******************/
00014 #define BUGGY_HALTED    0
00015 #define BUGGY_RUNNING   1
00016 #define BUGGY_PAUSED    2
00017 
00018 /*******************************
00019 Local function declarations
00020 *******************************/
00021 void onSonarEchoPulse(MicroBitEvent evt);
00022 void MoveForward(unsigned int Voltage, unsigned int Time_ms);
00023 void MoveBackward(unsigned int Voltage, unsigned int Time_ms);
00024 void RotateClockwise(unsigned int Voltage, unsigned int Time_ms);
00025 void RotateAnticlockwise(unsigned int Voltage, unsigned int Time_ms);
00026 void SendSonarTrigger();
00027 void onButtonA(MicroBitEvent);
00028 void onButtonB(MicroBitEvent);
00029 void PrintSonarTiming(void);
00030 void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms);
00031 void TestSonar();
00032 
00033 /*******************************
00034 Global variables for the microbit runtime
00035 ******************************/
00036 MicroBit uBit;
00037 MicroBitPin MotorSpeed_L(MICROBIT_ID_IO_P0, MICROBIT_PIN_P0, PIN_CAPABILITY_ANALOG);
00038 MicroBitPin MotorDirection_L(MICROBIT_ID_IO_P8, MICROBIT_PIN_P8, PIN_CAPABILITY_DIGITAL );
00039 MicroBitPin MotorSpeed_R(MICROBIT_ID_IO_P1, MICROBIT_PIN_P1, PIN_CAPABILITY_ANALOG);
00040 MicroBitPin MotorDirection_R(MICROBIT_ID_IO_P12, MICROBIT_PIN_P12, PIN_CAPABILITY_DIGITAL );
00041 //Note: The buggy has both the trigger and echo signals on the same microbit pin
00042 MicroBitPin Sonar(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
00043 MicroBitButton buttonA(MICROBIT_PIN_BUTTON_A, MICROBIT_ID_BUTTON_A);
00044 MicroBitButton buttonB(MICROBIT_PIN_BUTTON_B, MICROBIT_ID_BUTTON_B);
00045  
00046 
00047 /**********************************
00048 Global variables for the buggy interface
00049 *************************************/
00050 unsigned long SonarReturnPulseWidth = 0xFFFFFFFF;
00051 int TargetMotorRunTime = 0;
00052 int CurrentMotorRunTime = 0;
00053 unsigned int TimerStart = 0;
00054 unsigned int LastMotorVoltage_L = 0;
00055 unsigned int LastMotorVoltage_R = 0;
00056 unsigned int LastMotorDirection_L = 0;
00057 unsigned int LastMotorDirection_R = 0;
00058 int BuggyState = BUGGY_HALTED;
00059 Ticker SonarTriggerTimer;
00060 int MotorTestIndex = 0;
00061 bool MotorTestGo = false;
00062 
00063 /*****************************************************************************
00064 Start of function definitions
00065 *****************************************************************************/
00066 void InitialiseBuggy()
00067 {
00068     //Initialise the micro:bit runtime.
00069     uBit.init();
00070     
00071     //setup the message bus to listen for events from the ultrasonic sensor input
00072     uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_PULSE_HI, onSonarEchoPulse, MESSAGE_BUS_LISTENER_IMMEDIATE);
00073     /*The sw MicroBitButtons instantiated above also have events assigned to them. This line will 
00074     suppress MICROBIT_BUTTON_EVT_CLICK and MICROBIT_BUTTON_EVT_LONG_CLICK events on the button, otherwise
00075     2 events will be flagged*/
00076     buttonA.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
00077     buttonB.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
00078     //Listen out for the button A press event
00079     uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA, MESSAGE_BUS_LISTENER_IMMEDIATE);
00080     uBit.messageBus.listen(MICROBIT_ID_BUTTON_B, MICROBIT_BUTTON_EVT_CLICK, onButtonB, MESSAGE_BUS_LISTENER_IMMEDIATE);
00081         
00082     //Attach the SendSonarTrigger function to the timer and configure for it to trigger every 0.2 secs.
00083     SonarTriggerTimer.attach(&SendSonarTrigger, 0.2); 
00084 }
00085 
00086 void SendSonarTrigger()
00087 {
00088     //Turn the monitoring of the event off whilst the trigger pulse is sent.
00089     Sonar.eventOn(MICROBIT_PIN_EVENT_NONE);
00090     /*Set the trigger to high for 10 microseconds, then back to a low. This
00091     setDigitalValue function also changes the pin to an output pin*/
00092     Sonar.setDigitalValue(1);
00093     wait_us(10);
00094     Sonar.setDigitalValue(0);
00095     
00096     //Call this function to turn the sonar pin back to an input pin to await the echo.
00097     Sonar.getDigitalValue();
00098     //Turn the event back on so that this function responds again
00099     Sonar.eventOn(MICROBIT_PIN_EVENT_ON_PULSE);
00100     
00101     return;
00102     
00103 }
00104 
00105 void onSonarEchoPulse(MicroBitEvent evt)
00106 {
00107     //Read the pulse wdith of the returned echo.  This is returned in the timestamp of the event.
00108     SonarReturnPulseWidth = evt.timestamp;
00109     
00110     return;
00111 }
00112 
00113 unsigned int GetSonarTime_us()
00114 {
00115     return (unsigned int)SonarReturnPulseWidth;
00116 }
00117 
00118 bool hasLastCommandCompleted()
00119 {
00120     if(BuggyState == BUGGY_RUNNING)
00121     {
00122         CurrentMotorRunTime = uBit.systemTime() - TimerStart;
00123         if(CurrentMotorRunTime >= TargetMotorRunTime)
00124         {
00125             //Stop the motors by writing a 0 voltage and setting direction to forward.
00126             MotorDirection_L.setDigitalValue(0);
00127             MotorDirection_R.setDigitalValue(0);
00128             MotorSpeed_L.setAnalogValue(0);  
00129             MotorSpeed_R.setAnalogValue(0);
00130             
00131             //Update the buggy state to show that it is now halted
00132             BuggyState = BUGGY_HALTED;
00133             
00134             //completed
00135             return true;
00136         }
00137         else
00138         {
00139             //return not completed
00140             return false;
00141         }
00142     }
00143     
00144     //return no last command was running
00145     return false; 
00146 }
00147 
00148 void PauseLastCommand()
00149 {
00150     //Only do this if the buggy is actually running
00151     if(BuggyState == BUGGY_RUNNING)
00152     {
00153         //Store the amount of elapsed time before the command is paused.
00154         CurrentMotorRunTime = uBit.systemTime() - TimerStart;
00155         //Stop the motors by writing a 0 voltage and setting direction to forward.
00156         MotorDirection_L.setDigitalValue(0);
00157         MotorDirection_R.setDigitalValue(0);
00158         MotorSpeed_L.setAnalogValue(0);  
00159         MotorSpeed_R.setAnalogValue(0);
00160         
00161         //Update the buggy state to show that it is now halted
00162         BuggyState = BUGGY_PAUSED;
00163     }
00164 }
00165 
00166 void ContinueLastCommand()
00167 {
00168     //Only do this if the buggy is currently halted we have not yet reached the target run time
00169     if( (BuggyState == BUGGY_PAUSED) && (TargetMotorRunTime > CurrentMotorRunTime) )
00170     {
00171         //Update the target motor run time to only run for the incomplete time period and zero CurrentMotorRunTime
00172         TargetMotorRunTime = TargetMotorRunTime - CurrentMotorRunTime;
00173         CurrentMotorRunTime = 0;
00174         //Set the voltage of the motors as per the stored last voltage and direction command.
00175         MotorDirection_L.setDigitalValue(LastMotorDirection_L);
00176         MotorDirection_R.setDigitalValue(LastMotorDirection_R);
00177         MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);  
00178         MotorSpeed_R.setAnalogValue(LastMotorVoltage_R); 
00179         
00180         //Start the timer
00181         TimerStart = uBit.systemTime();
00182         
00183         //Update the buggy state to show that it is now running
00184         BuggyState = BUGGY_RUNNING;
00185     }
00186 }
00187 
00188 void MoveBuggy(int Command, unsigned int Voltage, unsigned int Time_ms)
00189 {
00190     //Initialise the variables for tracking the travel progress
00191     TargetMotorRunTime = Time_ms;
00192     CurrentMotorRunTime = 0;
00193     
00194     //Limit the voltage to 1024, which is the max A2D output value.
00195     if(Voltage > 1024)
00196     {
00197         Voltage = 1024;
00198     }
00199     
00200     switch(Command)
00201     {
00202         case MOVE_FORWARD:
00203             //Set the motor voltage as requested by the user
00204             LastMotorVoltage_L = Voltage;
00205             LastMotorVoltage_R = Voltage;
00206             //Set motor direction to forward
00207             LastMotorDirection_L = 0;
00208             LastMotorDirection_R = 0; 
00209             break;
00210             
00211         case MOVE_BACKWARD:
00212             /*Set the voltage of the motors as per the user request (1024 - value)
00213             In reverse, 0 is actually max speed and 1024 is stopped.*/
00214             LastMotorVoltage_L = 1024 - Voltage;
00215             LastMotorVoltage_R = 1024 - Voltage;
00216             //Set the motor direction to reverse
00217             LastMotorDirection_L = 1;
00218             LastMotorDirection_R = 1;
00219             break;
00220             
00221         case ROTATE_CLOCKWISE:
00222             /*Set the voltage of the motors as per the user request (1024 - value)
00223             In reverse, 0 is actually max speed and 1024 is stopped.*/
00224             LastMotorVoltage_L = Voltage;
00225             LastMotorVoltage_R = 1024 - Voltage;
00226             //Set the direction to left wheel forwards and right wheel backward
00227             LastMotorDirection_L = 0;
00228             LastMotorDirection_R = 1;
00229             break;
00230             
00231         case ROTATE_ANTICLOCKWISE:
00232             /*Set the voltage of the motors as per the user request (1024 - value)
00233             In reverse, 0 is actually max speed and 1024 is stopped.*/
00234             LastMotorVoltage_L = 1024 - Voltage;
00235             LastMotorVoltage_R = Voltage;
00236             //Set the direction to left wheel backwards and right wheel forward
00237             LastMotorDirection_L = 1;
00238             LastMotorDirection_R = 0;
00239             break;
00240             
00241         default:
00242         
00243             break;    
00244     }
00245     
00246     //Set the direction of the motors as per the command
00247     MotorDirection_L.setDigitalValue(LastMotorDirection_L);
00248     MotorDirection_R.setDigitalValue(LastMotorDirection_R);
00249     wait_ms(1);
00250     //Set the voltage of the motors as per the user request 
00251     MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);  
00252     MotorSpeed_R.setAnalogValue(LastMotorVoltage_R); 
00253     
00254     //Start the timer
00255     TimerStart = uBit.systemTime();
00256     
00257     //Update the buggy state to show that it is now running
00258     BuggyState = BUGGY_RUNNING;
00259     
00260     return;
00261 }
00262 
00263 
00264 void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms)
00265 {
00266     //Move the buggy forward
00267     MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
00268     //wait and check if the command has completed
00269     do
00270     {
00271         //sleep whilst we wait for the command to complete
00272         uBit.sleep(Time_ms);
00273     }while( hasLastCommandCompleted() == false );
00274     //Pause before doing the next command
00275     wait_ms(Pause_ms);
00276     
00277     //Move the buggy bacward    
00278     MoveBuggy(MOVE_BACKWARD, Voltage, Time_ms);
00279     //wait and check if the command has completed
00280     do
00281     {
00282         //sleep whilst we wait for the command to complete
00283         uBit.sleep(Time_ms);
00284     }while( hasLastCommandCompleted() == false );
00285     //Pause before doing the next command
00286     wait_ms(Pause_ms);
00287     
00288     //Rotate the buggy clockwise
00289     MoveBuggy(ROTATE_CLOCKWISE, Voltage, Time_ms);
00290     //wait and check if the command has completed
00291     do
00292     {
00293         //sleep whilst we wait for the command to complete
00294         uBit.sleep(Time_ms);
00295     }while( hasLastCommandCompleted() == false );
00296     //Pause before doing the next command
00297     wait_ms(Pause_ms);
00298     
00299     //Rotate the buggy anticloclwise
00300     MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, Time_ms);
00301     //wait and check if the command has completed
00302     do
00303     {
00304         //sleep whilst we wait for the command to complete
00305         uBit.sleep(Time_ms);
00306     }while( hasLastCommandCompleted() == false );
00307     //Pause before doing the next command
00308     wait_ms(Pause_ms);
00309     
00310     return;
00311 }
00312 
00313 void TestAntiCollision(unsigned int Voltage, unsigned int Time_ms, unsigned int SonarTime_us)
00314 {
00315     MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
00316     do
00317     {
00318         if(GetSonarTime_us() < SonarTime_us)
00319         {
00320             PauseLastCommand();
00321         }
00322         else
00323         {
00324             ContinueLastCommand();
00325         }
00326     }while( hasLastCommandCompleted() == false );
00327     
00328     return;
00329     
00330 }
00331 
00332 void TestSonar()
00333 {
00334     //Local variables
00335     MicroBitImage SonarImage;
00336     unsigned int SonarDistance;
00337     int NumPixels, x, y;
00338     int MaxDisplayDist = 100;
00339     int WholeRows, RemainderColumns;
00340        
00341     //Compute the distance in cm. the 58 is taken from the datasheet
00342     SonarDistance = GetSonarTime_us()/58;
00343     //limit to 1m
00344     if(SonarDistance > MaxDisplayDist)
00345     {
00346         SonarDistance = MaxDisplayDist;   
00347     }
00348        
00349     //Convert the distance to the number of pixels to light
00350     NumPixels = (SonarDistance*25)/MaxDisplayDist;
00351     //Convert into the number of whole pixel rows and remainder columns to light
00352     WholeRows = NumPixels/5;
00353     RemainderColumns = NumPixels%5;
00354    
00355     //First fill the whole rows
00356     for(y=0; y<WholeRows; y++)
00357     {
00358         for(x=0; x<5; x++)
00359         {
00360             uBit.display.image.setPixelValue(x, y, 200);
00361         }
00362     }
00363 
00364     //fill the partial row
00365     if(WholeRows < 5)
00366     {
00367         for(x=0; x<RemainderColumns; x++)
00368         {
00369             uBit.display.image.setPixelValue(x, y, 200);
00370         }
00371     }
00372     
00373     //Fill the remaining pixels in the partial row with 0
00374     for( ; x<5; x++)
00375     {
00376         uBit.display.image.setPixelValue(x, y, 0);
00377     }
00378     //Continue from the next row
00379     y++;
00380     for( ; y<5; y++)
00381     {
00382         for(x=0; x<5; x++)
00383         {
00384             uBit.display.image.setPixelValue(x, y, 0);
00385         }
00386     }
00387 
00388     return;
00389 }
00390 
00391 void PrintSonarTiming(void)
00392 {
00393     //Local variables
00394     int SonarTime;
00395     
00396     //read the latest sonar time
00397     SonarTime = GetSonarTime_us();
00398     //Print the time in micro secs
00399     uBit.display.printAsync(SonarTime);
00400     uBit.serial.printf("Time = %d us\n\r", SonarTime);
00401 }
00402 
00403 void MotorSpeedCharacterisation(void)
00404 {
00405     int Voltage;
00406     char OutBuf[7];
00407     int Counter;
00408     unsigned int LastSystemTime;
00409     
00410     //do this forever
00411     while(1)
00412     {
00413         if(MotorTestIndex < 10) //Move forward tests
00414         {
00415             Voltage = (MotorTestIndex * 100) + 100;
00416             sprintf(OutBuf, "F%d", Voltage);
00417         }
00418         else if(MotorTestIndex < 20)    //Rotate clockwise test
00419         {
00420             Voltage = ((MotorTestIndex-10) * 100) + 100;
00421             sprintf(OutBuf, "C%d", Voltage);
00422         }
00423         else    //Rotate anticlockwise tests
00424         {
00425             Voltage = ((MotorTestIndex-20) * 100) + 100;
00426             sprintf(OutBuf, "A%d", Voltage);
00427         }
00428         //Display the current target test
00429         uBit.display.print(OutBuf);
00430         
00431         //If button B has been pressed
00432         if(MotorTestGo == true)
00433         {
00434             //do the algorithm for counting down from 3
00435             Counter = 3;
00436             LastSystemTime = 0;
00437             uBit.display.printAsync(Counter);
00438             LastSystemTime = uBit.systemTime();
00439             while (Counter > 0)
00440             {
00441                 if( (uBit.systemTime()-LastSystemTime) > 1000)
00442                 {    
00443                     LastSystemTime = uBit.systemTime();
00444                     Counter = Counter - 1;
00445                     uBit.display.printAsync(Counter);
00446                 }
00447             }
00448             
00449             //run the selected motor characterisation test
00450             if(MotorTestIndex < 10) //Move forward tests
00451             {
00452                 MoveBuggy(MOVE_FORWARD, Voltage, 1000);
00453                 do
00454                 {
00455                     //Nothing
00456                 }while( hasLastCommandCompleted() == false );
00457             }
00458             else if(MotorTestIndex < 20)    //Rotate clockwise test
00459             {
00460                 MoveBuggy(ROTATE_CLOCKWISE, Voltage, 500);
00461                 do
00462                 {
00463                     //Nothing
00464                 }while( hasLastCommandCompleted() == false );
00465             }
00466             else    //Rotate anticlockwise tests
00467             {
00468                 MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, 500);
00469                 do
00470                 {
00471                     //Nothing
00472                 }while( hasLastCommandCompleted() == false );
00473             }
00474     
00475             MotorTestGo = false;    
00476         }
00477     }
00478 }
00479 
00480 void onButtonA(MicroBitEvent evt) 
00481 { 
00482     if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
00483     {
00484         MotorTestIndex++;
00485         if(MotorTestIndex > 30)
00486         {
00487             MotorTestIndex = 0; 
00488         }    
00489     }
00490 } 
00491 
00492 void onButtonB(MicroBitEvent evt) 
00493 { 
00494     if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
00495     {
00496         MotorTestGo = true;
00497     }
00498 } 
00499 
00500 void SelfTest()
00501 {
00502     int Counter = 3;
00503     unsigned int LastSystemTime = 0;
00504     
00505     //Display start message using the LEDs
00506     //uBit.display.scroll("Hello World");
00507     //Instead, display 3, 2, 1, 0
00508     uBit.display.printAsync(Counter);
00509     LastSystemTime = uBit.systemTime();
00510     while (Counter > 0)
00511     {
00512         if( (uBit.systemTime()-LastSystemTime) > 1000)
00513         {    
00514             //uBit.serial.printf("Counter=%d, ", Counter);
00515             LastSystemTime = uBit.systemTime();
00516             Counter = Counter - 1;
00517             uBit.display.printAsync(Counter);
00518         }
00519     }
00520         
00521     while(1)
00522     {
00523         //Run the sonar test for 10 secs
00524         LastSystemTime = uBit.systemTime();
00525         while( (uBit.systemTime()-LastSystemTime) < 10000)
00526         {    
00527             TestSonar();
00528         }
00529         //Display 0
00530         uBit.display.printAsync(Counter);
00531         //Run the motor test
00532         RunBasicBuggyMotorTest(500, 1000, 1000);
00533     }
00534 }
00535 
00536 void DisplaySonarTiming()
00537 {
00538     while(1)
00539     {
00540         PrintSonarTiming();
00541     }
00542     
00543 }