 /**
  * Interrupt service routine
  **/
-ISR(TIMER1_OVF_vect)
+void manageMotors()
 {
   loopCounter = (loopCounter + 1) % 200;
-  TCNT1 = 64286; // preload timer
-
+
+  if (rightSpeed == 100) stopRight();
+  else if (loopCounter >= rightSpeed)forwardRight();
+  else backwardRight();
+  
   if (leftSpeed == 100) stopLeft();
   else if (loopCounter >= leftSpeed)forwardLeft();
   else backwardLeft();
 
-  if (rightSpeed == 100) stopRight();
-  else if (loopCounter >= rightSpeed)forwardRight();
-  else backwardRight();
-}
-
-void initializeTimer1 () {
-  noInterrupts();           // disable all interrupts
-  TCCR1A = 0;
-  TCCR1B = 0;
-
-  TCNT1 = 64286;            // preload timer
-  TCCR1B |= (1 << CS10);    // 0 prescaler
-  TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
-  interrupts();             // enable all interrupts
 }
 
 void setup() {

{
  switch (serialInput[0])
  {
    case 'i':
    case 'I': // Initialize
      initializeTimer1();
      break;

    case 'r':
    case 'R': // Rotation
      if (serialInput[1] == 'L' || serialInput[1] == 'l') {

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

  motorTimer.begin(manageMotors, 30);

  // Ultrason
  ultraTimer.begin(ultraCheck, 50000);
  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 ;)
}

// Serial
