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.