 const int NUM_VEL_SAMPLES(10);
 double samples_wl[NUM_VEL_SAMPLES];
 double samples_wr[NUM_VEL_SAMPLES];
-int velocity_index = 0;
+int samples_w_index = 0;
 double set_point_wl;
 double set_point_wr;
 float motor_controller_p = 5.0;

  int publish_encoder_debug_info_freq_hz =
    ros_param_helper.getParam("publish_encoder_debug_info_freq_hz", 10);

  cmd_vel_timeout_threshold =
    ros_param_helper.getParam("cmd_vel_timeout_threshold", .5);

  motor_command_timeout =
    ros_param_helper.getParam("motor_command_timeout", 1);

  // Initialize all the hardware
  

  
  // Start serial port to TRex
  trex.begin(19200);
  return true;
}

// Callback for blink
