  */
 void doReadEncoders() {
   ros::Time current_time = ros_nh.now();
-  unsigned long new_encoder_left_pos = encoder_left.read();
-  unsigned long new_encoder_right_pos = encoder_right.read();
+  long new_encoder_left_pos = encoder_left.read();
+  long new_encoder_right_pos = encoder_right.read();
 
   // TODO(mwomack): handle overflow/reset to 0
-  unsigned long diff_l = new_encoder_left_pos - encoder_left_pos;
-  unsigned long diff_r = new_encoder_right_pos - encoder_right_pos;
+  double diff_l = static_cast<double>(new_encoder_left_pos) - encoder_left_pos;
+  double diff_r = static_cast<double>(new_encoder_right_pos) - encoder_right_pos;
   double diff_t = current_time.toSec() - last_encoder_read_time.toSec();
 
-  ang_v_samples_l[velocity_index] = (diff_l/TICKS_PER_RADIAN)/diff_t;
+  ang_v_samples_l[velocity_index] = ((-diff_l)/TICKS_PER_RADIAN)/diff_t;
   ang_v_samples_r[velocity_index] = (diff_r/TICKS_PER_RADIAN)/diff_t;
 
   velocity_index = (velocity_index + 1) % NUM_VEL_SAMPLES;


// TRex motor controller
HardwareSerial trex = HardwareSerial();
double motor_left_speed;
double motor_right_speed;

// Victoria constants
const unsigned long TICKS_PER_RADIAN(9072); // TODO(mwomack): Verify real value
const double TRACK_RADIUS(0.235);  // in meters
const double WHEEL_RADIUS(0.127);  // in meters
const double MAX_SPEED(4.71);      // in meters/second

// Motor encoders
Encoder encoder_left(ENCODER_LEFT_PIN_1, ENCODER_LEFT_PIN_2);
Encoder encoder_right(ENCODER_RIGHT_PIN_1, ENCODER_RIGHT_PIN_2);
unsigned long encoder_left_pos;
unsigned long encoder_right_pos;
ros::Time last_encoder_read_time;

const int NUM_VEL_SAMPLES(10);

  ros_encoder_debug_pub.publish(&ros_debug_msg);
}

void doTeensyDebug() {
  teensy_debug_msg.trex_err = trex_err;
  teensy_debug_msg.imu_err = imu_err;
  teensy_debug_msg.mag_err = mag_err;
  teensy_debug_msg.motor_speed_left = motor_left_speed;
  teensy_debug_msg.motor_speed_right = motor_right_speed;
  teensy_debug_msg.last_cmd_vel_time = last_cmd_vel_time;
  ros_teensy_debug_pub.publish(&teensy_debug_msg);
}

/*
 * Converts a frequency in hertz to the
 * number of milliseconds to match the
