File content as of revision 6:edbaeaaf08bb:

/***********************************************************************
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();
void onButtonA(MicroBitEvent);
void onButtonB(MicroBitEvent);
void PrintSonarTiming(void);
void RunBasicBuggyMotorTest(unsigned int Voltage, unsigned int Time_ms, unsigned int Pause_ms);
void TestSonar();

/*******************************
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);
MicroBitButton buttonA(MICROBIT_PIN_BUTTON_A, MICROBIT_ID_BUTTON_A);
MicroBitButton buttonB(MICROBIT_PIN_BUTTON_B, MICROBIT_ID_BUTTON_B);
 

/**********************************
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;
int MotorTestIndex = 0;
bool MotorTestGo = false;

/*****************************************************************************
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);
    /*The sw MicroBitButtons instantiated above also have events assigned to them. This line will 
    suppress MICROBIT_BUTTON_EVT_CLICK and MICROBIT_BUTTON_EVT_LONG_CLICK events on the button, otherwise
    2 events will be flagged*/
    buttonA.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
    buttonB.setEventConfiguration(MICROBIT_BUTTON_SIMPLE_EVENTS);
    //Listen out for the button A press event
    uBit.messageBus.listen(MICROBIT_ID_BUTTON_A, MICROBIT_BUTTON_EVT_CLICK, onButtonA, MESSAGE_BUS_LISTENER_IMMEDIATE);
    uBit.messageBus.listen(MICROBIT_ID_BUTTON_B, MICROBIT_BUTTON_EVT_CLICK, onButtonB, 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);
}

void MotorSpeedCharacterisation(void)
{
    int Voltage;
    char OutBuf[7];
    int Counter;
    unsigned int LastSystemTime;
    
    //do this forever
    while(1)
    {
        if(MotorTestIndex < 10) //Move forward tests
        {
            Voltage = (MotorTestIndex * 100) + 100;
            sprintf(OutBuf, "F%d", Voltage);
        }
        else if(MotorTestIndex < 20)    //Rotate clockwise test
        {
            Voltage = ((MotorTestIndex-10) * 100) + 100;
            sprintf(OutBuf, "C%d", Voltage);
        }
        else    //Rotate anticlockwise tests
        {
            Voltage = ((MotorTestIndex-20) * 100) + 100;
            sprintf(OutBuf, "A%d", Voltage);
        }
        //Display the current target test
        uBit.display.print(OutBuf);
        
        //If button B has been pressed
        if(MotorTestGo == true)
        {
            //do the algorithm for counting down from 3
            Counter = 3;
            LastSystemTime = 0;
            uBit.display.printAsync(Counter);
            LastSystemTime = uBit.systemTime();
            while (Counter > 0)
            {
                if( (uBit.systemTime()-LastSystemTime) > 1000)
                {    
                    LastSystemTime = uBit.systemTime();
                    Counter = Counter - 1;
                    uBit.display.printAsync(Counter);
                }
            }
            
            //run the selected motor characterisation test
            if(MotorTestIndex < 10) //Move forward tests
            {
                MoveBuggy(MOVE_FORWARD, Voltage, 1000);
                do
                {
                    //Nothing
                }while( hasLastCommandCompleted() == false );
            }
            else if(MotorTestIndex < 20)    //Rotate clockwise test
            {
                MoveBuggy(ROTATE_CLOCKWISE, Voltage, 500);
                do
                {
                    //Nothing
                }while( hasLastCommandCompleted() == false );
            }
            else    //Rotate anticlockwise tests
            {
                MoveBuggy(ROTATE_ANTICLOCKWISE, Voltage, 500);
                do
                {
                    //Nothing
                }while( hasLastCommandCompleted() == false );
            }
    
            MotorTestGo = false;    
        }
    }
}

void onButtonA(MicroBitEvent evt) 
{ 
    if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
    {
        MotorTestIndex++;
        if(MotorTestIndex > 30)
        {
            MotorTestIndex = 0; 
        }    
    }
} 

void onButtonB(MicroBitEvent evt) 
{ 
    if(evt.value == MICROBIT_BUTTON_EVT_CLICK)
    {
        MotorTestGo = true;
    }
} 

void SelfTest()
{
    int Counter = 3;
    unsigned int LastSystemTime = 0;
    
    //Display start message using the LEDs
    //uBit.display.scroll("Hello World");
    //Instead, display 3, 2, 1, 0
    uBit.display.printAsync(Counter);
    LastSystemTime = uBit.systemTime();
    while (Counter > 0)
    {
        if( (uBit.systemTime()-LastSystemTime) > 1000)
        {    
            //uBit.serial.printf("Counter=%d, ", Counter);
            LastSystemTime = uBit.systemTime();
            Counter = Counter - 1;
            uBit.display.printAsync(Counter);
        }
    }
        
    while(1)
    {
        //Run the sonar test for 10 secs
        LastSystemTime = uBit.systemTime();
        while( (uBit.systemTime()-LastSystemTime) < 10000)
        {    
            TestSonar();
        }
        //Display 0
        uBit.display.printAsync(Counter);
        //Run the motor test
        RunBasicBuggyMotorTest(500, 1000, 1000);
    }
}

void DisplaySonarTiming()
{
    while(1)
    {
        PrintSonarTiming();
    }
    
}