File content as of revision 11:12e73437dc9f:

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

float *TOFrangePtr[4];



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

int Servo1Pos;
int Servo2Pos;

/*--------------------------------------------------------------------------------
Function name: KeySub
Input Parameters: std_msgs::UInt8 &KeyPress
Output Parameters:
Description:
----------------------------------------------------------------------------------*/
void MotorKeySub(const std_msgs::UInt8 &keyPress)
{
    printf("%d", keyPress.data);

    // Lowercase chars //
    //  get keypress information at rasbarry pi to move the DC motor. 
    //"w,W" move forward, "a,A" move left side, "d,D" move right side,"s,S" move backward  
    // "q" stop the motor, "e" check the obstacles automatically  
    if (keyPress.data == 119) { // w
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.05);
    }

    else if (keyPress.data == 97) { // a
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Forwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.05);
    }

    else if (keyPress.data  == 100) { // d
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Backwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.05);
    }

    else if (keyPress.data  == 115) { // s
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Backwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.05);
    }


    // Upper Case Chars
    else if (keyPress.data == 87) { // W
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Forwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.387);
    }

    else if (keyPress.data == 65) { // A
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Forwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.387);
    }

    else if (keyPress.data  == 68) { // D
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Backwards(1.0f);
        MotorL.Forwards(1.0f);
        wait(0.387);
    }

    else if (keyPress.data  == 83) { // S
        if (keyPress.data  == 88 || 120) { // X
            MotorR.Stop();
            MotorL.Stop();
        }
        MotorR.Backwards(1.0f);
        MotorL.Backwards(1.0f);
        wait(0.387);
    }
    
    //Stop command
    else if (keyPress.data == 'q') {
        MotorR.Stop();
        MotorL.Stop();
   }
    else if (keyPress.data == 'e') {
        Check_for_obstacles(*TOFrangePtr);
   }

    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
----------------------------------------------------------------------------------*/
// using the 4 sensor to check the obstacles and escape the obstacles. 
//  TOFRange[0]is back sensor,  TOFRange[1] is leftside sensor, TOFRange[2] is front sensor and  TOFRange[4] is rightside sensor 
// devide the front sensor range to 200~150 and 150~0. 
// when robot detect the obstacles the buzzer and blue led are ON.  
void Check_for_obstacles(float 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;
}



/*--------------------------------------------------------------------------------
Function name: ServoKeySub
Input Parameters:
Output Parameters:
Description:
----------------------------------------------------------------------------------*/
/* Ascii values for arrow Keys
37(left arrow)
38(up arrow)
39(right arrow)
40(down arrow)
*/
// get the keypress data in rasbarry pi and  move  the 2 servo motor. 
//"j" move servo1 to left arrow, "l" move servo1 to right arrow, "i" move servo2 to up arrow, "k" move servo2 to down arrow.  
void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
{
    printf("%d", ServoKeyPress.data);

    if (ServoKeyPress.data == 106) {   // j for Pan Left
        if(Servo1Pos>-45) {
            Servo1Pos = Servo1Pos-IncSize;
            servo1.position(Servo1Pos);
            wait(0.01);
        }
    }

    else if (ServoKeyPress.data == 108) {   // l for Pan Right
        if(Servo1Pos<45) {
            Servo1Pos = Servo1Pos+IncSize;
            servo1.position(Servo1Pos);
            wait(0.01);
        }
    }


    else if (ServoKeyPress.data == 105) {   // i for Tilt Up
        if(Servo2Pos>-45){
        Servo2Pos = Servo2Pos-IncSize;
        servo2.position(Servo2Pos);
        wait(0.01);
        }
    }

    else if (ServoKeyPress.data == 107) {   // K for Tilt Down
        if(Servo2Pos<45){
        Servo2Pos = Servo2Pos+IncSize;
        servo2.position(Servo2Pos);
        wait(0.01);
         }
    }

    else if (ServoKeyPress.data == 44) {   //  for Up
        if(IncSize<20) {
            IncSize=(IncSize+1);
        }
    } else if (ServoKeyPress.data == 46) { //  for Up
        if(IncSize>1) {
            IncSize=(IncSize-1);
        }
    }



    pc.printf("Servo 1 = %d  Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
    servo1.position(Servo1Pos);
    servo2.position(Servo2Pos);

}