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 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..5a8ee68 --- /dev/null +++ b/examples/encoders/calibrated_sensor/calibration_save/calibration_save.ino @@ -0,0 +1,86 @@ +/** + * 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[50] = {0}; +float zero_electric_angle = 0; +Direction sensor_direction = Direction::CW; + +// 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, 50); + +// 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 + // 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); + + // 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 e3d579c..5f6c007 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -1,257 +1,304 @@ #include "CalibratedSensor.h" - // CalibratedSensor() // sensor - instance of original sensor object -CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) -{ +// n_lut - number of samples in the LUT +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut) : _wrapped(wrapped) { + this->n_lut = 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; + + // 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; +} + +// 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]; + memset(window_buffer, 0, window*sizeof(float)); + 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(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 - */ - - /* +void CalibratedSensor::calibrate(FOCMotor &motor, float* lut, float zero_electric_angle, Direction senor_direction) +{ + // 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]; // 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 + + + // find the first guess of the motor.zero_electric_angle + // and the sensor direction + // updates motor.zero_electric_angle + // updates motor.sensor_direction + // 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; + } + // 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 + motor.setPhaseVoltage(1, 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; iprint("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 { - theta_actual = -theta_actual; - error_f[i] = theta_actual - elec_angle/_NPP; - + _wrapped.update(); + elec_angle += deltaElectricalAngle; + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); } - else - { - error_f[i] = elec_angle/_NPP - theta_actual; + + // delay to settle in position before taking a position sample + _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); + + // 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; } - // if overflow happened track it as full rotation -// raw_f[i] = theta_actual; - - // storing the normalized angle every time the electrical angle 3PI/2 to calculate average zero electrical angle - if(i==(k*128+96)) - { - _delay(50); - avg_elec_angle += _normalizeAngle(directionSensor*_wrapped.getMechanicalAngle()*_NPP); - k += 1; - } + zero_angle_prev = zero_angle; + avg_elec_angle += zero_angle/n_ticks; + + // 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); - /* + /* backwards rotation */ - Serial.println("Rotating backwards"); - for(int i = 0; iprint("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 + { _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; + elec_angle -= deltaElectricalAngle; + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + } + + // delay to settle in position before taking a position sample + _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); + // 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; + + // 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. _wrapped.update(); - theta_absolute_post = _wrapped.getMechanicalAngle(); + 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; + 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); - // 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; - } - mean += error_filt[i]/n_ticks; - } + float error_mean = 0.0; + this->filter_error(error, error_mean, n_ticks, window); + _delay(1000); // calculate offset index - int index_offset = floor(raw_offset/(_2PI/n_lut)); + 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-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); + 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(""); + _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); } - - // de-allocate memory - delete error_filt; - delete error; - // delete raw_b; - delete error_b; - // delete raw_f; - delete error_f; + motor.monitor_port->println("};"); + _delay(1000); - Serial.println("Sensor Calibration Done."); + // 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.h b/src/encoders/calibrated/CalibratedSensor.h index 414c53a..e4cfcbb 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -4,13 +4,15 @@ #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 +23,10 @@ class CalibratedSensor: public Sensor{ /** * Calibrate method computes the LUT for the correction */ - virtual void calibrate(BLDCMotor& 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; - + float voltage_calibration = 1; 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 }; + // lut size - settable by the user + 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/README.md b/src/encoders/calibrated/README.md index 5768d92..9a1c823 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -81,11 +81,69 @@ 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 paramer for calibration. 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: +Once you do the calibration once, it will output something like this: -- Improve memory usage and performance -- Make calibration able to be saved/restored +``` +... + +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: + +```c++ + +// number of LUT entries +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; +Direction sensor_direction = Direction::CCW; + +// provide the sensor class and the number of points in the LUT +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor, N_LUT); + +... + +void setup() { + ... + // as LUT is provided to this function + sensor_calibrated.calibrate(motor, calibrationLut, zero_eletrical_angle, sensor_direction); + ... + + motor.linkSensor(&sensor_calibrated); + + ... + motor.initFOC(); + .... +} + + +``` + +## Future work +- Reduce the LUT size by using a more efficient LUT type - maybe pass to uint16_t +- Use a more eficient LUT interpolation method - maybe a polynomial interpolation \ No newline at end of file