   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));
 }
 
 void loop() {

  Serial.print(newSide);
  Serial.print(" ");
  Serial.print(newAngle);
  Serial.println(END_CMD_CHAR);

  // Handle servo data
  int newServoPwm = getServoPwm(newSide, newAngle);
  if (servoPwm != newServoPwm) {
    servoPwm = newServoPwm;
    servo.write(servoPwm);

  
  // 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 getServoPwm(int side, int angle, int servoFix) {
  angle *= 0.9;
  int result = 90;
  switch (side) {
    case SIDE_RIGHT:
      result -= angle;
      break;
    case SIDE_LEFT:
      result += angle;
      break;
  }
  result += servoFix;
  return result;
}

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