//! \brief Sets up the throttle module parameters initially
//! \param[in] handle The throttle handle
extern void THROTTLE_setParams(THROTTLE_Handle handle, \
const bool invert,
const _iq max_adc, \
const _iq min_adc, \
const _iq max_out, \
const _iq min_out)
{
THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;
obj->flagSw1 = false;
obj->flagSw2 = false;
obj->max_adc = max_adc;
obj->min_adc = min_adc;
obj->max_out = max_out;
obj->min_out = min_out;
obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);
obj->state = THROTTLE_Run;
obj->value = _IQ(0.0);
return;
}
void rate_generator()
{
static _iq time = 0;
_iq f;
if (system.state == running_state)
{
if (time < system.Tacc)
{
f = _IQmpy(_IQdiv(system.Fref, system.Tacc), time);
system.Fout = f;
time += pwm.pwm_period;
uf_characteristics();
pwm.angle_step = _IQmpy(system.Fout, pwm.pwm_period);
}
else
{
system.Fout = system.uf.F[1];
system.Uref = system.uf.U[1];
}
}
else if (system.state == ready_state)
{
time = 0;
system.Fout = system.uf.F[0];
system.Uref = system.uf.U[0];
}
}
void smopos_calc(SMOPOS *v)
{
_iq E0;
E0 = _IQ(0.5);
// Sliding mode current observer
v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha-v->Ealpha-v->Zalpha));
v->EstIbeta = _IQmpy(v->Fsmopos,v->EstIbeta) + _IQmpy(v->Gsmopos,(v->Vbeta-v->Ebeta-v->Zbeta));
// Current errors
v->IalphaError = v->EstIalpha - v->Ialpha;
v->IbetaError = v->EstIbeta - v->Ibeta;
// Sliding control calculator
if (_IQabs(v->IalphaError) < E0)
v->Zalpha = _IQmpy(v->Kslide,_IQdiv(v->IalphaError,E0));
else if (v->IalphaError >= E0)
v->Zalpha = v->Kslide;
else if (v->IalphaError <= -E0)
v->Zalpha = -v->Kslide;
if (_IQabs(v->IbetaError) < E0)
v->Zbeta = _IQmpy(v->Kslide,_IQdiv(v->IbetaError,E0));
else if (v->IbetaError >= E0)
v->Zbeta = v->Kslide;
else if (v->IbetaError <= -E0)
v->Zbeta = -v->Kslide;
// Sliding control filter -> back EMF calculator
v->Ealpha = v->Ealpha + _IQmpy(v->Kslf,(v->Zalpha-v->Ealpha));
v->Ebeta = v->Ebeta + _IQmpy(v->Kslf,(v->Zbeta-v->Ebeta));
// Rotor angle calculator -> Theta = atan(-Ealpha,Ebeta)
v->Theta = _IQatan2PU(-v->Ealpha,v->Ebeta);
}
extern void THROTTLE_runState(THROTTLE_Handle handle)
{
THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;
if(obj->flagSw1)
{
obj->result = _IQ(0.0);
obj->state = THROTTLE_CalMaxMin;
obj->max_adc = obj->min_out;
obj->min_adc = obj->max_out;
}
else
{
switch (obj->state)
{
case THROTTLE_CalMaxMin:
if (obj->value > obj->max_adc)
obj->max_adc = obj->value;
else if (obj->value < obj->min_adc)
obj->min_adc = obj->value;
else if (obj->flagSw2)
obj->state = THROTTLE_CalCalc;
break;
case THROTTLE_CalCalc:
{
obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);
obj->state = THROTTLE_Run;
}
break;
case THROTTLE_Run:
{
_iq result = _IQmpy(obj->value,obj->slope) + obj->offset;
obj->result = _IQsat(result,obj->max_out,obj->min_out);
}
break;
}
}
return;
}
__interrupt void ecapISR(void)
{
// Clear capture (CAP) interrupt flags
CAP_clearInt(halHandle->capHandle, CAP_Int_Type_All);
// Compute the PWM high period (rising edge to falling edge)
uint32_t PwmDuty = (uint32_t) CAP_getCap2(halHandle->capHandle) - (uint32_t) CAP_getCap1(halHandle->capHandle);
// Assign the appropriate speed command, combine 0-5% and 95-100%
// 0-100% speed is proportional to 1-2ms high period
// 60MHz * 2ms = 120000
// 0-1%
if (PwmDuty <= 61000)
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQ(0);
gMotorVars.Flag_Run_Identify = 0;
}
// 1-99%
if ((PwmDuty > 61000) && (PwmDuty < 119000))
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQdiv(PwmDuty - 60000, 60000);
gMotorVars.Flag_Run_Identify = 1;
}
// 99-100%
else if (PwmDuty >= 119000)
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQ(1.0);
gMotorVars.Flag_Run_Identify = 1;
}
// Catch all
else
{
gSpeedRef_duty = _IQ(0);
gMotorVars.Flag_Run_Identify = 0;
}
// Clears an interrupt defined by group number
PIE_clearInt(halHandle->pieHandle, PIE_GroupNumber_4);
} // end of ecapISR() function
extern void VS_FREQ_setProfile(VS_FREQ_Handle handle,
float_t LowFreq, float_t HighFreq,
float_t VoltMin, float_t VoltMax)
{
VS_FREQ_Obj *obj = (VS_FREQ_Obj *)handle;
obj->LowFreq = _IQ(LowFreq/obj->iqFullScaleFreq_Hz);
obj->HighFreq = _IQ(HighFreq/obj->iqFullScaleFreq_Hz);
obj->VoltMin = _IQ(VoltMin/obj->iqFullScaleVoltage_V);
obj->VoltMax = _IQ(VoltMax/obj->iqFullScaleVoltage_V);
obj->VfSlope = _IQdiv((obj->VoltMax - obj->VoltMin), (obj->HighFreq - obj->LowFreq));
obj->Vdq_gain.value[0] = _IQ(0.3);
obj->Vdq_gain.value[1] = _IQsqrt(_IQ(obj->maxVsMag_pu*obj->maxVsMag_pu) - _IQmpy(obj->Vdq_gain.value[0], obj->Vdq_gain.value[0]));
return;
} // end of VS_FREQ_setProfile() function
//----------------------------------------------------------------------------
// Main code:
//----------------------------------------------------------------------------
int main(void)
{
unsigned int i;
_iq tempX, tempY, tempP, tempM, tempMmax;
// char buffer[20];
int *WatchdogWDCR = (void *) 0x7029;
// Disable the watchdog:
asm(" EALLOW ");
*WatchdogWDCR = 0x0068;
asm(" EDIS ");
Step.Xsize = _IQ(STEP_X_SIZE);
Step.Ysize = _IQ(STEP_Y_SIZE);
Step.Yoffset = 0;
Step.X = 0;
Step.Y = Step.Yoffset;
// Fill the buffers with some initial value
for(i=0; i < DATA_LOG_SIZE; i++)
{
Dlog.Xwaveform[i] = _IQ(0.0);
Dlog.Ywaveform[i] = _IQ(0.0);
Dlog.Mag[i] = _IQ(0.0);
Dlog.Phase[i] = _IQ(0.0);
Dlog.Exp[i] = _IQ(0.0);
}
Step.GainX = _IQ(X_GAIN);
Step.FreqX = _IQ(X_FREQ);
Step.GainY = _IQ(Y_GAIN);
Step.FreqY = _IQ(Y_FREQ);
// Calculate maximum magnitude value:
tempMmax = _IQmag(Step.GainX, Step.GainY);
for(i=0; i < DATA_LOG_SIZE; i++)
{
// Calculate waveforms:
Step.X = Step.X + _IQmpy(Step.Xsize, Step.FreqX);
if( Step.X > _IQ(2*PI) )
Step.X -= _IQ(2*PI);
Step.Y = Step.Y + _IQmpy(Step.Ysize, Step.FreqY);
if( Step.Y > _IQ(2*PI) )
Step.Y -= _IQ(2*PI);
Dlog.Xwaveform[i] = tempX = _IQmpy(_IQsin(Step.X), Step.GainX);
Dlog.Ywaveform[i] = tempY = _IQmpy(_IQabs(_IQsin(Step.Y)), Step.GainY);
// Calculate normalized magnitude:
//
// Mag = sqrt(X^2 + Y^2)/sqrt(GainX^2 + GainY^2);
tempM = _IQmag(tempX, tempY);
Dlog.Mag[i] = _IQdiv(tempM, tempMmax);
// Calculate normalized phase:
//
// Phase = (long) (atan2PU(X,Y) * 360);
tempP = _IQatan2PU(tempY,tempX);
Dlog.Phase[i] = _IQmpyI32int(tempP, 360);
// Use the exp function
Dlog.Exp[i] = _IQexp(_IQmpy(_IQ(.075L),_IQ(i)));
// Use the asin function
Dlog.Asin[i] = _IQasin(Dlog.Xwaveform[i]);
}
}
void ScalarControl(void)
{
_iq Ds, Udc, angle;
_iq sin, cos;
Udc = _IQ(20.0); //system.Udc;
Ds = _IQdiv(_IQmpy(system.Uref, _IQ(K_MOD_SPACEPWM)), Udc);
if (Ds > _IQ(0.98))
Ds = _IQ(0.98);
system.Uout = _IQmpy( _IQmpy(Udc, Ds), _IQ(1/K_MOD_SPACEPWM));
angle = pwm.angle;
angle += pwm.angle_step;
if (angle >= _IQ(1.0))
{
angle -= _IQ(1.0);
pwm.period_out = 1;
}
pwm.angle = angle;
sin = _IQsinPU(angle);
cos = _IQcosPU(angle);
svgen_dq.Ualpha = _IQmpy(Ds, cos);
svgen_dq.Ubeta = _IQmpy(Ds, sin);
svgen_dq.calc(&svgen_dq);
#if 0
_iq Ds, Udc, Angle;
_iq sinT, cosT;
Udc = ctom.measData.UdcFast;
Ds = _IQdiv(_IQmpy(mtoc.mFControl.Uref, _IQ(K_MOD_SPACEPWM)), Udc);
if(Ds > _IQ(0.98)) Ds = _IQ(0.98);
ctom.cFControl.Uout = _IQmpy( _IQmpy(Udc, Ds), _IQ(1/K_MOD_SPACEPWM));
Angle = ctom.cFControl.Angle;
Angle += mtoc.mFControl.StepAngle;
if(Angle > _IQ( 1.0)){
Angle -= _IQ(1.0);
ctom.measData.periodOut = 1;
}
else if(Angle < _IQ(-1.0)){
Angle += _IQ(1.0);
ctom.measData.periodOut = 1;
}
ctom.cFControl.Angle = Angle;
sinT = _IQsinPU(Angle);
cosT = _IQcosPU(Angle);
svgen_dq.Ualpha = _IQmpy(Ds, cosT);
svgen_dq.Ubeta = _IQmpy(Ds, sinT);
svgen_dq.calc(&svgen_dq);
/*
pwm1.MfuncC1 = (s16)_IQtoIQ15(svgen_dq.Tc); // MfuncC1 is in Q15
pwm1.MfuncC2 = (s16)_IQtoIQ15(svgen_dq.Ta); // MfuncC2 is in Q15
pwm1.MfuncC3 = (s16)_IQtoIQ15(svgen_dq.Tb); // MfuncC3 is in Q15
pwm1.triol_update(&pwm1);
v->Tc = (s16)_IQtoIQ15(svgen_dq.Tc);
v->Ta = (s16)_IQtoIQ15(svgen_dq.Ta);
v->Tb = (s16)_IQtoIQ15(svgen_dq.Tb);
*/
#endif
}
void uf_characteristics()
{
system.Uref = _IQmpy(_IQdiv(system.uf.U[1] - system.uf.U[0], system.uf.F[1] - system.uf.F[0]), system.Fout);
}
void FREQCAL_Calc(FREQCAL *p)
{
unsigned long tmp;
_iq newp,oldp;
// Check unit Time out-event for speed calculation:
// Unit Timer is configured for 100Hz in INIT function
//**** Freq Calculation using QEP position counter ****//
// For a more detailed explanation of the calculation, read
// the description at the top of this file
if(EQep1Regs.QFLG.bit.UTO==1) // Unit Timeout event
{
/** Differentiator **/
newp=EQep1Regs.QPOSLAT; // Latched POSCNT value
oldp=p->oldpos;
if (newp>oldp)
tmp = newp - oldp; // x2-x1 in v=(x2-x1)/T equation
else
tmp = (0xFFFFFFFF-oldp)+newp;
p->freq_fr = _IQdiv(tmp,p->freqScaler_fr); // p->freq_fr = (x2-x1)/(T*10KHz)
tmp=p->freq_fr;
if (tmp>=_IQ(1)) // is freq greater than max freq (10KHz for this example)?
p->freq_fr = _IQ(1);
else
p->freq_fr = tmp;
p->freqhz_fr = _IQmpy(p->BaseFreq,p->freq_fr); // Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
// p->freqhz_fr = (p->freq_fr)*10kHz = (x2-x1)/T
// Update position counter
p->oldpos = newp;
//=======================================
EQep1Regs.QCLR.bit.UTO=1; // Clear interrupt flag
}
//**** Freq Calculation using QEP capture counter ****//
if(EQep1Regs.QEPSTS.bit.UPEVNT==1) // Unit Position Event
{
if(EQep1Regs.QEPSTS.bit.COEF==0) // No Capture overflow
tmp=(unsigned long)EQep1Regs.QCPRDLAT;
else // Capture overflow, saturate the result
tmp=0xFFFF;
p->freq_pr = _IQdiv(p->freqScaler_pr,tmp); // p->freq_pr = X/[(t2-t1)*10KHz]
tmp=p->freq_pr;
if (tmp>_IQ(1))
p->freq_pr = _IQ(1);
else
p->freq_pr = tmp;
p->freqhz_pr = _IQmpy(p->BaseFreq,p->freq_pr); // Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q
// p->freqhz_pr =( p->freq_pr)*10kHz = X/(t2-t1)
EQep1Regs.QEPSTS.all=0x88; // Clear Unit position event flag
// Clear overflow error flag
}
}
