Skip to content

Commit de755f4

Browse files
runger1101001runger1101001
authored
Merge pull request #436 from mcells/feat-inductance-measure
Add motor characterisation
2 parents add1f42 + 6ba2cf2 commit de755f4

File tree

5 files changed

+370
-0
lines changed

5 files changed

+370
-0
lines changed

‎examples/utils/calibration/measure_inductance_and_resistance/measure_inductance_and_resistance.ino

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
/**
2+
*
3+
* Motor characterisation example sketch.
4+
*
5+
*/
6+
#include <SimpleFOC.h>
7+
8+
9+
// BLDC motor & driver instance
10+
BLDCMotor motor = BLDCMotor(11);
11+
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
12+
13+
// encoder instance
14+
Encoder encoder = Encoder(2, 3, 500);
15+
// channel A and B callbacks
16+
void doA(){encoder.handleA();}
17+
void doB(){encoder.handleB();}
18+
19+
// current sensor
20+
InlineCurrentSense current_sense = InlineCurrentSense(0.01f, 50.0f, A0, A2);
21+
22+
// characterisation voltage set point variable
23+
float characteriseVolts = 0.0f;
24+
25+
// instantiate the commander
26+
Commander command = Commander(Serial);
27+
void onMotor(char* cmd){command.motor(&motor,cmd);}
28+
void characterise(char* cmd) {
29+
command.scalar(&characteriseVolts, cmd);
30+
motor.characteriseMotor(characteriseVolts);
31+
}
32+
33+
void setup() {
34+
35+
// use monitoring with serial
36+
Serial.begin(115200);
37+
// enable more verbose output for debugging
38+
// comment out if not needed
39+
SimpleFOCDebug::enable(&Serial);
40+
41+
// initialize encoder sensor hardware
42+
encoder.init();
43+
encoder.enableInterrupts(doA, doB);
44+
// link the motor to the sensor
45+
motor.linkSensor(&encoder);
46+
47+
// driver config
48+
// power supply voltage [V]
49+
driver.voltage_power_supply = 12;
50+
driver.init();
51+
// link driver
52+
motor.linkDriver(&driver);
53+
// link current sense and the driver
54+
current_sense.linkDriver(&driver);
55+
56+
// current sense init hardware
57+
current_sense.init();
58+
// link the current sense to the motor
59+
motor.linkCurrentSense(&current_sense);
60+
61+
// set torque mode:
62+
// TorqueControlType::dc_current
63+
// TorqueControlType::voltage
64+
// TorqueControlType::foc_current
65+
motor.torque_controller = TorqueControlType::foc_current;
66+
// set motion control loop to be used
67+
motor.controller = MotionControlType::torque;
68+
69+
// foc current control parameters (Arduino UNO/Mega)
70+
motor.PID_current_q.P = 5;
71+
motor.PID_current_q.I= 300;
72+
motor.PID_current_d.P= 5;
73+
motor.PID_current_d.I = 300;
74+
motor.LPF_current_q.Tf = 0.01f;
75+
motor.LPF_current_d.Tf = 0.01f;
76+
// foc current control parameters (stm/esp/due/teensy)
77+
// motor.PID_current_q.P = 5;
78+
// motor.PID_current_q.I= 1000;
79+
// motor.PID_current_d.P= 5;
80+
// motor.PID_current_d.I = 1000;
81+
// motor.LPF_current_q.Tf = 0.002f; // 1ms default
82+
// motor.LPF_current_d.Tf = 0.002f; // 1ms default
83+
84+
// comment out if not needed
85+
motor.useMonitoring(Serial);
86+
87+
// initialize motor
88+
motor.init();
89+
// align sensor and start FOC
90+
motor.initFOC();
91+
92+
// add commands M & L
93+
command.add('M',&onMotor,"Control motor");
94+
command.add('L', characterise, "Characterise motor L & R with the given voltage");
95+
96+
motor.disable();
97+
98+
Serial.println(F("Motor disabled and ready."));
99+
Serial.println(F("Control the motor and measure the inductance using the terminal. Type \"?\" for available commands:"));
100+
_delay(1000);
101+
}
102+
103+
void loop() {
104+
105+
// main FOC algorithm function
106+
// the faster you run this function the better
107+
// Arduino UNO loop ~1kHz
108+
// Bluepill loop ~10kHz
109+
motor.loopFOC();
110+
111+
// Motion control function
112+
// velocity, position or torque (defined in motor.controller)
113+
// this function can be run at much lower frequency than loopFOC() function
114+
// You can also use motor.move() and set the motor.target in the code
115+
motor.move();
116+
117+
// user communication
118+
command.run();
119+
}

