From 189a54cbc65cd3604fae61cc3e627ad7d737149c Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 10 Mar 2025 13:49:41 +0100 Subject: [PATCH 01/10] wip --- src/encoders/calibrated/CalibratedSensor.cpp | 489 ++++++++++-------- .../calibrated/CalibratedSensor.cpp.old | 257 +++++++++ src/encoders/calibrated/CalibratedSensor.h | 29 +- .../calibrated/CalibratedSensor.h.old | 61 +++ 4 files changed, 600 insertions(+), 236 deletions(-) create mode 100644 src/encoders/calibrated/CalibratedSensor.cpp.old create mode 100644 src/encoders/calibrated/CalibratedSensor.h.old diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index e3d579c..efb1925 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -1,257 +1,306 @@ #include "CalibratedSensor.h" - // CalibratedSensor() // sensor - instance of original sensor object -CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) -{ +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut) : _wrapped(wrapped) { + this->n_lut = n_lut; + this->calibrationLut = new float[n_lut](); }; - -CalibratedSensor::~CalibratedSensor() -{ - delete calibrationLut; +CalibratedSensor::~CalibratedSensor() { + // delete calibrationLut; }; // call update of calibrated sensor -void CalibratedSensor::update(){ - _wrapped.update(); - this->Sensor::update(); +void CalibratedSensor::update() +{ + _wrapped.update(); + this->Sensor::update(); }; // Retrieve the calibrated sensor angle -void CalibratedSensor::init() { - // assume wrapped sensor has already been initialized - this->Sensor::init(); // call superclass init +void CalibratedSensor::init() +{ + // assume wrapped sensor has already been initialized + this->Sensor::init(); // call superclass init } // Retrieve the calibrated sensor angle -float CalibratedSensor::getSensorAngle(){ - // raw encoder position e.g. 0-2PI - float rawAngle = _wrapped.getMechanicalAngle(); - - // index of the bucket that rawAngle is part of. - // e.g. rawAngle = 0 --> bucketIndex = 0. - // e.g. rawAngle = 2PI --> bucketIndex = 128. - int bucketIndex = floor(rawAngle/(_2PI/n_lut)); - float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex); - - // Extract the lower and upper LUT value in counts - float y0 = calibrationLut[bucketIndex]; - float y1 = calibrationLut[(bucketIndex+1)%n_lut]; - - // Linear Interpolation Between LUT values y0 and y1 using the remainder - // If remainder = 0, interpolated offset = y0 - // If remainder = 2PI/n_lut, interpolated offset = y1 - float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; - - // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians - float calibratedAngle = rawAngle+interpolatedOffset; - - // return calibrated angle in radians - return calibratedAngle; +float CalibratedSensor::getSensorAngle() +{ + // raw encoder position e.g. 0-2PI + float raw_angle = _wrapped.getMechanicalAngle(); + + // 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; + + // Get calibration values from the LUT + float y0 = calibrationLut[lut_index]; + float y1 = calibrationLut[(lut_index + 1) % n_lut]; + + // Linearly interpolate between the y0 and y1 values + // Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1) + // If distance = 0, interpolated offset = y0 + // If distance = 1, interpolated offset = y1 + float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution; + float offset = (1 - distance) * y0 + distance * y1; + + // Calculate the calibrated angle + return raw_angle - offset; } -void CalibratedSensor::calibrate(BLDCMotor& motor){ - - Serial.println("Starting Sensor Calibration."); - - int _NPP = motor.pole_pairs; // number of pole pairs which is user input - const int n_ticks = 128*_NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) - const int n2_ticks = 40; // increments between saved samples (for smoothing motion) - float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks); // Electrical Angle increments for calibration steps - float* error_f = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_f = new float[n_ticks](); // pointer to raw forward position - float* error_b = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_b = new float[n_ticks](); // pointer to raw backword position - float* error = new float[n_ticks](); // pointer to error array (average of forward & backward) - float* error_filt = new float[n_ticks](); // pointer to filtered error array (low pass filter) - const int window = 128; // window size for moving average filter of raw error - motor.zero_electric_angle = 0; // Set position sensor offset - - // find natural direction (this is copy of the init code) - // move one electrical revolution forward - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0f; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - // take and angle in the middle - _wrapped.update(); - float mid_angle = _wrapped.getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0f ; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } + +// find the first guess of the motor.zero_electric_angle +// and the sensor direction +// updates motor.zero_electric_angle +// updates motor.sensor_direction +void CalibratedSensor::alignSensor(FOCMotor &motor){ + motor.zero_electric_angle = 0; // Set position sensor offset + + // find natural direction (this is copy of the init code) + // move one electrical revolution forward + for (int i = 0; i <= 500; i++) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + // take and angle in the middle + _wrapped.update(); + float mid_angle = _wrapped.getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >= 0; i--) + { + float angle = _3PI_2 + _2PI * i / 500.0f; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + _wrapped.update(); + float end_angle = _wrapped.getAngle(); + motor.setPhaseVoltage(0, 0, 0); + _delay(200); + + // determine the direction the sensor moved + if (mid_angle < end_angle) + { + motor.monitor_port->println("MOT: sensor_direction==CCW"); + motor.sensor_direction = Direction::CCW; + } + else + { + motor.monitor_port->println("MOT: sensor_direction==CW"); + motor.sensor_direction = Direction::CW; + } + + // align the electrical phases of the motor and sensor + // set angle -90(270 = 3PI/2) degrees + motor.setPhaseVoltage(voltage_calibration, 0, _3PI_2); + _delay(700); + // read the sensor _wrapped.update(); - float end_angle = _wrapped.getAngle(); + motor.zero_electric_angle = _normalizeAngle(_electricalAngle(motor.sensor_direction*_wrapped.getAngle(), motor.pole_pairs)); + _delay(20); + motor.monitor_port->print("Zero Electrical Angle: "); + motor.monitor_port->println(motor.zero_electric_angle); + // stop everything motor.setPhaseVoltage(0, 0, 0); - _delay(200); - // determine the direction the sensor moved - int directionSensor; - if (mid_angle < end_angle) { - Serial.println("MOT: sensor_direction==CCW"); - directionSensor = -1; - motor.sensor_direction = Direction::CCW; - - } else{ - Serial.println("MOT: sensor_direction==CW"); - directionSensor = 1; - motor.sensor_direction = Direction::CW; - - } - - //Set voltage angle to zero, wait for rotor position to settle - // keep the motor in position while getting the initial positions - motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); - _delay(1000); - _wrapped.update(); - float theta_init = _wrapped.getAngle(); - float theta_absolute_init = _wrapped.getMechanicalAngle(); - - /* - Start Calibration - Loop over electrical angles from 0 to NPP*2PI, once forward, once backward - store actual position and error as compared to electrical angle - */ - - /* +} + +// Perform filtering to linearize position sensor eccentricity +// FIR n-sample average, where n = number of samples in the window +// This filter has zero gain at electrical frequency and all integer multiples +// So cogging effects should be completely filtered out +void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ + float window_buffer[window] = {0}; + float window_sum = 0; + int buffer_index = 0; + // fill the inital window buffer + for (int i = 0; i < window; i++) { + int ind = n_ticks - window/2 -1 + i; + window_buffer[i] = error[ind % n_ticks]; + window_sum += window_buffer[i]; + } + // calculate the moving average + error_mean = 0; + for (int i = 0; i < n_ticks; i++) + { + // Update buffer + window_sum -= window_buffer[buffer_index]; + window_buffer[buffer_index] = error[( i + window/2 ) %n_ticks]; + window_sum += window_buffer[buffer_index]; + // update the buffer index + buffer_index = (buffer_index + 1) % window; + + // Update filtered error + error[i] = window_sum / (float)window; + // update the mean value + error_mean += error[i] / n_ticks; + } + +} + +void CalibratedSensor::calibrate(FOCMotor &motor) +{ + + motor.monitor_port->println("Starting Sensor Calibration."); + + // Init inital angles + float theta_actual = 0.0; + float avg_elec_angle = 0.0; + + 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}; // 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; + + // find the first guess of the motor.zero_electric_angle + // and the sensor direction + // updates motor.zero_electric_angle + // updates motor.sensor_direction + this->alignSensor(motor); + + // Set voltage angle to zero, wait for rotor position to settle + // keep the motor in position while getting the initial positions + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + _delay(1000); + _wrapped.update(); + float theta_init = _wrapped.getAngle(); + float theta_absolute_init = _wrapped.getMechanicalAngle(); + + /* + Start Calibration + Loop over electrical angles from 0 to NPP*2PI, once forward, once backward + store actual position and error as compared to electrical angle + */ + + /* forwards rotation */ - Serial.println("Rotating forwards"); + motor.monitor_port->println("Rotating forwards"); int k = 0; - for(int i = 0; iprintln(2.0*error[i]*180/3.14); + + // calculate the current electrical zero angle + float zero_angle = (motor.sensor_direction*_wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + zero_angle = _normalizeAngle(zero_angle); + avg_elec_angle += zero_angle/n_ticks; } - + _delay(2000); - /* + /* backwards rotation */ - Serial.println("Rotating backwards"); - for(int i = 0; iprintln("Rotating backwards"); + for (int i = n_ticks - 1; i >= 0; i--) + { + for (int j = 0; j < n2_ticks; j++) // move to the next location + { _wrapped.update(); - theta_actual = _wrapped.getAngle()-theta_init; - if (directionSensor == -1) - { - theta_actual = -theta_actual; - error_b[i] = theta_actual - elec_angle/_NPP; - } - else - { - error_b[i] = elec_angle/_NPP - theta_actual; - } -// raw_b[i] = theta_actual; - } - - // get post calibration mechanical angle. - _wrapped.update(); - theta_absolute_post = _wrapped.getMechanicalAngle(); - - // done with the measurement - motor.setPhaseVoltage(0, 0, 0); - - // raw offset from initial position in absolute radians between 0-2PI - float raw_offset = (theta_absolute_init+theta_absolute_post)/2; - - // calculating the average zero electrica angle from the forward calibration. - motor.zero_electric_angle = avg_elec_angle/_NPP; - Serial.print( "Average Zero Electrical Angle: "); - Serial.println( motor.zero_electric_angle); - - // Perform filtering to linearize position sensor eccentricity - // FIR n-sample average, where n = number of samples in one electrical cycle - // This filter has zero gain at electrical frequency and all integer multiples - // So cogging effects should be completely filtered out - float mean = 0; - for (int i = 0; i n_ticks-1) { - ind -= n_ticks;} - error_filt[i] += error[ind]/(float)window; + elec_angle -= deltaElectricalAngle; + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); } - mean += error_filt[i]/n_ticks; - } - - // calculate offset index - int index_offset = floor(raw_offset/(_2PI/n_lut)); - // Build Look Up Table - for (int i = 0; i (n_lut-1)){ - ind -= n_lut; - } - if(ind < 0 ){ - ind += n_lut; - } - calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean); - //Serial.print(ind); - //Serial.print('\t'); - //Serial.println(calibrationLut[ind],5); - _delay(1); + // delay to settle in position before taking a position sample + _delay(50); + _wrapped.update(); + // calculate the error + theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init); + error[i] += 0.5 * (theta_actual - elec_angle / _NPP); + + // display the errror + motor.monitor_port->println(error[i]*180/3.14); + + // calculate the current electrical zero angle + float zero_angle = (motor.sensor_direction*_wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); + zero_angle = _normalizeAngle(zero_angle); + // zero_angle = zero_angle > _PI ? zero_angle - 2*_PI : zero_angle; + avg_elec_angle += zero_angle/n_ticks; } - - // de-allocate memory - delete error_filt; - delete error; - // delete raw_b; - delete error_b; - // delete raw_f; - delete error_f; - - Serial.println("Sensor Calibration Done."); + // // get post calibration mechanical angle. + // _wrapped.update(); + // float theta_absolute_post = _wrapped.getMechanicalAngle(); + + // // done with the measurement + // motor.setPhaseVoltage(0, 0, 0); + + // // raw offset from initial position in absolute radians between 0-2PI + // float raw_offset = (theta_absolute_init + theta_absolute_post) / 2; + + // // calculating the average zero electrical angle from the forward calibration. + // motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); + // motor.monitor_port->print("Average Zero Electrical Angle: "); + // motor.monitor_port->println(motor.zero_electric_angle); + // _delay(1000); + + // // Perform filtering to linearize position sensor eccentricity + // // FIR n-sample average, where n = number of samples in one electrical cycle + // // This filter has zero gain at electrical frequency and all integer multiples + // // So cogging effects should be completely filtered out + // float error_mean = 0.0; + // this->filter_error(error, error_mean, n_ticks, window); + + // _delay(1000); + // // calculate offset index + // int index_offset = floor((float)n_lut * raw_offset / _2PI); + // float dn = n_ticks / (float)n_lut; + + // motor.monitor_port->println("Constructing LUT: "); + // _delay(1000); + // // Build Look Up Table + // for (int i = 0; i < n_lut; i++) + // { + // int ind = index_offset + i*motor.sensor_direction; + // if (ind > (n_lut - 1)) ind -= n_lut; + // 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]; + // } + // motor.monitor_port->println("Calibration LUT: "); + // _delay(1000); + + // for (int i=0;i < n_lut; i++){ + // motor.monitor_port->println(calibrationLut[i]*180.0/3.14, 5); + // _delay(1); + // } + + + // de-allocate memory + // delete error_filt; + // delete error; + // delete raw_b; + // delete error_b; + // delete raw_f; + // delete error_f; + + motor.monitor_port->println("Sensor Calibration Done."); } - diff --git a/src/encoders/calibrated/CalibratedSensor.cpp.old b/src/encoders/calibrated/CalibratedSensor.cpp.old new file mode 100644 index 0000000..e3d579c --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.cpp.old @@ -0,0 +1,257 @@ +#include "CalibratedSensor.h" + + +// CalibratedSensor() +// sensor - instance of original sensor object +CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) +{ +}; + + +CalibratedSensor::~CalibratedSensor() +{ + delete calibrationLut; +}; + +// call update of calibrated sensor +void CalibratedSensor::update(){ + _wrapped.update(); + this->Sensor::update(); +}; + +// Retrieve the calibrated sensor angle +void CalibratedSensor::init() { + // assume wrapped sensor has already been initialized + this->Sensor::init(); // call superclass init +} + +// Retrieve the calibrated sensor angle +float CalibratedSensor::getSensorAngle(){ + // raw encoder position e.g. 0-2PI + float rawAngle = _wrapped.getMechanicalAngle(); + + // index of the bucket that rawAngle is part of. + // e.g. rawAngle = 0 --> bucketIndex = 0. + // e.g. rawAngle = 2PI --> bucketIndex = 128. + int bucketIndex = floor(rawAngle/(_2PI/n_lut)); + float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex); + + // Extract the lower and upper LUT value in counts + float y0 = calibrationLut[bucketIndex]; + float y1 = calibrationLut[(bucketIndex+1)%n_lut]; + + // Linear Interpolation Between LUT values y0 and y1 using the remainder + // If remainder = 0, interpolated offset = y0 + // If remainder = 2PI/n_lut, interpolated offset = y1 + float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; + + // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians + float calibratedAngle = rawAngle+interpolatedOffset; + + // return calibrated angle in radians + return calibratedAngle; +} + +void CalibratedSensor::calibrate(BLDCMotor& motor){ + + Serial.println("Starting Sensor Calibration."); + + int _NPP = motor.pole_pairs; // number of pole pairs which is user input + const int n_ticks = 128*_NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2_ticks = 40; // increments between saved samples (for smoothing motion) + float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks); // Electrical Angle increments for calibration steps + float* error_f = new float[n_ticks](); // pointer to error array rotating forwards + // float* raw_f = new float[n_ticks](); // pointer to raw forward position + float* error_b = new float[n_ticks](); // pointer to error array rotating forwards + // float* raw_b = new float[n_ticks](); // pointer to raw backword position + float* error = new float[n_ticks](); // pointer to error array (average of forward & backward) + float* error_filt = new float[n_ticks](); // pointer to filtered error array (low pass filter) + const int window = 128; // window size for moving average filter of raw error + motor.zero_electric_angle = 0; // Set position sensor offset + + // find natural direction (this is copy of the init code) + // move one electrical revolution forward + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0f; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + // take and angle in the middle + _wrapped.update(); + float mid_angle = _wrapped.getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0f ; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + _wrapped.update(); + float end_angle = _wrapped.getAngle(); + motor.setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + int directionSensor; + if (mid_angle < end_angle) { + Serial.println("MOT: sensor_direction==CCW"); + directionSensor = -1; + motor.sensor_direction = Direction::CCW; + + } else{ + Serial.println("MOT: sensor_direction==CW"); + directionSensor = 1; + motor.sensor_direction = Direction::CW; + + } + + //Set voltage angle to zero, wait for rotor position to settle + // keep the motor in position while getting the initial positions + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + _delay(1000); + _wrapped.update(); + float theta_init = _wrapped.getAngle(); + float theta_absolute_init = _wrapped.getMechanicalAngle(); + + /* + Start Calibration + Loop over electrical angles from 0 to NPP*2PI, once forward, once backward + store actual position and error as compared to electrical angle + */ + + /* + forwards rotation + */ + Serial.println("Rotating forwards"); + int k = 0; + for(int i = 0; i n_ticks-1) { + ind -= n_ticks;} + error_filt[i] += error[ind]/(float)window; + } + mean += error_filt[i]/n_ticks; + } + + // calculate offset index + int index_offset = floor(raw_offset/(_2PI/n_lut)); + + // Build Look Up Table + for (int i = 0; i (n_lut-1)){ + ind -= n_lut; + } + if(ind < 0 ){ + ind += n_lut; + } + calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean); + //Serial.print(ind); + //Serial.print('\t'); + //Serial.println(calibrationLut[ind],5); + _delay(1); + } + + // de-allocate memory + delete error_filt; + delete error; + // delete raw_b; + delete error_b; + // delete raw_f; + delete error_f; + + Serial.println("Sensor Calibration Done."); + +} + + diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index 414c53a..9f63106 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -4,13 +4,14 @@ #include "common/base_classes/Sensor.h" #include "BLDCMotor.h" #include "common/base_classes/FOCMotor.h" +#include "common/foc_utils.h" class CalibratedSensor: public Sensor{ public: // constructor of class with pointer to base class sensor and driver - CalibratedSensor(Sensor& wrapped); + CalibratedSensor(Sensor& wrapped, int n_lut = 200); ~CalibratedSensor(); /* @@ -21,11 +22,11 @@ class CalibratedSensor: public Sensor{ /** * Calibrate method computes the LUT for the correction */ - virtual void calibrate(BLDCMotor& motor); + virtual void calibrate(FOCMotor& motor); // voltage to run the calibration: user input - float voltage_calibration = 1; - + float voltage_calibration = 1; + int bucket_offset = 0; protected: /** @@ -42,20 +43,16 @@ class CalibratedSensor: public Sensor{ * delegate instance of Sensor class */ Sensor& _wrapped; + + 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? - const int n_lut { 128 } ; - // create pointer for lut memory - float* calibrationLut = new float[n_lut](); - - // Init inital angles - float theta_actual { 0.0 }; - float elecAngle { 0.0 }; - float elec_angle { 0.0 }; - float theta_absolute_post { 0.0 }; - float theta_absolute_init { 0.0 }; - float theta_init { 0.0 }; - float avg_elec_angle { 0.0 }; + int n_lut { 200 } ; + // create pointer for lut memory + // depending on the size of the lut + // will be allocated in the constructor + float* calibrationLut; }; #endif \ No newline at end of file diff --git a/src/encoders/calibrated/CalibratedSensor.h.old b/src/encoders/calibrated/CalibratedSensor.h.old new file mode 100644 index 0000000..414c53a --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.h.old @@ -0,0 +1,61 @@ +#ifndef __CALIBRATEDSENSOR_H__ +#define __CALIBRATEDSENSOR_H__ + +#include "common/base_classes/Sensor.h" +#include "BLDCMotor.h" +#include "common/base_classes/FOCMotor.h" + + +class CalibratedSensor: public Sensor{ + +public: + // constructor of class with pointer to base class sensor and driver + CalibratedSensor(Sensor& wrapped); + ~CalibratedSensor(); + + /* + Override the update function + */ + virtual void update() override; + + /** + * Calibrate method computes the LUT for the correction + */ + virtual void calibrate(BLDCMotor& motor); + + // voltage to run the calibration: user input + float voltage_calibration = 1; + +protected: + + /** + * getSenorAngle() method of CalibratedSensor class. + * This should call getAngle() on the wrapped instance, and then apply the correction to + * the value returned. + */ + virtual float getSensorAngle() override; + /** + * init method of CaibratedSensor - call after calibration + */ + virtual void init() override; + /** + * delegate instance of Sensor class + */ + Sensor& _wrapped; + + // lut size, currently constan. Perhaps to be made variable by user? + const int n_lut { 128 } ; + // create pointer for lut memory + float* calibrationLut = new float[n_lut](); + + // Init inital angles + float theta_actual { 0.0 }; + float elecAngle { 0.0 }; + float elec_angle { 0.0 }; + float theta_absolute_post { 0.0 }; + float theta_absolute_init { 0.0 }; + float theta_init { 0.0 }; + float avg_elec_angle { 0.0 }; +}; + +#endif \ No newline at end of file From f1850994a130dd92ae7668d53fe1dceb6e1ce12f Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 11 Mar 2025 13:52:40 +0100 Subject: [PATCH 02/10] a good state of the calibraiton code --- .../calibrated/calibrated.ino | 10 +- .../calibration_save/calibration_save.ino | 88 ++++++ src/encoders/calibrated/CalibratedSensor.cpp | 249 ++++++++--------- .../calibrated/CalibratedSensor.cpp.old | 257 ------------------ src/encoders/calibrated/CalibratedSensor.h | 3 +- .../calibrated/CalibratedSensor.h.old | 61 ----- 6 files changed, 208 insertions(+), 460 deletions(-) rename examples/encoders/{ => calibrated_sensor}/calibrated/calibrated.ino (84%) create mode 100644 examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino delete mode 100644 src/encoders/calibrated/CalibratedSensor.cpp.old delete mode 100644 src/encoders/calibrated/CalibratedSensor.h.old diff --git a/examples/encoders/calibrated/calibrated.ino b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino similarity index 84% rename from examples/encoders/calibrated/calibrated.ino rename to examples/encoders/calibrated_sensor/calibrated/calibrated.ino index 6cc6110..6e4e303 100644 --- a/examples/encoders/calibrated/calibrated.ino +++ b/examples/encoders/calibrated_sensor/calibrated/calibrated.ino @@ -1,10 +1,5 @@ /** - * Torque control example using voltage control loop. - * - * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers - * you a way to control motor torque by setting the voltage to the motor instead hte current. - * - * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + * The example demonstrates the usage of the calibrated sensor object. */ #include #include @@ -16,6 +11,8 @@ MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6); BLDCMotor motor = BLDCMotor(11); BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); // instantiate the calibrated sensor object +// argument 1 - sensor object +// argument 2 - number of samples in the LUT (default 200) CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); // voltage set point variable @@ -58,6 +55,7 @@ void setup() { // set voltage to run calibration sensor_calibrated.voltage_calibration = 6; // Running calibration + // it will ouptut the LUT and the zero electrical angle to the serial monitor !!!! sensor_calibrated.calibrate(motor); //Serial.println("Calibrating Sensor Done."); diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino new file mode 100644 index 0000000..7af47b6 --- /dev/null +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -0,0 +1,88 @@ +/** + * 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. + */ + +#include +#include +#include "encoders/calibrated/CalibratedSensor.h" + +// fill this array with the calibration values outputed by the calibration procedure +float calibrationLut[200] = {0}; + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14); +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +StepperDriver4PWM driver = StepperDriver4PWM(10, 9, 5, 6,8); +// instantiate the calibrated sensor object +// instantiate the calibrated sensor object +// 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, 200, calibrationLut); + +// voltage set point variable +float target_voltage = 2; + +// instantiate the commander +Commander command = Commander(Serial); + +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + sensor.init(); + // Link motor to sensor + motor.linkSensor(&sensor); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + 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); + + // 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(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + +void loop() { + + motor.loopFOC(); + motor.move(target_voltage); + command.run(); + motor.monitor(); + +} \ No newline at end of file diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index efb1925..3ee0fcb 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -2,9 +2,11 @@ // CalibratedSensor() // sensor - instance of original sensor object -CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut) : _wrapped(wrapped) { +// 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) { this->n_lut = n_lut; - this->calibrationLut = new float[n_lut](); + this->calibrationLut = lut; }; CalibratedSensor::~CalibratedSensor() { @@ -51,65 +53,6 @@ float CalibratedSensor::getSensorAngle() return raw_angle - offset; } - -// find the first guess of the motor.zero_electric_angle -// and the sensor direction -// updates motor.zero_electric_angle -// updates motor.sensor_direction -void CalibratedSensor::alignSensor(FOCMotor &motor){ - motor.zero_electric_angle = 0; // Set position sensor offset - - // find natural direction (this is copy of the init code) - // move one electrical revolution forward - for (int i = 0; i <= 500; i++) - { - float angle = _3PI_2 + _2PI * i / 500.0f; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - // take and angle in the middle - _wrapped.update(); - float mid_angle = _wrapped.getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >= 0; i--) - { - float angle = _3PI_2 + _2PI * i / 500.0f; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - _wrapped.update(); - float end_angle = _wrapped.getAngle(); - motor.setPhaseVoltage(0, 0, 0); - _delay(200); - - // determine the direction the sensor moved - if (mid_angle < end_angle) - { - motor.monitor_port->println("MOT: sensor_direction==CCW"); - motor.sensor_direction = Direction::CCW; - } - else - { - motor.monitor_port->println("MOT: sensor_direction==CW"); - motor.sensor_direction = Direction::CW; - } - - // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees - motor.setPhaseVoltage(voltage_calibration, 0, _3PI_2); - _delay(700); - // read the sensor - _wrapped.update(); - motor.zero_electric_angle = _normalizeAngle(_electricalAngle(motor.sensor_direction*_wrapped.getAngle(), motor.pole_pairs)); - _delay(20); - motor.monitor_port->print("Zero Electrical Angle: "); - motor.monitor_port->println(motor.zero_electric_angle); - // stop everything - motor.setPhaseVoltage(0, 0, 0); -} - // Perform filtering to linearize position sensor eccentricity // FIR n-sample average, where n = number of samples in the window // This filter has zero gain at electrical frequency and all integer multiples @@ -145,8 +88,14 @@ void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks void CalibratedSensor::calibrate(FOCMotor &motor) { - - motor.monitor_port->println("Starting Sensor Calibration."); + + if(this->calibrationLut != nullptr){ + motor.monitor_port->println("Using existing LUT."); + return; + }else{ + this->calibrationLut = new float[n_lut](); + motor.monitor_port->println("Starting Sensor Calibration."); + } // Init inital angles float theta_actual = 0.0; @@ -157,7 +106,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor) 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}; // pointer to error array (average of forward & backward) + float error[n_ticks] = {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; @@ -166,11 +115,24 @@ void CalibratedSensor::calibrate(FOCMotor &motor) // and the sensor direction // updates motor.zero_electric_angle // updates motor.sensor_direction - this->alignSensor(motor); + 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; + } + 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; + } + motor.linkSensor(this); // Set voltage angle to zero, wait for rotor position to settle // keep the motor in position while getting the initial positions - motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + motor.setPhaseVoltage(1, 0, elec_angle); _delay(1000); _wrapped.update(); float theta_init = _wrapped.getAngle(); @@ -185,8 +147,9 @@ void CalibratedSensor::calibrate(FOCMotor &motor) /* forwards rotation */ - motor.monitor_port->println("Rotating forwards"); - int k = 0; + motor.monitor_port->print("Rotating: "); + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CCW" : "CW" ); + float zero_angle_prev = 0.0; for (int i = 0; i < n_ticks; i++) { for (int j = 0; j < n2_ticks; j++) // move to the next location @@ -197,17 +160,22 @@ void CalibratedSensor::calibrate(FOCMotor &motor) } // delay to settle in position before taking a position sample - _delay(50); + _delay(30); _wrapped.update(); // calculate the error theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init); error[i] = 0.5 * (theta_actual - elec_angle / _NPP); - motor.monitor_port->println(2.0*error[i]*180/3.14); - // calculate the current electrical zero angle float zero_angle = (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){ + zero_angle = zero_angle - _2PI; + }else if(zero_angle - zero_angle_prev < -_PI){ + zero_angle = zero_angle + _2PI; + } + zero_angle_prev = zero_angle; avg_elec_angle += zero_angle/n_ticks; } @@ -216,8 +184,8 @@ void CalibratedSensor::calibrate(FOCMotor &motor) /* backwards rotation */ - k = 0; - motor.monitor_port->println("Rotating backwards"); + motor.monitor_port->print("Rotating: "); + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CW" : "CCW" ); for (int i = n_ticks - 1; i >= 0; i--) { for (int j = 0; j < n2_ticks; j++) // move to the next location @@ -228,78 +196,89 @@ void CalibratedSensor::calibrate(FOCMotor &motor) } // delay to settle in position before taking a position sample - _delay(50); + _delay(30); _wrapped.update(); // calculate the error theta_actual = motor.sensor_direction*(_wrapped.getAngle() - theta_init); error[i] += 0.5 * (theta_actual - elec_angle / _NPP); - - // display the errror - motor.monitor_port->println(error[i]*180/3.14); - // calculate the current electrical zero angle float zero_angle = (motor.sensor_direction*_wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); zero_angle = _normalizeAngle(zero_angle); - // zero_angle = zero_angle > _PI ? zero_angle - 2*_PI : zero_angle; + // remove the 2PI jumps + if(zero_angle - zero_angle_prev > _PI){ + zero_angle = zero_angle - _2PI; + }else if(zero_angle - zero_angle_prev < -_PI){ + zero_angle = zero_angle + _2PI; + } + zero_angle_prev = zero_angle; avg_elec_angle += zero_angle/n_ticks; } - // // get post calibration mechanical angle. - // _wrapped.update(); - // float theta_absolute_post = _wrapped.getMechanicalAngle(); - - // // done with the measurement - // motor.setPhaseVoltage(0, 0, 0); - - // // raw offset from initial position in absolute radians between 0-2PI - // float raw_offset = (theta_absolute_init + theta_absolute_post) / 2; - - // // calculating the average zero electrical angle from the forward calibration. - // motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); - // motor.monitor_port->print("Average Zero Electrical Angle: "); - // motor.monitor_port->println(motor.zero_electric_angle); - // _delay(1000); - - // // Perform filtering to linearize position sensor eccentricity - // // FIR n-sample average, where n = number of samples in one electrical cycle - // // This filter has zero gain at electrical frequency and all integer multiples - // // So cogging effects should be completely filtered out - // float error_mean = 0.0; - // this->filter_error(error, error_mean, n_ticks, window); - - // _delay(1000); - // // calculate offset index - // int index_offset = floor((float)n_lut * raw_offset / _2PI); - // float dn = n_ticks / (float)n_lut; - - // motor.monitor_port->println("Constructing LUT: "); - // _delay(1000); - // // Build Look Up Table - // for (int i = 0; i < n_lut; i++) - // { - // int ind = index_offset + i*motor.sensor_direction; - // if (ind > (n_lut - 1)) ind -= n_lut; - // 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]; - // } - // motor.monitor_port->println("Calibration LUT: "); - // _delay(1000); - - // for (int i=0;i < n_lut; i++){ - // motor.monitor_port->println(calibrationLut[i]*180.0/3.14, 5); - // _delay(1); - // } - - - // de-allocate memory - // delete error_filt; - // delete error; - // delete raw_b; - // delete error_b; - // delete raw_f; - // delete error_f; + // get post calibration mechanical angle. + _wrapped.update(); + float theta_absolute_post = _wrapped.getMechanicalAngle(); + + // done with the measurement + motor.setPhaseVoltage(0, 0, 0); + + // raw offset from initial position in absolute radians between 0-2PI + float raw_offset = (theta_absolute_init + theta_absolute_post) / 2; + + // calculating the average zero electrical angle from the forward calibration. + motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); + motor.monitor_port->print("Average Zero Electrical Angle: "); + motor.monitor_port->println(motor.zero_electric_angle); + _delay(1000); + + // Perform filtering to linearize position sensor eccentricity + // FIR n-sample average, where n = number of samples in one electrical cycle + // This filter has zero gain at electrical frequency and all integer multiples + // So cogging effects should be completely filtered out + float error_mean = 0.0; + this->filter_error(error, error_mean, n_ticks, window); + + _delay(1000); + // calculate offset index + int index_offset = floor((float)n_lut * raw_offset / _2PI); + float dn = n_ticks / (float)n_lut; + + motor.monitor_port->println("Constructing LUT: "); + _delay(1000); + // Build Look Up Table + for (int i = 0; i < n_lut; i++) + { + int ind = index_offset + i*motor.sensor_direction; + if (ind > (n_lut - 1)) ind -= n_lut; + 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]; + } + motor.monitor_port->println("Calibration LUT: "); + _delay(1000); + + // Display the LUT + motor.monitor_port->print("float calibrationLut["); + motor.monitor_port->print(n_lut); + motor.monitor_port->println("] = {"); + _delay(100); + for (int i=0;i < n_lut; i++){ + motor.monitor_port->print(calibrationLut[i],6); + if(i < n_lut - 1) motor.monitor_port->print(", "); + _delay(1); + } + motor.monitor_port->println("};"); + _delay(1000); + + // Display the zero electrical angle + motor.monitor_port->print("float zero_electric_angle = "); + motor.monitor_port->print(motor.zero_electric_angle,6); + motor.monitor_port->println(";"); + + // Display the sensor direction + motor.monitor_port->print("Direction sensor_direction = "); + motor.monitor_port->println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;"); + _delay(1000); motor.monitor_port->println("Sensor Calibration Done."); } diff --git a/src/encoders/calibrated/CalibratedSensor.cpp.old b/src/encoders/calibrated/CalibratedSensor.cpp.old deleted file mode 100644 index e3d579c..0000000 --- a/src/encoders/calibrated/CalibratedSensor.cpp.old +++ /dev/null @@ -1,257 +0,0 @@ -#include "CalibratedSensor.h" - - -// CalibratedSensor() -// sensor - instance of original sensor object -CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) -{ -}; - - -CalibratedSensor::~CalibratedSensor() -{ - delete calibrationLut; -}; - -// call update of calibrated sensor -void CalibratedSensor::update(){ - _wrapped.update(); - this->Sensor::update(); -}; - -// Retrieve the calibrated sensor angle -void CalibratedSensor::init() { - // assume wrapped sensor has already been initialized - this->Sensor::init(); // call superclass init -} - -// Retrieve the calibrated sensor angle -float CalibratedSensor::getSensorAngle(){ - // raw encoder position e.g. 0-2PI - float rawAngle = _wrapped.getMechanicalAngle(); - - // index of the bucket that rawAngle is part of. - // e.g. rawAngle = 0 --> bucketIndex = 0. - // e.g. rawAngle = 2PI --> bucketIndex = 128. - int bucketIndex = floor(rawAngle/(_2PI/n_lut)); - float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex); - - // Extract the lower and upper LUT value in counts - float y0 = calibrationLut[bucketIndex]; - float y1 = calibrationLut[(bucketIndex+1)%n_lut]; - - // Linear Interpolation Between LUT values y0 and y1 using the remainder - // If remainder = 0, interpolated offset = y0 - // If remainder = 2PI/n_lut, interpolated offset = y1 - float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; - - // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians - float calibratedAngle = rawAngle+interpolatedOffset; - - // return calibrated angle in radians - return calibratedAngle; -} - -void CalibratedSensor::calibrate(BLDCMotor& motor){ - - Serial.println("Starting Sensor Calibration."); - - int _NPP = motor.pole_pairs; // number of pole pairs which is user input - const int n_ticks = 128*_NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) - const int n2_ticks = 40; // increments between saved samples (for smoothing motion) - float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks); // Electrical Angle increments for calibration steps - float* error_f = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_f = new float[n_ticks](); // pointer to raw forward position - float* error_b = new float[n_ticks](); // pointer to error array rotating forwards - // float* raw_b = new float[n_ticks](); // pointer to raw backword position - float* error = new float[n_ticks](); // pointer to error array (average of forward & backward) - float* error_filt = new float[n_ticks](); // pointer to filtered error array (low pass filter) - const int window = 128; // window size for moving average filter of raw error - motor.zero_electric_angle = 0; // Set position sensor offset - - // find natural direction (this is copy of the init code) - // move one electrical revolution forward - for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0f; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - // take and angle in the middle - _wrapped.update(); - float mid_angle = _wrapped.getAngle(); - // move one electrical revolution backwards - for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0f ; - motor.setPhaseVoltage(voltage_calibration, 0, angle); - _wrapped.update(); - _delay(2); - } - _wrapped.update(); - float end_angle = _wrapped.getAngle(); - motor.setPhaseVoltage(0, 0, 0); - _delay(200); - // determine the direction the sensor moved - int directionSensor; - if (mid_angle < end_angle) { - Serial.println("MOT: sensor_direction==CCW"); - directionSensor = -1; - motor.sensor_direction = Direction::CCW; - - } else{ - Serial.println("MOT: sensor_direction==CW"); - directionSensor = 1; - motor.sensor_direction = Direction::CW; - - } - - //Set voltage angle to zero, wait for rotor position to settle - // keep the motor in position while getting the initial positions - motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); - _delay(1000); - _wrapped.update(); - float theta_init = _wrapped.getAngle(); - float theta_absolute_init = _wrapped.getMechanicalAngle(); - - /* - Start Calibration - Loop over electrical angles from 0 to NPP*2PI, once forward, once backward - store actual position and error as compared to electrical angle - */ - - /* - forwards rotation - */ - Serial.println("Rotating forwards"); - int k = 0; - for(int i = 0; i n_ticks-1) { - ind -= n_ticks;} - error_filt[i] += error[ind]/(float)window; - } - mean += error_filt[i]/n_ticks; - } - - // calculate offset index - int index_offset = floor(raw_offset/(_2PI/n_lut)); - - // Build Look Up Table - for (int i = 0; i (n_lut-1)){ - ind -= n_lut; - } - if(ind < 0 ){ - ind += n_lut; - } - calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean); - //Serial.print(ind); - //Serial.print('\t'); - //Serial.println(calibrationLut[ind],5); - _delay(1); - } - - // de-allocate memory - delete error_filt; - delete error; - // delete raw_b; - delete error_b; - // delete raw_f; - delete error_f; - - Serial.println("Sensor Calibration Done."); - -} - - diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index 9f63106..99e2707 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -11,7 +11,8 @@ class CalibratedSensor: public Sensor{ public: // constructor of class with pointer to base class sensor and driver - CalibratedSensor(Sensor& wrapped, int n_lut = 200); + CalibratedSensor(Sensor& wrapped, int n_lut = 200, float* lut = nullptr); + ~CalibratedSensor(); /* diff --git a/src/encoders/calibrated/CalibratedSensor.h.old b/src/encoders/calibrated/CalibratedSensor.h.old deleted file mode 100644 index 414c53a..0000000 --- a/src/encoders/calibrated/CalibratedSensor.h.old +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef __CALIBRATEDSENSOR_H__ -#define __CALIBRATEDSENSOR_H__ - -#include "common/base_classes/Sensor.h" -#include "BLDCMotor.h" -#include "common/base_classes/FOCMotor.h" - - -class CalibratedSensor: public Sensor{ - -public: - // constructor of class with pointer to base class sensor and driver - CalibratedSensor(Sensor& wrapped); - ~CalibratedSensor(); - - /* - Override the update function - */ - virtual void update() override; - - /** - * Calibrate method computes the LUT for the correction - */ - virtual void calibrate(BLDCMotor& motor); - - // voltage to run the calibration: user input - float voltage_calibration = 1; - -protected: - - /** - * getSenorAngle() method of CalibratedSensor class. - * This should call getAngle() on the wrapped instance, and then apply the correction to - * the value returned. - */ - virtual float getSensorAngle() override; - /** - * init method of CaibratedSensor - call after calibration - */ - virtual void init() override; - /** - * delegate instance of Sensor class - */ - Sensor& _wrapped; - - // lut size, currently constan. Perhaps to be made variable by user? - const int n_lut { 128 } ; - // create pointer for lut memory - float* calibrationLut = new float[n_lut](); - - // Init inital angles - float theta_actual { 0.0 }; - float elecAngle { 0.0 }; - float elec_angle { 0.0 }; - float theta_absolute_post { 0.0 }; - float theta_absolute_init { 0.0 }; - float theta_init { 0.0 }; - float avg_elec_angle { 0.0 }; -}; - -#endif \ No newline at end of file From cec679ded36ba9f9ea4ec2b62279a2770c560a45 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 11 Mar 2025 14:00:41 +0100 Subject: [PATCH 03/10] a bit more info about the saving and loading of the LUT --- src/encoders/calibrated/README.md | 41 ++++++++++++++++++++++++++++--- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index 5768d92..485d16b 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -81,11 +81,44 @@ void setup() { Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-drivers/blob/master/examples/encoders/calibrated/sensor_calibration.ino) in our examples directory. +## EDIT March 2025 -## Roadmap +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. -Possible future improvements we've thought about: +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. -- Improve memory usage and performance -- Make calibration able to be saved/restored +Your code will look something like this: +```c++ + +// number of LUT entries +const N_LUT = 100; +// Lookup table that has been ouptut from the calibration process +float calibrationLut[N_LUT] = {...}; + +// provide the sensor class and the number of points in the LUT +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT, calibrationLut); + +... + +void setup() { + ... + // as LUT is provided this function does not need to be called + sensor_calibrated.calibrate(motor); + ... + + 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(); + .... +} + + +``` From bec64065e3950ca4730fe5fea9c4d8074f306c8f Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 11 Mar 2025 14:02:47 +0100 Subject: [PATCH 04/10] resize the example --- .../calibrated_sensor/calibration_save/calibration_save.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino index 7af47b6..852214f 100644 --- a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -8,7 +8,7 @@ #include "encoders/calibrated/CalibratedSensor.h" // fill this array with the calibration values outputed by the calibration procedure -float calibrationLut[200] = {0}; +float calibrationLut[100] = {0}; // magnetic sensor instance - SPI MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 14); @@ -20,7 +20,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, 200, calibrationLut); +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, 100, calibrationLut); // voltage set point variable float target_voltage = 2; From ab90dbaab454fc0d66dd0a99a09c7c131a249fec Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 11 Mar 2025 14:04:45 +0100 Subject: [PATCH 05/10] some init issues --- src/encoders/calibrated/CalibratedSensor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 3ee0fcb..9bc1c2a 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -58,7 +58,7 @@ float CalibratedSensor::getSensorAngle() // This filter has zero gain at electrical frequency and all integer multiples // So cogging effects should be completely filtered out void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ - float window_buffer[window] = {0}; + float window_buffer[window] = {0.0}; float window_sum = 0; int buffer_index = 0; // fill the inital window buffer @@ -106,7 +106,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor) 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}; // pointer to error array (average of forward & backward) + 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; From cab7af59abfcf25dad472ba0e0357fb93e5a2996 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:01:00 +0100 Subject: [PATCH 06/10] calibration update --- .../calibration_save/calibration_save.ino | 14 ++--- src/encoders/calibrated/CalibratedSensor.cpp | 59 ++++++++++++------- src/encoders/calibrated/CalibratedSensor.h | 9 ++- src/encoders/calibrated/README.md | 20 ++++--- 4 files changed, 59 insertions(+), 43 deletions(-) diff --git a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino index 852214f..5a8ee68 100644 --- a/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino +++ b/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(); diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 9bc1c2a..750da3c 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/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. diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h index 99e2707..e4cfcbb 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/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 diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index 485d16b..abfcb67 100644 --- a/src/encoders/calibrated/README.md +++ b/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,25 +96,22 @@ 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(); .... @@ -122,3 +119,8 @@ void setup() { ``` + +## 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 \ No newline at end of file From e174615ffc53e23ee985dc03c6c91acd9a09982f Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:05:19 +0100 Subject: [PATCH 07/10] memory issues --- .github/workflows/ccpp.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 3f98490..e9e2b82 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,14 +26,14 @@ jobs: - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:nano - sketches-exclude: calibrated mt6816_spi smoothing simplefocnano_torque_voltage + sketches-exclude: calibrated calibration_save mt6816_spi smoothing simplefocnano_torque_voltage required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC - sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega + sketches-exclude: calibrated calibration_save smoothing simplefocnano_torque_voltage simplefocnano_atmega - arduino-boards-fqbn: arduino:samd:nano_33_iot required-libraries: Simple FOC - sketches-exclude: calibrated smoothing + sketches-exclude: calibrated calibration_save smoothing - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC sketches-exclude: calibrated smoothing simplefocnano_torque_voltage simplefocnano_atmega From 98edcc531450bf0244d86e4a94cd00cdb80c26af Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:09:50 +0100 Subject: [PATCH 08/10] due variable size issue --- src/encoders/calibrated/CalibratedSensor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 750da3c..38fe521 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -56,7 +56,8 @@ float CalibratedSensor::getSensorAngle() // This filter has zero gain at electrical frequency and all integer multiples // So cogging effects should be completely filtered out void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ - float window_buffer[window] = {0.0}; + float window_buffer[window]; + memset(window_buffer, 0, window*sizeof(float)); float window_sum = 0; int buffer_index = 0; // fill the inital window buffer @@ -116,7 +117,8 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri 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) + float error[n_ticks]; // pointer to error array (average of forward & backward) + memset(error, 0, n_ticks*sizeof(float)); // 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 From 01cde01521e8539878a51714645d602d5da97ad5 Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:42:31 +0100 Subject: [PATCH 09/10] added info to README + removed prints --- src/encoders/calibrated/CalibratedSensor.cpp | 20 +++++++------- src/encoders/calibrated/README.md | 29 ++++++++++++++++++-- 2 files changed, 36 insertions(+), 13 deletions(-) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index 38fe521..5f6c007 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -188,10 +188,10 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri 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); + // motor.monitor_port->print(">zero:"); + // motor.monitor_port->println(zero_angle); + // motor.monitor_port->print(">zero_average:"); + // motor.monitor_port->println(avg_elec_angle/2.0); } _delay(2000); @@ -227,10 +227,10 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri 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); + // motor.monitor_port->print(">zero:"); + // motor.monitor_port->println(zero_angle); + // motor.monitor_port->print(">zero_average:"); + // motor.monitor_port->println(avg_elec_angle/2.0); } // get post calibration mechanical angle. @@ -261,7 +261,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri int index_offset = floor((float)n_lut * raw_offset / _2PI); float dn = n_ticks / (float)n_lut; - motor.monitor_port->println("Constructing LUT: "); + motor.monitor_port->println("Constructing LUT."); _delay(1000); // Build Look Up Table for (int i = 0; i < n_lut; i++) @@ -273,7 +273,7 @@ void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electri // negate the error if the sensor is in the opposite direction calibrationLut[ind] = motor.sensor_direction * calibrationLut[ind]; } - motor.monitor_port->println("Calibration LUT: "); + motor.monitor_port->println(""); _delay(1000); // Display the LUT diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index abfcb67..8135f5b 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -86,6 +86,29 @@ Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC- 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 paramer for calibration. This allows you to save the LUT and load it on startup to avoid recalibration on each startup. +Once you do the calibration once, it will output something like this: + +``` +... + +Starting Sensor Calibration. +MOT: Align sensor. +MOT: sensor_direction==CCW +MOT: PP check: OK! +MOT: Zero elec. angle: 3.17 +MOT: No current sense. +MOT: Ready.Rotating: CCW +Rotating: CW +Average Zero Electrical Angle: 4.01 +Constructing LUT. + +float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; +float zero_electric_angle = 4.007072; +Direction sensor_direction = Direction::CCW; +Sensor Calibration Done +... +``` + 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. Your code will look something like this: @@ -95,9 +118,9 @@ Your code will look something like this: // number of LUT entries 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; +float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; +float zero_electric_angle = 4.007072; +Direction sensor_direction = Direction::CCW; // provide the sensor class and the number of points in the LUT CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT); From b4a32629d599491ef7ed900e0e67bbd3e1e5de6d Mon Sep 17 00:00:00 2001 From: gospar Date: Wed, 12 Mar 2025 10:55:41 +0100 Subject: [PATCH 10/10] typon in the readme --- src/encoders/calibrated/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index 8135f5b..9a1c823 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -116,7 +116,7 @@ Your code will look something like this: ```c++ // number of LUT entries -const N_LUT = 100; +const N_LUT = 50; // Lookup table that has been ouptut from the calibration process float calibrationLut[50] = {0.003486, 0.005795, 0.007298, 0.008303, 0.008771, 0.007551, 0.005986, 0.004115, 0.001361, -0.001392, -0.004069, -0.007474, -0.010420, -0.013135, -0.014891, -0.017415, -0.018328, -0.019125, -0.018849, -0.017193, -0.015152, -0.012422, -0.008579, -0.003970, 0.000678, 0.005211, 0.009821, 0.013280, 0.016470, 0.018127, 0.018376, 0.016969, 0.016716, 0.015466, 0.013602, 0.011431, 0.008646, 0.006092, 0.003116, 0.000409, -0.002342, -0.004367, -0.005932, -0.006998, -0.007182, -0.007175, -0.006017, -0.003746, -0.001783, 0.000948}; float zero_electric_angle = 4.007072;