File content as of revision 4:bb9c8df5bef9:

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

m3pi m3pi;

RawSerial dev(p28,p27);
DigitalOut led1(LED1);

// default serial buffer length is 8 bytes
const int BUFF_LENGTH = 8;
const int MAX_SPEED = 255;
const int MIN_SPEED = -255;

uint8_t rx_buf[BUFF_LENGTH];
char str_buf[BUFF_LENGTH];
char battery[BUFF_LENGTH] = "";
char str[BUFF_LENGTH] = "";

void drive(int left, int right) {
    // bounds check speeds
    if (right > MIN_SPEED && right < MAX_SPEED &&
        left > MIN_SPEED && left < MAX_SPEED) {
                
        m3pi.left_motor(left);
        m3pi.right_motor(right);
    }
}    

/**
 * Receive commands in the form of a one-character leading command,
 * followed by two optional wheel speeds, if driving forwards or back.
 * Wheel speeds are unsigned integers in the range of 0 (stop) to 255.
 */
void process_cmd() {
    char cmd = (char)rx_buf[0];
    int right = (int)rx_buf[1];
    int left = (int)rx_buf[2];
    
    switch(cmd) {
        case 'f': {
            // forward
            drive(left, right);
            break;
        } case 'r': {
            // reverse
            drive(-left, -right);
            break;
        } case 'b': {
            // return battery level as 8-character string repr of float
            sprintf(battery, "%f", m3pi.battery());
            // FIXME: this does not get sent as a singe byte,
            // but sometimes several
            dev.puts(battery);
            break;
        } case 's': {
            m3pi.stop();
            //m3pi.printf("stop");
            break;
        } case 'z': {
            // spin left
            m3pi.left(0.1);
            break;
        } case 'y': {
            // spin right
            m3pi.right(0.1);
            break;
        } default: {
            m3pi.stop();
            m3pi.cls();
            m3pi.locate(0, 0);
            // print the buffer contents as ASCII on the first line
            m3pi.printf(str_buf);
            m3pi.locate(0, 1);
            m3pi.printf("?");
        }
    }
}

void dev_recv() {
    
    // toggle the LED whenever a data transmission is received
    led1 = !led1;
    
    int i = 0;
    for (i=0; i < BUFF_LENGTH; i++) {
        // let buffer empty if it's not ready to be read yet
        int count = 0;
        while (count <= 10) {
            if (dev.readable())
                break;
            wait(0.01f);
            count++;
        }
        
        // if still nothing to read after waiting, must have received
        // less than BUFF_LENGTH bytes
        if (!dev.readable()) {
            break;
        }
        
        uint8_t b = dev.getc();
        rx_buf[i] = b;
        str_buf[i] = (char)b;
    }
    
    process_cmd();
}

int main() {
    dev.baud(9600);
    // use CTS-only flow control
    dev.set_flow_control(Serial::CTS, p26);
    dev.attach(&dev_recv, Serial::RxIrq);
    m3pi.locate(0, 1);
    m3pi.printf("BT LE");

    while(1) {
        sleep();
    }
}