  * to be between -1 and 1. Values outside this range
  * will be pinned.
  */
-void setMotors(double motor_left_speed, double motor_right_speed) {
-  byte command_byte_left = 0xC4;
+void setMotors(double new_motor_left_speed, double new_motor_right_speed) {
+  motor_left_speed = new_motor_left_speed;
+  motor_right_speed = new_motor_right_speed;
+  
+  byte command_byte_left = 0xCC;
   if (motor_left_speed < 0) {
+    command_byte_left = command_byte_left | 0x02;
+  } else {
     command_byte_left = command_byte_left | 0x01;
+  }
+  int trex_left_speed = min(127, 127.0 * abs(motor_left_speed));
+
+  byte command_byte_right = 0xC4;
+  if (motor_right_speed < 0) {
+    command_byte_right = command_byte_right | 0x01;
   } else {
-    command_byte_left = command_byte_left | 0x02;
-  }
-  int trex_left_speed = min(127, 127.0 * abs(motor_left_speed));
-
-  byte command_byte_right = 0xCC;
-  if (motor_right_speed < 0) {
     command_byte_right = command_byte_right | 0x02;
-  } else {
-    command_byte_right = command_byte_right | 0x01;
   }
   int trex_right_speed = min(127, 127.0 * abs(motor_right_speed));
   


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

  // If no cmd_vel messages withing threshold time, something is wrong, stop the robot.
  cmd_vel_timeout_stop = (current_time.toSec() - last_cmd_vel_time.toSec()) > cmd_vel_timeout_threshold;
  if (cmd_vel_timeout_stop) {
    setMotors(0, 0);
  }
}

/*
 * Callback used by ros_cmd_vel_sub to handle cmd_vel commands, 
 * set the motor speed.
 */
void cmdVelCallback(const geometry_msgs::Twist& cmd_vel_msg) {
  last_cmd_vel_time = ros_nh.now();
