From 58ec9781ba86cbfe252a6f4015446d1605b39414 Mon Sep 17 00:00:00 2001 From: mcells <33664753+mcells@users.noreply.github.com> Date: Wed, 9 Oct 2024 14:34:58 +0200 Subject: [PATCH] Add motor characterisation --- src/BLDCMotor.h | 10 ++ src/StepperMotor.h | 11 ++ src/common/base_classes/FOCMotor.cpp | 221 +++++++++++++++++++++++++++ src/common/base_classes/FOCMotor.h | 9 ++ 4 files changed, 251 insertions(+) diff --git a/src/BLDCMotor.h b/src/BLDCMotor.h index c261e405..386c5f19 100644 --- a/src/BLDCMotor.h +++ b/src/BLDCMotor.h @@ -81,6 +81,16 @@ class BLDCMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a BLDCMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.5f); + } + private: // FOC methods diff --git a/src/StepperMotor.h b/src/StepperMotor.h index 7e7810d8..208e453e 100644 --- a/src/StepperMotor.h +++ b/src/StepperMotor.h @@ -83,6 +83,17 @@ class StepperMotor: public FOCMotor */ void setPhaseVoltage(float Uq, float Ud, float angle_el) override; + /** + * Measure resistance and inductance of a StepperMotor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * TODO: determine the correction factor + * @param voltage The voltage applied to the motor + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage){ + return FOCMotor::characteriseMotor(voltage, 1.0f); + } + private: /** Sensor alignment to electrical 0 angle of the motor */ diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 5d8f8127..06dfe63a 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -92,6 +92,227 @@ void FOCMotor::useMonitoring(Print &print){ #endif } +// Measure resistance and inductance of a motor +int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){ + if (!this->current_sense || !this->current_sense->initialized) + { + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized"); + return 1; + } + + if (voltage <= 0.0f){ + SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero"); + return 2; + } + voltage = _constrain(voltage, 0.0f, voltage_limit); + + SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still..."); + + float current_electric_angle = electricalAngle(); + + // Apply zero volts and measure a zero reference + setPhaseVoltage(0, 0, current_electric_angle); + _delay(500); + + PhaseCurrent_s zerocurrent_raw = current_sense->readAverageCurrents(); + DQCurrent_s zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + + // Ramp and hold the voltage to measure resistance + // 300 ms of ramping + current_electric_angle = electricalAngle(); + for(int i=0; i < 100; i++){ + setPhaseVoltage(0, voltage/100.0*((float)i), current_electric_angle); + _delay(3); + } + _delay(10); + PhaseCurrent_s r_currents_raw = current_sense->readAverageCurrents(); + DQCurrent_s r_currents = current_sense->getDQCurrents(current_sense->getABCurrents(r_currents_raw), current_electric_angle); + + // Zero again + setPhaseVoltage(0, 0, current_electric_angle); + + + if (fabsf(r_currents.d - zerocurrent.d) < 0.2f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low"); + return 3; + } + + float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d)); + if (resistance <= 0.0f) + { + SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0"); + return 4; + } + + SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance); + _delay(100); + + + // Start inductance measurement + SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still..."); + + unsigned long t0 = 0; + unsigned long t1 = 0; + float Ltemp = 0; + float Ld = 0; + float Lq = 0; + float d_electrical_angle = 0; + + uint iterations = 40; // how often the algorithm gets repeated. + uint cycles = 3; // averaged measurements for each iteration + uint risetime_us = 200; // initially short for worst case scenario with low inductance + uint settle_us = 100000; // initially long for worst case scenario with high inductance + + // Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it) + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + + for (size_t axis = 0; axis < 2; axis++) + { + for (size_t i = 0; i < iterations; i++) + { + // current_electric_angle = i * _2PI / iterations; // <-- Do a sweep of the inductance. Use eg. for graphing + float inductanced = 0.0f; + + float qcurrent = 0.0f; + float dcurrent = 0.0f; + for (size_t j = 0; j < cycles; j++) + { + // read zero current + zerocurrent_raw = current_sense->readAverageCurrents(20); + zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle); + + // step the voltage + setPhaseVoltage(0, voltage, current_electric_angle); + t0 = micros(); + delayMicroseconds(risetime_us); // wait a little bit + + PhaseCurrent_s l_currents_raw = current_sense->getPhaseCurrents(); + t1 = micros(); + setPhaseVoltage(0, 0, current_electric_angle); + + DQCurrent_s l_currents = current_sense->getDQCurrents(current_sense->getABCurrents(l_currents_raw), current_electric_angle); + delayMicroseconds(settle_us); // wait a bit for the currents to go to 0 again + + if (t0 > t1) continue; // safety first + + // calculate the inductance + float dt = (t1 - t0)/1000000.0f; + if (l_currents.d - zerocurrent.d <= 0 || (voltage - resistance * (l_currents.d - zerocurrent.d)) <= 0) + { + continue; + } + + inductanced += fabsf(- (resistance * dt) / log((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor; + + qcurrent+= l_currents.q - zerocurrent.q; // average the measured currents + dcurrent+= l_currents.d - zerocurrent.d; + } + qcurrent /= cycles; + dcurrent /= cycles; + + float delta = qcurrent / (fabsf(dcurrent) + fabsf(qcurrent)); + + + inductanced /= cycles; + Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4; + + float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R) + // SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant); + + // Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes) + risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000); + settle_us = 10 * risetime_us; + + + // Serial.printf(">inductance:%f:%f|xy\n", current_electric_angle, Ltemp * 1000.0f); // <-- Plot an angle sweep + + + /** + * How this code works: + * If we apply a current spike in the d´-axis, there will be cross coupling to the q´-axis current, if we didn´t use the actual d-axis (ie. d´ != d). + * This has to do with saliency (Ld != Lq). + * The amount of cross coupled current is somewhat proportional to the angle error, which means that if we iteratively change the angle to min/maximise this current, we get the correct d-axis (and q-axis). + */ + if (axis) + { + qcurrent *= -1.0f; // to d or q axis + } + + if (qcurrent < 0) + { + current_electric_angle+=fabsf(delta); + } else + { + current_electric_angle-=fabsf(delta); + } + current_electric_angle = _normalizeAngle(current_electric_angle); + + + // Average the d-axis angle further for calculating the electrical zero later + if (axis) + { + d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9 + current_electric_angle * 0.1; + } + + } + + // We know the minimum is 0.5*PI radians away, so less iterations are needed. + current_electric_angle += 0.5f * _PI; + current_electric_angle = _normalizeAngle(current_electric_angle); + iterations /= 2; + + if (axis == 0) + { + Lq = Ltemp; + }else + { + Ld = Ltemp; + } + + } + + if (sensor) + { + /** + * The d_electrical_angle should now be aligned to the d axis or the -d axis. We can therefore calculate two possible electrical zero angles. + * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count). + */ + + float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle); + float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI); + float estimated_zero_electric_angle = 0.0f; + if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle)) + { + estimated_zero_electric_angle = estimated_zero_electric_angle_A; + } else + { + estimated_zero_electric_angle = estimated_zero_electric_angle_B; + } + + SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle); + SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle); + } + + + SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!"); + SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f); + SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f); + if (Ld > Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error."); + } + if (Ld * 2.0f < Lq) + { + SIMPLEFOC_DEBUG("WARN: MOT: Measured Q inductance is more than twice the D inductance. This is probably wrong. From experience, the lower value is probably close to reality."); + } + + return 0; + +} + // utility function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! void FOCMotor::monitor() { diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 8ae0e8af..93ee1106 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -149,6 +149,15 @@ class FOCMotor */ float electricalAngle(); + /** + * Measure resistance and inductance of a motor and print results to debug. + * If a sensor is available, an estimate of zero electric angle will be reported too. + * @param voltage The voltage applied to the motor + * @param correction_factor Is 1.5 for 3 phase motors, because we measure over a series-parallel connection. TODO: what about 2 phase motors? + * @returns 0 for success, >0 for failure + */ + int characteriseMotor(float voltage, float correction_factor); + // state variables float target; //!< current target value - depends of the controller float feed_forward_velocity = 0.0f; //!< current feed forward velocity