   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);
+  // Calculate and set  refrence wheel angular velocity
+  setReferenceVelocity(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 * (set_point_wl - sensed_wl);
  double wr = motor_controller_p * (set_point_wr - sensed_wr);

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

  encoder_right_pos = 0.0;
  motor_left_speed = 0.0;
  motor_right_speed = 0.0;
  reference_wl = 0.0;
  reference_wr = 0.0;
  pose.x = 0.0;
  pose.y = 0.0;
  pose.theta = 0.0;
