calibration update

Author: askuric

examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino

@@ -8,7 +8,9 @@
 #include "encoders/calibrated/CalibratedSensor.h"
 
 // fill this array with the calibration values outputed by the calibration procedure
-float calibrationLut[100] = {0};
+float calibrationLut[50] = {0};
+float zero_electric_angle = 0;
+Direction sensor_direction = Direction::CW;
 
 // magnetic sensor instance - SPI
 MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14);
@@ -20,7 +22,7 @@ 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, 100, calibrationLut);
+CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, 50);
 
 // voltage set point variable
 float target_voltage = 2;
@@ -56,16 +58,12 @@ void setup() {
   motor.init();
 
   // Running calibration
-  // as the Lookup table (LUT) has been provided as an argument this function will not do anything
-  sensor_calibrated.calibrate(motor); 
+  // the function will setup everything for the provided calibration LUT
+  sensor_calibrated.calibrate(motor, calibrationLut, zero_electric_angle, sensor_direction);
 
   // Linking sensor to motor object
   motor.linkSensor(&sensor_calibrated);
 
-  // write the sensor direction and zero electrical angle outputed by the calibration  
-  motor.sensor_direction = Direction::CW; // replace with the value outputed by the calibration
-  motor.zero_electric_angle = 0.0;        // replace with the value outputed by the calibration
-
   // calibrated init FOC
   motor.initFOC();
 

src/encoders/calibrated/CalibratedSensor.cpp

@@ -3,10 +3,8 @@
 // CalibratedSensor()
 // sensor              - instance of original sensor object
 // n_lut               - number of samples in the LUT
-// lut                 - pointer to the LUT array
-CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float* lut) : _wrapped(wrapped) {
+CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut) : _wrapped(wrapped) {
 	this->n_lut = n_lut;
-	this->calibrationLut = lut;
 };
 
 CalibratedSensor::~CalibratedSensor() {
@@ -36,7 +34,7 @@ float CalibratedSensor::getSensorAngle()
 	// Calculate the resolution of the LUT in radians
     float lut_resolution = _2PI / n_lut;
     // Calculate LUT index
-    int lut_index = raw_angle / lut_resolution + bucket_offset;
+    int lut_index = raw_angle / lut_resolution;
 
     // Get calibration values from the LUT
     float y0 = calibrationLut[lut_index];
@@ -86,49 +84,59 @@ void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks
 
 }
 
-void CalibratedSensor::calibrate(FOCMotor &motor)
+void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction)
 {
-	
-	if(this->calibrationLut != nullptr){
-		motor.monitor_port->println("Using existing LUT.");
+	// 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.");
 	}
+
+	// Calibration variables
 
     // Init inital angles
     float theta_actual = 0.0;
     float avg_elec_angle = 0.0;
+	// set the inital electric angle to 0
+    float elec_angle = 0.0;
 
+	// Calibration parameters
+	// The motor will take a n_pos samples per electrical cycle
+	// which amounts to n_ticks (n_pos * motor.pole_pairs) samples per mechanical rotation
+	// Additionally, the motor will take n2_ticks steps to reach any of the n_ticks posiitons
+	// incrementing the electrical angle by deltaElectricalAngle each time
 	int n_pos = 5;
 	int _NPP = motor.pole_pairs;								      // number of pole pairs which is user input
 	const int n_ticks = n_pos * _NPP;							      // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
 	const int n2_ticks = 5;										      // increments between saved samples (for smoothing motion)
 	float deltaElectricalAngle = _2PI * _NPP / (n_ticks * n2_ticks);  // Electrical Angle increments for calibration steps
-	float error[n_ticks] = {0.0};	         							  // pointer to error array (average of forward & backward)
-	const int window = 5;			 // window size for moving average filter of raw error
-	// set the electric angle to 0
-    float elec_angle = 0.0;
+	float error[n_ticks] = {0.0};	         						  // pointer to error array (average of forward & backward)
+	// The fileter window size is set to n_pos - one electrical cycle 
+	// important for cogging filtering !!!
+	const int window = n_pos; // window size for moving average filter of raw error
+	
 
 	// find the first guess of the motor.zero_electric_angle 
 	// and the sensor direction
 	// updates motor.zero_electric_angle
 	// updates motor.sensor_direction
-	bool skip_align_current_sense = false;
-	if(motor.current_sense != nullptr){
-		skip_align_current_sense = motor.current_sense->skip_align;
-		motor.current_sense->skip_align = true;
-	}
+	// temporarily unlink the sensor and current sense
+	CurrentSense *current_sense = motor.current_sense;
+	motor.current_sense = nullptr;
 	motor.linkSensor(&this->_wrapped);
 	if(!motor.initFOC()){
 		motor.monitor_port->println("Failed to align the sensor.");
 		return;
 	}
-	if(motor.current_sense != nullptr){
-		motor.current_sense->skip_align = skip_align_current_sense;
-	}
+	// link back the sensor and current sense
 	motor.linkSensor(this);
+	motor.linkCurrentSense(current_sense);
 
 	// Set voltage angle to zero, wait for rotor position to settle
 	// keep the motor in position while getting the initial positions
@@ -177,8 +185,12 @@ void CalibratedSensor::calibrate(FOCMotor &motor)
 		}
 		zero_angle_prev = zero_angle;
 		avg_elec_angle += zero_angle/n_ticks;
