From e96c5b83e44d064bf46e314bd96a70cee5f13d19 Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Thu, 15 May 2025 15:09:41 +0200 Subject: [PATCH] CalibratedSensor: settle time as parameter --- src/encoders/calibrated/CalibratedSensor.cpp | 15 ++++++++------- src/encoders/calibrated/CalibratedSensor.h | 4 ++-- 2 files changed, 10 insertions(+), 9 deletions(-) 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