   // 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;
+    setReferenceVelocity(0, 0);
+  }
+}
+
+void setReferenceVelocity(double new_wl, double new_wr) {
+  reference_wl = new_wl;
+  reference_wr = new_wr;
 }
 
 /*

  double vl = cmd_vel_msg.linear.x - (cmd_vel_msg.angular.z * track_radius);
  double vr = cmd_vel_msg.linear.x + (cmd_vel_msg.angular.z * track_radius);

  // Calculate wheel angular velocity
  setSetPoint(vl / wheel_radius, vr / wheel_radius);
}

void doMotorController() {

  double sensed_wl = getAngularVelocityFromSamples(samples_wl);
  double sensed_wr = getAngularVelocityFromSamples(samples_wr);
  
  double wl = motor_controller_p * (reference_wl - sensed_wl);
  double wr = motor_controller_p * (reference_wr - sensed_wr);

  // Calculate motor speed between -1 and 1
  double new_left_speed = max(-1, min(1, wl / max_speed));
