00001 // mbed Robot Racing Wii example
00002 //  Connects to Wii Wheel/Remote to get control input to drive m3pi robots
00003 //  Also has RFID reader to trigger different powerups/slowdowns
00004 //  Robot has a simple inertial model to give it a more natural car-like driving acceleration and deceleration
00005 
00006 #include "mbed.h"
00007 
00008 #include "USBHost.h" // Peter Barratt's USB Bluetooth Dongle code
00009 #include "m3pi.h"    // m3pi robot controls
00010 #include "Wiimote.h" // Wii Remote message decoding
00011 #include "ID12RFID.h" // RFID reader
00012 
00013 Serial pc(USBTX, USBRX);
00014 m3pi m3pi;
00015 
00016 ID12RFID rfid(p14);
00017 
00018 #define ACCELERATE 0.02
00019 #define DECAY 0.01
00020 
00021 static float clamp(float v, float mn, float mx) {
00022     return (v < mn) ? mn : (v > mx) ? mx : v;
00023 }
00024 
00025 Timer banana;
00026 
00027 // indicate a banana has been hit
00028 void banana_hit() {
00029     banana.reset();
00030     banana.start();
00031 }
00032 
00033 // see if there is any impact from bananas!
00034 // returns 1.0 for full pain, decaying to 0.0 over time
00035 float banana_impact() {
00036     if(banana > 2.0) {
00037         banana.stop();
00038         banana.reset();
00039     }
00040     if(banana > 0) {
00041         return clamp(2.0 - banana, 0.0, 1.0);    
00042     } else {
00043         return 0;
00044      }
00045 }
00046 
00047 #define NORMAL_SPEED 0.3
00048 
00049 volatile float max_speed = NORMAL_SPEED;
00050 volatile float velocity = 0;
00051 
00052 Timer boost;
00053 
00054 void boost_impact() {
00055     if(boost > 2.0) {
00056         boost.stop();
00057         boost.reset();
00058         max_speed = NORMAL_SPEED;
00059     }
00060 }
00061 
00062 void boost_hit() {
00063     max_speed = .7;
00064     velocity = .7;
00065     boost.reset();
00066     boost.start();    
00067 }
00068 
00069 void slowmo_hit() {
00070     max_speed = 0.15;
00071     velocity = 0.0;
00072     boost.reset();
00073     boost.start();
00074 }
00075 
00076 // this is called by the USB infrastructure when a wii message comes in
00077 void wii_data(char * data) {
00078     
00079     Wiimote wii;
00080     wii.decode(data);
00081 
00082     // temporary action triggers
00083     if(wii.up) { banana_hit(); }
00084     if(wii.down) { boost_hit(); }
00085     if(wii.left) { slowmo_hit(); }
00086         
00087     if(wii.two) {           // active acceleration
00088         velocity += ACCELERATE;
00089         if(velocity > max_speed) { velocity = max_speed; }
00090     } else if(wii.one) {    // active deceleration/reverse
00091         velocity -= ACCELERATE;
00092         if(velocity < -max_speed) { velocity = -max_speed; }
00093     } else {                // decay to zero
00094         if(velocity < 0) {
00095             velocity += DECAY;
00096             if(velocity > 0) { velocity = 0; }
00097         } else {
00098             velocity -= DECAY;
00099             if(velocity < 0) { velocity = 0; }
00100         }
00101     }
00102      
00103     float factor = wii.wheel / 190.0f; 
00104     
00105     float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
00106     float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
00107     
00108     boost_impact();
00109     float impact = banana_impact();
00110 
00111     if(impact > 0.0) {
00112         m3pi.left_motor(1 * impact);
00113         m3pi.right_motor(-1 * impact);
00114         velocity = 0;
00115     } else {
00116         m3pi.left_motor(velocity * right_factor);
00117         m3pi.right_motor(velocity * left_factor);
00118     }
00119 }
00120 
00121 DigitalOut led_active(LED4);
00122 
00123 int main() {
00124 
00125     m3pi.locate(0,1);
00126     m3pi.printf("WiiWheel");
00127 
00128 
00129     pc.baud(38400);
00130 
00131     printf("Hello Wii Racing...\n");
00132 
00133     USBInit();
00134 
00135 
00136     while (1) {
00137         USBLoop();
00138         led_active = !led_active;
00139         if(rfid.readable()) {
00140             int id = rfid.read();
00141             // ignore id for now
00142             banana_hit();
00143         }
00144     }
00145 }