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@7:c6781a58f666, 2014-12-05 (annotated)

Committer:
hotwheelharry
Date:
Fri Dec 05 23:39:56 2014 +0000
Revision:
7:c6781a58f666
Parent:
6:e905d3ec8545
Child:
8:36b2ef26a0b1
ir sensor reverse

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 7:c6781a58f666 114 tank.SetMotors(-0.5,-0.5);
hotwheelharry 7:c6781a58f666 115 continue;
hotwheelharry 3:c1620db50a75 116 }else{
hotwheelharry 0:79485480cd7e 117 }
hotwheelharry 3:c1620db50a75 118
hotwheelharry 3:c1620db50a75 119 if(controlState.triggerRight > 0x80){ //shoot
hotwheelharry 0:79485480cd7e 120 controller.rumble(255,255);
hotwheelharry 3:c1620db50a75 121 led = 1;
mtaylor33 6:e905d3ec8545 122 fire = 1;
hotwheelharry 3:c1620db50a75 123 }else{
hotwheelharry 3:c1620db50a75 124 controller.rumble(0,0);
hotwheelharry 3:c1620db50a75 125 led = 0;
mtaylor33 6:e905d3ec8545 126 fire = 0;
hotwheelharry 0:79485480cd7e 127 }
hotwheelharry 0:79485480cd7e 128
hotwheelharry 3:c1620db50a75 129 if( sqrt(x*x + y*y) > 0.25 ) {
hotwheelharry 3:c1620db50a75 130 tank.SetMotors(tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 131 //m.lock();
hotwheelharry 3:c1620db50a75 132 //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
hotwheelharry 3:c1620db50a75 133 //m.unlock();
hotwheelharry 0:79485480cd7e 134 } else {
hotwheelharry 3:c1620db50a75 135 tank.SetMotors(0.0,0.0);
hotwheelharry 0:79485480cd7e 136 }
hotwheelharry 0:79485480cd7e 137 }
hotwheelharry 3:c1620db50a75 138
hotwheelharry 3:c1620db50a75 139 //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n");
hotwheelharry 0:79485480cd7e 140 tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second.
hotwheelharry 0:79485480cd7e 141 Thread::wait(500);
hotwheelharry 0:79485480cd7e 142 controller.connect();
hotwheelharry 0:79485480cd7e 143 }
hotwheelharry 0:79485480cd7e 144 }
hotwheelharry 2:5e870c215495 145
mtaylor33 6:e905d3ec8545 146 int start = 0;
mtaylor33 6:e905d3ec8545 147
hotwheelharry 2:5e870c215495 148 int main() {
mtaylor33 6:e905d3ec8545 149 if(start == 0) {tank.SetMotors(0,0); fire = 0; start++;}
hotwheelharry 2:5e870c215495 150 led = 1;
hotwheelharry 3:c1620db50a75 151
hotwheelharry 3:c1620db50a75 152 led4 = 1;
hotwheelharry 3:c1620db50a75 153 //pc.baud(19200);
hotwheelharry 3:c1620db50a75 154 //pc.printf("TANK\r\n");
hotwheelharry 2:5e870c215495 155
hotwheelharry 2:5e870c215495 156 Thread t_controller(thread_controller);
hotwheelharry 2:5e870c215495 157
hotwheelharry 2:5e870c215495 158 //Thread audio_thread(thread_audio_run);
hotwheelharry 2:5e870c215495 159
hotwheelharry 2:5e870c215495 160 //audio.playMario();
hotwheelharry 2:5e870c215495 161
hotwheelharry 3:c1620db50a75 162
hotwheelharry 3:c1620db50a75 163 //tank.SetMotors(1.0,-1.0);
hotwheelharry 3:c1620db50a75 164 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 165 //tank.SetMotors(-1.0,1.0);
hotwheelharry 3:c1620db50a75 166 //Thread::wait(1000);
hotwheelharry 3:c1620db50a75 167 //tank.SetMotors(0,0);
hotwheelharry 3:c1620db50a75 168
hotwheelharry 2:5e870c215495 169 while(1){
hotwheelharry 3:c1620db50a75 170 led4 = !led4;
mtaylor33 6:e905d3ec8545 171 // fire = !fire;
mtaylor33 6:e905d3ec8545 172 Thread::wait(5000);
hotwheelharry 2:5e870c215495 173 }
hotwheelharry 2:5e870c215495 174 }
hotwheelharry 3:c1620db50a75 175