   encoder_right_pos = 0.0;
   motor_left_speed = 0.0;
   motor_right_speed = 0.0;
-  set_point_wl = 0.0;
-  set_point_wr = 0.0;
+  reference_wl = 0.0;
+  reference_wr = 0.0;
   pose.x = 0.0;
   pose.y = 0.0;
   pose.theta = 0.0;

double samples_wl[NUM_VEL_SAMPLES];
double samples_wr[NUM_VEL_SAMPLES];
int samples_w_index = 0;
double set_point_wl;
double set_point_wr;
float motor_controller_p = 5.0;

// IMU and Magnetometer

  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));
