 // Command Velocity Subscriber
 ros::Subscriber<geometry_msgs::Twist> cmd_vel_topic("cmd_vel", &cmd_vel_callback);
 
-
+unsigned long last_battery_published = 0;
 ////////////////////////////////////////////////////////////////////////////////
 
 void setup()
-{
-  // initialize serial:
-  // Serial.begin(9600);
-  // reserve 200 bytes for the inputString:
-  // inputString.reserve(200);
-  
+{  
   pinMode(LED_PIN, OUTPUT);
   
   // Initialize Encoders

  // Measure left encoder manually
  measureLeftEncoder();
  
  // Timer updates
  t.update();

  nh.spinOnce();
}

////////////////////////////////////////////////////////////////////////////////

// cmd_x  : linear x velocity (forward velocity)
// cmd_th : angular z velocity (rotation of heading) 
void diff_drive(double cmd_x, double cmd_th) {
  int vl = (cmd_x - cmd_th * WHEEL_OFFSET_M / 2.0) * MAX_MOTOR_SPEED / MAX_REAL_SPEED;
  int vr = (cmd_x + cmd_th * WHEEL_OFFSET_M / 2.0) * MAX_MOTOR_SPEED / MAX_REAL_SPEED;
  motors.setLeftSpeed(vl);
  motors.setRightSpeed(vr);
}

// Drive left and right wheel motors for dt milliseconds, blocking
void drive(int vl, int vr) {
  motors.setLeftSpeed(vl);
  motors.setRightSpeed(vr);
}

// Stops motors
void drive_stop() {
  motors.setLeftSpeed(0);
  motors.setRightSpeed(0);
}

// Battery Voltage

        else {encoder_left_pos++;}
    }
    encoder_left_pin_A_last = encoder_right_val;
}


// Smoothly move to pan tilt pos 0-100 range for both (50 centered)
void moveTo(int pan, int tilt)
{
  if (vl != 0 || vr != 0) {
    // WARNING
    // Left Encoder will miss steps due to the delay in this while loop!
  }
  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;
    setPan( map(pan, 0, 100, SERVO_PAN_MIN, SERVO_PAN_MAX) );
    setTilt( map(tilt, 0, 100, SERVO_TILT_MIN, SERVO_TILT_MAX) );
    delay(PAN_TILT_MOVE_DELAY); // ms
  }
}


// Moves to pan and tilt in range 0 to 100, where 50 is centered
void setPanTilt(int pan, int tilt)
{
  setPan( map(pan, 0, 100, SERVO_PAN_MIN, SERVO_PAN_MAX) );
  setTilt( map(tilt, 0, 100, SERVO_TILT_MIN, SERVO_TILT_MAX) );
}

// Center Pan/Tilt servos
void setZero()
{
  moveTo(50,50);
}

// Given degree, constrain to 0-180, then convert to microsecond delay
void setPan(int pan_deg)
{
  pan_deg = constrain(pan_deg, SERVO_PAN_MIN, SERVO_PAN_MAX);
  servo_pan.write(map(pan_deg, 0, 180, ST2_MIN, ST2_MAX));
}

void setTilt(int tilt_deg)
{
  tilt_deg = constrain(tilt_deg, SERVO_TILT_MIN, SERVO_TILT_MAX);
  servo_tilt.write(map(tilt_deg, 0, 180, ST2_MIN, ST2_MAX));
}