 #include <Servo.h>
 
+// debug mode
+#define debugUltrason true
+
+// Pins
 #define rightMotorPin1 11
 #define rightMotorPin2 10
 
 #define leftMotorPin1 8
 #define leftMotorPin2 9
 
-IntervalTimer motorTimer;
+#define colorPin 2
+#define jumperPin 3
 
 #define ultraTrig 20
 #define ultraEcho 21
 
 #define starterServoPin 19
 
+// Constants
 #define rotateTimeLeft 570
 #define rotateTimeRight 500
 
-#define pinColor 2
-
-#define JUMPER 3
-bool endScript = false;
+#define minDistanceInCm 20
+
 IntervalTimer ultraTimer;
+IntervalTimer motorTimer;
+
 int ultraBuffer[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int ultraCompteur;
-int ultraSum;
-
-// Globals vars
+int ultraCount = 1;
+int ultraSum = 0;
+boolean hasEnnemy = false;
+
+volatile int loopCounter = 0;
+volatile int leftSpeed = 100;
+volatile int rightSpeed = 100;
+
 Servo starterServo;
 
 // # Low level controls
-// ## Motor Right
-void backwardRight() {
+void _backwardRight() {
   digitalWrite(rightMotorPin1, HIGH);
   digitalWrite(rightMotorPin2, LOW);
 }
 
-void forwardRight() {
+void _forwardRight() {
   digitalWrite(rightMotorPin1, LOW);
   digitalWrite(rightMotorPin2, HIGH);
 }
 
+void _stopRight() {
+  digitalWrite(rightMotorPin1, LOW);
+  digitalWrite(rightMotorPin2, LOW);
+}
+
+// ## Motor Left
+void _backwardLeft() {
+  digitalWrite(leftMotorPin1, HIGH);
+  digitalWrite(leftMotorPin2, LOW);
+}
+
+void _forwardLeft() {
+  digitalWrite(leftMotorPin1, LOW);
+  digitalWrite(leftMotorPin2, HIGH);
+}
+
+void _stopLeft() {
+  digitalWrite(leftMotorPin1, LOW);
+  digitalWrite(leftMotorPin2, LOW);
+}
+
 long ultraMesure() {
-  /* Utilisation du capteur Ultrason HC-SR04 */
-
   long lecture_echo;
   long cm;
 

    case 'd':
    case 'D':
      Serial.println("Right on");
      setRightSpeed( 30);
      break;

    case 'l':
    case 'L':
      Serial.println("left on");
      setLeftSpeed( 30);
      break;

    case 'j':
    case 'J':
      JUMPER != JUMPER;
      if (JUMPER) Serial.println("Jumper On!");
      else Serial.println("Jumper off");
      break;


    default:
      Serial.print("Unkwnown command (");

      delay(400);
      stopMotors();
      break;
      
    case 'b':
    case 'B': // Back direction
      Serial.println("Revert revert revert!");
      setLeftSpeed( (serialInput[1] * 100 + serialInput[2] * 10 + serialInput[3]) * -1);
      setRightSpeed( (serialInput[1] * 100 + serialInput[2] * 10 + serialInput[3]) * -1);
      break;
      
    case 's':
    case 'S': // Starting block
      Serial.println("Lock'n'load!");
      prepareToStart();
      break;
      
    case 'g':
    case 'G': // GO !!!
      Serial.println("Smooth splosh");
      smoothSploch();
      break;

    case 'W':
    case 'w': // Emergy stop
      Serial.println("Emergency Stop!!");
