 #include <Servo.h>
 
-#define rightMotorPin1 12
-#define rightMotorPin2 11
-
-#define leftMotorPin1 9
-#define leftMotorPin2 10
-
-#define ultraTrig 14
-#define ultraEcho 15
-
-#define starterServoPin 18
+#define rightMotorPin1 6
+#define rightMotorPin2 5
+
+#define leftMotorPin1 3
+#define leftMotorPin2 4
+
+IntervalTimer motorTimer;
+
+#define ultraTrig 20
+#define ultraEcho 21
+
+#define starterServoPin 19
 
 #define rotateTime 600
 
+IntervalTimer ultraTimer;
 int ultraBuffer[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 int ultraCompteur;
 int ultraSum;

/**
 * Interrupt service routine
 **/
ISR(TIMER1_OVF_vect)
{
  loopCounter = (loopCounter + 1) % 200;
  TCNT1 = 64286; // preload timer

  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() {

      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]);
