Merge pull request #58 from Schnilz/dev

CalibratedSensor: Allow saving of lut at runtime

Author: runger1101001

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