RoboLab 3_2 full code

#define MAX_SPEED 250
//Left Motor connection
int enL = 11;
int in1 = 12;
int in2 = 13;
//Right Motor connection
int enR = 6;
int in3 = 7;
int in4 = 8;
//Ultrasonic Sensor connection
int trigPin 9;
int echoPin 10;

int measurement;

void setup()
{
  //Assign DDR
  pinMode(enL,OUTPUT);
  pinMode(in1,OUTPUT);
  pinMode(in2,OUTPUT);
  pinMode(enR,OUTPUT);
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  //Set motor initial state as "off"
  digitalWrite(enL,LOW);
  digitalWrite(in1,LOW);
  digitalWrite(in2,LOW);
  digitalWrite(enR,LOW);
  digitalWrite(in3,LOW);
  digitalWrite(in4,LOW);
}

/**
 * Behaviour is implemented here
 */
void loop()
{
  measurement = measureDistance();
  if(measurement > 200){
    moveForeward();
  } else if (measurement < 30) {
    stop();
  } else {
    moveForeward(map(measurement,30,200,50,255));
  }
}

/**
 * Sets drive to forward movement
 */
void moveForward()
{
  setL(250);
  setR(250);
}

/**
 * Sets drive to forward movement with speed control
 */
void moveForward(int a)
{
  if(a > MAX_SPEED) {
    setL(MAX_SPEED);
    setR(MAX_SPEED);
  } else if (a <= 0) {
    stop();
  } else {
    setL(a);
    setR(a);
  }
}

/**
 * Sets drive to backward movement
 */
void moveBackward()
{
  setL(-250);
  setR(-250);
}

/**
 * Sets drive to backward movement with speed control
 */
void moveBackward(int a)
{
  if(a > MAX_SPEED) {
    setL(MAX_SPEED * (-1));
    setR(MAX_SPEED * (-1));
  } else if (a <= 0) {
    stop();
  } else {
    setL(a *(-1));
    setR(a *(-1));
  }
}

/**
 * Sets drive to move counterclockwise
 */
void moveLeft()
{
  setL(50);
  setR(250);
}

/**
 * Sets drive to move counterclockwise with speed control
 */
void moveLeft(int a)
{
  if(a > MAX_SPEED) {
    setL(MAX_SPEED/2);
    setR(MAX_SPEED);
  } else if (a < MAX_SPEED * (-1)) {
    setL(MAX_SPEED/-2);
    setR(MAX_SPEED * (-1));
  } else {
    setL(a/2);
    setR(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  }
}

/**
 * Sets drive to move clockwise
 */
void moveRight()
{
  setL(250);
  setR(50);
}

/**
 * Sets drive to move counterclockwise with speed control
 */
void moveRight(int a)
{
  if(a > MAX_SPEED) {
    setR(MAX_SPEED/2);
    setL(MAX_SPEED);
  } else if (a < MAX_SPEED * (-1)) {
    setR(MAX_SPEED/-2);
    setL(MAX_SPEED * (-1));
  } else {
    setR(a/2);
    setL(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  }
}

/**
 * Sets drive to stop fully
 */
void stop()
{
  setL(0);
  setR(0);
}


/**
 * Left Motor control
 */
void setL(int a)
{
  digitalWrite(in1,(a>0)?HIGH:LOW);
  digitalWrite(in2,(a<0)?HIGH:LOW);
  analogWrite(enL,abs(a));
}


/**
 * Right Motor control
 */
void setR(int a)
{
  digitalWrite(in3,(a>0)?HIGH:LOW);
  digitalWrite(in4,(a<0)?HIGH:LOW);
  analogWrite(enR,abs(a));
}

/**
 * Method to measure distance using the Ultrasonic Sensor
 */
int measureDistance(){
  long duration;
  int distance;
  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance= duration*0.034/2;
  return distance;
}

/**
 * Return sign of number
 */
int getSign(int a){
  return a/abs(a);
}