Revision 0:345f76c72b9a, committed 2015-02-25

Comitter:

347467

Date:

Wed Feb 25 08:25:16 2015 +0000

Commit message:

Nog niet kloar;

Changed in this revision

diff -r 000000000000 -r 345f76c72b9a Audio.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Audio.h	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,418 @@
+
+#include "Speaker.h"
+#include <vector>
+
+//This needs work to get threading working
+
+class Audio{
+private:
+    Mutex mutex;
+    Speaker& speaker;
+    bool playing;
+    int code;       //sound code
+    
+    bool getplaying(){
+        bool b;
+        mutex.lock();
+        b = playing;
+        mutex.unlock();
+        return b;
+    }
+    int getcode(){
+        int c;
+        mutex.lock();
+        c = code;
+        mutex.unlock();   
+        return c;
+    }
+public:
+    Audio(Speaker& ao) : speaker(ao)
+    {   
+    }
+    void play(int code){
+        mutex.lock();
+        this->code = code;
+        this->playing = true;
+        mutex.unlock();
+    }
+    void stop(){
+        mutex.lock();
+        this->playing = false;
+        mutex.unlock();   
+    }
+    
+    void run(){
+        while(true){    //service audio requests...
+            
+            //if not playing, wait fast
+            if(!getplaying()){
+                Thread::wait(1);
+                continue;   
+            }
+            
+            
+            int code = getcode();
+            
+            switch(code){
+                case 0: // up
+                    speaker.PlayNote(500.0, 0.05, 1.0);
+                    speaker.PlayNote(600.0, 0.05, 1.0);
+                    speaker.PlayNote(700.0, 0.05, 1.0);
+                    break;
+                case 1: // down
+                    speaker.PlayNote(500.0, 0.05, 1.0);
+                    speaker.PlayNote(550.0, 0.05, 1.0);
+                    break;
+                case 2: //left
+                    speaker.PlayNote(400.0, 0.1, 1.0);
+                    speaker.PlayNote(500.0, 0.1, 1.0);
+                    break;
+                case 3:  // right
+                    speaker.PlayNote(600.0, 0.05, 1.0);
+                    speaker.PlayNote(500.0, 0.05, 1.0);
+                    break;
+                    
+                // trigger sound
+                case 4:
+                
+                    speaker.PlayNote(500.0, 0.05, 1.0);
+                    speaker.PlayNote(600.0, 0.05, 1.0);
+                    speaker.PlayNote(700.0, 0.05, 1.0);
+                    break;
+                    
+                // mario!
+                case 5:
+                    playMario();
+                    break;
+                default: break; 
+            }
+            
+            
+            //Thread::wait(10);
+        }
+    }
+    
+    
+    void playNote(float a, float b, float c){
+        speaker.PlayNote(a,b,c);
+    }
+    
+public:
+    
+    void playMario(){
+          
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(510.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(770.0, 0.08, 1.0);
+        Thread::wait(413);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(431);
+        playNote(510.0, 0.08, 1.0);
+        Thread::wait(338);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(300);
+        playNote(320.0, 0.08, 1.0);
+        Thread::wait(375);
+        playNote(440.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(480.0, 0.06, 1.0);
+        Thread::wait(248);
+        playNote(450.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(150);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(150);
+        playNote(760.0, 0.04, 1.0);
+        Thread::wait(113);
+        playNote(860.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(700.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(760.0, 0.04, 1.0);
+        Thread::wait(263);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(520.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(580.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(480.0, 0.06, 1.0);
+        Thread::wait(375);
+        playNote(510.0, 0.08, 1.0);
+        Thread::wait(338);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(300);
+        playNote(320.0, 0.08, 1.0);
+        Thread::wait(375);
+        playNote(440.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(480.0, 0.06, 1.0);
+        Thread::wait(248);
+        playNote(450.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(150);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(150);
+        playNote(760.0, 0.04, 1.0);
+        Thread::wait(113);
+        playNote(860.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(700.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(760.0, 0.04, 1.0);
+        Thread::wait(263);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(520.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(580.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(480.0, 0.06, 1.0);
+        Thread::wait(375);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(570.0, 0.08, 1.0);
+        Thread::wait(165);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.15, 1.0);
+        Thread::wait(225);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(570.0, 0.08, 1.0);
+        Thread::wait(315);
+        playNote(585.0, 0.08, 1.0);
+        Thread::wait(338);
+        playNote(550.0, 0.08, 1.0);
+        Thread::wait(315);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(270);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(570.0, 0.08, 1.0);
+        Thread::wait(165);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.15, 1.0);
+        Thread::wait(225);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(1020.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(760.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(720.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(680.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(620.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(650.0, 0.11, 1.0);
+        Thread::wait(225);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(570.0, 0.08, 1.0);
+        Thread::wait(315);
+        playNote(585.0, 0.08, 1.0);
+        Thread::wait(338);
+        playNote(550.0, 0.08, 1.0);
+        Thread::wait(315);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(270);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(263);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(580.0, 0.06, 1.0);
+        Thread::wait(263);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(380.0, 0.06, 1.0);
+        Thread::wait(450);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(263);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(580.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(413);
+        playNote(870.0, 0.06, 1.0);
+        Thread::wait(244);
+        playNote(760.0, 0.06, 1.0);
+        Thread::wait(450);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(500.0, 0.04, 1.0);
+        Thread::wait(263);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(580.0, 0.06, 1.0);
+        Thread::wait(263);
+        playNote(660.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(500.0, 0.06, 1.0);
+        Thread::wait(225);
+        playNote(430.0, 0.06, 1.0);
+        Thread::wait(113);
+        playNote(380.0, 0.06, 1.0);
+        Thread::wait(450);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(113);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(510.0, 0.08, 1.0);
+        Thread::wait(75);
+        playNote(660.0, 0.08, 1.0);
+        Thread::wait(225);
+        playNote(770.0, 0.08, 1.0);
+        Thread::wait(413);
+        playNote(380.0, 0.08, 1.0);
+        Thread::wait(431);
+   
+    }
+};
\ No newline at end of file

diff -r 000000000000 -r 345f76c72b9a Speaker.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Speaker.h	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,52 @@
+class Speaker
+{
+public:
+    Speaker(PinName pin) : _pin(pin) {
+// _pin(pin) means pass pin to the Speaker Constructor
+// precompute 64 sample points on one sine wave cycle
+// used for continuous sine wave output later
+        for(int k=0; k<64; k++) {
+            Analog_out_data[k] = int (65536.0 * ((1.0 + sin((float(k)/64.0*6.28318530717959)))/2.0));
+            // scale the sine wave to 16-bits - as needed for AnalogOut write_u16 arg
+        }
+
+    }
+// class method to play a note based on AnalogOut class
+    void PlayNote(float frequency, float duration, float volume) {
+        // scale samples using current volume level arg
+        for(int k=0; k<64; k++) {
+            Analog_scaled_data[k] = Analog_out_data[k] * volume;
+        }
+        // reset to start of sample array
+        i=0;
+        // turn on timer interrupts to start sine wave output
+        Sample_Period.attach(this, &Speaker::Sample_timer_interrupt, 1.0/(frequency*64.0));
+        // play note for specified time
+        wait(duration);
+        // turns off timer interrupts
+        Sample_Period.detach();
+        // sets output to mid range - analog zero
+        this->_pin.write_u16(32768);
+
+    }
+private:
+// sets up specified pin for analog using AnalogOut class
+    AnalogOut _pin;
+    // set up a timer to be used for sample rate interrupts
+    Ticker Sample_Period;
+
+    //variables used by interrupt routine and PlayNote
+    volatile int i;
+    short unsigned Analog_out_data[64];
+    short unsigned Analog_scaled_data[64];
+
+// Interrupt routine
+// used to output next analog sample whenever a timer interrupt occurs
+    void Sample_timer_interrupt(void) {
+        // send next analog sample out to D to A
+        this->_pin.write_u16(Analog_scaled_data[i]);
+        // increment pointer and wrap around back to 0 at 64
+        i = (i+1) & 0x03F;
+    }
+};
+

diff -r 000000000000 -r 345f76c72b9a Traxster.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Traxster.cpp	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,46 @@
+
+#include "Traxster.h"
+#include "utils.h"
+#include "rtos.h"
+
+//extern Serial pc;
+//extern Mutex m;
+
+int getMotorSpeedInt(float f){
+    
+    int max = 40;
+    
+    if(f > 1.0)
+        f = 1.0;
+
+    if(f < -1.0)
+        f = -1.0;
+    
+    return   (int)round( (float)max * f );
+}
+
+void Traxster::SetMotors(float fm1, float fm2)
+{
+    int m1 = getMotorSpeedInt(fm1);
+    int m2 = getMotorSpeedInt(fm2);
+    
+    //clear robot msgs
+    while (rob.readable())
+        rob.getc();
+        
+    if (m1 == 0 && m2 == 0)
+    {
+        rob.puts("stop\r");
+    }
+    else
+    {
+        //m.lock();
+        //pc.printf("mogo 1:%d 2:%d\r\n", m1, m2);
+        //m.unlock();
+        rob.printf("mogo 1:%d 2:%d\r", m1, m2);
+    }
+    
+    //clear robot msgs
+    while (rob.readable())
+        rob.getc();
+}

diff -r 000000000000 -r 345f76c72b9a Traxster.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Traxster.h	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,16 @@
+#include "mbed.h"
+
+class Traxster { 
+private:
+    Serial& rob;
+public:   
+    Traxster(Serial& robotComm  /* and other controls or sensors...*/)
+    : rob(robotComm)
+    {
+        rob.baud(19200);
+    };
+    
+    // Set motor speeds. [-1, +1] of max speed for motor.
+    void SetMotors(float m1, float m2);
+    
+};
\ No newline at end of file

diff -r 000000000000 -r 345f76c72b9a USBHost.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHost.lib	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/hotwheelharry/code/USBHost/#8e7d225f2fd7

diff -r 000000000000 -r 345f76c72b9a USBHostXpad.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostXpad.lib	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/hotwheelharry/code/USBHostXpad/#bbbbd88de858

diff -r 000000000000 -r 345f76c72b9a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,540 @@
+#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);
+I2C i2cMod(p28, p27);
+
+Serial robotCom(p13, p14);
+AnalogIn    ir(p20);
+Speaker     speaker(p18);  // the pin must be the AnalogOut pin - p18
+
+DigitalIn   DI1(p5);
+DigitalIn   DI2(p6);
+DigitalIn   DI3(p7);
+DigitalIn   DI4(p8);
+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
+AnalogIn    pot(p19);         //potmeter
+//I2C         U1(p27,p28);        //Ultrasoonsensorwaarde lv1 
+//I2C         U2(p27,p28);        //Ultrasoonsensorwaarde rv1
+AnalogIn    lslv(p15);           //lasersensorwaarde lv2
+AnalogIn    lsrv(p16);           //lasersensorwaarde rv2
+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);
+
+int i2cAddress1 = 0xF2;
+int i2cAddress2 = 0xE0;
+
+void sendStartRangingCommand1(void){
+    const char command[] = {0x00, 0x51};
+    i2cMod.write(i2cAddress1, command, 2);
+}
+void sendStartRangingCommand2(void){
+    const char command[] = {0x00, 0x51};
+    i2cMod.write(i2cAddress2, command, 2);
+}
+int readRange1(void){
+    const char command[]  = {0x02};           //Address of range register
+    char response[] = {0x00, 0x00};
+    i2cMod.write(i2cAddress1, command, 1, 1);  //Send command
+    i2cMod.read(i2cAddress1, response, 2);     //Read 16bits result
+    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
+    return range; 
+}
+
+int readRange2(void){
+    const char command[]  = {0x02};           //Address of range register
+    char response[] = {0x00, 0x00};
+    i2cMod.write(i2cAddress2, command, 1, 1);  //Send command
+    i2cMod.read(i2cAddress2, response, 2);     //Read 16bits result
+    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
+    return range; 
+}
+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() {
+
+    while(1) {
+        sendStartRangingCommand1();
+        wait(0.07);
+        int U1 = readRange1();
+        wait (0.2);
+        sendStartRangingCommand2();
+        wait(0.07);
+        int U2 = readRange2();
+        
+        
+        
+        if (DI2 == 1 && DI4 == 1){
+            // sensoren
+              
+            float AS; // snelheid afhangelijk van
+            float Q;       // De afstand vanaf waar de ultrasonesensor iets moet detecteren in CM.
+            float I;        // de afstand vanaf waar de infraroodsensor iets moet detecteren.
+            float x;        // persentage voor het draaien van de motoren
+            float y;        // persentage voor het draaien van de motoren
+            x = 0.5;
+            y = 0.25;
+            Q = 300;    // groter dan een waarde 300 = 3 meter detecteerd de sensor iets
+            I = 1.5;    // groeten dan een waarde 300 = 3meter
+            AS =pot*3.3;
+
+            if (U1<=Q && U2<=Q && lslv>=I && lsrv>=I)//alles iets detecteerd
+                {
+                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);
+        
+                while(1)
+                    {
+                    DQ1= 1; //Lv  
+                    DQ2= 0; //Rv
+                    DQ3= 1; //La
+                    DQ4= 0; //Ra
+                    PWM1 = AS*0.5; //LV 50% van AS
+                    PWM2 = AS*0.5; //RV 50% van AS
+                    PWM3 = AS*0.5; //LA 50% van AS
+                    PWM4 = AS*0.5; //RA 50% van AS
+        
+                    if(U1 >= Q  && U2 >= Q)
+                        {
+                        break;
+                        }
+   
+                }
+            }
+   
+   
+            else if (U1<=Q && U2<=Q && lslv<=I && lsrv>=I)//rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien
+            {
+                while(1)
+                {
+                DQ1 = 1; //lv
+                DQ2 = 1; //rv
+                DQ3 = 1; //La
+                DQ4 = 1; //Ra
+                PWM1 = AS*x;
+                PWM2 = AS*y;
+                PWM3 = AS*x;
+                PWM4 = AS*y;
+        
+                if(U1 >= Q  && U2 >= Q)
+                    {
+                     break;}
+                }    
+            }
+        
+            else if (U1<=Q && U2<=Q && lslv>=I && lsrv<=I)//lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
+            {
+                while(1)
+                    {
+                    DQ1 = 1; //lv
+                    DQ2 = 1; //rv
+                    DQ3 = 1; //la
+                    DQ4 = 1; //Ra
+                    PWM1 = AS*y;
+                    PWM2 = AS*x;
+                    PWM3 = AS*y;
+                    PWM4 = AS*x;
+        
+        
+                    if(U1 >= Q  && U2 >= Q)
+                        {
+                        break;}
+                    }    
+            }
+            
+            else if (U1>Q && U2<Q && lslv<I && lsrv>I)//rv1 rv2 iets gedetecteerd hebben, naar links draaien)
+            {
+                 while(1)
+                    {
+                    DQ1 = 1; //lv
+                    DQ2 = 1; //rv
+                    DQ3 = 1; //la
+                    DQ4 = 1; //ra
+                    PWM1 = AS*x; 
+                    PWM2 = AS*y;
+                    PWM3 = AS*x;
+                    PWM4 = AS*y;
+        
+                        if(U1>Q  && U2 > Q)
+                            {
+                                break;}
+                    }
+            }
+        
+        
+            else if (U1<Q && U2>Q && lslv>I && lsrv<I)//lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
+            {
+                 while(1)
+                    {
+                    DQ1 = 1; //lv
+                    DQ2 = 1; //rv
+                    DQ3 = 1; //La
+                    DQ4 = 1; //Ra
+                    PWM1 = AS*y;
+                    PWM2 = AS*x;
+                    PWM3 = AS*y;
+                    PWM4 = AS*x;
+        
+                    if(U1 > Q  && U2 > Q)
+                        {
+                        break;}
+                }
+            }
+    
+    
+            else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
+                {
+                if ( lslv>=lsrv)
+                    {
+                    while(1)
+                        {    
+                        DQ1 = 1; //lv
+                        DQ2 = 1; //rv
+                        DQ3 = 1; //La
+                        DQ4 = 1; //Ra
+                        PWM1 = AS*y;
+                        PWM2 = AS*x;
+                        PWM3 = AS*y;
+                        PWM4 = AS*x;
+        
+                        if(U1<Q  && U2 > Q)
+                            {
+                            break;}
+                        }
+                }
+        
+                else if ( lslv<=lsrv )
+                    {
+                    while(1)
+                        {
+                        DQ1 = 1; //lv
+                        DQ2 = 1; //rv
+                        DQ3 = 1; //la
+                        DQ4 = 1; //Ra
+                        PWM1 = AS*x;
+                        PWM2 = AS*y;
+                        PWM3 = AS*x;
+                        PWM4 = AS*y;
+        
+                        if( U1 > Q  && U2 > Q)
+                            {
+                            break;}
+                        }     
+                    }
+        
+        
+                else if (U1<Q && U2>Q && lslv<I && lsrv<I)//lv1 iets gedetecteerd heeft, naar rechts draaien)
+                    {
+                    while(1)
+                        {
+        
+                        DQ1 = 1; //lv
+                        DQ2 = 1; //rv
+                        DQ3 = 1; //la
+                        DQ4 = 1; //ra
+                        PWM1 = AS*y;
+                        PWM2 = AS*x;
+                        PWM3 = AS*y;
+                        PWM4 = AS*x;
+                        if(U1>Q  && U2 > Q)
+                            {
+                            break;}
+                            
+                        }
+                }
+//////////////////////////////////////////////////////////////////////////////
+        else if (DI1 == 1 && DI4 == 1)
+           {
+            // Handmatig
+          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);
+           }
+        }
+        }}}}
+        
+        
+        
+        
+       
\ No newline at end of file

diff -r 000000000000 -r 345f76c72b9a mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#318e02f48146

diff -r 000000000000 -r 345f76c72b9a mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac
\ No newline at end of file

diff -r 000000000000 -r 345f76c72b9a ultrasoon.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasoon.h	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,85 @@
+//#include "mbed.h"
+
+I2C i2cMod(p28, p27);
+//Serial PC(USBTX, USBRX);       //Debug port to PC
+
+int i2cAddress1 = 0xF2;
+int i2cAddress2 = 0xE0;
+
+void sendStartRangingCommand1(void){
+    const char command[] = {0x00, 0x51};
+    i2cMod.write(i2cAddress1, command, 2);
+}
+void sendStartRangingCommand2(void){
+    const char command[] = {0x00, 0x51};
+    i2cMod.write(i2cAddress2, command, 2);
+}
+/* 
+int readRange1(void){
+    const char command[]  = {0x02};           //Address of range register
+    char response[] = {0x00, 0x00};
+    i2cMod.write(i2cAddress1, command, 1, 1);  //Send command
+    i2cMod.read(i2cAddress1, response, 2);     //Read 16bits result
+    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
+    return range; 
+}
+
+int readRange2(void){
+    const char command[]  = {0x02};           //Address of range register
+    char response[] = {0x00, 0x00};
+    i2cMod.write(i2cAddress2, command, 1, 1);  //Send command
+    i2cMod.read(i2cAddress2, response, 2);     //Read 16bits result
+    int range = (response[0]<<8)+response[1]; //Shift two bytes into int   
+    return range; 
+}
+
+                    //void setAddress(int address) {
+                     //Send address change sequence
+                     //    char command[] = {0x00, 0xA0};
+                    //   i2cMod.write(i2cAddress, command, 2);
+                    //   command[1] = 0xAA;
+                    //   i2cMod.write(i2cAddress, command, 2);
+                     //   command[1] = 0xA5;
+                     //   i2cMod.write(i2cAddress, command, 2);
+                     //   command[1] = address;
+                    //   i2cMod.write(i2cAddress, command, 2);
+                    // }
+
+    //int main() {
+      //      //Read software version
+    //const char command[]  = {0x00};           //Address of swversion register
+    //char response[] = {0x00, 0x00};
+    //i2cMod.write(i2cAddress1, command, 1, 1);  //Send command
+    //i2cMod.read(i2cAddress1, response, 2);     //Read 16bits result
+    //int swversion = (response[0]<<8)+response[1]; //Shift two bytes into int
+    //PC.printf(" Software version1: %i", swversion);
+    //PC.printf("\n\r");
+    
+                          //Read software version
+                            //const char command[]  = {0x00};           //Address of swversion register
+                            // char response[] = {0x00, 0x00};
+                            // i2cMod.write(i2cAddress2, command, 1, 1);  //Send command
+                            // i2cMod.read(i2cAddress2, response, 2);     //Read 16bits result
+                            // int swversion = (response[0]<<8)+response[1]; //Shift two bytes into int
+                            // PC.printf(" Software version2: %i", swversion);
+                            // PC.printf("\n\r");
+    
+    
+    //setAddress(0xF2) ;
+        
+    //while(true){
+     //   sendStartRangingCommand1();
+     //   wait(0.07);
+     //   int range1 = readRange1();
+     //   PC.printf(" Range1: %i", range1);
+     //   PC.printf("\n\r");
+     //   wait (0.2);
+     
+    // sendStartRangingCommand2();
+    //    wait(0.07);
+    //    int range2 = readRange2();
+    //    PC.printf(" Range2: %i", range2);
+    //    PC.printf("\n\r");
+    //    wait(1.0);
+     //}
+//}
\ No newline at end of file

diff -r 000000000000 -r 345f76c72b9a utils.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/utils.cpp	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,23 @@
+#include "utils.h"
+
+double ghettoFloor(double d){
+    return double((long long)d);
+}
+double round(double d)
+{
+  return ghettoFloor(d + 0.5);
+}
+
+float xpadNormalizeAnalog(int x){
+    float res = 0;
+    if(x > 0){
+        res = (float)x/32767.0;
+    }else{
+        res = (float)x/32768.0;
+    }   
+    return res;
+}
+float xpadNormalizeTrigger(int x){
+    float res = (float)x / (float)0xff;
+    return res;
+}

diff -r 000000000000 -r 345f76c72b9a utils.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/utils.h	Wed Feb 25 08:25:16 2015 +0000
@@ -0,0 +1,26 @@
+
+#ifndef utilshhh
+#define utilshhh
+
+
+double ghettoFloor(double d);
+double round(double d);
+
+float xpadNormalizeAnalog(int x);
+float xpadNormalizeTrigger(int x);
+struct Xbox360ControllerState{
+    int buttons;
+    int analogLeftX;
+    int analogLeftY;
+    int analogRightX;
+    int analogRightY;
+    int triggerLeft;
+    int triggerRight;
+    Xbox360ControllerState(){};
+    Xbox360ControllerState(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r)
+    : buttons(buttons), analogLeftX(stick_lx), analogLeftY(stick_ly), analogRightX(stick_rx), analogRightY(stick_ry), triggerLeft(trigger_l), triggerRight(trigger_r)
+    {}
+};
+
+
+#endif
\ No newline at end of file