‎src/BLDCMotor.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,16 @@ class BLDCMotor: public FOCMotor
8181
*/
8282
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
8383

84+
/**
85+
* Measure resistance and inductance of a BLDCMotor and print results to debug.
86+
* If a sensor is available, an estimate of zero electric angle will be reported too.
87+
* @param voltage The voltage applied to the motor
88+
* @returns 0 for success, >0 for failure
89+
*/
90+
int characteriseMotor(float voltage){
91+
return FOCMotor::characteriseMotor(voltage, 1.5f);
92+
}
93+
8494
private:
8595
// FOC methods
8696

‎src/StepperMotor.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,17 @@ class StepperMotor: public FOCMotor
8383
*/
8484
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;
8585

86+
/**
87+
* Measure resistance and inductance of a StepperMotor and print results to debug.
88+
* If a sensor is available, an estimate of zero electric angle will be reported too.
89+
* TODO: determine the correction factor
90+
* @param voltage The voltage applied to the motor
91+
* @returns 0 for success, >0 for failure
92+
*/
93+
int characteriseMotor(float voltage){
94+
return FOCMotor::characteriseMotor(voltage, 1.0f);
95+
}
96+
8697
private:
8798

8899
/** Sensor alignment to electrical 0 angle of the motor */

‎src/common/base_classes/FOCMotor.cpp

Lines changed: 221 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,227 @@ void FOCMotor::useMonitoring(Print &print){
9292
#endif
9393
}
9494

