diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp
index 5f6c007..896e453 100644
--- a/src/encoders/calibrated/CalibratedSensor.cpp
+++ b/src/encoders/calibrated/CalibratedSensor.cpp
@@ -28,10 +28,11 @@ void CalibratedSensor::init()
 // Retrieve the calibrated sensor angle
 float CalibratedSensor::getSensorAngle()
 {
-	// raw encoder position e.g. 0-2PI
-	float raw_angle = _wrapped.getMechanicalAngle();
+    // raw encoder position e.g. 0-2PI
+    float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI);
+    raw_angle += raw_angle < 0 ? _2PI:0;
 
-	// Calculate the resolution of the LUT in radians
+    // Calculate the resolution of the LUT in radians
     float lut_resolution = _2PI / n_lut;
     // Calculate LUT index
     int lut_index = raw_angle / lut_resolution;
@@ -41,7 +42,7 @@ float CalibratedSensor::getSensorAngle()
     float y1 = calibrationLut[(lut_index + 1) % n_lut];
 
     // Linearly interpolate between the y0 and y1 values
-	// Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1)
+    // Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1)
     // If distance = 0, interpolated offset = y0
     // If distance = 1, interpolated offset = y1
     float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution;
@@ -85,7 +86,7 @@ void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks
 
 }
 
-void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction)
+void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction, int settle_time_ms)
 {
 	// if the LUT is already defined, skip the calibration
 	if(lut != nullptr){
@@ -170,7 +171,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri
 		}
 
 		// delay to settle in position before taking a position sample
-		_delay(30);
+		_delay(settle_time_ms);
 		_wrapped.update();
 		// calculate the error
 		theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init);
@@ -210,7 +211,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri
 		}
 
 		// delay to settle in position before taking a position sample
-		_delay(30);
+		_delay(settle_time_ms);
 		_wrapped.update();
 		// calculate the error
 		theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init);
diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h
index e4cfcbb..bc4ac13 100644
--- a/src/encoders/calibrated/CalibratedSensor.h
+++ b/src/encoders/calibrated/CalibratedSensor.h
@@ -23,7 +23,7 @@ class CalibratedSensor: public Sensor{
     /**
     * Calibrate method computes the LUT for the correction
     */
-    virtual void calibrate(FOCMotor& motor, float* lut = nullptr, float zero_electric_angle = 0.0, Direction senor_direction= Direction::CW);
+    virtual void calibrate(FOCMotor& motor, float* lut = nullptr, float zero_electric_angle = 0.0, Direction senor_direction= Direction::CW, int settle_time_ms = 30);
 
     // voltage to run the calibration: user input
     float voltage_calibration = 1;    
@@ -55,4 +55,4 @@ class CalibratedSensor: public Sensor{
     float* calibrationLut;
 };
 
-#endif
\ No newline at end of file
+#endif