   // Motor Left
   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);


    case 'r':
    case 'R': // Rotation
      if (serialInput[1] == 'L' || serialInput[1] == 'l'){
        rotateLeft();
        Serial.println("Step left!");
        }
      else {
        rotateRight();
        Serial.println("Step right!");
        }
      break;
    case 'f':
    case 'F': // Front direction
    Serial.println("Go!");
      setLeftSpeed( serialInput[1] * 100 + serialInput[2] * 10 + serialInput[3]);
      setRightSpeed( serialInput[1] * 100 + serialInput[2] * 10 + serialInput[3]);
      delay(1500);


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
