 
 // TRex motor controller
 HardwareSerial trex = HardwareSerial();
+bool trex_err;
+
+// Timeout, in seconds, for motor controller to receive a command,
+// after which the controller will shut down the motors.
+double motor_command_timeout;
+
+// Last motor speed set on motors.
 double motor_left_speed;
 double motor_right_speed;
 
 // Victoria constants
-const unsigned long TICKS_PER_RADIAN(9072); // TODO(mwomack): Verify real value
+const double TICKS_PER_RADIAN(9072);
 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
+const double MAX_SPEED(4.71);      // in radians/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;
+long encoder_left_pos;
+long encoder_right_pos;
 ros::Time last_encoder_read_time;
 
 const int NUM_VEL_SAMPLES(10);

 */
double getAngularVelocityFromSamples(double* samples) {
  double total = 0;
  for (int count = 0; count < NUM_VEL_SAMPLES; count++) {
    total += samples[count];
  }
  return total / NUM_VEL_SAMPLES;
}

// Callback for blink

 */
void doReadEncoders() {
  ros::Time current_time = ros_nh.now();
  long new_encoder_left_pos = encoder_left.read();
  long new_encoder_right_pos = encoder_right.read();

  // TODO(mwomack): handle overflow/reset to 0
  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_r[velocity_index] = (diff_r/TICKS_PER_RADIAN)/diff_t;

  velocity_index = (velocity_index + 1) % NUM_VEL_SAMPLES;
