File content as of revision 8:262c6c40bff9:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "Buzzer.h"
#include "Motor.h"
#include "LED.h"
#include <ros.h>
#include <std_msgs/UInt8.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);


void keySub(const std_msgs::UInt8 &keyPress)
{
    printf("%d", keyPress.data);
    
    if (keyPress.data == 8) 
    {
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.5);
    }
    
    else if (keyPress.data == 4) 
    {
        MotorR.Forwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.5);
    } 
    
    else if (keyPress.data  == 6) 
    {
        MotorR.Backwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.5);
    } 
    
    else if (keyPress.data  == 2) 
    {
        MotorR.Backwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.5);
    } 
    
    else if (keyPress.data  == 5) 
    {
        MotorR.Stop();
        MotorL.Stop();
        wait(0.5);
    } 
    
    else 
    {
        MotorR.Stop();
        MotorL.Stop(); 
    }
}

    /*--------------------------------------------------------------------------------
    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;
    }