File content as of revision 8:36b2ef26a0b1:

#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 );   
                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");
    
    //int i = 0;
    //while(i++ < 10){
    //    audio.playMario();
    //}
    
    
    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);
    }
}