File content as of revision 3:83ac04b7ee07:

// Craciun Catalin
//  main.h
//   © 2014 Catalin Craciun

#include "mbed.h"
#include "ZumoRobotManager.h"

using namespace std;

#define RGBLED_ON 0
#define RGBLED_OFF 1

DigitalOut LEDRED(LED_RED);
DigitalOut LEDBLUE(LED_BLUE);
DigitalOut LEDGREEN(LED_GREEN);

ZumoRobotManager *zumoRobotManager;

Timer inputTimer;
Serial bluetooth(PTC4, PTC3);

/**
 *  The string receiver from bluetooth
 */
char bluetoothInput[20];
float velocity;

/**
 *  This method waits for the robot's password in order to unlock it
 */
void waitForPassword();
/**
 *  Method for updating the robot
 */
void update();
/**
 *  Method for initialising the robot
 *  It initialises the bluetooth's baud rate, the leds, creates a new instance of ZumoRobotManager and sets the 
 * timer for controlling the bluetooth's transmission flow
 */
void init();

void waitForPassword() {

    // Turning off the leds
    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_OFF;
    while (true) {
        // Waiting to read the password of the bluetooth
        if (bluetooth.readable()) {
            bluetooth.gets(bluetoothInput, 15);
            if (zumoRobotManager -> checkPassword(bluetoothInput)) {
                // Sending a "successfuly message" back to the device which sent the password
                bluetooth.printf("$suc");
                break;
            } else {
                // Sending a "failure message" back to the device which sent the password
                bluetooth.printf("$fai");   
            }
        }
    }
    
    update();
}

void update() {
    
    // Updating loop

    // Setting the timer for controlling the bluetooth flow
    inputTimer.start(); Serial pc(USBTX, USBRX);
    // Lighting the green led
    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_ON;
    while (true) {
        if (inputTimer.read_ms() >= 10 && bluetooth.readable()) {
            inputTimer.reset();
            bluetooth.gets(bluetoothInput, 15);
            // Checking the type of input received
            if ((bluetoothInput[0] == '+' || bluetoothInput[0] == '-') && 
                (bluetoothInput[1] == '+' || bluetoothInput[1] == '-') &&
                bluetoothInput[2] >= '0' && bluetoothInput[2] <='9' &&
                bluetoothInput[3] >= '0' && bluetoothInput[3] <='9' &&
                bluetoothInput[4] >= '0' && bluetoothInput[4] <='9' &&
                bluetoothInput[5] >= '0' && bluetoothInput[5] <='9') {
                    /// Got a "velocity descriptor" string
                    velocity = ((bluetoothInput[2] - '0') * 10.0f + (bluetoothInput[3] - '0') * 1.0f) * 0.01f;
                    if (bluetoothInput[0] == '-')
                        velocity = -velocity;
                    zumoRobotManager -> setVelocityX(velocity);
                    
                    velocity = ((bluetoothInput[4] - '0') * 10.0f + (bluetoothInput[5] - '0') * 1.0f) * 0.01f;
                    if (bluetoothInput[1] == '-')
                        velocity = -velocity;
                    zumoRobotManager -> setVelocityY(velocity);
            } else if (bluetoothInput[0] == 'c' &&
                       bluetoothInput[1] == '$') {
                // Got a "colour macro command" (macro command referring to the color of the RGB led on robot)
                char colour = bluetoothInput[2];
                if (colour == 'r') { 
                    LEDRED = RGBLED_ON; LEDBLUE = RGBLED_OFF; LEDGREEN = RGBLED_OFF;   
                }  else if (colour == 'g') {
                    LEDRED = RGBLED_OFF; LEDBLUE = RGBLED_OFF; LEDGREEN = RGBLED_ON;
                } else if (colour == 'b') {
                    LEDRED = RGBLED_OFF; LEDBLUE = RGBLED_ON; LEDGREEN = RGBLED_OFF;
                }  
            } else if (bluetoothInput[0] == '$') {
                // Received a "macro command" (macro command = a command controlling the status of the device or so)
                if (bluetoothInput[1] == 'o' &&
                    bluetoothInput[2] == 'u' &&
                    bluetoothInput[3] == 't') {
                    // Connection should be ended
                    break;   
                }
            }
        }
    }
    
    // Connection was terminated, retrieving the "locked" status
    waitForPassword();
}

void init() {

    // Initialising the robot
    
    // Creating a new instance of ZumoRobotManager
    zumoRobotManager = new ZumoRobotManager();
    
    // Turning off the leds
    LEDBLUE = RGBLED_OFF; LEDRED = RGBLED_OFF; LEDGREEN = RGBLED_OFF;

    // Setting the bluetoot's baud rate for communication
    bluetooth.baud(9600);
}

int main() {
    
    // Calling init
    init();
    // Locking the robot
    waitForPassword();

    return 0;
}