File content as of revision 10:7e3987c8fa37:

#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 DQ1(p9); //motor lv
PwmOut PWM1(p21);  //motor lv
DigitalOut DQ2(p10);//motor rv
PwmOut PWM2 (p22); //Motor rv
DigitalOut DQ3(p11);//motor la
PwmOut PWM3(p23);  //motor la
DigitalOut DQ4(p12);//motor ra
PwmOut PWM4 (p24); //Motor ra
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){  //LBA bottom right
    pc.printf("1\r\n");
        
    
    
    
        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){  //LBA bottom left
    pc.printf("2\r\n");
    
   
    
        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
            
            float AS = 1.0; // acutele snelheid
            if(controlState.triggerRight > 0x80){  //Rechtdoor rijden
                //controller.rumble(255,255);
                fire = 1;
                trigger = true;
                
                   
            }else{
                //controller.rumble(0,0);
                fire = 0;
            }
            
            //lazzzzeeer sight
            if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ //Knop X laser=DQ3
            //Stoppen
            DQ1= 1; //lv
            DQ2= 1; //rv
            DQ3= 1; //la
            DQ4= 1; //ra
            PWM1 = AS*0; //lv in de vrij
            PWM2 = AS*0; //rv in de vrij
            PWM3 = AS*0; //la in de vrij
            PWM4 = AS*0; //ra in de vrij
            wait(2.0);
        
                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;
                
                float z = 0.5;        // persentage voor het draaien van de motoren
                float j = 0.75;        // persentage voor het draaien van de motoren
                if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){ //vooruit
                    
                    DQ1= 1; //lv
                    DQ2= 1; //rv
                    DQ3= 1; //la
                    DQ4= 1; //ra
                    PWM1 = AS*1.5;
                    PWM2 = AS*1.5;
                    PWM3 = AS*1.5;
                    PWM4 = AS*1.5;
                } else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ //Achteruit
                    DQ1= 0; //lv
                    DQ2= 0; //rv
                    DQ3= 0; //la
                    DQ4= 0; //ra
                    PWM1 = AS*1.5;
                    PWM2 = AS*1.5;
                    PWM3 = AS*1.5;
                    PWM4 = AS*1.5;
                    
                } else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ //links
                    led1=1; //links
    DQ1 = 0; //lv
    DQ2 = 1; //rv
    DQ3 = 0; //La
    DQ4 = 1; //Ra
    PWM1 = AS*z;
    PWM2 = AS*j;
    PWM3 = AS*z;
    PWM4 = AS*j;
                } else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){//rechts
                    led3=1; //rechts
        
    DQ1 = 1; //lv
    DQ2 = 0; //rv
    DQ3 = 1; //la
    DQ4 = 0; //Ra
    PWM1 = AS*j;
    PWM2 = AS*z;
    PWM3 = AS*j;
    PWM4 = AS*z;
    
                } 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;
    led2= 1;
    
    
    pc.baud(9600);
    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= !DQ4;
    //    fire = !fire;
        Thread::wait(1000);
    }
}