From 5dc056317bc4d88229573f358d9848f09c9cd565 Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Mon, 16 Jun 2025 20:29:19 +0200 Subject: [PATCH] CalibratedSensor: allow saving of lut at runtime Before this change you needed to recompile the code with the calibration data. This change allows you to write code that saves the data to non volatile storage and load on the next power cycle --- .../calibration_save/calibration_save.ino | 21 +++++++--- src/encoders/calibrated/CalibratedSensor.cpp | 38 ++++++++++--------- src/encoders/calibrated/CalibratedSensor.h | 16 +++++--- 3 files changed, 47 insertions(+), 28 deletions(-) diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino index 5a8ee68..da49b73 100644 --- a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -1,6 +1,10 @@ /** * The example demonstrates the calibration of the magnetic sensor with the calibration procedure and saving the calibration data. * So that the calibration procedure does not have to be run every time the motor is powered up. + * + * 1. Run this Sketch as is and wait for the calibration data to be generated and printed over Serial. + * 2. Then copy the output from Serial to calibrationLut, zero_electric_angle and sensor_direction + * 3. Change values_provided to true and run the Sketch again to see the motor skipping the calibration. */ #include @@ -10,7 +14,9 @@ // fill this array with the calibration values outputed by the calibration procedure float calibrationLut[50] = {0}; float zero_electric_angle = 0; -Direction sensor_direction = Direction::CW; +Direction sensor_direction = Direction::UNKNOWN; + +const bool values_provided = false; // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14); @@ -22,7 +28,8 @@ StepperDriver4PWM driver = StepperDriver4PWM(10, 9, 5, 6,8); // argument 1 - sensor object // argument 2 - number of samples in the LUT (default 200) // argument 3 - pointer to the LUT array (defualt nullptr) -CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, 50); +CalibratedSensor sensor_calibrated = CalibratedSensor( + sensor, sizeof(calibrationLut) / sizeof(calibrationLut[0])); // voltage set point variable float target_voltage = 2; @@ -57,9 +64,13 @@ void setup() { // initialize motor motor.init(); - // Running calibration - // the function will setup everything for the provided calibration LUT - sensor_calibrated.calibrate(motor, calibrationLut, zero_electric_angle, sensor_direction); + if(values_provided) { + motor.zero_electric_angle = zero_electric_angle; + motor.sensor_direction = sensor_direction; + } else { + // Running calibration + sensor_calibrated.calibrate(motor); + } // Linking sensor to motor object motor.linkSensor(&sensor_calibrated); diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 896e453..089b17a 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -3,12 +3,15 @@ // CalibratedSensor() // sensor - instance of original sensor object // n_lut - number of samples in the LUT -CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut) : _wrapped(wrapped) { - this->n_lut = n_lut; -}; +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float *lut) + : _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) { + }; CalibratedSensor::~CalibratedSensor() { // delete calibrationLut; + if(allocated) { + delete []calibrationLut; + } }; // call update of calibrated sensor @@ -28,6 +31,9 @@ void CalibratedSensor::init() // Retrieve the calibrated sensor angle float CalibratedSensor::getSensorAngle() { + if(!calibrationLut) { + return _wrapped.getMechanicalAngle(); + } // raw encoder position e.g. 0-2PI float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI); raw_angle += raw_angle < 0 ? _2PI:0; @@ -86,19 +92,15 @@ 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, int settle_time_ms) +void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) { // if the LUT is already defined, skip the calibration - if(lut != nullptr){ - motor.monitor_port->println("Using the provided LUT."); - motor.zero_electric_angle = zero_electric_angle; - motor.sensor_direction = senor_direction; - this->calibrationLut = lut; - return; - }else{ - this->calibrationLut = new float[n_lut](); - motor.monitor_port->println("Starting Sensor Calibration."); + + if(calibrationLut == NULL) { + allocated = true; + calibrationLut = new float[n_lut]; } + motor.monitor_port->println("Starting Sensor Calibration."); // Calibration variables @@ -174,11 +176,11 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri _delay(settle_time_ms); _wrapped.update(); // calculate the error - theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init); + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); error[i] = 0.5 * (theta_actual - elec_angle / _NPP); // calculate the current electrical zero angle - float zero_angle = (motor.sensor_direction*_wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); zero_angle = _normalizeAngle(zero_angle); // remove the 2PI jumps if(zero_angle - zero_angle_prev > _PI){ @@ -214,10 +216,10 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri _delay(settle_time_ms); _wrapped.update(); // calculate the error - theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init); + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); error[i] += 0.5 * (theta_actual - elec_angle / _NPP); // calculate the current electrical zero angle - float zero_angle = (motor.sensor_direction*_wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); zero_angle = _normalizeAngle(zero_angle); // remove the 2PI jumps if(zero_angle - zero_angle_prev > _PI){ @@ -272,7 +274,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri if (ind < 0) ind += n_lut; calibrationLut[ind] = (float)(error[(int)(i * dn)] - error_mean); // negate the error if the sensor is in the opposite direction - calibrationLut[ind] = motor.sensor_direction * calibrationLut[ind]; + calibrationLut[ind] = (int)motor.sensor_direction * calibrationLut[ind]; } motor.monitor_port->println(""); _delay(1000); diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index bc4ac13..29a0a8d 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -10,8 +10,13 @@ class CalibratedSensor: public Sensor{ public: - // constructor of class with pointer to base class sensor and driver - CalibratedSensor(Sensor& wrapped, int n_lut = 200); + /** + * @brief Constructor of class with pointer to base class sensor and driver + * @param wrapped the wrapped sensor which needs calibration + * @param n_lut the number of entries in the lut + * @param lut the look up table (if null, the lut will be allocated on the heap) + */ + CalibratedSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr); ~CalibratedSensor(); @@ -23,7 +28,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, int settle_time_ms = 30); + virtual void calibrate(FOCMotor& motor, int settle_time_ms = 30); // voltage to run the calibration: user input float voltage_calibration = 1; @@ -49,9 +54,10 @@ class CalibratedSensor: public Sensor{ // lut size - settable by the user int n_lut { 200 } ; - // create pointer for lut memory + // pointer for lut memory // depending on the size of the lut - // will be allocated in the constructor + // will be allocated in the calibrate function if not given. + bool allocated; float* calibrationLut; };