File content as of revision 6:2cc2aac35868:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "Buzzer.h"
#include "LED.h"
#include "Motor.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
cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
cBuzzer cBuzzer(Buzz);

/*--------------------------------------------------------------------------------
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) {
       cBuzzer.Beep();
       cRGB_LED.red_led();
        if(TOFRange[2]>150) {   //Provided its 15cm away...
            
            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.red_led();
        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.red_led();
        MotorR.Forwards(0.1f);
        MotorL.Forwards(0.9f);
        
    } else if(TOFRange[0]<200) {
       cBuzzer.Beep();
       cRGB_LED.red_led();
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
    }

    else {
       cRGB_LED.green_led();
        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;
}