Diff: Buggy.cpp

Revision:

2:47b7a55b0805

Child:

4:2d939ef2b09c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Buggy.cpp	Mon Jul 31 21:48:36 2017 +0000
@@ -0,0 +1,384 @@
+/***********************************************************************
+This file contains the different function calls that can be used to control
+the buggy and read and control the ultrasonic sensor on the buggy
+*************************************************************************/
+/************
+Includes
+**************/
+#include "Buggy.h"
+#include <stdio.h>
+
+/******************
+Definition of constants
+******************/
+#define BUGGY_HALTED    0
+#define BUGGY_RUNNING   1
+#define BUGGY_PAUSED    2
+
+/*******************************
+Local function declarations
+*******************************/
+void onSonarEchoPulse(MicroBitEvent evt);
+void MoveForward(unsigned int Voltage, unsigned int Time_ms);
+void MoveBackward(unsigned int Voltage, unsigned int Time_ms);
+void RotateClockwise(unsigned int Voltage, unsigned int Time_ms);
+void RotateAnticlockwise(unsigned int Voltage, unsigned int Time_ms);
+void SendSonarTrigger();
+
+/*******************************
+Global variables for the microbit runtime
+******************************/
+MicroBit uBit;
+MicroBitPin MotorSpeed_L(MICROBIT_ID_IO_P0, MICROBIT_PIN_P0, PIN_CAPABILITY_ANALOG);
+MicroBitPin MotorDirection_L(MICROBIT_ID_IO_P8, MICROBIT_PIN_P8, PIN_CAPABILITY_DIGITAL );
+MicroBitPin MotorSpeed_R(MICROBIT_ID_IO_P1, MICROBIT_PIN_P1, PIN_CAPABILITY_ANALOG);
+MicroBitPin MotorDirection_R(MICROBIT_ID_IO_P12, MICROBIT_PIN_P12, PIN_CAPABILITY_DIGITAL );
+//Note: The buggy has both the trigger and echo signals on the same microbit pin
+MicroBitPin Sonar(MICROBIT_ID_IO_P13, MICROBIT_PIN_P13, PIN_CAPABILITY_DIGITAL);
+ 
+
+/**********************************
+Global variables for the buggy interface
+*************************************/
+unsigned long SonarReturnPulseWidth = 0xFFFFFFFF;
+int TargetMotorRunTime = 0;
+int CurrentMotorRunTime = 0;
+unsigned int TimerStart = 0;
+unsigned int LastMotorVoltage_L = 0;
+unsigned int LastMotorVoltage_R = 0;
+unsigned int LastMotorDirection_L = 0;
+unsigned int LastMotorDirection_R = 0;
+int BuggyState = BUGGY_HALTED;
+Ticker SonarTriggerTimer;
+
+/*****************************************************************************
+Start of function definitions
+*****************************************************************************/
+void InitialiseBuggy()
+{
+    //Initialise the micro:bit runtime.
+    uBit.init();
+    
+    //setup the message bus to listen for events from the ultrasonic sensor input
+    uBit.messageBus.listen(MICROBIT_ID_IO_P13, MICROBIT_PIN_EVT_PULSE_HI, onSonarEchoPulse, MESSAGE_BUS_LISTENER_IMMEDIATE);
+    
+    //Attach the SendSonarTrigger function to the timer and configure for it to trigger every 0.2 secs.
+    SonarTriggerTimer.attach(&SendSonarTrigger, 0.2); 
+}
+
+void SendSonarTrigger()
+{
+    //Turn the monitoring of the event off whilst the trigger pulse is sent.
+    Sonar.eventOn(MICROBIT_PIN_EVENT_NONE);
+    /*Set the trigger to high for 10 microseconds, then back to a low. This
+    setDigitalValue function also changes the pin to an output pin*/
+    Sonar.setDigitalValue(1);
+    wait_us(10);
+    Sonar.setDigitalValue(0);
+    
+    //Call this function to turn the sonar pin back to an input pin to await the echo.
+    Sonar.getDigitalValue();
+    //Turn the event back on so that this function responds again
+    Sonar.eventOn(MICROBIT_PIN_EVENT_ON_PULSE);
+    
+    return;
+    
+}
+
+void onSonarEchoPulse(MicroBitEvent evt)
+{
+    //Read the pulse wdith of the returned echo.  This is returned in the timestamp of the event.
+    SonarReturnPulseWidth = evt.timestamp;
+    
+    return;
+}
+
+unsigned int GetSonarTime_us()
+{
+    return (unsigned int)SonarReturnPulseWidth;
+}
+
+bool hasLastCommandCompleted()
+{
+    if(BuggyState == BUGGY_RUNNING)
+    {
+        CurrentMotorRunTime = uBit.systemTime() - TimerStart;
+        if(CurrentMotorRunTime >= TargetMotorRunTime)
+        {
+            //Stop the motors by writing a 0 voltage and setting direction to forward.
+            MotorDirection_L.setDigitalValue(0);
+            MotorDirection_R.setDigitalValue(0);
+            MotorSpeed_L.setAnalogValue(0);  
+            MotorSpeed_R.setAnalogValue(0);
+            
+            //Update the buggy state to show that it is now halted
+            BuggyState = BUGGY_HALTED;
+            
+            //completed
+            return true;
+        }
+        else
+        {
+            //return not completed
+            return false;
+        }
+    }
+    
+    //return no last command was running
+    return false; 
+}
+
+void PauseLastCommand()
+{
+    //Only do this if the buggy is actually running
+    if(BuggyState == BUGGY_RUNNING)
+    {
+        //Store the amount of elapsed time before the command is paused.
+        CurrentMotorRunTime = uBit.systemTime() - TimerStart;
+        //Stop the motors by writing a 0 voltage and setting direction to forward.
+        MotorDirection_L.setDigitalValue(0);
+        MotorDirection_R.setDigitalValue(0);
+        MotorSpeed_L.setAnalogValue(0);  
+        MotorSpeed_R.setAnalogValue(0);
+        
+        //Update the buggy state to show that it is now halted
+        BuggyState = BUGGY_PAUSED;
+    }
+}
+
+void ContinueLastCommand()
+{
+    //Only do this if the buggy is currently halted we have not yet reached the target run time
+    if( (BuggyState == BUGGY_PAUSED) && (TargetMotorRunTime > CurrentMotorRunTime) )
+    {
+        //Update the target motor run time to only run for the incomplete time period and zero CurrentMotorRunTime
+        TargetMotorRunTime = TargetMotorRunTime - CurrentMotorRunTime;
+        CurrentMotorRunTime = 0;
+        //Set the voltage of the motors as per the stored last voltage and direction command.
+        MotorDirection_L.setDigitalValue(LastMotorDirection_L);
+        MotorDirection_R.setDigitalValue(LastMotorDirection_R);
+        MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);  
+        MotorSpeed_R.setAnalogValue(LastMotorVoltage_R); 
+        
+        //Start the timer
+        TimerStart = uBit.systemTime();
+        
+        //Update the buggy state to show that it is now running
+        BuggyState = BUGGY_RUNNING;
+    }
+}
+
+void MoveBuggy(int Command, unsigned int Voltage, unsigned int Time_ms)
+{
+    //Initialise the variables for tracking the travel progress
+    TargetMotorRunTime = Time_ms;
+    CurrentMotorRunTime = 0;
+    
+    //Limit the voltage to 1024, which is the max A2D output value.
+    if(Voltage > 1024)
+    {
+        Voltage = 1024;
+    }
+    
+    switch(Command)
+    {
+        case MOVE_FORWARD:
+            //Set the motor voltage as requested by the user
+            LastMotorVoltage_L = Voltage;
+            LastMotorVoltage_R = Voltage;
+            //Set motor direction to forward
+            LastMotorDirection_L = 0;
+            LastMotorDirection_R = 0; 
+            break;
+            
+        case MOVE_BACKWARD:
+            /*Set the voltage of the motors as per the user request (1024 - value)
+            In reverse, 0 is actually max speed and 1024 is stopped.*/
+            LastMotorVoltage_L = 1024 - Voltage;
+            LastMotorVoltage_R = 1024 - Voltage;
+            //Set the motor direction to reverse
+            LastMotorDirection_L = 1;
+            LastMotorDirection_R = 1;
+            break;
+            
+        case ROTATE_CLOCKWISE:
+            /*Set the voltage of the motors as per the user request (1024 - value)
+            In reverse, 0 is actually max speed and 1024 is stopped.*/
+            LastMotorVoltage_L = Voltage;
+            LastMotorVoltage_R = 1024 - Voltage;
+            //Set the direction to left wheel forwards and right wheel backward
+            LastMotorDirection_L = 0;
+            LastMotorDirection_R = 1;
+            break;
+            
+        case ROTATE_ANTICLOCKWISE:
+            /*Set the voltage of the motors as per the user request (1024 - value)
+            In reverse, 0 is actually max speed and 1024 is stopped.*/
+            LastMotorVoltage_L = 1024 - Voltage;
+            LastMotorVoltage_R = Voltage;
+            //Set the direction to left wheel backwards and right wheel forward
+            LastMotorDirection_L = 1;
+            LastMotorDirection_R = 0;
+            break;
+            
+        default:
+        
+            break;    
+    }
+    
+    //Set the direction of the motors as per the command
+    MotorDirection_L.setDigitalValue(LastMotorDirection_L);
+    MotorDirection_R.setDigitalValue(LastMotorDirection_R);
+    wait_ms(1);
+    //Set the voltage of the motors as per the user request 
+    MotorSpeed_L.setAnalogValue(LastMotorVoltage_L);  
+    MotorSpeed_R.setAnalogValue(LastMotorVoltage_R); 
+    
+    //Start the timer
+    TimerStart = uBit.systemTime();
+    
+    //Update the buggy state to show that it is now running
+    BuggyState = BUGGY_RUNNING;
+    
+    return;
+}
+
+
+void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms)
+{
+    //Move the buggy forward
+    MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
+    //wait and check if the command has completed
+    do
+    {
+        //sleep whilst we wait for the command to complete
+        uBit.sleep(Time_ms);
+    }while( hasLastCommandCompleted() == false );
+    //Pause before doing the next command
+    wait_ms(Pause_ms);
+    
+    //Move the buggy bacward    
+    MoveBuggy(MOVE_BACKWARD, Voltage, Time_ms);
+    //wait and check if the command has completed
+    do
+    {
+        //sleep whilst we wait for the command to complete
+        uBit.sleep(Time_ms);
+    }while( hasLastCommandCompleted() == false );
+    //Pause before doing the next command
+    wait_ms(Pause_ms);
+    
+    //Rotate the buggy clockwise
+    MoveBuggy(ROTATE_CLOCKWISE, Voltage, Time_ms);
+    //wait and check if the command has completed
+    do
+    {
+        //sleep whilst we wait for the command to complete
+        uBit.sleep(Time_ms);
+    }while( hasLastCommandCompleted() == false );
+    //Pause before doing the next command
+    wait_ms(Pause_ms);
+    
+    //Rotate the buggy anticloclwise
+    MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, Time_ms);
+    //wait and check if the command has completed
+    do
+    {
+        //sleep whilst we wait for the command to complete
+        uBit.sleep(Time_ms);
+    }while( hasLastCommandCompleted() == false );
+    //Pause before doing the next command
+    wait_ms(Pause_ms);
+    
+    return;
+}
+
+void TestAntiCollision(unsigned int Voltage, unsigned int Time_ms, unsigned int SonarTime_us)
+{
+    MoveBuggy(MOVE_FORWARD, Voltage, Time_ms);
+    do
+    {
+        if(GetSonarTime_us() < SonarTime_us)
+        {
+            PauseLastCommand();
+        }
+        else
+        {
+            ContinueLastCommand();
+        }
+    }while( hasLastCommandCompleted() == false );
+    
+    return;
+    
+}
+
+void TestSonar()
+{
+    //Local variables
+    MicroBitImage SonarImage;
+    unsigned int SonarDistance;
+    int NumPixels, x, y;
+    int MaxDisplayDist = 100;
+    int WholeRows, RemainderColumns;
+       
+    //Compute the distance in cm. the 58 is taken from the datasheet
+    SonarDistance = GetSonarTime_us()/58;
+    //limit to 1m
+    if(SonarDistance > MaxDisplayDist)
+    {
+        SonarDistance = MaxDisplayDist;   
+    }
+       
+    //Convert the distance to the number of pixels to light
+    NumPixels = (SonarDistance*25)/MaxDisplayDist;
+    //Convert into the number of whole pixel rows and remainder columns to light
+    WholeRows = NumPixels/5;
+    RemainderColumns = NumPixels%5;
+   
+    //First fill the whole rows
+    for(y=0; y<WholeRows; y++)
+    {
+        for(x=0; x<5; x++)
+        {
+            uBit.display.image.setPixelValue(x, y, 200);
+        }
+    }
+
+    //fill the partial row
+    if(WholeRows < 5)
+    {
+        for(x=0; x<RemainderColumns; x++)
+        {
+            uBit.display.image.setPixelValue(x, y, 200);
+        }
+    }
+    
+    //Fill the remaining pixels in the partial row with 0
+    for( ; x<5; x++)
+    {
+        uBit.display.image.setPixelValue(x, y, 0);
+    }
+    //Continue from the next row
+    y++;
+    for( ; y<5; y++)
+    {
+        for(x=0; x<5; x++)
+        {
+            uBit.display.image.setPixelValue(x, y, 0);
+        }
+    }
+
+    return;
+}
+
+void PrintSonarTiming(void)
+{
+    //Local variables
+    int SonarTime;
+    
+    //read the latest sonar time
+    SonarTime = GetSonarTime_us();
+    //Print the time in micro secs
+    uBit.display.printAsync(SonarTime);
+    uBit.serial.printf("Time = %d us\n\r", SonarTime);
+}