File content as of revision 5:854aea46d7b8:

#include "mbed.h"
#include "m3pi.h"
#include "Protocol.h"

Serial xbee(p28, p27);
m3pi m3pi;

void set_command();
void execute();

int commands[3];

int main()
{
    m3pi.cls();
    
    // Set initial "default" commands for m3pi
    // Action (stop), Direction (left), and Duration (zero), respectively
    commands[0] = 6; commands[1] = 0; commands[2] = 0;             
    
    while (1) {
        if (xbee.readable()) {
            xbee.scanf("|%i,%i,%i@", &commands[0], &commands[1], &commands[2]);
        }
        
        m3pi.locate(0,1);
        m3pi.printf("%i,%i,%i", commands[0], commands[1], commands[2]);
        execute();
    }
}

void execute()
{
    float speed = 0.25;         // Default initial speed
    float turn_speed = 0.125;   // Default turn speed
    
    // For ease in switch block readability
    int action = commands[0];
    int direction = commands[1];
    int duration = commands[2];

    switch (action) {
        case MOVE:
            if (direction == FORWARD) {
                m3pi.forward(speed);
            } else if (direction == LEFT) {
                m3pi.left(turn_speed);
                m3pi.forward(speed);
            } else if (direction == RIGHT) {
                m3pi.right(turn_speed);
                m3pi.forward(speed);
            } else if (direction == BACKWARD) {
                m3pi.backward(speed);
            } break;
            
        case TURN:
            if (direction == LEFT) {
                m3pi.left(turn_speed);
            } else if (direction == RIGHT) {
                m3pi.right(turn_speed);
            } break;
            
        case RUN:
            if (direction == FORWARD) {
                speed *= 2;
                m3pi.forward(speed);
            } else if (direction == LEFT) {
                m3pi.left(turn_speed);
                speed *= 2;
                m3pi.forward(speed);
            } else if (direction == RIGHT) {
                m3pi.right(turn_speed);
                speed *= 2;
                m3pi.forward(speed);
            } else if (direction == BACKWARD) {
                speed *= 2;
                m3pi.backward(speed);
            } break;
            
        case STOP:
            m3pi.stop();
            break;
    }
    
    wait(duration);
    m3pi.stop();
    commands[0] = 6; commands[1] = 0; commands[2] = 0; // Reset "defaults"
}