File content as of revision 0:0ab65a1aef12:

// mbed Robot Racing Wii example
//  Connects to Wii Wheel/Remote to get control input to drive m3pi robots
//  Also has RFID reader to trigger different powerups/slowdowns
//  Robot has a simple inertial model to give it a more natural car-like driving acceleration and deceleration

#include "mbed.h"

#include "USBHost.h" // Peter Barratt's USB Bluetooth Dongle code
#include "m3pi.h"    // m3pi robot controls
#include "Wiimote.h" // Wii Remote message decoding
#include "ID12RFID.h" // RFID reader

Serial pc(USBTX, USBRX);
m3pi m3pi;

ID12RFID rfid(p14);

#define ACCELERATE 0.02
#define DECAY 0.01

static float clamp(float v, float mn, float mx) {
    return (v < mn) ? mn : (v > mx) ? mx : v;
}

Timer banana;

// indicate a banana has been hit
void banana_hit() {
    banana.reset();
    banana.start();
}

// see if there is any impact from bananas!
// returns 1.0 for full pain, decaying to 0.0 over time
float banana_impact() {
    if(banana > 2.0) {
        banana.stop();
        banana.reset();
    }
    if(banana > 0) {
        return clamp(2.0 - banana, 0.0, 1.0);    
    } else {
        return 0;
     }
}

#define NORMAL_SPEED 0.3

volatile float max_speed = NORMAL_SPEED;
volatile float velocity = 0;

Timer boost;

void boost_impact() {
    if(boost > 2.0) {
        boost.stop();
        boost.reset();
        max_speed = NORMAL_SPEED;
    }
}

void boost_hit() {
    max_speed = .7;
    velocity = .7;
    boost.reset();
    boost.start();    
}

void slowmo_hit() {
    max_speed = 0.15;
    velocity = 0.0;
    boost.reset();
    boost.start();
}

// this is called by the USB infrastructure when a wii message comes in
void wii_data(char * data) {
    
    Wiimote wii;
    wii.decode(data);

    // temporary action triggers
    if(wii.up) { banana_hit(); }
    if(wii.down) { boost_hit(); }
    if(wii.left) { slowmo_hit(); }
        
    if(wii.two) {           // active acceleration
        velocity += ACCELERATE;
        if(velocity > max_speed) { velocity = max_speed; }
    } else if(wii.one) {    // active deceleration/reverse
        velocity -= ACCELERATE;
        if(velocity < -max_speed) { velocity = -max_speed; }
    } else {                // decay to zero
        if(velocity < 0) {
            velocity += DECAY;
            if(velocity > 0) { velocity = 0; }
        } else {
            velocity -= DECAY;
            if(velocity < 0) { velocity = 0; }
        }
    }
     
    float factor = wii.wheel / 190.0f; 
    
    float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
    float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
    
    boost_impact();
    float impact = banana_impact();

    if(impact > 0.0) {
        m3pi.left_motor(1 * impact);
        m3pi.right_motor(-1 * impact);
        velocity = 0;
    } else {
        m3pi.left_motor(velocity * right_factor);
        m3pi.right_motor(velocity * left_factor);
    }
}

DigitalOut led_active(LED4);

int main() {

    m3pi.locate(0,1);
    m3pi.printf("WiiWheel");


    pc.baud(38400);

    printf("Hello Wii Racing...\n");

    USBInit();


    while (1) {
        USBLoop();
        led_active = !led_active;
        if(rfid.readable()) {
            int id = rfid.read();
            // ignore id for now
            banana_hit();
        }
    }
}