-	}
 
+		motor.monitor_port->print(">error:");
+		motor.monitor_port->println(zero_angle);
+		motor.monitor_port->print(">zero_angle:");
+		motor.monitor_port->println(avg_elec_angle);
+	}
 	_delay(2000);
 
 	/*
@@ -212,6 +224,11 @@ void CalibratedSensor::calibrate(FOCMotor &motor)
 		}
 		zero_angle_prev = zero_angle;
 		avg_elec_angle += zero_angle/n_ticks;
+
+		motor.monitor_port->print(">error:");
+		motor.monitor_port->println(zero_angle);
+		motor.monitor_port->print(">zero_angle:");
+		motor.monitor_port->println(avg_elec_angle);
 	}
 
 	// get post calibration mechanical angle.

src/encoders/calibrated/CalibratedSensor.h

@@ -11,7 +11,7 @@ class CalibratedSensor: public Sensor{
 
 public:
     // constructor of class with pointer to base class sensor and driver
-    CalibratedSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr);
+    CalibratedSensor(Sensor& wrapped, int n_lut = 200);
 
     ~CalibratedSensor();
 
@@ -23,11 +23,10 @@ class CalibratedSensor: public Sensor{
     /**
     * Calibrate method computes the LUT for the correction
     */
-    virtual void calibrate(FOCMotor& motor);
+    virtual void calibrate(FOCMotor& motor, float* lut = nullptr, float zero_electric_angle = 0.0, Direction senor_direction= Direction::CW);
 
     // voltage to run the calibration: user input
-    float voltage_calibration = 1;                        
-    int bucket_offset = 0;
+    float voltage_calibration = 1;    
 protected:
 
     /**
@@ -48,7 +47,7 @@ class CalibratedSensor: public Sensor{
     void alignSensor(FOCMotor &motor);
     void filter_error(float* error, float &error_mean, int n_ticks, int window);
 
-     // lut size, currently constan. Perhaps to be made variable by user?
+     // lut size - settable by the user
     int  n_lut { 200 } ;
     // create pointer for lut memory 
     // depending on the size of the lut

src/encoders/calibrated/README.md

@@ -84,7 +84,7 @@ Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-
 ## EDIT March 2025
 
 The code has been rewritten to reduce its memory footprint and allow more flexible Lookup table (LUT) sizing. 
-Additionally, the calibrated sensor class now supports providing the saved LUT as a parameter to the constructor. This allows you to save the LUT and load it on startup to avoid recalibration on each startup.
+Additionally, the calibrated sensor class now supports providing the saved LUT as a paramer for calibration. This allows you to save the LUT and load it on startup to avoid recalibration on each startup.
 
 The LUT and sensor's zero angle and direction are outputed by the calibration process to the Serial terminal. So you can copy and paste them into your code.
 
@@ -96,29 +96,31 @@ Your code will look something like this:
 const N_LUT = 100;
 // Lookup table that has been ouptut from the calibration process
 float calibrationLut[N_LUT] = {...};
+float zero_eletrical_angle = 0.0;
+Direction sensor_direction = Direction::CW;
 
 // provide the sensor class and the number of points in the LUT
-CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT, calibrationLut);
+CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT);
 
 ... 
 
 void setup() {
   ...
-  // as LUT is provided this function does not need to be called
-  sensor_calibrated.calibrate(motor); 
+  // as LUT is provided to this function
+  sensor_calibrated.calibrate(motor, calibrationLut, zero_eletrical_angle, sensor_direction);
   ...
 
   motor.linkSensor(&sensor_calibrated);
 
-  
-  // write the sensor direction and zero electrical angle outputed by the calibration  
-  motor.sensor_direction = Direction::CW; // replace with the value outputed by the calibration
-  motor.zero_electric_angle = 0.0;        // replace with the value outputed by the calibration
-
   ... 
   motor.initFOC();
   ....
 }
 
 
 ```
+
+## Future work
+
+- Reduce the LUT size by using a more efficient LUT type - maybe pass to uint16_t
+- Use a more eficient LUT interpolation method - maybe a polynomial interpolation