   double wr = getAngularVelocityFromSamples(samples_wr);
   
   // Convert to wheel linear velocity
-  double vl = wl * WHEEL_RADIUS;
-  double vr = wr * WHEEL_RADIUS;
+  double vl = wl * wheel_radius;
+  double vr = wr * wheel_radius;
   
   // Calculate robot vx and vth
   double vx = (vl + vr)/2;
-  double vth = (vr - vl)/(2 * TRACK_RADIUS);
+  double vth = (vr - vl)/(2 * track_radius);
   
   // Calculate the deltas since
   double dt = current_time.toSec() - last_raw_odom_publish_time.toSec();

  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));
  double new_right_speed = max(-1, min(1, wr / MAX_SPEED));

  setMotors(new_left_speed, new_right_speed);
}

  last_cmd_vel_time = ros_nh.now();
  
  // Calculate wheel linear velocity
  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() {
