Revision 1:da390b3b1330, committed 2015-03-02

Comitter:

juliandekker

Date:

Mon Mar 02 10:58:30 2015 +0000

Parent:

0:345f76c72b9a

Commit message:

xbox controller robot

Changed in this revision

--- a/main.cpp	Wed Feb 25 08:25:16 2015 +0000
+++ b/main.cpp	Mon Mar 02 10:58:30 2015 +0000
@@ -1,48 +1,203 @@
 #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
+Serial pc   (USBTX, USBRX);
+I2C i2cMod  (p28, p27);
+DigitalOut  handmode(p5);                   // hand moet 1 worden (niet connected
+DigitalIn   automode(p6);                   // auto
+DigitalIn   externemode(p7);                // extern
+DigitalIn   aanuit(p8);                     // aan uit
+DigitalOut  motorLeftFrontDirAV(p9);       //motor lv 1= vooruit 
+DigitalOut  motorRightFrontDirAV(p10);      //MOTOR rv 1= vooruit
+DigitalOut  motorLeftRearDirAV(p11);        //motor la 1= vooruit
+DigitalOut  motorRightRearDirAV(p12);       //MOTOR ra 1= vooruit        
+PwmOut      motorLeftFrontVelAV(p21);       //motor lv
+PwmOut      motorRightFrontVelAV (p22);      //Motor rv
+PwmOut      motorLeftRearVelAV(p23);        //motor la
+PwmOut      motorRightRearVelAV (p24);      //Motor ra
+AnalogIn    pot(p19);                       //potmeter
+AnalogIn    laserLeft(p15);                 //lasersensorwaarde lv2
+AnalogIn    laserRight(p16);                //lasersensorwaarde rv2
+DigitalOut  led1(LED1);      
+DigitalOut  led2(LED2);      
+DigitalOut  led3(LED3);      
+DigitalOut  led4(LED4); 
+     
 
-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);
+float SonarLeft;               // afstandswaarde ultrasoon sensor 1(links?)
+float SonarRight;               // afstandswaarde ultrasoon sensor 2 (rechts?)
+float AS = 0.5;               // actuele snelheid
+float z = 0.5;          // percentage voor het draaien van de motoren
+float j = 0.75;         // persentage voor het draaien van de motoren
+float Q = 300;          // De afstand vanaf waar de ultrasonesensor iets moet detecteren in CM. // groter dan een waarde 300 = 3 meter detecteerd de sensor iets
+float I = 1.5;          // de afstand vanaf waar de infraroodsensor iets moet detecteren.
+float x = 0.5;          // persentage voor het draaien van de motoren
+float y = 0.25;         // persentage voor het draaien van de motoren
+float motorAcc = 0.3;   // Motor versnelling    pwm per seconde
+float motorDec = 0.5;   // Motor vertraging     pwm per seconde
+bool motorLeftFrontDirSP = 0;       //motordirection leftfront setpoint
+bool motorRightFrontDirSP = 0;      //motordirection leftfront setpoint
+bool motorLeftRearDirSP = 0;        //motordirection leftfront setpoint
+bool motorRightRearDirSP = 0;       //motordirection leftfront setpoint
+float motorLeftFrontVelSP =0;       // motor snelheid leftfront setpoint
+float motorRightFrontVelSP =0;       // motor snelheid leftfront setpoint
+float motorLeftRearVelSP =0;       // motor snelheid leftfront setpoint
+float motorRightRearVelSP =0;       // motor snelheid leftfront setpoint
 
 Xbox360ControllerState controlState;
-Traxster tank(robotCom);
+
+// Turn right program
+void turnRight(){
+    led3 = 1; //rechts        
+    motorLeftFrontDirSP = 1; //lv
+    motorRightFrontDirSP = 0; //rv
+    motorLeftRearDirSP = 1; //la
+    motorRightRearDirSP = 0; //Ra
+    motorLeftFrontVelSP = AS*j;
+    motorRightFrontVelSP = AS*z;
+    motorLeftRearVelSP = AS*j;
+    motorRightRearVelSP = AS*z;
+    pc.printf("rechts\r\n");
+}
+
+// Turn Left program
+void turnLeft(){
+    motorLeftFrontDirSP = 1; //lv
+    motorRightFrontDirSP = 1; //rv
+    motorLeftRearDirSP = 1; //La
+    motorRightRearDirSP = 1; //Ra
+    motorLeftFrontVelSP = AS*x;
+    motorRightFrontVelSP = AS*y;
+    motorLeftRearVelSP = AS*x;
+    motorRightRearVelSP = AS*y;
+    pc.printf("links\r\n");
+}
+
+void forward (){
+    motorLeftFrontDirSP = 1; //lv
+    motorRightFrontDirSP = 1; //rv
+    motorLeftRearDirSP = 1; //La
+    motorRightRearDirSP = 1; //Ra
+    motorLeftFrontVelSP = AS;
+    motorRightFrontVelSP = AS;
+    motorLeftRearVelSP = AS;
+    motorRightRearVelSP = AS;
+    pc.printf("rechtsvooruit\r\n");
+} 
+
+void reverse(){
+    motorLeftFrontDirSP = 0; //lv
+    motorRightFrontDirSP = 0; //rv
+    motorLeftRearDirSP = 0; //La
+    motorRightRearDirSP = 0; //Ra
+    motorLeftFrontVelSP = AS;
+    motorRightFrontVelSP = AS;
+    motorLeftRearVelSP = AS;
+    motorRightRearVelSP = AS;
+    pc.printf("achteruit\r\n");   
+}
 