95+
// Measure resistance and inductance of a motor
96+
int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){
97+
if (!this->current_sense || !this->current_sense->initialized)
98+
{
99+
SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: CS unconfigured or not initialized");
100+
return 1;
101+
}
102+
103+
if (voltage <= 0.0f){
104+
SIMPLEFOC_DEBUG("ERR: MOT: Cannot characterise motor: Voltage is negative or less than zero");
105+
return 2;
106+
}
107+
voltage = _constrain(voltage, 0.0f, voltage_limit);
108+
109+
SIMPLEFOC_DEBUG("MOT: Measuring phase to phase resistance, keep motor still...");
110+
111+
float current_electric_angle = electricalAngle();
112+
113+
// Apply zero volts and measure a zero reference
114+
setPhaseVoltage(0, 0, current_electric_angle);
115+
_delay(500);
116+
117+
PhaseCurrent_s zerocurrent_raw = current_sense->readAverageCurrents();
118+
DQCurrent_s zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle);
119+
120+
121+
// Ramp and hold the voltage to measure resistance
122+
// 300 ms of ramping
123+
current_electric_angle = electricalAngle();
124+
for(int i=0; i < 100; i++){
125+
setPhaseVoltage(0, voltage/100.0*((float)i), current_electric_angle);
126+
_delay(3);
127+
}
128+
_delay(10);
129+
PhaseCurrent_s r_currents_raw = current_sense->readAverageCurrents();
130+
DQCurrent_s r_currents = current_sense->getDQCurrents(current_sense->getABCurrents(r_currents_raw), current_electric_angle);
131+
132+
// Zero again
133+
setPhaseVoltage(0, 0, current_electric_angle);
134+
135+
136+
if (fabsf(r_currents.d - zerocurrent.d) < 0.2f)
137+
{
138+
SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: measured current too low");
139+
return 3;
140+
}
141+
142+
float resistance = voltage / (correction_factor * (r_currents.d - zerocurrent.d));
143+
if (resistance <= 0.0f)
144+
{
145+
SIMPLEFOC_DEBUG("ERR: MOT: Motor characterisation failed: Calculated resistance <= 0");
146+
return 4;
147+
}
148+
149+
SIMPLEFOC_DEBUG("MOT: Estimated phase to phase resistance: ", 2.0f * resistance);
150+
_delay(100);
151+
152+
153+
// Start inductance measurement
154+
SIMPLEFOC_DEBUG("MOT: Measuring inductance, keep motor still...");
155+
156+
unsigned long t0 = 0;
157+
unsigned long t1 = 0;
158+
float Ltemp = 0;
159+
float Ld = 0;
160+
float Lq = 0;
161+
float d_electrical_angle = 0;
162+
163+
unsigned int iterations = 40; // how often the algorithm gets repeated.
164+
unsigned int cycles = 3; // averaged measurements for each iteration
165+
unsigned int risetime_us = 200; // initially short for worst case scenario with low inductance
166+
unsigned int settle_us = 100000; // initially long for worst case scenario with high inductance
167+
168+
// Pre-rotate the angle to the q-axis (only useful with sensor, else no harm in doing it)
169+
current_electric_angle += 0.5f * _PI;
170+
current_electric_angle = _normalizeAngle(current_electric_angle);
171+
172+
for (size_t axis = 0; axis < 2; axis++)
173+
{
174+
for (size_t i = 0; i < iterations; i++)
175+
{
176+
// current_electric_angle = i * _2PI / iterations; // <-- Do a sweep of the inductance. Use eg. for graphing
177+
float inductanced = 0.0f;
178+
179+
float qcurrent = 0.0f;
180+
float dcurrent = 0.0f;
181+
for (size_t j = 0; j < cycles; j++)
182+
{
183+
// read zero current
184+
zerocurrent_raw = current_sense->readAverageCurrents(20);
185+
zerocurrent = current_sense->getDQCurrents(current_sense->getABCurrents(zerocurrent_raw), current_electric_angle);
186+
187+
// step the voltage
188+
setPhaseVoltage(0, voltage, current_electric_angle);
189+
t0 = micros();
190+
delayMicroseconds(risetime_us); // wait a little bit
191+
192+
PhaseCurrent_s l_currents_raw = current_sense->getPhaseCurrents();
193+
t1 = micros();
194+
setPhaseVoltage(0, 0, current_electric_angle);
195+
196+
DQCurrent_s l_currents = current_sense->getDQCurrents(current_sense->getABCurrents(l_currents_raw), current_electric_angle);
197+
delayMicroseconds(settle_us); // wait a bit for the currents to go to 0 again
198+
199+
if (t0 > t1) continue; // safety first
200+
201+
// calculate the inductance
202+
float dt = (t1 - t0)/1000000.0f;
203+
if (l_currents.d - zerocurrent.d <= 0 || (voltage - resistance * (l_currents.d - zerocurrent.d)) <= 0)
204+
{
205+
continue;
206+
}
207+
208+
inductanced += fabsf(- (resistance * dt) / log((voltage - resistance * (l_currents.d - zerocurrent.d)) / voltage))/correction_factor;
209+
210+
qcurrent+= l_currents.q - zerocurrent.q; // average the measured currents
211+
dcurrent+= l_currents.d - zerocurrent.d;
212+
}
213+
qcurrent /= cycles;
214+
dcurrent /= cycles;
215+
216+
float delta = qcurrent / (fabsf(dcurrent) + fabsf(qcurrent));
217+
218+
219+
inductanced /= cycles;
220+
Ltemp = i < 2 ? inductanced : Ltemp * 0.6 + inductanced * 0.4;
221+
222+
float timeconstant = fabsf(Ltemp / resistance); // Timeconstant of an RL circuit (L/R)
223+
// SIMPLEFOC_DEBUG("MOT: Estimated time constant in us: ", 1000000.0f * timeconstant);
224+
225+
// Wait as long as possible (due to limited timing accuracy & sample rate), but as short as needed (while the current still changes)
226+
risetime_us = _constrain(risetime_us * 0.6f + 0.4f * 1000000 * 0.6f * timeconstant, 100, 10000);
227+
settle_us = 10 * risetime_us;
228+
229+
230+
// Serial.printf(">inductance:%f:%f|xy\n", current_electric_angle, Ltemp * 1000.0f); // <-- Plot an angle sweep
231+
232+
233+
/**
234+
* How this code works:
235+
* 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).
236+
* This has to do with saliency (Ld != Lq).
237+
* 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).
238+
*/
239+
if (axis)
240+
{
241+
qcurrent *= -1.0f; // to d or q axis
242+
}
243+
244+
if (qcurrent < 0)
245+
{
246+
current_electric_angle+=fabsf(delta);
247+
} else
248+
{
249+
current_electric_angle-=fabsf(delta);
250+
}
251+
current_electric_angle = _normalizeAngle(current_electric_angle);
252+
253+
254+
// Average the d-axis angle further for calculating the electrical zero later
255+
if (axis)
256+
{
257+
d_electrical_angle = i < 2 ? current_electric_angle : d_electrical_angle * 0.9 + current_electric_angle * 0.1;
258+
}
259+
260+
}
261+
262+
// We know the minimum is 0.5*PI radians away, so less iterations are needed.
263+
current_electric_angle += 0.5f * _PI;
264+
current_electric_angle = _normalizeAngle(current_electric_angle);
265+
iterations /= 2;
266+
267+
if (axis == 0)
268+
{
269+
Lq = Ltemp;
270+
}else
271+
{
272+
Ld = Ltemp;
273+
}
274+
275+
}
276+
277+
if (sensor)
278+
{
279+
/**
280+
* 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.
281+
* 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).
282+
*/
283+
284+
float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle);
285+
float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI);
286+
float estimated_zero_electric_angle = 0.0f;
287+
if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle))
288+
{
289+
estimated_zero_electric_angle = estimated_zero_electric_angle_A;
290+
} else
291+
{
292+
estimated_zero_electric_angle = estimated_zero_electric_angle_B;
293+
}
294+
295+
SIMPLEFOC_DEBUG("MOT: Newly estimated electrical zero: ", estimated_zero_electric_angle);
296+
SIMPLEFOC_DEBUG("MOT: Current electrical zero: ", zero_electric_angle);
297+
}
298+
299+
300+
SIMPLEFOC_DEBUG("MOT: Inductance measurement complete!");
301+
SIMPLEFOC_DEBUG("MOT: Measured D-inductance in mH: ", Ld * 1000.0f);
302+
SIMPLEFOC_DEBUG("MOT: Measured Q-inductance in mH: ", Lq * 1000.0f);
303+
if (Ld > Lq)
304+
{
305+
SIMPLEFOC_DEBUG("WARN: MOT: Measured inductance is larger in D than in Q axis. This is normally a sign of a measurement error.");
306+
}
307+
if (Ld * 2.0f < Lq)
308+
{
309+
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.");
310+
}
311+
312+
return 0;
313+
314+
}
315+
95316
// utility function intended to be used with serial plotter to monitor motor variables
96317
// significantly slowing the execution down!!!!
97318
void FOCMotor::monitor() {

‎src/common/base_classes/FOCMotor.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,15 @@ class FOCMotor
149149
*/
150150
float electricalAngle();
151151

152+
/**
153+
* Measure resistance and inductance of a motor and print results to debug.
154+
* If a sensor is available, an estimate of zero electric angle will be reported too.
155+
* @param voltage The voltage applied to the motor
156+
* @param correction_factor Is 1.5 for 3 phase motors, because we measure over a series-parallel connection. TODO: what about 2 phase motors?
157+
* @returns 0 for success, >0 for failure
158+
*/
159+
int characteriseMotor(float voltage, float correction_factor);
160+
152161
// state variables
153162
float target; //!< current target value - depends of the controller
154163
float feed_forward_velocity = 0.0f; //!< current feed forward velocity

0 commit comments

Comments
 (0)