 
 //#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;


int moduleJob;

int moveServo(int servo, int from, int to, int s) {
  if (to > from) {
    for (uint16_t pulselen = from; pulselen < to; pulselen++) {
      pwm.setPWM(servo, 0, pulselen);
      delay(s);
    }
  } else {
     for (uint16_t pulselen = from; pulselen > to; pulselen--) {
      pwm.setPWM(servo, 0, pulselen);
      delay(s);
    }

  return to;
}

int moveServo(int servo, int from, int to)
{
  return moveServo(servo, from, to, speed);
}

void releaseServos()
{
  pwm.setPWM(SERVO_TURN, 0, 0);
  pwm.setPWM(SERVO_GRAB, 0, 0);
  pwm.setPWM(SERVO_LIFT, 0, 0);
}

void setup()
{
  initBT();
  pwm.begin();
  pwm.setPWMFreq(50);
  pwm.setPWM(SERVO_TURN, 0, posTurn);
