jordy morsinkhof / Mbed 2 deprecated Handmatig

Niet van mijzelf

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

Fork of x4180_Tank by C K

main.cpp@10:7e3987c8fa37, 2015-02-25 (annotated)

Committer:
347467
Date:
Wed Feb 25 08:38:37 2015 +0000
Revision:
10:7e3987c8fa37
Parent:
9:789350244478 
Xboxcontroller;

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 8:36b2ef26a0b1 13 Serial pc(USBTX, USBRX);
hotwheelharry 8:36b2ef26a0b1 14
hotwheelharry 0:79485480cd7e 15 Serial robotCom(p13, p14);
hotwheelharry 0:79485480cd7e 16 AnalogIn ir(p20);
hotwheelharry 8:36b2ef26a0b1 17 Speaker speaker(p18); // the pin must be the AnalogOut pin - p18
347467 10:7e3987c8fa37 18 DigitalOut DQ1(p9); //motor lv
347467 10:7e3987c8fa37 19 PwmOut PWM1(p21); //motor lv
347467 10:7e3987c8fa37 20 DigitalOut DQ2(p10);//motor rv
347467 10:7e3987c8fa37 21 PwmOut PWM2 (p22); //Motor rv
347467 10:7e3987c8fa37 22 DigitalOut DQ3(p11);//motor la
347467 10:7e3987c8fa37 23 PwmOut PWM3(p23); //motor la
347467 10:7e3987c8fa37 24 DigitalOut DQ4(p12);//motor ra
347467 10:7e3987c8fa37 25 PwmOut PWM4 (p24); //Motor ra
hotwheelharry 8:36b2ef26a0b1 26 DigitalOut led1(LED1); //shows trigger
hotwheelharry 8:36b2ef26a0b1 27 DigitalOut led2(LED2); //shows collision prevention
hotwheelharry 8:36b2ef26a0b1 28 DigitalOut led3(LED3); //shows laser pointer active
hotwheelharry 8:36b2ef26a0b1 29 DigitalOut led4(LED4); //shows cpu activity
347467 10:7e3987c8fa37 30
mtaylor33 6:e905d3ec8545 31 DigitalOut fire(p8);
hotwheelharry 8:36b2ef26a0b1 32 DigitalOut laser(p27);
hotwheelharry 0:79485480cd7e 33
hotwheelharry 0:79485480cd7e 34
347467 10:7e3987c8fa37 35
347467 10:7e3987c8fa37 36
hotwheelharry 8:36b2ef26a0b1 37 Audio audio(speaker);
hotwheelharry 0:79485480cd7e 38 Xbox360ControllerState controlState;
hotwheelharry 0:79485480cd7e 39 Traxster tank(robotCom);
hotwheelharry 0:79485480cd7e 40
jmccranie3 1:3bae10d2507c 41 //comment added
hotwheelharry 0:79485480cd7e 42
hotwheelharry 0:79485480cd7e 43 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 44 //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 45 controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
hotwheelharry 0:79485480cd7e 46 };
hotwheelharry 0:79485480cd7e 47
hotwheelharry 8:36b2ef26a0b1 48 void thread_audio_run(void const* arg){
hotwheelharry 8:36b2ef26a0b1 49 audio.run();
hotwheelharry 8:36b2ef26a0b1 50 }
hotwheelharry 0:79485480cd7e 51
hotwheelharry 3:c1620db50a75 52 float tank_L(float x, float y){
hotwheelharry 3:c1620db50a75 53 float scale = 0.0;
hotwheelharry 3:c1620db50a75 54
347467 10:7e3987c8fa37 55
347467 10:7e3987c8fa37 56
347467 10:7e3987c8fa37 57 if(x >= 0.0 && y <= 0){ //LBA bottom right
347467 10:7e3987c8fa37 58 pc.printf("1\r\n");
347467 10:7e3987c8fa37 59
347467 10:7e3987c8fa37 60
347467 10:7e3987c8fa37 61
347467 10:7e3987c8fa37 62
hotwheelharry 3:c1620db50a75 63 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 64 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 65 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 66 scale = theta * 4.0 + 1.0;
hotwheelharry 3:c1620db50a75 67
347467 10:7e3987c8fa37 68
347467 10:7e3987c8fa37 69 } else if(x <= 0.0 && y <= 0){ //LBA bottom left
347467 10:7e3987c8fa37 70 pc.printf("2\r\n");
347467 10:7e3987c8fa37 71
347467 10:7e3987c8fa37 72
347467 10:7e3987c8fa37 73
hotwheelharry 3:c1620db50a75 74 scale = -1.0;
hotwheelharry 3:c1620db50a75 75
hotwheelharry 3:c1620db50a75 76 } else if(x <= 0.0 && y >= 0){ //top left
347467 10:7e3987c8fa37 77
347467 10:7e3987c8fa37 78
hotwheelharry 3:c1620db50a75 79 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 80 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 81 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 82 scale = -theta * 4.0 - 1.0;
hotwheelharry 3:c1620db50a75 83
hotwheelharry 3:c1620db50a75 84 } else { //top-right
347467 10:7e3987c8fa37 85
347467 10:7e3987c8fa37 86
hotwheelharry 3:c1620db50a75 87 scale = 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 3:c1620db50a75 94 float tank_R(float x, float y){
hotwheelharry 3:c1620db50a75 95 float scale = 0.0;
hotwheelharry 3:c1620db50a75 96
hotwheelharry 3:c1620db50a75 97 if(x >= 0.0 && y <= 0){ //bottom right
347467 10:7e3987c8fa37 98
hotwheelharry 3:c1620db50a75 99 scale = -1.0;
hotwheelharry 3:c1620db50a75 100
hotwheelharry 3:c1620db50a75 101 } else if(x <= 0.0 && y <= 0){ //bottom left
hotwheelharry 3:c1620db50a75 102 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 103 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 104 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 105 scale = -theta*4.0 + 1.0;
hotwheelharry 3:c1620db50a75 106
hotwheelharry 3:c1620db50a75 107 } else if(x <= 0.0 && y >= 0){ //top left
hotwheelharry 3:c1620db50a75 108 scale = 1.0;
hotwheelharry 3:c1620db50a75 109
hotwheelharry 3:c1620db50a75 110 } else { //top-right
hotwheelharry 3:c1620db50a75 111 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 112 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 113 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 114 scale = theta*4.0 - 1.0;
hotwheelharry 3:c1620db50a75 115 }
hotwheelharry 3:c1620db50a75 116
hotwheelharry 3:c1620db50a75 117 End:
hotwheelharry 3:c1620db50a75 118 scale *= sqrt(x*x + y*y);
hotwheelharry 3:c1620db50a75 119 return scale;
hotwheelharry 3:c1620db50a75 120 }
hotwheelharry 2:5e870c215495 121 void thread_controller(void const* arg){
hotwheelharry 0:79485480cd7e 122
hotwheelharry 8:36b2ef26a0b1 123 tank.SetMotors(0,0);
hotwheelharry 2:5e870c215495 124 USBHostXpad controller;
hotwheelharry 0:79485480cd7e 125 controller.attachEvent(&onControlInput);
hotwheelharry 0:79485480cd7e 126
hotwheelharry 0:79485480cd7e 127 while(1){
hotwheelharry 8:36b2ef26a0b1 128 tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 129 bool wasdisconnected = true;
hotwheelharry 0:79485480cd7e 130 //acts as a failsafe
hotwheelharry 0:79485480cd7e 131 while(controller.connected()) {
hotwheelharry 3:c1620db50a75 132 if(wasdisconnected){
347467 10:7e3987c8fa37 133 pc.printf("Controller Status >> Connected!\r\n");
hotwheelharry 3:c1620db50a75 134 controller.led( USBHostXpad::LED1_ON );
hotwheelharry 9:789350244478 135 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 3:c1620db50a75 136 wasdisconnected = false;
hotwheelharry 0:79485480cd7e 137 }
hotwheelharry 3:c1620db50a75 138
hotwheelharry 8:36b2ef26a0b1 139 // left bumper disables auto-reverse
hotwheelharry 8:36b2ef26a0b1 140 bool collisionAvoidance = !((bool)(controlState.buttons & USBHostXpad::XPAD_PAD_LB));
hotwheelharry 8:36b2ef26a0b1 141 bool trigger = false;
hotwheelharry 8:36b2ef26a0b1 142
hotwheelharry 8:36b2ef26a0b1 143 // trigger rely - airsoft gun
347467 10:7e3987c8fa37 144
347467 10:7e3987c8fa37 145 float AS = 1.0; // acutele snelheid
347467 10:7e3987c8fa37 146 if(controlState.triggerRight > 0x80){ //Rechtdoor rijden
347467 10:7e3987c8fa37 147 //controller.rumble(255,255);
hotwheelharry 8:36b2ef26a0b1 148 fire = 1;
hotwheelharry 8:36b2ef26a0b1 149 trigger = true;
347467 10:7e3987c8fa37 150
347467 10:7e3987c8fa37 151
hotwheelharry 8:36b2ef26a0b1 152 }else{
347467 10:7e3987c8fa37 153 //controller.rumble(0,0);
hotwheelharry 8:36b2ef26a0b1 154 fire = 0;
hotwheelharry 8:36b2ef26a0b1 155 }
hotwheelharry 8:36b2ef26a0b1 156
hotwheelharry 8:36b2ef26a0b1 157 //lazzzzeeer sight
347467 10:7e3987c8fa37 158 if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ //Knop X laser=DQ3
347467 10:7e3987c8fa37 159 //Stoppen
347467 10:7e3987c8fa37 160 DQ1= 1; //lv
347467 10:7e3987c8fa37 161 DQ2= 1; //rv
347467 10:7e3987c8fa37 162 DQ3= 1; //la
347467 10:7e3987c8fa37 163 DQ4= 1; //ra
347467 10:7e3987c8fa37 164 PWM1 = AS*0; //lv in de vrij
347467 10:7e3987c8fa37 165 PWM2 = AS*0; //rv in de vrij
347467 10:7e3987c8fa37 166 PWM3 = AS*0; //la in de vrij
347467 10:7e3987c8fa37 167 PWM4 = AS*0; //ra in de vrij
347467 10:7e3987c8fa37 168 wait(2.0);
347467 10:7e3987c8fa37 169
hotwheelharry 8:36b2ef26a0b1 170 laser = 1;
hotwheelharry 8:36b2ef26a0b1 171 }else{
hotwheelharry 8:36b2ef26a0b1 172 laser = 0;
hotwheelharry 8:36b2ef26a0b1 173 }
hotwheelharry 8:36b2ef26a0b1 174
hotwheelharry 3:c1620db50a75 175
hotwheelharry 8:36b2ef26a0b1 176 //dpad sounds
hotwheelharry 8:36b2ef26a0b1 177 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 178 int code = 0;
347467 10:7e3987c8fa37 179
347467 10:7e3987c8fa37 180 float z = 0.5; // persentage voor het draaien van de motoren
347467 10:7e3987c8fa37 181 float j = 0.75; // persentage voor het draaien van de motoren
347467 10:7e3987c8fa37 182 if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){ //vooruit
347467 10:7e3987c8fa37 183
347467 10:7e3987c8fa37 184 DQ1= 1; //lv
347467 10:7e3987c8fa37 185 DQ2= 1; //rv
347467 10:7e3987c8fa37 186 DQ3= 1; //la
347467 10:7e3987c8fa37 187 DQ4= 1; //ra
347467 10:7e3987c8fa37 188 PWM1 = AS*1.5;
347467 10:7e3987c8fa37 189 PWM2 = AS*1.5;
347467 10:7e3987c8fa37 190 PWM3 = AS*1.5;
347467 10:7e3987c8fa37 191 PWM4 = AS*1.5;
347467 10:7e3987c8fa37 192 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ //Achteruit
347467 10:7e3987c8fa37 193 DQ1= 0; //lv
347467 10:7e3987c8fa37 194 DQ2= 0; //rv
347467 10:7e3987c8fa37 195 DQ3= 0; //la
347467 10:7e3987c8fa37 196 DQ4= 0; //ra
347467 10:7e3987c8fa37 197 PWM1 = AS*1.5;
347467 10:7e3987c8fa37 198 PWM2 = AS*1.5;
347467 10:7e3987c8fa37 199 PWM3 = AS*1.5;
347467 10:7e3987c8fa37 200 PWM4 = AS*1.5;
347467 10:7e3987c8fa37 201
347467 10:7e3987c8fa37 202 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ //links
347467 10:7e3987c8fa37 203 led1=1; //links
347467 10:7e3987c8fa37 204 DQ1 = 0; //lv
347467 10:7e3987c8fa37 205 DQ2 = 1; //rv
347467 10:7e3987c8fa37 206 DQ3 = 0; //La
347467 10:7e3987c8fa37 207 DQ4 = 1; //Ra
347467 10:7e3987c8fa37 208 PWM1 = AS*z;
347467 10:7e3987c8fa37 209 PWM2 = AS*j;
347467 10:7e3987c8fa37 210 PWM3 = AS*z;
347467 10:7e3987c8fa37 211 PWM4 = AS*j;
347467 10:7e3987c8fa37 212 } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){//rechts
347467 10:7e3987c8fa37 213 led3=1; //rechts
347467 10:7e3987c8fa37 214
347467 10:7e3987c8fa37 215 DQ1 = 1; //lv
347467 10:7e3987c8fa37 216 DQ2 = 0; //rv
347467 10:7e3987c8fa37 217 DQ3 = 1; //la
347467 10:7e3987c8fa37 218 DQ4 = 0; //Ra
347467 10:7e3987c8fa37 219 PWM1 = AS*j;
347467 10:7e3987c8fa37 220 PWM2 = AS*z;
347467 10:7e3987c8fa37 221 PWM3 = AS*j;
347467 10:7e3987c8fa37 222 PWM4 = AS*z;
347467 10:7e3987c8fa37 223
hotwheelharry 8:36b2ef26a0b1 224 } else if (trigger){
hotwheelharry 8:36b2ef26a0b1 225 code = 4;
hotwheelharry 8:36b2ef26a0b1 226 }else if (controlState.buttons & USBHostXpad::XPAD_PAD_Y){
hotwheelharry 8:36b2ef26a0b1 227 code = 5;
hotwheelharry 8:36b2ef26a0b1 228 }
hotwheelharry 8:36b2ef26a0b1 229
hotwheelharry 8:36b2ef26a0b1 230 //pc.printf("Audio play: %d\r\n",code);
hotwheelharry 8:36b2ef26a0b1 231 audio.play(code);
hotwheelharry 8:36b2ef26a0b1 232 }else{
hotwheelharry 8:36b2ef26a0b1 233 //pc.printf("Audio Stop!\r\n");
hotwheelharry 8:36b2ef26a0b1 234 audio.stop();
hotwheelharry 8:36b2ef26a0b1 235 }
hotwheelharry 8:36b2ef26a0b1 236
hotwheelharry 8:36b2ef26a0b1 237
hotwheelharry 8:36b2ef26a0b1 238
hotwheelharry 8:36b2ef26a0b1 239 //update output leds
347467 10:7e3987c8fa37 240 led1= fire;
347467 10:7e3987c8fa37 241 led2= collisionAvoidance;
347467 10:7e3987c8fa37 242 led3= laser;
hotwheelharry 8:36b2ef26a0b1 243
hotwheelharry 8:36b2ef26a0b1 244 // on collision, reverse tank
hotwheelharry 8:36b2ef26a0b1 245 if(ir > 0.75 && collisionAvoidance){
347467 10:7e3987c8fa37 246 // controller.rumble(255,255);
hotwheelharry 7:c6781a58f666 247 tank.SetMotors(-0.5,-0.5);
hotwheelharry 7:c6781a58f666 248 continue;
hotwheelharry 3:c1620db50a75 249 }else{
hotwheelharry 0:79485480cd7e 250 }
hotwheelharry 0:79485480cd7e 251
hotwheelharry 8:36b2ef26a0b1 252 // map joystick input to tracks - if joystick is being moved
hotwheelharry 8:36b2ef26a0b1 253 float y = xpadNormalizeAnalog(controlState.analogLeftY);
hotwheelharry 8:36b2ef26a0b1 254 float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX);
hotwheelharry 8:36b2ef26a0b1 255 //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX); //flips left/right when in reverse
hotwheelharry 3:c1620db50a75 256 if( sqrt(x*x + y*y) > 0.25 ) {
hotwheelharry 3:c1620db50a75 257 tank.SetMotors(tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 258 //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
hotwheelharry 0:79485480cd7e 259 } else {
hotwheelharry 3:c1620db50a75 260 tank.SetMotors(0.0,0.0);
hotwheelharry 0:79485480cd7e 261 }
hotwheelharry 8:36b2ef26a0b1 262
hotwheelharry 8:36b2ef26a0b1 263
hotwheelharry 8:36b2ef26a0b1 264
hotwheelharry 0:79485480cd7e 265 }
hotwheelharry 3:c1620db50a75 266
hotwheelharry 3:c1620db50a75 267 //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
hotwheelharry 0:79485480cd7e 268 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 0:79485480cd7e 269 Thread::wait(500);
hotwheelharry 0:79485480cd7e 270 controller.connect();
hotwheelharry 0:79485480cd7e 271 }
hotwheelharry 0:79485480cd7e 272 }
hotwheelharry 2:5e870c215495 273
mtaylor33 6:e905d3ec8545 274
hotwheelharry 2:5e870c215495 275 int main() {
hotwheelharry 8:36b2ef26a0b1 276 tank.SetMotors(0,0);
hotwheelharry 8:36b2ef26a0b1 277 fire = 0;
hotwheelharry 8:36b2ef26a0b1 278 laser = 1;
347467 10:7e3987c8fa37 279 led1 = 1;
347467 10:7e3987c8fa37 280 led2= 1;
hotwheelharry 8:36b2ef26a0b1 281
hotwheelharry 3:c1620db50a75 282
347467 10:7e3987c8fa37 283 pc.baud(9600);
hotwheelharry 8:36b2ef26a0b1 284 pc.printf("TANK\r\n");
hotwheelharry 8:36b2ef26a0b1 285
hotwheelharry 8:36b2ef26a0b1 286
hotwheelharry 2:5e870c215495 287
hotwheelharry 2:5e870c215495 288 Thread t_controller(thread_controller);
hotwheelharry 8:36b2ef26a0b1 289 Thread audio_thread(thread_audio_run);
hotwheelharry 2:5e870c215495 290
hotwheelharry 2:5e870c215495 291
hotwheelharry 3:c1620db50a75 292
hotwheelharry 3:c1620db50a75 293 //tank.SetMotors(1.0,-1.0);
hotwheelharry 3:c1620db50a75 294 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 295 //tank.SetMotors(-1.0,1.0);
hotwheelharry 3:c1620db50a75 296 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 297 //tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 298
hotwheelharry 2:5e870c215495 299 while(1){
347467 10:7e3987c8fa37 300 led4= !DQ4;
mtaylor33 6:e905d3ec8545 301 // fire = !fire;
hotwheelharry 8:36b2ef26a0b1 302 Thread::wait(1000);
hotwheelharry 2:5e870c215495 303 }
hotwheelharry 2:5e870c215495 304 }
hotwheelharry 3:c1620db50a75 305