File content as of revision 7:8248af58df5a:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "Buzzer.h"
#include "Motor.h"
#include "LED.h"

cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
cMotor MotorR(M2_PWM, M2_IN1, M2_IN2);  //Right motor class and pin initialisation

//Class definitions
cBuzzer cBuzzer(Buzz);
cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);


/*--------------------------------------------------------------------------------
Function name: update_power_levels
Input Parameters: vBatt
Output Parameters: N/A
Description: Function used to send the battery level over to the LED class locally from global source files
----------------------------------------------------------------------------------*/
void update_power_levels(float vBatt)
{
    cRGB_LED.record_power(vBatt);
}

/*--------------------------------------------------------------------------------
Function name: Check_for_obstacles
Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Output Parameters: N/A
Description: Simple obstacle avoidance functionality
----------------------------------------------------------------------------------*/
void Check_for_obstacles(uint8_t TOFRange[4])
{
    //If top right sensor(6) detects something
    if (TOFRange[2]<200) {

        if(TOFRange[2]>150) {   //Provided its 15cm away...
            cBuzzer.Beep();
            cRGB_LED.blue_led();
            wait(0.02);
            cRGB_LED.display_power();
            if(TOFRange[3]<200) {   //...and the back sensor detects something
                //Smooth turn right
                MotorR.Forwards(0.8f);
                MotorL.Forwards(0.2f);

            } else if(TOFRange[1]<200) { //...and the top left sensor detects something
                //Smooth turn left
                MotorR.Forwards(0.2f);
                MotorL.Forwards(0.8f);

            } else {
                MotorR.Forwards(0.8f);
                MotorL.Forwards(0.2f);
            }

        } else {

            if(TOFRange[3]<200) {
                MotorR.Backwards(0.1f);
                MotorL.Backwards(0.9f);

            } else if(TOFRange[1]<200) {
                MotorR.Backwards(0.9f);
                MotorL.Backwards(0.1f);

            } else {
                MotorR.Backwards(0.1f);
                MotorL.Backwards(0.9f);
            }
        }

    } else if(TOFRange[3]<200) {
        cBuzzer.Beep();
        cRGB_LED.blue_led();
        wait(0.02);
        cRGB_LED.display_power();

        if(TOFRange[1]<200) {
            MotorR.Forwards(1.0f);
            MotorL.Forwards(1.0f);
        } else {

            MotorR.Forwards(0.9f);
            MotorL.Forwards(0.1f);
        }

    } else if(TOFRange[1]<200) {
        cBuzzer.Beep();
        cRGB_LED.blue_led();
        wait(0.02);
        cRGB_LED.display_power();
        MotorR.Forwards(0.1f);
        MotorL.Forwards(0.9f);

    } else if(TOFRange[0]<200) {
        cBuzzer.Beep();
        cRGB_LED.blue_led();
        wait(0.02);
        cRGB_LED.display_power();
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
    }

    else {
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
    }
}

/*--------------------------------------------------------------------------------
Function name: cMotor
Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Output Parameters: N/A
Description: Class constructor (Initialisation upon creating class)
----------------------------------------------------------------------------------*/
cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
{

    // Set initial condition of PWM
    _pwm.period(0.001); //1KHz
    _pwm = 0;

    // Initial condition of output enables
    _fwd = 0;
    _rev = 0;
}

/*--------------------------------------------------------------------------------
Function name: Forwards
Input Parameters: float speed - PWM duty between 0-1
Output Parameters: N/A
Description: Drives the motor forwards at a designated speed
----------------------------------------------------------------------------------*/
void cMotor::Forwards(float speed)
{
    _fwd = 1;
    _rev = 0;
    _pwm = speed;
}

/*--------------------------------------------------------------------------------
Function name: Backwards
Input Parameters: float speed - PWM duty between 0-1
Output Parameters: N/A
Description: Drives the motor backwards at a designated speed
----------------------------------------------------------------------------------*/
void cMotor::Backwards(float speed)
{
    _fwd = 0;
    _rev = 1;
    _pwm = speed;
}

/*--------------------------------------------------------------------------------
Function name: Stop
Input Parameters: N/A
Output Parameters: N/A
Description: Drives the motor backwards at a designated speed
----------------------------------------------------------------------------------*/
void cMotor::Stop()
{
    _fwd = 0;
    _rev = 0;
    _pwm = 0;
}