C K / Mbed 2 deprecated x4180_Tank

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   USBHost USBHostXpad mbed-rtos mbed

main.cpp@4:868a07a5496f, 2014-11-26 (annotated)

Committer:
hotwheelharry
Date:
Wed Nov 26 23:35:46 2014 +0000
Revision:
4:868a07a5496f
Parent:
3:c1620db50a75
Child:
6:e905d3ec8545
removed y joystick flip

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