+void halt(){
+    motorLeftFrontDirSP = 0; //lv
+    motorRightFrontDirSP = 0; //rv
+    motorLeftRearDirSP = 0; //La
+    motorRightRearDirSP = 0; //Ra
+    motorLeftFrontVelSP = 0;
+    motorRightFrontVelSP = 0;
+    motorLeftRearVelSP = 0;
+    motorRightRearVelSP = 0;
+    pc.printf("stop\r\n");   
+}
+                                                            ///todo nul moet nog stilstaan worden
+//Thread MotorController    
+void thread_motorControl(void const* arg){
+    int controlInterval = 100;// was100
+    while(1){
+        Thread::wait(controlInterval);
+        //MotorLeftFront
+        if(motorLeftFrontDirSP == motorLeftFrontDirAV){
+            if (motorLeftFrontVelSP > motorLeftFrontVelAV){
+                motorLeftFrontVelAV=motorLeftFrontVelAV+(motorAcc/controlInterval*10);
+            }
+            else if(motorLeftFrontVelSP < motorLeftFrontVelAV){
+                motorLeftFrontVelAV=motorLeftFrontVelAV-(motorAcc/controlInterval*10);
+            }
+        }
+        else {
+             //regel naar nul
+            if ((motorLeftFrontVelAV-(motorDec/controlInterval*10))> 0){
+                motorLeftFrontVelAV= motorLeftFrontVelAV-(motorDec/controlInterval*10);
+            } 
+            else{
+                motorLeftFrontDirAV = not motorLeftFrontDirAV; 
+            }
+        } 
+        //MotorRightFront
+        if(motorRightFrontDirSP == motorRightFrontDirAV){
+            if (motorRightFrontVelSP > motorRightFrontVelAV){
+                motorRightFrontVelAV=motorRightFrontVelAV+(motorAcc/controlInterval*10);
+            }
+            else if(motorRightFrontVelSP < motorRightFrontVelAV){
+                motorRightFrontVelAV=motorRightFrontVelAV-(motorAcc/controlInterval*10);
+            }
+        }
+        else {
+             //regel naar nul
+            if ((motorRightFrontVelAV-(motorDec/controlInterval*10))> 0){
+                motorRightFrontVelAV= motorRightFrontVelAV-(motorDec/controlInterval*10);
+            } 
+            else{
+                motorRightFrontDirAV = not motorRightFrontDirAV; 
+            }
+        }
+        //motorLeftRear
+        if(motorLeftRearDirSP == motorLeftRearDirAV){
+            if (motorLeftRearVelSP > motorLeftRearVelAV){
+                motorLeftRearVelAV=motorLeftRearVelAV+(motorAcc/controlInterval*10);
+            }
+            else if(motorLeftRearVelSP < motorLeftRearVelAV){
+                motorLeftRearVelAV=motorLeftRearVelAV-(motorAcc/controlInterval*10);
+            }
+        }
+        else {
+             //regel naar nul
+            if ((motorLeftRearVelAV-(motorDec/controlInterval*10))> 0){
+                motorLeftRearVelAV= motorLeftRearVelAV-(motorDec/controlInterval*10);
+            } 
+            else{
+                motorLeftRearDirAV = not motorLeftRearDirAV; 
+            }
+        } 
+        //motorRightRear
+        if(motorRightRearDirSP == motorRightRearDirAV){
+            if (motorRightRearVelSP > motorRightRearVelAV){
+                motorRightRearVelAV=motorRightRearVelAV+(motorAcc/controlInterval*10);
+            }
+            else if(motorRightRearVelSP < motorRightRearVelAV){
+                motorRightRearVelAV=motorRightRearVelAV-(motorAcc/controlInterval*10);
+            }
+        }
+        else {
+             //regel naar nul
+            if ((motorRightRearVelAV-(motorDec/controlInterval*10))> 0){
+                motorRightRearVelAV= motorRightRearVelAV-(motorDec/controlInterval*10);
+            } 
+            else{
+                motorRightRearDirAV = not motorRightRearDirAV; 
+            }
+        }  
+        pc.printf ("dirAV : %d ", motorLeftFrontDirAV.read());
+        pc.printf ("velocityAV :%F", motorLeftFrontVelAV.read()); 
+        pc.printf ("dirSP : %d ", motorLeftFrontDirSP);
+        pc.printf ("velocitySP :%F\r\n", motorLeftFrontVelSP); 
+    }
+}    
+
+// programma tbv uitlezen ultasoon sensoren
 int i2cAddress1 = 0xF2;
 int i2cAddress2 = 0xE0;
 
@@ -71,468 +226,128 @@
     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();
