 void setup() {
   pinMode(engineForwardPin, OUTPUT);
   pinMode(engineBackwardPin, OUTPUT);
+  pinMode(enginePowerPin, OUTPUT);
+  pinMode(servoPin, OUTPUT);
 
   Serial.begin(9600);
   bluetooth.begin(9600);
   servo.attach(servoPin);
 
-  digitalWrite(engineForwardPin, HIGH);
-  digitalWrite(engineBackwardPin, LOW);
-  analogWrite(servoPin, getServoPwm(side, angle, servoFix));
-  analogWrite(enginePowerPin, getEnginePwm(direction, speed));
+  setServo(side, angle);
+  setDirectionPins(direction);
+  setEnginePower(direction, speed);
 }
 
 void loop() {

  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);
    Serial.println("Servo moved");
  }
  
  // Handle engine data
  if (direction != newDirection) {
    // slow down
    for (int i = speed ; i >= MIN_ENGINE_PWM ; i--) {
      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);
    direction = newDirection;
    
    // speed up
    for (int i = MIN_ENGINE_PWM ; i <= newSpeed ; i++) {
      delay(10);
      enginePwm = getEnginePwm(newDirection, i);
      analogWrite(enginePowerPin, enginePwm);
    }
    speed = newSpeed;
    
    Serial.println("Direction changed");
  }

  // Changed speed only
  if (speed != newSpeed) {
    speed = newSpeed;
    enginePwm = getEnginePwm(direction, speed);
    analogWrite(enginePowerPin, enginePwm);
    Serial.print("Speed changed: ");
    Serial.println(enginePwm);
  }
}

// Returns [0 - 180]


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

const int DIRECTION_FORWARD = 1;
