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