 double motor_right_speed;
 
 // Victoria constants
-#define TICKS_PER_RADIAN 11 // TODO(mwomack): Need the real value here
+#define TICKS_PER_RADIAN 9072 // TODO(mwomack): Verify real value
 #define TRACK_RADIUS 0.235  // in meters
 #define WHEEL_RADIUS 0.127  // in meters
 #define MAX_SPEED 4.71      // in meters/second

  last_encoder_read_time = current_time;
}

void doPublishOdom() {
  ros::Time current_time = ros_nh.now();
  double wl = getAngularVelocityFromSamples(ang_vel_samples_l);
  double wr = getAngularVelocityFromSamples(ang_vel_samples_r);
  double vl = wl * WHEEL_RADIUS;
  double vr = wr * WHEEL_RADIUS;
  double vx = (vl + vr)/2;
  double vth = (vr - vl)/(2 * TRACK_RADIUS);
  
  double dt = current_time.toSec() - last_odom_publish_time.toSec();
  double delta_x = (vx * cos(th)) * dt;
  double delta_y = (vx * sin(th)) * dt;
  double delta_th = vth * dt;

  x += delta_x;
  y += delta_y;
  th += delta_th;

  // Setup Timer callbacks
  timer.every((1.0/read_encoders_freq_hz) * 1000, doReadEncoders);
  timer.every((1.0/publish_odom_freq_hz) * 1000, doPublishOdom);
  timer.every((1.0/publish_imu_freq_hz) * 1000, doPublishImu);
  timer.every((1.0/publish_magnetometer_freq_hz) * 1000, doPublishMagnetometer);
  timer.every(BLINK_DURATION, doBlink);
  
  current_time = ros_nh.now();
