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

Author: Schnilz

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 <SimpleFOC.h>
@@ -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);

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

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