File content as of revision 3:bebb73b82a17:

#include "mbed.h"
#include "HCSR04.h"

// Set's the Serial port
Serial pc(USBTX, USBRX);

// Defines the LED colors
DigitalOut led(LED_RED);
DigitalOut led2(LED_GREEN);

// Defines the sensors (Left and Right)
HCSR04 sensorLEFT(PTA12, PTD4);
HCSR04 sensorRIGHT(PTA4, PTA5);

// Get the left distance variable
int distLeft(int dLEFT){
    return dLEFT;    
}

// Get the right distance variable
int distRight(int dRIGHT){
    return dRIGHT;    
}


//SELVKØRENDE I MØRKET!
void selfDrive(){
    
    int dLEFT = sensorLEFT.distance(CM);
    int dRIGHT = sensorRIGHT.distance(CM);
    
    // Writes the left and right distance variable
       pc.printf("SENSOR Left: %d \n\r\v",distLeft(dLEFT));
       pc.printf("SENSOR Right: %d \n\r\v",distRight(dRIGHT));
    
        
    //Drej til venstre for at rette op
    if(dRIGHT > 60 && dLEFT > 60 || BT.getc()=='p'){
      control = 0;
    }
    
    else if(dRIGHT > 35){
      leftForward = 0.75;
      leftBackward = 0.25;
      rightForward = 0.25;
      rightBackward = 0.75;
    }
    
    else if(dLEFT > 35){
      leftForward = 0.25;
      leftBackward = 0.75;
      rightForward = 0.75;
      rightBackward = 0.25;
    }
    
    else if(dRIGHT > 50){
      leftForward = 0.1;
      leftBackward = 0.9;
      rightForward = 0.9;
      rightBackward = 0.1;
    }     
    
    else if(dLEFT > 50){
      leftForward = 0.1;
      leftBackward = 0.9;
      rightForward = 0.9;
      rightBackward = 0.1;
      
    else{
      leftForward = 1;
      leftBackward = 0;
      rightForward = 1;
      rightBackward = 0;
    }         
}     
// The main() function
int main()
{
    
    
    // The loop() function
    while(1) {
        //left and right distance variables
                selfControl(dRIGHT, dLEFT);    
        
        
        // Delay
        wait(0.5);
    }
    
}