File content as of revision 6:e905d3ec8545:

#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 led(LED1);
DigitalOut led4(LED4);
DigitalOut fire(p8);

Mutex m;


//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){
    
    USBHostXpad controller;
    controller.attachEvent(&onControlInput);
    
    while(1){
        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 joystick controls
            float y = xpadNormalizeAnalog(controlState.analogLeftY);
            float x = -1.0 *  xpadNormalizeAnalog(controlState.analogLeftX);
            //float x = -1.0 * y / abs(y) *  xpadNormalizeAnalog(controlState.analogLeftX);
            
            if(ir > 0.65){      // on collision, stop tank
                controller.rumble(255,255);
                tank.SetMotors(0,0); 
            }else{
            }
            
            if(controlState.triggerRight > 0x80){   //shoot
                controller.rumble(255,255);
                led = 1;
                fire = 1;
            }else{
                controller.rumble(0,0);
                led = 0;
                fire = 0;
            }
           
            if( sqrt(x*x + y*y) > 0.25 ) {
                tank.SetMotors(tank_L(x,y), tank_R(x,y));
                //m.lock();
                //pc.printf("motors: %f, %f\r\n", tank_L(x,y), tank_R(x,y));
                //m.unlock();
            } 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 start = 0;

int main() {
    if(start == 0) {tank.SetMotors(0,0); fire = 0; start++;}
    led = 1;
    
    led4 = 1;
    //pc.baud(19200);
    //pc.printf("TANK\r\n");
    
    Thread t_controller(thread_controller);
    
    //Thread audio_thread(thread_audio_run);
    
    //audio.playMario();
    
    
    //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(5000);
    }
}