 }
 
 // Handle API commands
-void handleApiCommands(String command) {
-  if (command.startsWith("pickup(0)")) {
+void handleApiCommands(String command)
+{
+  if (command.startsWith("pickup(0)"))
+  {
     moduleJob = JOB_PICKUP_BACK;
   }
-  if (command.startsWith("pickup(1)")) {
+  if (command.startsWith("pickup(1)"))
+  {
     moduleJob = JOB_PICKUP_FRONT;
   }
-  if (command.startsWith("drop(0)")) {
+  if (command.startsWith("drop(0)"))
+  {
     moduleJob = JOB_DROP_BACK;
   }
-  if (command.startsWith("drop(1)")) {
+  if (command.startsWith("drop(1)"))
+  {
     moduleJob = JOB_DROP_FRONT;
   }
 }
 
 // Listen to incomminc commands from BLUETOOTH
-void listenBT() {
+void listenBT()
+{
   String buffer = "";
-  #ifdef BLUETOOTH
-    if (Serial1.available()) {
-      buffer = Serial1.readStringUntil('\n');
-    }
-  #else
-    if (Serial.available()) {
-      buffer = Serial.readStringUntil('\n');
-    }
-  #endif
-  if (buffer != "") {
+#ifdef BLUETOOTH
+  if (Serial1.available())
+  {
+    buffer = Serial1.readStringUntil('\n');
+  }
+#else
+  if (Serial.available())
+  {
+    buffer = Serial.readStringUntil('\n');
+  }
+#endif
+  if (buffer != "")
+  {
     handleApiCommands(buffer);
   }
 }
 
 // Send message over BLUETOOTH
-void sendBT(String msg) {
-  #ifdef BLUETOOTH
-    Serial1.println(msg);
-  #else
-    Serial.println(msg);
-  #endif
+void sendBT(String msg)
+{
+#ifdef BLUETOOTH
+  Serial1.println(msg);
+#else
+  Serial.println(msg);
+#endif
 }
 
 // A small helper
-void error(const __FlashStringHelper*err) {
-  #ifdef BLUETOOTH
-    Serial1.println(err);
-  #else
-    Serial.println(err);
-  #endif
-}
-
-void initBT() {
-  #ifdef BLUETOOTH
-    Serial1.begin(9600);
-  #else
-    Serial.begin(9600);
-    while (!Serial) {
-      delay(1);
-    }
-  #endif
+void error(const __FlashStringHelper *err)
+{
+#ifdef BLUETOOTH
+  Serial1.println(err);
+#else
+  Serial.println(err);
+#endif
+}
+
+void initBT()
+{
+#ifdef BLUETOOTH
+  Serial1.begin(9600);
+#else
+  Serial.begin(9600);
+  while (!Serial)
+  {
+    delay(1);
+  }
+#endif
   sendBT("ready");
 }

      moduleJob = JOB_IDLE;
      sendBT("success(1)");
    }
    if (moduleJob == JOB_PICKUP_BACK || moduleJob == JOB_PICKUP_FRONT) {
      posGrab = moveServo(SERVO_GRAB, posGrab, GRAB_OPEN);
      if (moduleJob == JOB_PICKUP_BACK) {
        posTurn = moveServo(SERVO_TURN, posTurn, TURN_BACK);
      } else {
        posTurn = moveServo(SERVO_TURN, posTurn, TURN_FRONT);
      }
      posLift = moveServo(SERVO_LIFT, posLift, LIFT_DOWN_PICKUP);

}

// Stupid, most simple implemetation
void loop()
{
  listenBT();

  if (moduleJob != JOB_IDLE)
  {
    if (moduleJob == JOB_DROP_BACK || moduleJob == JOB_DROP_FRONT)
    {
      if (moduleJob == JOB_DROP_BACK)
      {
        posTurn = moveServo(SERVO_TURN, posTurn, TURN_BACK);
      }
      else
      {
        posTurn = moveServo(SERVO_TURN, posTurn, TURN_FRONT);
      }
      posLift = moveServo(SERVO_LIFT, posLift, LIFT_DOWN_DROP);
