Re: Field oriented control of induction motors
Posted: Mon Feb 13, 2023 12:27 pm
So, rather late last night I wrote a simpler FOC implementation. The full code is here: https://github.com/catphish/stm32-sine/ ... n-sine.cpp
First we fetch some settings. slipconst is a time constant. It's a combination of motor parameters and the PWM frequency. It doesn't really matter how it's derived, it just needs to be tuned to the specific motor, and it determines the slip. curkp controls the current gains. idtarget is the d-axis current target. We set this to a fixed value for now. throtcur is multiplied by the torque request to get the q-axis current target iqtarget.
The next part is standard, we fetch the rotor angle from the encoder, and the three phase currents from the ADC.
Next we run a Park and Clarke transform using the last known rotor field angle to derive the Id and Iq currents.
This block of code estimates the slip and advances the rotor field angle accordingly We assume that slip is proportional to iqtarget divided by idtarget, disregarding the difference between target and reality, as well as the rotor time constant. slipIncr is integrated into slipAngle and added the the physical rotor angle to arrive at a rotor field angle. The accuracy of this process is largely determined by the value of slipconst.
The slip and electrical frequencies are converted back to Hz for display calculation and use elsewhere in the application.
Here we run an extremely simple I-only PID to apply corrections to Ud and Uq according to the measured error in Id and Iq. Each voltage is limited to a value that prevents the combined total from exceeding the DC bus voltage.
And finally, as one might expect, we run the inverse Park and Clarke before outputting the voltages to the inverter.
This code *appears* to work!
I'm sure there will still be lots to do to get this accurate, and I apologize for not using the existing FOC loop. It just did a lot of things that may not apply to my motor and and I wanted to keep it simple to begin with. Sadly with no load on the motor most tests quickly exceed the voltage of the supply.
I think it's worth appreciating that ultimately, this code is still just controlling current and slip, much like my "current control" version. The substantial difference is that by handling d-axis and q-axis current independently, it should now be able to control negative currents.
Video of this code running on my bench.
First we fetch some settings. slipconst is a time constant. It's a combination of motor parameters and the PWM frequency. It doesn't really matter how it's derived, it just needs to be tuned to the specific motor, and it determines the slip. curkp controls the current gains. idtarget is the d-axis current target. We set this to a fixed value for now. throtcur is multiplied by the torque request to get the q-axis current target iqtarget.
Code: Select all
// Fetch settings
int dir = Param::GetInt(Param::dir);
s32fp slipconst = Param::GetInt(Param::slipconst);
s32fp curkp = Param::Get(Param::curkp);
s32fp idtarget = Param::Get(Param::idtarget);
s32fp throtcur = Param::Get(Param::throtcur);
s32fp iqtarget = FP_MUL(throtcur, torqueRequest) * dir;
Code: Select all
// Update rotor angle from encoder
Encoder::UpdateRotorAngle(dir);
// Process currents
s32fp il2 = GetCurrent(AnaIn::il1, ilofs[0], Param::Get(Param::il1gain));
s32fp il3 = GetCurrent(AnaIn::il2, ilofs[1], Param::Get(Param::il2gain));
s32fp il1 = -il2 - il3;
s32fp ilMax = GetIlMax(il1, il2);
Param::SetFixed(Param::il1, il1);
Param::SetFixed(Param::il2, il2);
Param::SetFixed(Param::ilmax, ilMax);
Code: Select all
// Calculate measured IQ and ID
FOC::SetAngle(angle);
FOC::ParkClarke(il1, il2);
Param::SetFixed(Param::iq, FOC::iq);
Param::SetFixed(Param::id, FOC::id);
Code: Select all
// Increment rotor field angle
int32_t slipIncr = 0;
if(idtarget > 0)
{
slipIncr = iqtarget;
slipIncr <<= 12;
slipIncr /= slipconst;
slipIncr <<= 12;
slipIncr /= idtarget;
}
static uint32_t slipAngle = 0;
slipAngle += slipIncr;
slipAngle &= 0xFFFFFF;
uint16_t rotorAngle = Encoder::GetRotorAngle();
angle = -polePairRatio * rotorAngle + (slipAngle >> 8);
Param::SetFixed(Param::fslipspnt, fslip);
Code: Select all
// Caculate slip frequency in Hz
fslip = (slipIncr * pwmfrq) >> 19;
Param::SetFixed(Param::fslipspnt, fslip);
// Calculate rotor frequency in Hz
frq = ABS(polePairRatio * Encoder::GetRotorFrequency() + fslip);
Param::SetFixed(Param::fstat, frq);
Code: Select all
// Apply corrections to IQ and ID voltages
static int32_t ud = 0;
int32_t dError = idtarget - FOC::id;
ud += FP_MUL(curkp, dError)/1000;
if(ud > 26737) ud = 26737;
if(ud < -26737) ud = -26737;
static int32_t uq = 0;
int32_t qError = iqtarget - FOC::iq;
uq += FP_MUL(curkp, qError)/1000;
if(uq > 26737) uq = 26737;
if(uq < -26737) uq = -26737;
Param::SetFixed(Param::ud, ud);
Param::SetFixed(Param::uq, uq);
Code: Select all
// Inverse Park Clarke
FOC::SetAngle(angle);
FOC::InvParkClarke(ud, uq);
/* Match to PWM resolution */
timer_set_oc_value(PWM_TIMER, TIM_OC1, FOC::DutyCycles[0] >> shiftForTimer);
timer_set_oc_value(PWM_TIMER, TIM_OC2, FOC::DutyCycles[1] >> shiftForTimer);
timer_set_oc_value(PWM_TIMER, TIM_OC3, FOC::DutyCycles[2] >> shiftForTimer);
I think it's worth appreciating that ultimately, this code is still just controlling current and slip, much like my "current control" version. The substantial difference is that by handling d-axis and q-axis current independently, it should now be able to control negative currents.
Video of this code running on my bench.
Here are my calcs in case they are of any use?