Simons Wii controlled m3pi program
Dependencies: mbed m3pi ID12RFIDIRQ
main.cpp
- Committer:
- chris
- Date:
- 2011-05-18
- Revision:
- 0:0ab65a1aef12
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(); } } }