 #define ENCODER_RIGHT_PIN_B 2 // (SPECIAL) Analog pin A2
 #define ENCODER_RIGHT_PIN_B_THRESHOLD 450 // Read value threshold crossing
 
+// Pan Tilt Servo pins
+#define SERVO_PAN_PIN 5 // Pan
+#define SERVO_TILT_PIN 6 // Tilt
+
+// Pan servo center min/max range (0-180 degrees with 90 degrees centred)
+#define SERVO_PAN_MIN 40
+#define SERVO_PAN_MAX 140
+
+// Tilt servo center min/max range (0-180 degrees with 90 degrees centred)
+#define SERVO_TILT_MIN 50
+#define SERVO_TILT_MAX 130
+// ServoTimer2 min/max microsecond delays 544us - 2400us ~ 0 - 180 degrees
+#define ST2_MIN 544
+#define ST2_MAX 2400
+
+#define PAN_TILT_MOVE_DELAY 2 // ms delay for smooth movement
 
 // ROS
 ros::NodeHandle nh;
 geometry_msgs::Twist twist_msg;
 
 ZumoMotors motors; // Motor controller
-Timer t;
-
+// Timer t;
+
+// Pan Tilt variables
+uint8_t pan, tilt;
+
+void set_position_callback(const rosserial_arduino::pan_tilt& msg)
+{
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+  pan = msg.pan;
+  tilt = msg.tilt;
+  
+  moveTo(int(pan), int(tilt));
+}
+
+// Set up pan/tilt subscriber
+ros::Subscriber<rosserial_arduino::pan_tilt> pan_tilt_sub("pantilt/set_position", set_position_callback);
+
+// Servo objects
+ServoTimer2 servo_pan;
+ServoTimer2 servo_tilt;
+
+// Servo positions (0-180 degrees with 90 degrees centred)
+int pan_pos = 0;
+int tilt_pos = 0;
 
 /// Left Encoder
 int encoder_left_pos = 0;

#define USER_PUSHBUTTON_PIN 12
#define LED_PIN 13

// Encoders
#define ENCODER_LEFT_PIN_A 2
#define ENCODER_LEFT_PIN_B 11

  // Initial speed is zero
  drive_stop();

  // Attach Pan Tilt Servos
  servo_pan.attach(SERVO_PAN_PIN);
  servo_tilt.attach(SERVO_TILT_PIN);

  // Init pan/tilt to center
  setZero();

  nh.initNode();
  nh.advertise(battery_pub); // Battery Voltage Publisher

  nh.subscribe(cmd_vel_topic); // Command Velocity Subscriber
  nh.subscribe(pan_tilt_sub); // Pan/Tilt Subscriber

  
  last_battery_published = millis();
}

void loop()
