Chris Styles / Mbed 2 deprecated m3pi_WiiRacing

Simons Wii controlled m3pi program

Dependencies:   mbed m3pi ID12RFIDIRQ

main.cpp@0:0ab65a1aef12, 2011-05-18 (annotated)

Committer:
chris
Date:
Wed May 18 14:50:46 2011 +0000
Revision:
0:0ab65a1aef12
Added some printing to the m3pi screen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:0ab65a1aef12 1 // mbed Robot Racing Wii example
chris 0:0ab65a1aef12 2 // Connects to Wii Wheel/Remote to get control input to drive m3pi robots
chris 0:0ab65a1aef12 3 // Also has RFID reader to trigger different powerups/slowdowns
chris 0:0ab65a1aef12 4 // Robot has a simple inertial model to give it a more natural car-like driving acceleration and deceleration
chris 0:0ab65a1aef12 5
chris 0:0ab65a1aef12 6 #include "mbed.h"
chris 0:0ab65a1aef12 7
chris 0:0ab65a1aef12 8 #include "USBHost.h" // Peter Barratt's USB Bluetooth Dongle code
chris 0:0ab65a1aef12 9 #include "m3pi.h" // m3pi robot controls
chris 0:0ab65a1aef12 10 #include "Wiimote.h" // Wii Remote message decoding
chris 0:0ab65a1aef12 11 #include "ID12RFID.h" // RFID reader
chris 0:0ab65a1aef12 12
chris 0:0ab65a1aef12 13 Serial pc(USBTX, USBRX);
chris 0:0ab65a1aef12 14 m3pi m3pi;
chris 0:0ab65a1aef12 15
chris 0:0ab65a1aef12 16 ID12RFID rfid(p14);
chris 0:0ab65a1aef12 17
chris 0:0ab65a1aef12 18 #define ACCELERATE 0.02
chris 0:0ab65a1aef12 19 #define DECAY 0.01
chris 0:0ab65a1aef12 20
chris 0:0ab65a1aef12 21 static float clamp(float v, float mn, float mx) {
chris 0:0ab65a1aef12 22 return (v < mn) ? mn : (v > mx) ? mx : v;
chris 0:0ab65a1aef12 23 }
chris 0:0ab65a1aef12 24
chris 0:0ab65a1aef12 25 Timer banana;
chris 0:0ab65a1aef12 26
chris 0:0ab65a1aef12 27 // indicate a banana has been hit
chris 0:0ab65a1aef12 28 void banana_hit() {
chris 0:0ab65a1aef12 29 banana.reset();
chris 0:0ab65a1aef12 30 banana.start();
chris 0:0ab65a1aef12 31 }
chris 0:0ab65a1aef12 32
chris 0:0ab65a1aef12 33 // see if there is any impact from bananas!
chris 0:0ab65a1aef12 34 // returns 1.0 for full pain, decaying to 0.0 over time
chris 0:0ab65a1aef12 35 float banana_impact() {
chris 0:0ab65a1aef12 36 if(banana > 2.0) {
chris 0:0ab65a1aef12 37 banana.stop();
chris 0:0ab65a1aef12 38 banana.reset();
chris 0:0ab65a1aef12 39 }
chris 0:0ab65a1aef12 40 if(banana > 0) {
chris 0:0ab65a1aef12 41 return clamp(2.0 - banana, 0.0, 1.0);
chris 0:0ab65a1aef12 42 } else {
chris 0:0ab65a1aef12 43 return 0;
chris 0:0ab65a1aef12 44 }
chris 0:0ab65a1aef12 45 }
chris 0:0ab65a1aef12 46
chris 0:0ab65a1aef12 47 #define NORMAL_SPEED 0.3
chris 0:0ab65a1aef12 48
chris 0:0ab65a1aef12 49 volatile float max_speed = NORMAL_SPEED;
chris 0:0ab65a1aef12 50 volatile float velocity = 0;
chris 0:0ab65a1aef12 51
chris 0:0ab65a1aef12 52 Timer boost;
chris 0:0ab65a1aef12 53
chris 0:0ab65a1aef12 54 void boost_impact() {
chris 0:0ab65a1aef12 55 if(boost > 2.0) {
chris 0:0ab65a1aef12 56 boost.stop();
chris 0:0ab65a1aef12 57 boost.reset();
chris 0:0ab65a1aef12 58 max_speed = NORMAL_SPEED;
chris 0:0ab65a1aef12 59 }
chris 0:0ab65a1aef12 60 }
chris 0:0ab65a1aef12 61
chris 0:0ab65a1aef12 62 void boost_hit() {
chris 0:0ab65a1aef12 63 max_speed = .7;
chris 0:0ab65a1aef12 64 velocity = .7;
chris 0:0ab65a1aef12 65 boost.reset();
chris 0:0ab65a1aef12 66 boost.start();
chris 0:0ab65a1aef12 67 }
chris 0:0ab65a1aef12 68
chris 0:0ab65a1aef12 69 void slowmo_hit() {
chris 0:0ab65a1aef12 70 max_speed = 0.15;
chris 0:0ab65a1aef12 71 velocity = 0.0;
chris 0:0ab65a1aef12 72 boost.reset();
chris 0:0ab65a1aef12 73 boost.start();
chris 0:0ab65a1aef12 74 }
chris 0:0ab65a1aef12 75
chris 0:0ab65a1aef12 76 // this is called by the USB infrastructure when a wii message comes in
chris 0:0ab65a1aef12 77 void wii_data(char * data) {
chris 0:0ab65a1aef12 78
chris 0:0ab65a1aef12 79 Wiimote wii;
chris 0:0ab65a1aef12 80 wii.decode(data);
chris 0:0ab65a1aef12 81
chris 0:0ab65a1aef12 82 // temporary action triggers
chris 0:0ab65a1aef12 83 if(wii.up) { banana_hit(); }
chris 0:0ab65a1aef12 84 if(wii.down) { boost_hit(); }
chris 0:0ab65a1aef12 85 if(wii.left) { slowmo_hit(); }
chris 0:0ab65a1aef12 86
chris 0:0ab65a1aef12 87 if(wii.two) { // active acceleration
chris 0:0ab65a1aef12 88 velocity += ACCELERATE;
chris 0:0ab65a1aef12 89 if(velocity > max_speed) { velocity = max_speed; }
chris 0:0ab65a1aef12 90 } else if(wii.one) { // active deceleration/reverse
chris 0:0ab65a1aef12 91 velocity -= ACCELERATE;
chris 0:0ab65a1aef12 92 if(velocity < -max_speed) { velocity = -max_speed; }
chris 0:0ab65a1aef12 93 } else { // decay to zero
chris 0:0ab65a1aef12 94 if(velocity < 0) {
chris 0:0ab65a1aef12 95 velocity += DECAY;
chris 0:0ab65a1aef12 96 if(velocity > 0) { velocity = 0; }
chris 0:0ab65a1aef12 97 } else {
chris 0:0ab65a1aef12 98 velocity -= DECAY;
chris 0:0ab65a1aef12 99 if(velocity < 0) { velocity = 0; }
chris 0:0ab65a1aef12 100 }
chris 0:0ab65a1aef12 101 }
chris 0:0ab65a1aef12 102
chris 0:0ab65a1aef12 103 float factor = wii.wheel / 190.0f;
chris 0:0ab65a1aef12 104
chris 0:0ab65a1aef12 105 float left_factor = (factor >= 0.0) ? 1.0 : 1.0 - (-factor);
chris 0:0ab65a1aef12 106 float right_factor = (factor <= 0.0) ? 1.0 : 1.0 - factor;
chris 0:0ab65a1aef12 107
chris 0:0ab65a1aef12 108 boost_impact();
chris 0:0ab65a1aef12 109 float impact = banana_impact();
chris 0:0ab65a1aef12 110
chris 0:0ab65a1aef12 111 if(impact > 0.0) {
chris 0:0ab65a1aef12 112 m3pi.left_motor(1 * impact);
chris 0:0ab65a1aef12 113 m3pi.right_motor(-1 * impact);
chris 0:0ab65a1aef12 114 velocity = 0;
chris 0:0ab65a1aef12 115 } else {
chris 0:0ab65a1aef12 116 m3pi.left_motor(velocity * right_factor);
chris 0:0ab65a1aef12 117 m3pi.right_motor(velocity * left_factor);
chris 0:0ab65a1aef12 118 }
chris 0:0ab65a1aef12 119 }
chris 0:0ab65a1aef12 120
chris 0:0ab65a1aef12 121 DigitalOut led_active(LED4);
chris 0:0ab65a1aef12 122
chris 0:0ab65a1aef12 123 int main() {
chris 0:0ab65a1aef12 124
chris 0:0ab65a1aef12 125 m3pi.locate(0,1);
chris 0:0ab65a1aef12 126 m3pi.printf("WiiWheel");
chris 0:0ab65a1aef12 127
chris 0:0ab65a1aef12 128
chris 0:0ab65a1aef12 129 pc.baud(38400);
chris 0:0ab65a1aef12 130
chris 0:0ab65a1aef12 131 printf("Hello Wii Racing...\n");
chris 0:0ab65a1aef12 132
chris 0:0ab65a1aef12 133 USBInit();
chris 0:0ab65a1aef12 134
chris 0:0ab65a1aef12 135
chris 0:0ab65a1aef12 136 while (1) {
chris 0:0ab65a1aef12 137 USBLoop();
chris 0:0ab65a1aef12 138 led_active = !led_active;
chris 0:0ab65a1aef12 139 if(rfid.readable()) {
chris 0:0ab65a1aef12 140 int id = rfid.read();
chris 0:0ab65a1aef12 141 // ignore id for now
chris 0:0ab65a1aef12 142 banana_hit();
chris 0:0ab65a1aef12 143 }
chris 0:0ab65a1aef12 144 }
chris 0:0ab65a1aef12 145 }