     ros_param_helper.getParam("publish_teensy_debug_info_freq_hz", 1);
 
   cmd_vel_timeout_threshold =
-    ros_param_helper.getParam("cmd_vel_timeout_threshold", .5);
+    ros_param_helper.getParam("cmd_vel_timeout_threshold", 0.5);
+
+  motor_controller_cmd_timeout = 
+    ros_param_helper.getParam("motor_controller_cmd_timeout", 0.5);
 
   ticks_per_radian = 
     ros_param_helper.getParam("victoria_ticks_per_radian", 9072);

HardwareSerial trex = HardwareSerial();
bool trex_err;

// Last motor speed set on motors.
double motor_left_speed;
double motor_right_speed;

  
  // Start serial port to TRex
  trex.begin(19200);
  while (!trex) { }

  // Set serial timeout to motor_controller_cmd_timeout
  trex.write(0xAF); // Set configuration parameter
  trex.write(0x07); // Serial timeout parameter
  byte timeout = max(127, motor_controller_cmd_timeout * 10);
  trex.write(timeout); // timeout in 10ths of a second
  trex.write(0x55); // constant format byte 1
  trex.write(0x2A); // constant format byte 2
  while (!trex.available()) {}
  return (trex.read() == 0x00);
}

// Callback for blink
