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@3:c1620db50a75, 2014-11-26 (annotated)

Committer:
hotwheelharry
Date:
Wed Nov 26 22:53:15 2014 +0000
Revision:
3:c1620db50a75
Parent:
2:5e870c215495
Child:
4:868a07a5496f
Movement works. Solenoid digital fire signal and rumble added. Still need audio, and fix the bug where the controller doesn't work when not plugged into usb. It has to do with output to the serial port, not sure what else.

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