jordy morsinkhof / Mbed 2 deprecated Handmatig

Niet van mijzelf

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

Fork of x4180_Tank by C K

main.cpp@6:e905d3ec8545, 2014-12-05 (annotated)

Committer:
mtaylor33
Date:
Fri Dec 05 23:25:35 2014 +0000
Revision:
6:e905d3ec8545
Parent:
4:868a07a5496f
Child:
7:c6781a58f666
working prototype

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 3:c1620db50a75 14 //Serial pc(USBTX, USBRX);
hotwheelharry 0:79485480cd7e 15 Serial robotCom(p13, p14);
hotwheelharry 0:79485480cd7e 16 AnalogIn ir(p20);
hotwheelharry 2:5e870c215495 17 //Speaker speaker(p18); // the pin must be the AnalogOut pin - p18
hotwheelharry 2:5e870c215495 18 DigitalOut led(LED1);
hotwheelharry 3:c1620db50a75 19 DigitalOut led4(LED4);
mtaylor33 6:e905d3ec8545 20 DigitalOut fire(p8);
hotwheelharry 3:c1620db50a75 21
hotwheelharry 3:c1620db50a75 22 Mutex m;
hotwheelharry 0:79485480cd7e 23
hotwheelharry 0:79485480cd7e 24
hotwheelharry 2:5e870c215495 25 //Audio audio(speaker);
hotwheelharry 0:79485480cd7e 26 Xbox360ControllerState controlState;
hotwheelharry 0:79485480cd7e 27 Traxster tank(robotCom);
hotwheelharry 0:79485480cd7e 28
jmccranie3 1:3bae10d2507c 29 //comment added
hotwheelharry 0:79485480cd7e 30
hotwheelharry 0:79485480cd7e 31 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 32 //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 33 controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
hotwheelharry 0:79485480cd7e 34 };
hotwheelharry 0:79485480cd7e 35
hotwheelharry 2:5e870c215495 36 //void thread_audio_run(void const* arg){
hotwheelharry 2:5e870c215495 37 // audio.run();
hotwheelharry 2:5e870c215495 38 //}
hotwheelharry 0:79485480cd7e 39
hotwheelharry 3:c1620db50a75 40 float tank_L(float x, float y){
hotwheelharry 3:c1620db50a75 41 float scale = 0.0;
hotwheelharry 3:c1620db50a75 42
hotwheelharry 3:c1620db50a75 43 if(x >= 0.0 && y <= 0){ //bottom right
hotwheelharry 3:c1620db50a75 44 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 45 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 46 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 47 scale = theta * 4.0 + 1.0;
hotwheelharry 3:c1620db50a75 48
hotwheelharry 3:c1620db50a75 49 } else if(x <= 0.0 && y <= 0){ //bottom left
hotwheelharry 3:c1620db50a75 50 scale = -1.0;
hotwheelharry 3:c1620db50a75 51
hotwheelharry 3:c1620db50a75 52 } else if(x <= 0.0 && y >= 0){ //top left
hotwheelharry 3:c1620db50a75 53 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 54 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 55 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 56 scale = -theta * 4.0 - 1.0;
hotwheelharry 3:c1620db50a75 57
hotwheelharry 3:c1620db50a75 58 } else { //top-right
hotwheelharry 3:c1620db50a75 59 scale = 1.0;
hotwheelharry 3:c1620db50a75 60 }
hotwheelharry 3:c1620db50a75 61
hotwheelharry 3:c1620db50a75 62 End:
hotwheelharry 3:c1620db50a75 63 scale *= sqrt(x*x + y*y);
hotwheelharry 3:c1620db50a75 64 return scale;
hotwheelharry 3:c1620db50a75 65 }
hotwheelharry 3:c1620db50a75 66 float tank_R(float x, float y){
hotwheelharry 3:c1620db50a75 67 float scale = 0.0;
hotwheelharry 3:c1620db50a75 68
hotwheelharry 3:c1620db50a75 69 if(x >= 0.0 && y <= 0){ //bottom right
hotwheelharry 3:c1620db50a75 70 scale = -1.0;
hotwheelharry 3:c1620db50a75 71
hotwheelharry 3:c1620db50a75 72 } else if(x <= 0.0 && y <= 0){ //bottom left
hotwheelharry 3:c1620db50a75 73 if(y == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 74 if(x == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 75 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 76 scale = -theta*4.0 + 1.0;
hotwheelharry 3:c1620db50a75 77
hotwheelharry 3:c1620db50a75 78 } else if(x <= 0.0 && y >= 0){ //top left
hotwheelharry 3:c1620db50a75 79 scale = 1.0;
hotwheelharry 3:c1620db50a75 80
hotwheelharry 3:c1620db50a75 81 } else { //top-right
hotwheelharry 3:c1620db50a75 82 if(y == 0){ scale = -1.0; goto End;}
hotwheelharry 3:c1620db50a75 83 if(x == 0){ scale = 1.0; goto End;}
hotwheelharry 3:c1620db50a75 84 float theta = atan(y/x) / 3.14159;
hotwheelharry 3:c1620db50a75 85 scale = theta*4.0 - 1.0;
hotwheelharry 3:c1620db50a75 86 }
hotwheelharry 3:c1620db50a75 87
hotwheelharry 3:c1620db50a75 88 End:
hotwheelharry 3:c1620db50a75 89 scale *= sqrt(x*x + y*y);
hotwheelharry 3:c1620db50a75 90 return scale;
hotwheelharry 3:c1620db50a75 91 }
hotwheelharry 2:5e870c215495 92 void thread_controller(void const* arg){
hotwheelharry 0:79485480cd7e 93
hotwheelharry 2:5e870c215495 94 USBHostXpad controller;
hotwheelharry 0:79485480cd7e 95 controller.attachEvent(&onControlInput);
hotwheelharry 0:79485480cd7e 96
hotwheelharry 0:79485480cd7e 97 while(1){
hotwheelharry 3:c1620db50a75 98 bool wasdisconnected = true;
hotwheelharry 0:79485480cd7e 99 //acts as a failsafe
hotwheelharry 0:79485480cd7e 100 while(controller.connected()) {
hotwheelharry 3:c1620db50a75 101 if(wasdisconnected){
hotwheelharry 3:c1620db50a75 102 //pc.printf("Controller Status >> Connected!\r\n");
hotwheelharry 3:c1620db50a75 103 controller.led( USBHostXpad::LED1_ON );
hotwheelharry 3:c1620db50a75 104 wasdisconnected = false;
hotwheelharry 0:79485480cd7e 105 }
hotwheelharry 3:c1620db50a75 106
hotwheelharry 3:c1620db50a75 107 //left joystick controls
hotwheelharry 3:c1620db50a75 108 float y = xpadNormalizeAnalog(controlState.analogLeftY);
hotwheelharry 4:868a07a5496f 109 float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX);
hotwheelharry 4:868a07a5496f 110 //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX);
hotwheelharry 3:c1620db50a75 111
mtaylor33 6:e905d3ec8545 112 if(ir > 0.65){ // on collision, stop tank
hotwheelharry 3:c1620db50a75 113 controller.rumble(255,255);
hotwheelharry 3:c1620db50a75 114 tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 115 }else{
hotwheelharry 0:79485480cd7e 116 }
hotwheelharry 3:c1620db50a75 117
hotwheelharry 3:c1620db50a75 118 if(controlState.triggerRight > 0x80){ //shoot
hotwheelharry 0:79485480cd7e 119 controller.rumble(255,255);
hotwheelharry 3:c1620db50a75 120 led = 1;
mtaylor33 6:e905d3ec8545 121 fire = 1;
hotwheelharry 3:c1620db50a75 122 }else{
hotwheelharry 3:c1620db50a75 123 controller.rumble(0,0);
hotwheelharry 3:c1620db50a75 124 led = 0;
mtaylor33 6:e905d3ec8545 125 fire = 0;
hotwheelharry 0:79485480cd7e 126 }
hotwheelharry 0:79485480cd7e 127
hotwheelharry 3:c1620db50a75 128 if( sqrt(x*x + y*y) > 0.25 ) {
hotwheelharry 3:c1620db50a75 129 tank.SetMotors(tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 130 //m.lock();
hotwheelharry 3:c1620db50a75 131 //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 132 //m.unlock();
hotwheelharry 0:79485480cd7e 133 } else {
hotwheelharry 3:c1620db50a75 134 tank.SetMotors(0.0,0.0);
hotwheelharry 0:79485480cd7e 135 }
hotwheelharry 0:79485480cd7e 136 }
hotwheelharry 3:c1620db50a75 137
hotwheelharry 3:c1620db50a75 138 //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
hotwheelharry 0:79485480cd7e 139 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 0:79485480cd7e 140 Thread::wait(500);
hotwheelharry 0:79485480cd7e 141 controller.connect();
hotwheelharry 0:79485480cd7e 142 }
hotwheelharry 0:79485480cd7e 143 }
hotwheelharry 2:5e870c215495 144
mtaylor33 6:e905d3ec8545 145 int start = 0;
mtaylor33 6:e905d3ec8545 146
hotwheelharry 2:5e870c215495 147 int main() {
mtaylor33 6:e905d3ec8545 148 if(start == 0) {tank.SetMotors(0,0); fire = 0; start++;}
hotwheelharry 2:5e870c215495 149 led = 1;
hotwheelharry 3:c1620db50a75 150
hotwheelharry 3:c1620db50a75 151 led4 = 1;
hotwheelharry 3:c1620db50a75 152 //pc.baud(19200);
hotwheelharry 3:c1620db50a75 153 //pc.printf("TANK\r\n");
hotwheelharry 2:5e870c215495 154
hotwheelharry 2:5e870c215495 155 Thread t_controller(thread_controller);
hotwheelharry 2:5e870c215495 156
hotwheelharry 2:5e870c215495 157 //Thread audio_thread(thread_audio_run);
hotwheelharry 2:5e870c215495 158
hotwheelharry 2:5e870c215495 159 //audio.playMario();
hotwheelharry 2:5e870c215495 160
hotwheelharry 3:c1620db50a75 161
hotwheelharry 3:c1620db50a75 162 //tank.SetMotors(1.0,-1.0);
hotwheelharry 3:c1620db50a75 163 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 164 //tank.SetMotors(-1.0,1.0);
hotwheelharry 3:c1620db50a75 165 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 166 //tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 167
hotwheelharry 2:5e870c215495 168 while(1){
hotwheelharry 3:c1620db50a75 169 led4 = !led4;
mtaylor33 6:e905d3ec8545 170 // fire = !fire;
mtaylor33 6:e905d3ec8545 171 Thread::wait(5000);
hotwheelharry 2:5e870c215495 172 }
hotwheelharry 2:5e870c215495 173 }
hotwheelharry 3:c1620db50a75 174