File content as of revision 2:dfc1315b4b03:

#include "mbed.h"
#include "tsi_sensor.h"
#include <string>
#include "HCSR04.h"

/* This defines will be replaced by PinNames soon
#if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
#define ELEC0 9
#define ELEC1 10
#elif defined (TARGET_KL05Z)
#define ELEC0 9
#define ELEC1 8
#else
#error TARGET NOT DEFINED
#endif*/

PwmOut LED(LED1);
PwmOut leftForward(PTD5);
PwmOut leftBackward(PTA13);
PwmOut rightForward(PTA5);
PwmOut rightBackward(PTA4);

PwmOut grabIn(PTE30);
PwmOut grabOut(PTC1);
PwmOut heightUp(PTD4);
PwmOut heightDown(PTA12);

// Defines the sensors (Left and Right)
HCSR04 sensorLEFT(PTC12, PTC13);
HCSR04 sensorRIGHT(PTC16, PTC17);

Serial pc(USBTX, USBRX);
Serial BT(PTC4, PTC3);
char command = 0;
char store;
float frontSpeed = 0;
float backSpeed = 0;
float rightSpeed = 0;
float leftSpeed = 0;
bool pilot = false;


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));
    */
        char diller = BT.getc();
        BT.putc(diller);
       // Break out of loop if 'p'
       if(diller == 'p' && pilot == true){
            pilot = false;      
        }
        
    //Drej til venstre for at rette op    
    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;
    }         
}     

void motorControl(char input) {
  switch (input) {
    case 'w':
    rightSpeed = 0;
    leftSpeed = 0;
    BT.printf("FORWARDS!");
      if (frontSpeed < 0.99) {
        frontSpeed = frontSpeed + 0.25f;
}
      if (backSpeed > 0.1) {
        backSpeed = backSpeed - 0.25f;
      }
      leftForward = frontSpeed;
      leftBackward = backSpeed;
      rightForward = frontSpeed;
      rightBackward = backSpeed;
      
      break;

    case 's':
    rightSpeed = 0;
    leftSpeed = 0;
    if (frontSpeed > 0.1) {
        frontSpeed = frontSpeed - 0.25f;
      }
    if (backSpeed < 0.99) {
        backSpeed = backSpeed + 0.25f;
      }
      leftForward = frontSpeed;
      leftBackward = backSpeed;
      rightForward = frontSpeed;
      rightBackward = backSpeed;
      break;

    case 'a':
      if (leftSpeed > 0.1) {
        leftSpeed = leftSpeed - 0.10f;
      }
      if (rightSpeed < 0.99) {
        rightSpeed = rightSpeed + 0.10f;
      }
      leftForward = rightSpeed;
      leftBackward = leftSpeed;
      rightForward = leftSpeed;
      rightBackward = rightSpeed;
      break;

    case 'd':
      if (leftSpeed < 0.99) {
        leftSpeed = leftSpeed + 0.10f;
      }
      if (rightSpeed > 0.1) {
        rightSpeed = rightSpeed - 0.10f;
      }
      leftForward = rightSpeed;
      leftBackward = leftSpeed;
      rightForward = leftSpeed;
      rightBackward = rightSpeed;
      break;
      
    case 'r':
      // Go up
      heightDown = 0;
      heightUp.write(0.5);
      wait(0.25);
      heightUp = 0;
      break;
      
    case 'f':
      // Down
      heightUp = 0;
      heightDown.write(0.5);
      wait(0.25);
      heightDown = 0;
      break;
      
    case 'g':
      // Close the grab
      grabOut = 0;
      grabIn.write(0.5);
      wait(0.3);
      grabIn = 0;
      break;
    case 'h': 
      // Open the grab
      grabIn = 0;
      grabOut.write(0.5);
      wait(0.3);
      grabOut = 0;
      break;
      
      
    case 'x':
      frontSpeed = 0;
      backSpeed = 0;
      rightSpeed = 0;
      leftSpeed = 0;
      leftForward = 0;
      rightForward = 0;
      leftBackward = 0;
      rightBackward = 0;
      grabIn = 0;
      grabOut = 0;
      heightUp = 0;
      heightDown = 0;
      break;
      
    case 'p':
        if(pilot == false){
            pilot = true;
            while(1){ 
                selfDrive();
            }
        }
    break;
    
  }
}
int main(void) {
  LED = 0.1;
  leftForward = 0;
  rightForward = 0;
  leftBackward = 0;
  rightBackward = 0;
  while (1) {

        command = BT.getc();
        BT.putc(command);
        motorControl(command);
    }
  }