Diff: main.cpp

Revision:

0:0ab65a1aef12

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 18 14:50:46 2011 +0000
@@ -0,0 +1,145 @@
+// 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();
+        }
+    }
+}