tankkk
Dependencies: USBHost USBHostXpad mbed-rtos mbed
main.cpp
- Committer:
- hotwheelharry
- Date:
- 2014-12-06
- Revision:
- 9:789350244478
- Parent:
- 8:36b2ef26a0b1
File content as of revision 9:789350244478:
#define DEBUG 0 #include "mbed.h" #include "rtos.h" #include "Traxster.h" #include "USBHostXpad.h" #include "utils.h" #include "Audio.h" #include <cmath> Serial pc(USBTX, USBRX); Serial robotCom(p13, p14); AnalogIn ir(p20); Speaker speaker(p18); // the pin must be the AnalogOut pin - p18 DigitalOut led1(LED1); //shows trigger DigitalOut led2(LED2); //shows collision prevention DigitalOut led3(LED3); //shows laser pointer active DigitalOut led4(LED4); //shows cpu activity DigitalOut fire(p8); DigitalOut laser(p27); Audio audio(speaker); Xbox360ControllerState controlState; Traxster tank(robotCom); //comment added void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){ //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); controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r); }; void thread_audio_run(void const* arg){ audio.run(); } float tank_L(float x, float y){ float scale = 0.0; if(x >= 0.0 && y <= 0){ //bottom right if(y == 0){ scale = 1.0; goto End;} if(x == 0){ scale = -1.0; goto End;} float theta = atan(y/x) / 3.14159; scale = theta * 4.0 + 1.0; } else if(x <= 0.0 && y <= 0){ //bottom left scale = -1.0; } else if(x <= 0.0 && y >= 0){ //top left if(y == 0){ scale = -1.0; goto End;} if(x == 0){ scale = 1.0; goto End;} float theta = atan(y/x) / 3.14159; scale = -theta * 4.0 - 1.0; } else { //top-right scale = 1.0; } End: scale *= sqrt(x*x + y*y); return scale; } float tank_R(float x, float y){ float scale = 0.0; if(x >= 0.0 && y <= 0){ //bottom right scale = -1.0; } else if(x <= 0.0 && y <= 0){ //bottom left if(y == 0){ scale = 1.0; goto End;} if(x == 0){ scale = -1.0; goto End;} float theta = atan(y/x) / 3.14159; scale = -theta*4.0 + 1.0; } else if(x <= 0.0 && y >= 0){ //top left scale = 1.0; } else { //top-right if(y == 0){ scale = -1.0; goto End;} if(x == 0){ scale = 1.0; goto End;} float theta = atan(y/x) / 3.14159; scale = theta*4.0 - 1.0; } End: scale *= sqrt(x*x + y*y); return scale; } void thread_controller(void const* arg){ tank.SetMotors(0,0); USBHostXpad controller; controller.attachEvent(&onControlInput); while(1){ tank.SetMotors(0,0); bool wasdisconnected = true; //acts as a failsafe while(controller.connected()) { if(wasdisconnected){ //pc.printf("Controller Status >> Connected!\r\n"); controller.led( USBHostXpad::LED1_ON ); tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second. wasdisconnected = false; } // left bumper disables auto-reverse bool collisionAvoidance = !((bool)(controlState.buttons & USBHostXpad::XPAD_PAD_LB)); bool trigger = false; // trigger rely - airsoft gun if(controlState.triggerRight > 0x80){ controller.rumble(255,255); fire = 1; trigger = true; }else{ controller.rumble(0,0); fire = 0; } //lazzzzeeer sight if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ laser = 1; }else{ laser = 0; } //dpad sounds if(trigger || (controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y))){ int code = 0; if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){ code = 0; } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ code = 1; } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ code = 2; } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){ code = 3; } else if (trigger){ code = 4; }else if (controlState.buttons & USBHostXpad::XPAD_PAD_Y){ code = 5; } //pc.printf("Audio play: %d\r\n",code); audio.play(code); }else{ //pc.printf("Audio Stop!\r\n"); audio.stop(); } //update output leds led1 = fire; led2 = collisionAvoidance; led3 = laser; // on collision, reverse tank if(ir > 0.75 && collisionAvoidance){ controller.rumble(255,255); tank.SetMotors(-0.5,-0.5); continue; }else{ } // map joystick input to tracks - if joystick is being moved float y = xpadNormalizeAnalog(controlState.analogLeftY); float x = -1.0 * xpadNormalizeAnalog(controlState.analogLeftX); //float x = -1.0 * y / abs(y) * xpadNormalizeAnalog(controlState.analogLeftX); //flips left/right when in reverse if( sqrt(x*x + y*y) > 0.25 ) { tank.SetMotors(tank_L(x,y), tank_R(x,y)); //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y)); } else { tank.SetMotors(0.0,0.0); } } //pc.printf("Controller Status >> DISCONNECTED! Reconnecting...\r\n"); tank.SetMotors(0.0,0.0); //stop, wait for controller to reconnect for a second. Thread::wait(500); controller.connect(); } } int main() { tank.SetMotors(0,0); fire = 0; laser = 1; led1 = 1; led4 = 1; pc.baud(19200); pc.printf("TANK\r\n"); Thread t_controller(thread_controller); Thread audio_thread(thread_audio_run); //tank.SetMotors(1.0,-1.0); //Thread::wait(1000); //tank.SetMotors(-1.0,1.0); //Thread::wait(1000); //tank.SetMotors(0,0); while(1){ led4 = !led4; // fire = !fire; Thread::wait(1000); } }