   return cm;
 }
 
-void ultraCheck(void) {
-  int i = ultraCompteur % 10;
-  int first_i = (ultraCompteur + 1) % 10;
+/**
+ * Motors livecycle
+ **/
+void _motorsLivecycle()
+{
+  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();
+
+}
+
+/**
+ * Ultrason livecycle
+ **/
+void _ultrasonLivecycle() {
+  int i = ultraCount % 10;
+  int j = (ultraCount + 1) % 10;
   ultraBuffer[i] = ultraMesure();
   ultraSum += ultraBuffer[i];
-  ultraSum -= ultraBuffer[first_i];
-  Serial.println(ultraSum / 10);
-
-  if (ultraSum / 10 < 20) {
-    digitalWrite(13, HIGH);
-  } else {
-    digitalWrite(13, LOW);
-  }
-  ultraCompteur++;
-}
-
+  ultraSum -= ultraBuffer[j];
+  if (debugUltrason) Serial.println(ultraSum / 10);
+
+  hasEnnemy = (ultraSum / 10 < minDistanceInCm);
+  ultraCount++;
+}
+
+/**
+ * Returns true if robot is on green side.
+ **/
 boolean isGreen() {
-  return digitalRead(pinColor);
-}
-
-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);
-}
-
-volatile int loopCounter = 0;
-volatile int leftSpeed = 100;
-volatile int rightSpeed = 100;
+  return digitalRead(colorPin);
+}
 
 // Motors controls methods
+
+/**
+ * Set left motor speed
+ * @param int speed - between -100 and 100
+ **/
 void setLeftSpeed(int speed) {
   leftSpeed = speed + 100;
 }
 
+/**
+ * Set right motor speed
+ * @param int speed - between -100 and 100
+ *
+ **/
 void setRightSpeed(int speed) {
   rightSpeed = speed + 100;
 }
 
+/**
+ * Stop motors (freewheel)
+ **/
 void stopMotors() {
   rightSpeed = 100;
   leftSpeed = 100;

    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!!");
