   } 
 
   // Attach Encoder Interrupts
-  attachInterrupt(0, measureLeftEncoder, CHANGE); // 0 = pin 2 interrupt
-  attachInterrupt(1, measureRightEncoder, CHANGE); // 1 = pin 3 interrupt
+  attachInterrupt(1, measureLeftEncoder, CHANGE); // 1 = pin 3 interrupt
+  attachInterrupt(0, measureRightEncoder, CHANGE); // 0 = pin 2 interrupt
 
   // Left motor power wiring is reversed.
   motors.flipLeftMotor(true);
   
   // Initial speed is zero
-  drive_stop();
-
+  drive(0, 0);
+
+  // Initialize PIDs
+  initPIDs();
 
   // Init pan/tilt to center
-  setZero();
-
+  setPanTiltZero();
+
+  nh.getHardware()->setBaud(ROS_SERIAL_BAUD_RATE);
   nh.initNode();
   nh.advertise(battery_pub); // Battery Voltage Publisher
 

{
  tilt_deg = constrain(tilt_deg, SERVO_TILT_MIN, SERVO_TILT_MAX);
  servo_tilt.write(map(tilt_deg, 0, 180, ST2_MIN, ST2_MAX));
}
  encoder_last_velocity_time = micros();
}

void updateMotorPIDs()
{
  // Update wheel velocities (pid inputs)
  updateWheelPositionVelocity();

  // run pid controller, setting pid outputs
  // pid_left.Compute();
  // pid_right.Compute();
  doLeftPID();
  doRightPID();

  // If desired motor speed is less than threshold, zero it out
  if (abs(pid_left_output) < MOTOR_SPEED_CUTOFF) { pid_left_output = 0; }
  if (abs(pid_right_output) < MOTOR_SPEED_CUTOFF) { pid_right_output = 0; }

  // Set motor speeds
  motors.setLeftSpeed(pid_left_output);
  motors.setRightSpeed(pid_right_output);
}

// Init PIDs for zumo motors
void initPIDs()
{
  // Init setpoints in case they aren't yet set
  pid_left_setpoint = 0;
  pid_right_setpoint = 0;

  // pid_left.SetMode(AUTOMATIC);
  // pid_left.SetOutputLimits(-400,400);
  // pid_left.SetSampleTime(20);
  // pid_right.SetMode(AUTOMATIC);
  // pid_right.SetOutputLimits(-400,400);
  // pid_right.SetSampleTime(20);
}

// Smoothly move to pan tilt pos 0-100 range for both (50 centered)
void moveTo(int pan, int tilt)
{
  while (abs(pan_pos - pan) + abs(tilt_pos - tilt) > 0) {
    pan_pos += pan_pos > pan ? -1 : pan_pos < pan ? 1 : 0;
    tilt_pos += tilt_pos > tilt ? -1 : tilt_pos < tilt ? 1 : 0;
