   stopMotors();
 }
 
-// Vitesse entre 0 et 100
-void goBackward(int vitesse) {
-  setLeftSpeed(-vitesse);
-  setRightSpeed(-vitesse);
-}
-
-// Vitesse entre 0 et 100
-void goForward(int vitesse) {
-  setLeftSpeed(vitesse);
-  setRightSpeed(vitesse);
+/**
+ * Go backward
+ * @param int speed - between 0 and 100
+ **/
+void goBackward(int speed) {
+  setLeftSpeed(-speed);
+  setRightSpeed(-speed);
+}
+
+/**
+ * Go forward
+ * @param int speed - between 0 and 100
+ **/
+void goForward(int speed) {
+  setLeftSpeed(speed);
+  setRightSpeed(speed);
 }
 
 // Servo control methods
+/**
+ * Do a smooth sploch! (vertical to horizontal)
+ **/
 void smoothSploch () {
   for (int i = 135; i > 30; i--) {
     starterServo.write(i);
     delay(50);
   }
   starterServo.detach();
-  //  initializeTimer1();
-}
-
+}
+
+/**
+ * Put the robot in vertical mode
+ **/
 void prepareToStart () {
   starterServo.attach(starterServoPin);
   starterServo.write(135);
 }
 
-/**
- * Interrupt service routine
- **/
-void manageMotors()
-{
-  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();
-
-}
 
 void setup() {
   // Motor Right

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

        Serial.println("Step right!");
      }
      break;
      
    case 'f':
    case 'F': // Front direction
      Serial.println("Go!");
