   double diff_r = static_cast<double>(new_encoder_right_pos) - encoder_right_pos;
   double diff_t = current_time.toSec() - last_encoder_read_time.toSec();
 
-  samples_wl[velocity_index] = ((-diff_l)/TICKS_PER_RADIAN)/diff_t;
-  samples_wr[velocity_index] = (diff_r/TICKS_PER_RADIAN)/diff_t;
-
-  velocity_index = (velocity_index + 1) % NUM_VEL_SAMPLES;
+  samples_wl[samples_w_index] = ((-diff_l)/ticks_per_radian)/diff_t;
+  samples_wr[samples_w_index] = (diff_r/ticks_per_radian)/diff_t;
+
+  samples_w_index = (samples_w_index + 1) % NUM_VEL_SAMPLES;
   encoder_left_pos = new_encoder_left_pos;
   encoder_right_pos = new_encoder_right_pos;
   last_encoder_read_time = current_time;

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

  // Set default parameters
  // Set the serial timeout (see TRex parameter configuration documentation)
  trex.write(0xAF); // Set configuration parameter command
  trex.write(0x08); // Serial timeout parameter number
  // Timeout in 10ths of second
  byte timeout = max(127, (byte)(motor_command_timeout * 10));
  trex.write(50); 
  trex.write(0x55); // format byte 1
  trex.write(0x2A); // format byte 2

  while(!trex.available()) { }
  
  return (trex.read() == 0x00);
}

// Callback for blink

  // 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) {
    setSetPoint(0, 0);
  }
}

void setSetPoint(double new_set_point_wl, double new_set_point_wr) {
  set_point_wl = new_set_point_wl;
  set_point_wr = new_set_point_wr;
}

/*
