|
| 1 | +#include "CalibratedSensor.h" |
| 2 | + |
| 3 | +// CalibratedSensor() |
| 4 | +// sensor - instance of original sensor object |
| 5 | +// n_lut - number of samples in the LUT |
| 6 | +CalibratedSensor::CalibratedSensor(Sensor &wrapped, int n_lut, float *lut) |
| 7 | + : _wrapped(wrapped), n_lut(n_lut), allocated(false), calibrationLut(lut) { |
| 8 | + }; |
| 9 | + |
| 10 | +CalibratedSensor::~CalibratedSensor() { |
| 11 | + // delete calibrationLut; |
| 12 | + if(allocated) { |
| 13 | + delete []calibrationLut; |
| 14 | + } |
| 15 | +}; |
| 16 | + |
| 17 | +// call update of calibrated sensor |
| 18 | +void CalibratedSensor::update() |
| 19 | +{ |
| 20 | + _wrapped.update(); |
| 21 | + this->Sensor::update(); |
| 22 | +}; |
| 23 | + |
| 24 | +// Retrieve the calibrated sensor angle |
| 25 | +void CalibratedSensor::init() |
| 26 | +{ |
| 27 | + // assume wrapped sensor has already been initialized |
| 28 | + this->Sensor::init(); // call superclass init |
| 29 | +} |
| 30 | + |
| 31 | +// Retrieve the calibrated sensor angle |
| 32 | +float CalibratedSensor::getSensorAngle() |
| 33 | +{ |
| 34 | + if(!calibrationLut) { |
| 35 | + return _wrapped.getMechanicalAngle(); |
| 36 | + } |
| 37 | + // raw encoder position e.g. 0-2PI |
| 38 | + float raw_angle = fmodf(_wrapped.getMechanicalAngle(), _2PI); |
| 39 | + raw_angle += raw_angle < 0 ? _2PI:0; |
| 40 | + |
| 41 | + // Calculate the resolution of the LUT in radians |
| 42 | + float lut_resolution = _2PI / n_lut; |
| 43 | + // Calculate LUT index |
| 44 | + int lut_index = raw_angle / lut_resolution; |
| 45 | + |
| 46 | + // Get calibration values from the LUT |
| 47 | + float y0 = calibrationLut[lut_index]; |
| 48 | + float y1 = calibrationLut[(lut_index + 1) % n_lut]; |
| 49 | + |
| 50 | + // Linearly interpolate between the y0 and y1 values |
| 51 | + // Calculate the relative distance from the y0 (raw_angle has to be between y0 and y1) |
| 52 | + // If distance = 0, interpolated offset = y0 |
| 53 | + // If distance = 1, interpolated offset = y1 |
| 54 | + float distance = (raw_angle - lut_index * lut_resolution) / lut_resolution; |
| 55 | + float offset = (1 - distance) * y0 + distance * y1; |
| 56 | + |
| 57 | + // Calculate the calibrated angle |
| 58 | + return raw_angle - offset; |
| 59 | +} |
| 60 | + |
| 61 | +// Perform filtering to linearize position sensor eccentricity |
| 62 | +// FIR n-sample average, where n = number of samples in the window |
| 63 | +// This filter has zero gain at electrical frequency and all integer multiples |
| 64 | +// So cogging effects should be completely filtered out |
| 65 | +void CalibratedSensor::filter_error(float* error, float &error_mean, int n_ticks, int window){ |
| 66 | + float window_buffer[window]; |
| 67 | + memset(window_buffer, 0, window*sizeof(float)); |
| 68 | + float window_sum = 0; |
| 69 | + int buffer_index = 0; |
| 70 | + // fill the inital window buffer |
| 71 | + for (int i = 0; i < window; i++) { |
| 72 | + int ind = n_ticks - window/2 -1 + i; |
| 73 | + window_buffer[i] = error[ind % n_ticks]; |
| 74 | + window_sum += window_buffer[i]; |
| 75 | + } |
| 76 | + // calculate the moving average |
| 77 | + error_mean = 0; |
| 78 | + for (int i = 0; i < n_ticks; i++) |
| 79 | + { |
| 80 | + // Update buffer |
| 81 | + window_sum -= window_buffer[buffer_index]; |
| 82 | + window_buffer[buffer_index] = error[( i + window/2 ) %n_ticks]; |
| 83 | + window_sum += window_buffer[buffer_index]; |
| 84 | + // update the buffer index |
| 85 | + buffer_index = (buffer_index + 1) % window; |
| 86 | + |
| 87 | + // Update filtered error |
| 88 | + error[i] = window_sum / (float)window; |
| 89 | + // update the mean value |
| 90 | + error_mean += error[i] / n_ticks; |
| 91 | + } |
| 92 | + |
| 93 | +} |
| 94 | + |
| 95 | +void CalibratedSensor::calibrate(FOCMotor &motor, int settle_time_ms) |
| 96 | +{ |
| 97 | + // if the LUT is already defined, skip the calibration |
| 98 | + |
| 99 | + if(calibrationLut == NULL) { |
| 100 | + allocated = true; |
| 101 | + calibrationLut = new float[n_lut]; |
| 102 | + } |
| 103 | + motor.monitor_port->println("Starting Sensor Calibration."); |
| 104 | + |
| 105 | + // Calibration variables |
| 106 | + |
| 107 | + // Init inital angles |
| 108 | + float theta_actual = 0.0; |
| 109 | + float avg_elec_angle = 0.0; |
| 110 | + // set the inital electric angle to 0 |
| 111 | + float elec_angle = 0.0; |
| 112 | + |
| 113 | + // Calibration parameters |
| 114 | + // The motor will take a n_pos samples per electrical cycle |
| 115 | + // which amounts to n_ticks (n_pos * motor.pole_pairs) samples per mechanical rotation |
| 116 | + // Additionally, the motor will take n2_ticks steps to reach any of the n_ticks posiitons |
| 117 | + // incrementing the electrical angle by deltaElectricalAngle each time |
| 118 | + int n_pos = 5; |
| 119 | + int _NPP = motor.pole_pairs; // number of pole pairs which is user input |
| 120 | + const int n_ticks = n_pos * _NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) |
| 121 | + const int n2_ticks = 5; // increments between saved samples (for smoothing motion) |
| 122 | + float deltaElectricalAngle = _2PI * _NPP / (n_ticks * n2_ticks); // Electrical Angle increments for calibration steps |
| 123 | + float error[n_ticks]; // pointer to error array (average of forward & backward) |
| 124 | + memset(error, 0, n_ticks*sizeof(float)); |
| 125 | + // The fileter window size is set to n_pos - one electrical cycle |
| 126 | + // important for cogging filtering !!! |
| 127 | + const int window = n_pos; // window size for moving average filter of raw error |
| 128 | + |
| 129 | + |
| 130 | + // find the first guess of the motor.zero_electric_angle |
| 131 | + // and the sensor direction |
| 132 | + // updates motor.zero_electric_angle |
| 133 | + // updates motor.sensor_direction |
| 134 | + // temporarily unlink the sensor and current sense |
| 135 | + CurrentSense *current_sense = motor.current_sense; |
| 136 | + motor.current_sense = nullptr; |
| 137 | + motor.linkSensor(&this->_wrapped); |
| 138 | + if(!motor.initFOC()){ |
| 139 | + motor.monitor_port->println("Failed to align the sensor."); |
| 140 | + return; |
| 141 | + } |
| 142 | + // link back the sensor and current sense |
| 143 | + motor.linkSensor(this); |
| 144 | + motor.linkCurrentSense(current_sense); |
| 145 | + |
| 146 | + // Set voltage angle to zero, wait for rotor position to settle |
| 147 | + // keep the motor in position while getting the initial positions |
| 148 | + motor.setPhaseVoltage(1, 0, elec_angle); |
| 149 | + _delay(1000); |
| 150 | + _wrapped.update(); |
| 151 | + float theta_init = _wrapped.getAngle(); |
| 152 | + float theta_absolute_init = _wrapped.getMechanicalAngle(); |
| 153 | + |
| 154 | + /* |
| 155 | + Start Calibration |
| 156 | + Loop over electrical angles from 0 to NPP*2PI, once forward, once backward |
| 157 | + store actual position and error as compared to electrical angle |
| 158 | + */ |
| 159 | + |
| 160 | + /* |
| 161 | + forwards rotation |
| 162 | + */ |
| 163 | + motor.monitor_port->print("Rotating: "); |
| 164 | + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CCW" : "CW" ); |
| 165 | + float zero_angle_prev = 0.0; |
| 166 | + for (int i = 0; i < n_ticks; i++) |
| 167 | + { |
| 168 | + for (int j = 0; j < n2_ticks; j++) // move to the next location |
| 169 | + { |
| 170 | + _wrapped.update(); |
| 171 | + elec_angle += deltaElectricalAngle; |
| 172 | + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); |
| 173 | + } |
| 174 | + |
| 175 | + // delay to settle in position before taking a position sample |
| 176 | + _delay(settle_time_ms); |
| 177 | + _wrapped.update(); |
| 178 | + // calculate the error |
| 179 | + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); |
| 180 | + error[i] = 0.5 * (theta_actual - elec_angle / _NPP); |
| 181 | + |
| 182 | + // calculate the current electrical zero angle |
| 183 | + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); |
| 184 | + zero_angle = _normalizeAngle(zero_angle); |
| 185 | + // remove the 2PI jumps |
| 186 | + if(zero_angle - zero_angle_prev > _PI){ |
| 187 | + zero_angle = zero_angle - _2PI; |
| 188 | + }else if(zero_angle - zero_angle_prev < -_PI){ |
| 189 | + zero_angle = zero_angle + _2PI; |
| 190 | + } |
| 191 | + zero_angle_prev = zero_angle; |
| 192 | + avg_elec_angle += zero_angle/n_ticks; |
| 193 | + |
| 194 | + // motor.monitor_port->print(">zero:"); |
| 195 | + // motor.monitor_port->println(zero_angle); |
| 196 | + // motor.monitor_port->print(">zero_average:"); |
| 197 | + // motor.monitor_port->println(avg_elec_angle/2.0); |
| 198 | + } |
| 199 | + _delay(2000); |
| 200 | + |
| 201 | + /* |
| 202 | + backwards rotation |
| 203 | + */ |
| 204 | + motor.monitor_port->print("Rotating: "); |
| 205 | + motor.monitor_port->println( motor.sensor_direction == Direction::CCW ? "CW" : "CCW" ); |
| 206 | + for (int i = n_ticks - 1; i >= 0; i--) |
| 207 | + { |
| 208 | + for (int j = 0; j < n2_ticks; j++) // move to the next location |
| 209 | + { |
| 210 | + _wrapped.update(); |
| 211 | + elec_angle -= deltaElectricalAngle; |
| 212 | + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); |
| 213 | + } |
| 214 | + |
| 215 | + // delay to settle in position before taking a position sample |
| 216 | + _delay(settle_time_ms); |
| 217 | + _wrapped.update(); |
| 218 | + // calculate the error |
| 219 | + theta_actual = (int)motor.sensor_direction * (_wrapped.getAngle() - theta_init); |
| 220 | + error[i] += 0.5 * (theta_actual - elec_angle / _NPP); |
| 221 | + // calculate the current electrical zero angle |
| 222 | + float zero_angle = ((int)motor.sensor_direction * _wrapped.getMechanicalAngle() * _NPP ) - (elec_angle + _PI_2); |
| 223 | + zero_angle = _normalizeAngle(zero_angle); |
| 224 | + // remove the 2PI jumps |
| 225 | + if(zero_angle - zero_angle_prev > _PI){ |
| 226 | + zero_angle = zero_angle - _2PI; |
| 227 | + }else if(zero_angle - zero_angle_prev < -_PI){ |
| 228 | + zero_angle = zero_angle + _2PI; |
| 229 | + } |
| 230 | + zero_angle_prev = zero_angle; |
| 231 | + avg_elec_angle += zero_angle/n_ticks; |
| 232 | + |
| 233 | + // motor.monitor_port->print(">zero:"); |
| 234 | + // motor.monitor_port->println(zero_angle); |
| 235 | + // motor.monitor_port->print(">zero_average:"); |
| 236 | + // motor.monitor_port->println(avg_elec_angle/2.0); |
| 237 | + } |
| 238 | + |
| 239 | + // get post calibration mechanical angle. |
| 240 | + _wrapped.update(); |
| 241 | + float theta_absolute_post = _wrapped.getMechanicalAngle(); |
| 242 | + |
| 243 | + // done with the measurement |
| 244 | + motor.setPhaseVoltage(0, 0, 0); |
| 245 | + |
| 246 | + // raw offset from initial position in absolute radians between 0-2PI |
| 247 | + float raw_offset = (theta_absolute_init + theta_absolute_post) / 2; |
| 248 | + |
| 249 | + // calculating the average zero electrical angle from the forward calibration. |
| 250 | + motor.zero_electric_angle = _normalizeAngle(avg_elec_angle / (2.0)); |
| 251 | + motor.monitor_port->print("Average Zero Electrical Angle: "); |
| 252 | + motor.monitor_port->println(motor.zero_electric_angle); |
| 253 | + _delay(1000); |
| 254 | + |
| 255 | + // Perform filtering to linearize position sensor eccentricity |
| 256 | + // FIR n-sample average, where n = number of samples in one electrical cycle |
| 257 | + // This filter has zero gain at electrical frequency and all integer multiples |
| 258 | + // So cogging effects should be completely filtered out |
| 259 | + float error_mean = 0.0; |
| 260 | + this->filter_error(error, error_mean, n_ticks, window); |
| 261 | + |
| 262 | + _delay(1000); |
| 263 | + // calculate offset index |
| 264 | + int index_offset = floor((float)n_lut * raw_offset / _2PI); |
| 265 | + float dn = n_ticks / (float)n_lut; |
| 266 | + |
| 267 | + motor.monitor_port->println("Constructing LUT."); |
| 268 | + _delay(1000); |
| 269 | + // Build Look Up Table |
| 270 | + for (int i = 0; i < n_lut; i++) |
| 271 | + { |
| 272 | + int ind = index_offset + i*motor.sensor_direction; |
| 273 | + if (ind > (n_lut - 1)) ind -= n_lut; |
| 274 | + if (ind < 0) ind += n_lut; |
| 275 | + calibrationLut[ind] = (float)(error[(int)(i * dn)] - error_mean); |
| 276 | + // negate the error if the sensor is in the opposite direction |
| 277 | + calibrationLut[ind] = (int)motor.sensor_direction * calibrationLut[ind]; |
| 278 | + } |
| 279 | + motor.monitor_port->println(""); |
| 280 | + _delay(1000); |
| 281 | + |
| 282 | + // Display the LUT |
| 283 | + motor.monitor_port->print("float calibrationLut["); |
| 284 | + motor.monitor_port->print(n_lut); |
| 285 | + motor.monitor_port->println("] = {"); |
| 286 | + _delay(100); |
| 287 | + for (int i=0;i < n_lut; i++){ |
| 288 | + motor.monitor_port->print(calibrationLut[i],6); |
| 289 | + if(i < n_lut - 1) motor.monitor_port->print(", "); |
| 290 | + _delay(1); |
| 291 | + } |
| 292 | + motor.monitor_port->println("};"); |
| 293 | + _delay(1000); |
| 294 | + |
| 295 | + // Display the zero electrical angle |
| 296 | + motor.monitor_port->print("float zero_electric_angle = "); |
| 297 | + motor.monitor_port->print(motor.zero_electric_angle,6); |
| 298 | + motor.monitor_port->println(";"); |
| 299 | + |
| 300 | + // Display the sensor direction |
| 301 | + motor.monitor_port->print("Direction sensor_direction = "); |
| 302 | + motor.monitor_port->println(motor.sensor_direction == Direction::CCW ? "Direction::CCW;" : "Direction::CW;"); |
| 303 | + _delay(1000); |
| 304 | + |
| 305 | + motor.monitor_port->println("Sensor Calibration Done."); |
| 306 | +} |
| 307 | + |
0 commit comments