 
 //#define BLUETOOTH
 //Servo Ports
-#define SERVO_TURN 4    //Turn Servo
-#define SERVO_LIFT 5    //Lift Servo
-#define SERVO_GRAB 6    //Grab Servo
+#define SERVO_TURN 0 //Turn Servo
+#define SERVO_LIFT 1 //Lift Servo
+#define SERVO_GRAB 2 //Grab Servo
 //PULSES
-#define TURN_BACK 165 //(BACK)Turn Servo
-#define TURN_MIDDLE 330 //(FRONT) ...
-#define TURN_FRONT 505 //(FRONT) ...
+#define TURN_BACK 165        //(BACK)Turn Servo
+#define TURN_MIDDLE 330      //(FRONT) ...
+#define TURN_FRONT 505       //(FRONT) ...
 #define LIFT_DOWN_PICKUP 340 //(DOWN)Lift Servo
-#define LIFT_DOWN_DROP 390 //(DOWN)Lift Servo
-#define LIFT_UP 510 //(UP)
-#define GRAB_CLOSE 380 //150 //(CLOSED)Grab Servo
-#define GRAB_OPEN 50 //395 //(OPEN)
-
-Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
+#define LIFT_DOWN_DROP 390   //(DOWN)Lift Servo
+#define LIFT_UP 510          //(UP)
+#define GRAB_CLOSE 380       //150 //(CLOSED)Grab Servo
+#define GRAB_OPEN 50         //395 //(OPEN)
+
+Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
 
 int posTurn = TURN_MIDDLE;
 int posLift = LIFT_UP;

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


int speed = 10;

enum jobs
{
  JOB_IDLE,
  JOB_PICKUP_BACK,
  JOB_DROP_BACK,
