       stopMotors();
       break;
 
+    case 'd':
+    case 'D':
+      Serial.println("Right on");
+      setRightSpeed( 30);
+      break;
+
+    case 'l':
+    case 'L':
+      Serial.println("left on");
+      setLeftSpeed( 30);
+      break;
+
+
     default:
       Serial.print("Unkwnown command (");
       Serial.print(serialInput[0]);

  pinMode(rightMotorPin1, OUTPUT);
  pinMode(rightMotorPin2, OUTPUT);

  // Ultrason
  pinMode(ultraTrig, OUTPUT);
  digitalWrite(ultraTrig, LOW);
  pinMode(ultraEcho, INPUT);
  ultraCompteur = 1;
  ultraSum = 0;
  
  //DebugLed
  pinMode(13,OUTPUT);

  // Serial
  Serial.begin(9600);

  initializeTimer1();
}

void loop() {
  // TODO Script to win here ;)
  int i = ultraCompteur % 10;
  int first_i = (ultraCompteur + 1) % 10;
  ultraBuffer[i] = ultraMesure();
  ultraSum += ultraBuffer[i];
  ultraSum -= ultraBuffer[first_i];

if (ultraSum/10 < 20){
  digitalWrite(13,HIGH);
  } else {
    digitalWrite(13,LOW);
    }
  ultraCompteur++;


}

// Serial

/**
 * Interrupt service routine
 **/
void manageMotors()
{
  loopCounter = (loopCounter + 1) % 200;

  if (rightSpeed == 100) stopRight();
  else if (loopCounter >= rightSpeed)forwardRight();
  else backwardRight();
  
  if (leftSpeed == 100) stopLeft();
  else if (loopCounter >= leftSpeed)forwardLeft();
  else backwardLeft();

}

void setup() {
