   Serial.print(servoFix);
   Serial.println(END_CMD_CHAR);
 
-  // Handle servo data
-  int newServoPwm = getServoPwm(newSide, newAngle, servoFix);
-  if (servoPwm != newServoPwm) {
-    servoPwm = newServoPwm;
-    servo.write(servoPwm);
+  // Side or angle has changed
+  if (side != newSide || angle != newAngle) {
+    setServo(newSide, newAngle);
+
+    side = newSide;
+    angle = newAngle;
     Serial.println("Servo moved");
   }
-  
-  // Handle engine data
+
+   // Direction has changed
   if (direction != newDirection) {
-    // slow down
-    for (int i = speed ; i >= MIN_ENGINE_PWM ; i--) {
+    // Slow down
+    for (int speedIterator = speed ; speedIterator >= MIN_ENGINE_SPEED ; setEnginePower(direction, --speedIterator)) {
       delay(10);
-      enginePwm = getEnginePwm(direction, i);
-      analogWrite(enginePowerPin, enginePwm);
     }
 
-    // set direction
-    digitalWrite(engineForwardPin, newDirection == DIRECTION_FORWARD ? HIGH : LOW);
-    digitalWrite(engineBackwardPin, newDirection == DIRECTION_BACKWARD ? HIGH : LOW);
+    // Change direction
+    setDirectionPins(newDirection);
+    
+    // Speed up
+    for (int speedIterator = MIN_ENGINE_SPEED ; speedIterator <= newSpeed ; setEnginePower(newDirection, speedIterator++)) {
+      delay(10);
+    }
+
+    speed = newSpeed;
     direction = newDirection;
-    
-    // speed up
-    for (int i = MIN_ENGINE_PWM ; i <= newSpeed ; i++) {
-      delay(10);
-      enginePwm = getEnginePwm(newDirection, i);
-      analogWrite(enginePowerPin, enginePwm);
-    }
+    Serial.println("Direction changed");
+
+  // Only speed has changed
+  } else if (speed != newSpeed) {
+    setEnginePower(direction, newSpeed);
+
     speed = newSpeed;
-    
-    Serial.println("Direction changed");
+    Serial.print("Speed changed");
   }
+}
 
-  // Changed speed only
-  if (speed != newSpeed) {
-    speed = newSpeed;
-    enginePwm = getEnginePwm(direction, speed);
-    analogWrite(enginePowerPin, enginePwm);
-    Serial.print("Speed changed: ");
-    Serial.println(enginePwm);
-  }
+void setServo(int side, int angle) {
+  int servoPwm = getServoPwm(side, angle, servoFix);
+  servo.write(servoPwm);
+}
+
+void setDirectionPins(int direction) {
+    digitalWrite(engineForwardPin, direction == DIRECTION_FORWARD ? HIGH : LOW);
+    digitalWrite(engineBackwardPin, direction == DIRECTION_BACKWARD ? HIGH : LOW);
+}
+
+void setEnginePower(int direction, int speed) {
+    analogWrite(enginePowerPin, getEnginePwm(speed >= MIN_ENGINE_SPEED ? speed : 0));
 }
 
 // Returns [0 - 180]

}

// Returns [0 - 255]
int getEnginePwm(int direction, int speed) {
  return speed * 2.55;
}



int side = SIDE_RIGHT;
int angle = 0;
int servoFix = 50;

const int DIRECTION_FORWARD = 1;