+// Thread Sonar ranging
+void thread_sonarControl(void const* arg){
+    while (1){
+         // sensoren automatisch     
+        sendStartRangingCommand1();
+        Thread::wait(0.07);
+        SonarLeft = readRange1();
+        Thread::wait (0.2);
+        sendStartRangingCommand2();
+        Thread::wait(0.07);
+        SonarRight = readRange2();
+    }
 }
 
-float tank_L(float x, float y){
-    float scale = 0.0;
-    
+// Programm tbv xbox controller
+void onControlInput(int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r){
+    controlState = Xbox360ControllerState(buttons, stick_lx, stick_ly, stick_rx, stick_ry, trigger_l, trigger_r);
+}
 
-    
-    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;
-}
+// programma tbv xbox controller
 void thread_controller(void const* arg){
-    
-    tank.SetMotors(0,0); 
     USBHostXpad controller;
     controller.attachEvent(&onControlInput);
     
-    while(1){
-        tank.SetMotors(0,0); 
+    while(1){    
+        // connect programma
         bool wasdisconnected = true;
         //acts as a failsafe
-        while(controller.connected()) {
-            if(wasdisconnected){
-                //pc.printf("Controller Status >> Connected!\r\n");
+        while(controller.connected()){        
+            if(wasdisconnected){            
                 controller.led( USBHostXpad::LED1_ON );   
-                tank.SetMotors(0.0,0.0);    //stop, wait for controller to reconnect for a second.
+                //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
+            if( controlState.buttons & USBHostXpad::XPAD_PAD_X){ 
             //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;   
+                halt();
+            }        
+            //dpad xbox controller
+            if(controlState.buttons & (USBHostXpad::XPAD_HAT_UP | USBHostXpad::XPAD_HAT_DOWN | USBHostXpad::XPAD_HAT_LEFT | USBHostXpad::XPAD_HAT_RIGHT | USBHostXpad::XPAD_PAD_Y)& automode == 0  & externemode == 0 & aanuit == 1){                           
+                //vooruit
+                pc.printf (" handmatig \n  ");
+                if(controlState.buttons & USBHostXpad::XPAD_HAT_UP){
+                    forward();
+                } 
+                //Achteruit
+                else if(controlState.buttons & USBHostXpad::XPAD_HAT_DOWN){ 
+                    reverse();                    
+                } 
+                //links
+                else if(controlState.buttons & USBHostXpad::XPAD_HAT_LEFT){ 
+                    turnLeft();
+                } 
+                //rechts
+                else if(controlState.buttons & USBHostXpad::XPAD_HAT_RIGHT){
+                    turnRight();
+                }
             }
-            
-            
-            //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);
+        //stop, wait for controller to reconnect for a second.
+        Thread::wait(500); /// was 500
         controller.connect();
     }
 }
-int main() {
 
+//////////////////////////////////////////////////////////////////
+int main() {    
+    Thread t_controller(thread_controller);
+    Thread t_motorControl(thread_motorControl);
+    Thread t_sonarControl(thread_sonarControl);    
     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;
-                        }
-   
+        if (aanuit == 1 & automode == 0 & externemode == 0){
+            handmode = 1;
+            pc.printf (" handmode \r\n  ");
+            
+        }
+        if(aanuit == 1 & automode ==1){   
+            //AS =pot*3.3;                  aanzetten weer
+            //alles iets detecteerd dus draaien
+            if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight>=I){                     
+                turnRight();          
+            }
+            //rv1 rv2 en lv1 iets gedetecteerd hebben, naar links draaien 
+            else if (SonarLeft<=Q && SonarRight<=Q && laserLeft<=I && laserRight>=I){       
+                turnLeft();   
+            }
+            //lv1 lv2 en rv1 iets gedetecteerd hebben, naar rechts draaien)
+            else if (SonarLeft<=Q && SonarRight<=Q && laserLeft>=I && laserRight<=I){ 
+                turnRight();
+            }
+            //rv1 rv2 iets gedetecteerd hebben, naar links draaien)    
+            else if (SonarLeft>Q && SonarRight<Q && laserLeft<I && laserRight>I){
+                turnLeft();
+            }      
+            //lv1 lv2 iets gedetecteerd hebben, naar rechts draaien)
+            else if (SonarLeft<Q && SonarRight>Q && laserLeft>I && laserRight<I){
+                turnRight();
+            }    
+            //lv1 rv1 iets gedetecteerd hebben, naar links of rechts draaien)
+            else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
+                if ( laserLeft>=laserRight){
+                    turnLeft();
+                }
+                else if ( laserLeft<=laserRight ){
+                    turnRight();
                 }
             }
-   
-   
-            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;}
-                }
+            //lv1 iets gedetecteerd heeft, naar rechts draaien)
+            else if (SonarLeft<Q && SonarRight>Q && laserLeft<I && laserRight<I){
+                turnRight();
             }
-    
-    
-            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);
-           }
+        }
+        else if (externemode == 1 & aanuit == 1){
+            halt(); ///still todo
         }
-        }}}}
+       /* else {
+            pc.printf (" hallo \n  ");
+            halt();
+        }*/
+    }
+}