C K / Mbed 2 deprecated x4180_Tank

tankkk

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

main.cpp@9:789350244478, 2014-12-06 (annotated)

Committer:
hotwheelharry
Date:
Sat Dec 06 05:20:03 2014 +0000
Revision:
9:789350244478
Parent:
8:36b2ef26a0b1 
stop at atartup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hotwheelharry 3:c1620db50a75 1
hotwheelharry 3:c1620db50a75 2 #define DEBUG 0
hotwheelharry 3:c1620db50a75 3
hotwheelharry 0:79485480cd7e 4 #include "mbed.h"
hotwheelharry 0:79485480cd7e 5 #include "rtos.h"
hotwheelharry 0:79485480cd7e 6 #include "Traxster.h"
hotwheelharry 0:79485480cd7e 7 #include "USBHostXpad.h"
hotwheelharry 0:79485480cd7e 8 #include "utils.h"
hotwheelharry 0:79485480cd7e 9 #include "Audio.h"
hotwheelharry 3:c1620db50a75 10 #include <cmath>
hotwheelharry 0:79485480cd7e 11
hotwheelharry 3:c1620db50a75 12
hotwheelharry 3:c1620db50a75 13
hotwheelharry 8:36b2ef26a0b1 14 Serial pc(USBTX, USBRX);
hotwheelharry 8:36b2ef26a0b1 15
hotwheelharry 0:79485480cd7e 16 Serial robotCom(p13, p14);
hotwheelharry 0:79485480cd7e 17 AnalogIn ir(p20);
hotwheelharry 8:36b2ef26a0b1 18 Speaker speaker(p18); // the pin must be the AnalogOut pin - p18
hotwheelharry 8:36b2ef26a0b1 19 DigitalOut led1(LED1); //shows trigger
hotwheelharry 8:36b2ef26a0b1 20 DigitalOut led2(LED2); //shows collision prevention
hotwheelharry 8:36b2ef26a0b1 21 DigitalOut led3(LED3); //shows laser pointer active
hotwheelharry 8:36b2ef26a0b1 22 DigitalOut led4(LED4); //shows cpu activity
mtaylor33 6:e905d3ec8545 23 DigitalOut fire(p8);
hotwheelharry 8:36b2ef26a0b1 24 DigitalOut laser(p27);
hotwheelharry 0:79485480cd7e 25
hotwheelharry 0:79485480cd7e 26
hotwheelharry 8:36b2ef26a0b1 27 Audio audio(speaker);
hotwheelharry 0:79485480cd7e 28 Xbox360ControllerState controlState;
hotwheelharry 0:79485480cd7e 29 Traxster tank(robotCom);
hotwheelharry 0:79485480cd7e 30
jmccranie3 1:3bae10d2507c 31 //comment added
hotwheelharry 0:79485480cd7e 32
hotwheelharry 0:79485480cd7e 33 void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
hotwheelharry 3:c1620db50a75 34 //pc.printf("buttons: %04x Lstick X,Y: %-5d, %-5d Rstick X,Y: %-5d, %-5d LTrig: %02x (%d) RTrig: %02x (%d)\r\n", buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l,trigger_l, trigger_r,trigger_r);
hotwheelharry 0:79485480cd7e 35 controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
hotwheelharry 0:79485480cd7e 36 };
hotwheelharry 0:79485480cd7e 37
hotwheelharry 8:36b2ef26a0b1 38 void thread_audio_run(void const* arg){
hotwheelharry 8:36b2ef26a0b1 39 audio.run();
hotwheelharry 8:36b2ef26a0b1 40 }
hotwheelharry 0:79485480cd7e 41
hotwheelharry 3:c1620db50a75 42 float tank_L(float x, float y){
hotwheelharry 3:c1620db50a75 43 float scale = 0.0;
hotwheelharry 3:c1620db50a75 44
hotwheelharry 3:c1620db50a75 45 if(x >= 0.0 && y <= 0){ //bottom right
hotwheelharry 3:c1620db50a75 46 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 47 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 48 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 49 scale = theta * 4.0 + 1.0;
hotwheelharry 3:c1620db50a75 50
hotwheelharry 3:c1620db50a75 51 } else if(x <= 0.0 && y <= 0){ //bottom left
hotwheelharry 3:c1620db50a75 52 scale = -1.0;
hotwheelharry 3:c1620db50a75 53
hotwheelharry 3:c1620db50a75 54 } else if(x <= 0.0 && y >= 0){ //top left
hotwheelharry 3:c1620db50a75 55 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 56 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 57 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 58 scale = -theta * 4.0 - 1.0;
hotwheelharry 3:c1620db50a75 59
hotwheelharry 3:c1620db50a75 60 } else { //top-right
hotwheelharry 3:c1620db50a75 61 scale = 1.0;
hotwheelharry 3:c1620db50a75 62 }
hotwheelharry 3:c1620db50a75 63
hotwheelharry 3:c1620db50a75 64 End:
hotwheelharry 3:c1620db50a75 65 scale *= sqrt(x*x + y*y);
hotwheelharry 3:c1620db50a75 66 return scale;
hotwheelharry 3:c1620db50a75 67 }
hotwheelharry 3:c1620db50a75 68 float tank_R(float x, float y){
hotwheelharry 3:c1620db50a75 69 float scale = 0.0;
hotwheelharry 3:c1620db50a75 70
hotwheelharry 3:c1620db50a75 71 if(x >= 0.0 && y <= 0){ //bottom right
hotwheelharry 3:c1620db50a75 72 scale = -1.0;
hotwheelharry 3:c1620db50a75 73
hotwheelharry 3:c1620db50a75 74 } else if(x <= 0.0 && y <= 0){ //bottom left
hotwheelharry 3:c1620db50a75 75 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 76 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 77 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 78 scale = -theta*4.0 + 1.0;
hotwheelharry 3:c1620db50a75 79
hotwheelharry 3:c1620db50a75 80 } else if(x <= 0.0 && y >= 0){ //top left
hotwheelharry 3:c1620db50a75 81 scale = 1.0;
hotwheelharry 3:c1620db50a75 82
hotwheelharry 3:c1620db50a75 83 } else { //top-right
hotwheelharry 3:c1620db50a75 84 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 85 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 86 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 87 scale = theta*4.0 - 1.0;
hotwheelharry 3:c1620db50a75 88 }
hotwheelharry 3:c1620db50a75 89
hotwheelharry 3:c1620db50a75 90 End:
hotwheelharry 3:c1620db50a75 91 scale *= sqrt(x*x + y*y);
hotwheelharry 3:c1620db50a75 92 return scale;
hotwheelharry 3:c1620db50a75 93 }
hotwheelharry 2:5e870c215495 94 void thread_controller(void const* arg){
hotwheelharry 0:79485480cd7e 95
hotwheelharry 8:36b2ef26a0b1 96 tank.SetMotors(0,0);
hotwheelharry 2:5e870c215495 97 USBHostXpad controller;
hotwheelharry 0:79485480cd7e 98 controller.attachEvent(&onControlInput);
hotwheelharry 0:79485480cd7e 99
hotwheelharry 0:79485480cd7e 100 while(1){
hotwheelharry 8:36b2ef26a0b1 101 tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 102 bool wasdisconnected = true;
hotwheelharry 0:79485480cd7e 103 //acts as a failsafe
hotwheelharry 0:79485480cd7e 104 while(controller.connected()) {
hotwheelharry 3:c1620db50a75 105 if(wasdisconnected){
hotwheelharry 3:c1620db50a75 106 //pc.printf("Controller Status >> Connected!\r\n");
hotwheelharry 3:c1620db50a75 107 controller.led( USBHostXpad::LED1_ON );
hotwheelharry 9:789350244478 108 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 3:c1620db50a75 109 wasdisconnected = false;
hotwheelharry 0:79485480cd7e 110 }
hotwheelharry 3:c1620db50a75 111
hotwheelharry 8:36b2ef26a0b1 112 // left bumper disables auto-reverse
hotwheelharry 8:36b2ef26a0b1 113 bool collisionAvoidance = !((bool)(controlState.buttons & USBHostXpad::XPAD_PAD_LB));
hotwheelharry 8:36b2ef26a0b1 114 bool trigger = false;
hotwheelharry 8:36b2ef26a0b1 115
hotwheelharry 8:36b2ef26a0b1 116 // trigger rely - airsoft gun
hotwheelharry 8:36b2ef26a0b1 117 if(controlState.triggerRight > 0x80){
hotwheelharry 8:36b2ef26a0b1 118 controller.rumble(255,255);
hotwheelharry 8:36b2ef26a0b1 119 fire = 1;
hotwheelharry 8:36b2ef26a0b1 120 trigger = true;
hotwheelharry 8:36b2ef26a0b1 121 }else{
hotwheelharry 8:36b2ef26a0b1 122 controller.rumble(0,0);
hotwheelharry 8:36b2ef26a0b1 123 fire = 0;
hotwheelharry 8:36b2ef26a0b1 124 }
hotwheelharry 8:36b2ef26a0b1 125
hotwheelharry 8:36b2ef26a0b1 126 //lazzzzeeer sight
hotwheelharry 8:36b2ef26a0b1 127 if( controlState.buttons & USBHostXpad::XPAD_PAD_X){
hotwheelharry 8:36b2ef26a0b1 128 laser = 1;
hotwheelharry 8:36b2ef26a0b1 129 }else{
hotwheelharry 8:36b2ef26a0b1 130 laser = 0;
hotwheelharry 8:36b2ef26a0b1 131 }
hotwheelharry 8:36b2ef26a0b1 132
hotwheelharry 3:c1620db50a75 133
hotwheelharry 8:36b2ef26a0b1 134 //dpad sounds
hotwheelharry 8:36b2ef26a0b1 135 if(trigger || (controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y))){
hotwheelharry 8:36b2ef26a0b1 136 int code = 0;
hotwheelharry 8:36b2ef26a0b1 137 if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){
hotwheelharry 8:36b2ef26a0b1 138 code = 0;
hotwheelharry 8:36b2ef26a0b1 139 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){
hotwheelharry 8:36b2ef26a0b1 140 code = 1;
hotwheelharry 8:36b2ef26a0b1 141 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){
hotwheelharry 8:36b2ef26a0b1 142 code = 2;
hotwheelharry 8:36b2ef26a0b1 143 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){
hotwheelharry 8:36b2ef26a0b1 144 code = 3;
hotwheelharry 8:36b2ef26a0b1 145 } else if (trigger){
hotwheelharry 8:36b2ef26a0b1 146 code = 4;
hotwheelharry 8:36b2ef26a0b1 147 }else if (controlState.buttons & USBHostXpad::XPAD_PAD_Y){
hotwheelharry 8:36b2ef26a0b1 148 code = 5;
hotwheelharry 8:36b2ef26a0b1 149 }
hotwheelharry 8:36b2ef26a0b1 150
hotwheelharry 8:36b2ef26a0b1 151 //pc.printf("Audio play: %d\r\n",code);
hotwheelharry 8:36b2ef26a0b1 152 audio.play(code);
hotwheelharry 8:36b2ef26a0b1 153 }else{
hotwheelharry 8:36b2ef26a0b1 154 //pc.printf("Audio Stop!\r\n");
hotwheelharry 8:36b2ef26a0b1 155 audio.stop();
hotwheelharry 8:36b2ef26a0b1 156 }
hotwheelharry 8:36b2ef26a0b1 157
hotwheelharry 8:36b2ef26a0b1 158
hotwheelharry 8:36b2ef26a0b1 159
hotwheelharry 8:36b2ef26a0b1 160 //update output leds
hotwheelharry 8:36b2ef26a0b1 161 led1 = fire;
hotwheelharry 8:36b2ef26a0b1 162 led2 = collisionAvoidance;
hotwheelharry 8:36b2ef26a0b1 163 led3 = laser;
hotwheelharry 8:36b2ef26a0b1 164
hotwheelharry 8:36b2ef26a0b1 165 // on collision, reverse tank
hotwheelharry 8:36b2ef26a0b1 166 if(ir > 0.75 && collisionAvoidance){
hotwheelharry 3:c1620db50a75 167 controller.rumble(255,255);
hotwheelharry 7:c6781a58f666 168 tank.SetMotors(-0.5,-0.5);
hotwheelharry 7:c6781a58f666 169 continue;
hotwheelharry 3:c1620db50a75 170 }else{
hotwheelharry 0:79485480cd7e 171 }
hotwheelharry 0:79485480cd7e 172
hotwheelharry 8:36b2ef26a0b1 173 // map joystick input to tracks - if joystick is being moved
hotwheelharry 8:36b2ef26a0b1 174 float y = xpadNormalizeAnalog(controlState.analogLeftY);
hotwheelharry 8:36b2ef26a0b1 175 float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX);
hotwheelharry 8:36b2ef26a0b1 176 //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX); //flips left/right when in reverse
hotwheelharry 3:c1620db50a75 177 if( sqrt(x*x + y*y) > 0.25 ) {
hotwheelharry 3:c1620db50a75 178 tank.SetMotors(tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 179 //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
hotwheelharry 0:79485480cd7e 180 } else {
hotwheelharry 3:c1620db50a75 181 tank.SetMotors(0.0,0.0);
hotwheelharry 0:79485480cd7e 182 }
hotwheelharry 8:36b2ef26a0b1 183
hotwheelharry 8:36b2ef26a0b1 184
hotwheelharry 8:36b2ef26a0b1 185
hotwheelharry 0:79485480cd7e 186 }
hotwheelharry 3:c1620db50a75 187
hotwheelharry 3:c1620db50a75 188 //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
hotwheelharry 0:79485480cd7e 189 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 0:79485480cd7e 190 Thread::wait(500);
hotwheelharry 0:79485480cd7e 191 controller.connect();
hotwheelharry 0:79485480cd7e 192 }
hotwheelharry 0:79485480cd7e 193 }
hotwheelharry 2:5e870c215495 194
mtaylor33 6:e905d3ec8545 195
hotwheelharry 2:5e870c215495 196 int main() {
hotwheelharry 8:36b2ef26a0b1 197 tank.SetMotors(0,0);
hotwheelharry 8:36b2ef26a0b1 198 fire = 0;
hotwheelharry 8:36b2ef26a0b1 199 laser = 1;
hotwheelharry 8:36b2ef26a0b1 200 led1 = 1;
hotwheelharry 8:36b2ef26a0b1 201 led4 = 1;
hotwheelharry 8:36b2ef26a0b1 202
hotwheelharry 3:c1620db50a75 203
hotwheelharry 8:36b2ef26a0b1 204 pc.baud(19200);
hotwheelharry 8:36b2ef26a0b1 205 pc.printf("TANK\r\n");
hotwheelharry 8:36b2ef26a0b1 206
hotwheelharry 8:36b2ef26a0b1 207
hotwheelharry 2:5e870c215495 208
hotwheelharry 2:5e870c215495 209 Thread t_controller(thread_controller);
hotwheelharry 8:36b2ef26a0b1 210 Thread audio_thread(thread_audio_run);
hotwheelharry 2:5e870c215495 211
hotwheelharry 2:5e870c215495 212
hotwheelharry 3:c1620db50a75 213
hotwheelharry 3:c1620db50a75 214 //tank.SetMotors(1.0,-1.0);
hotwheelharry 3:c1620db50a75 215 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 216 //tank.SetMotors(-1.0,1.0);
hotwheelharry 3:c1620db50a75 217 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 218 //tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 219
hotwheelharry 2:5e870c215495 220 while(1){
hotwheelharry 3:c1620db50a75 221 led4 = !led4;
mtaylor33 6:e905d3ec8545 222 // fire = !fire;
hotwheelharry 8:36b2ef26a0b1 223 Thread::wait(1000);
hotwheelharry 2:5e870c215495 224 }
hotwheelharry 2:5e870c215495 225 }
hotwheelharry 3:c1620db50a75 226