   // Measure left encoder manually
   measureLeftEncoder();
   
-  // Timer updates
-  t.update();
+  // Publish battery voltage every second
+  if (millis() - last_battery_published > BATTERY_VOLTAGE_PUBLISH_RATE)
+  {
+    publishBatteryVoltage();
+    last_battery_published = millis();
+  }
 
   nh.spinOnce();
 }
 
 ////////////////////////////////////////////////////////////////////////////////
+// Motor drive speeds
+int vl = 0, vr = 0;
 
 // 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;
+  vl = (cmd_x - cmd_th * WHEEL_OFFSET_M / 2.0) * MAX_MOTOR_SPEED / MAX_REAL_SPEED;
+  vr = (cmd_x + cmd_th * WHEEL_OFFSET_M / 2.0) * MAX_MOTOR_SPEED / MAX_REAL_SPEED;
+  drive();
+}
+
+// Drive left and right wheel motors vased on vl and vr
+void drive() {
   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);
+  vl = 0;
+  vr = 0;
+  drive();
 }
 
 // Battery Voltage

#include <ros.h>
#include <ZumoMotors.h>
#include "Timer.h"
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>

// Distance between both wheels
#define WHEEL_OFFSET_M 0.1 // 10cm

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

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