void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback, iqReference, gSpdError, gUp, gUi;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
// Run the SpinTAC Controller
// Note that the library internal ramp generator is used to set the speed reference
STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); // Internal ramp generator does not provide Acceleration Reference
STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
STVELCTL_run(stObj->velCtlHandle);
// select SpinTAC Velocity Controller
iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);
if(gMotorVars.SpinTAC.VelCtlEnb == true) {
// Set the Iq reference that came out of SpinTAC Velocity Control
CTRL_setIq_ref_pu(ctrlHandle, iqReference);
// Update internal PI integrator, if running SpinTAC
gSpdError = TRAJ_getIntValue(ctrlObj->trajHandle_spd) - speedFeedback;
gUp = _IQmpy(gSpdError, CTRL_getKp(ctrlHandle, CTRL_Type_PID_spd));
gUi = _IQmpy(gSpdError, CTRL_getKi(ctrlHandle, CTRL_Type_PID_spd));
CTRL_setUi(ctrlHandle, CTRL_Type_PID_spd, iqReference - gUp - gUi);
}
}
interrupt void mainISR(void)
{
// toggle status LED
if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
{
HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
gLEDcnt = 0;
}
// acknowledge the ADC interrupt
HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
// convert the ADC data
HAL_readAdcData(halHandle,&gAdcData);
// run the controller
CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);
// write the PWM compare values
HAL_writePwmData(halHandle,&gPwmData);
if(FW_getFlag_enableFw(fwHandle) == true)
{
FW_incCounter(fwHandle);
if(FW_getCounter(fwHandle) > FW_getNumIsrTicksPerFwTick(fwHandle))
{
_iq refValue;
_iq fbackValue;
_iq output;
FW_clearCounter(fwHandle);
refValue = gMotorVars.VsRef;
fbackValue = gMotorVars.Vs;
FW_run(fwHandle, refValue, fbackValue, &output);
CTRL_setId_ref_pu(ctrlHandle, output);
gMotorVars.IdRef_A = _IQmpy(CTRL_getId_ref_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
}
}
else
{
CTRL_setId_ref_pu(ctrlHandle, _IQmpy(gMotorVars.IdRef_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
}
// setup the controller
CTRL_setup(ctrlHandle);
return;
} // end of mainISR() function
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 ST_runVelMove(ST_Handle handle, CTRL_Handle ctrlHandle)
{
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Run SpinTAC Move
// If we are not in reset, and the SpeedRef_krpm has been modified
if((EST_getState(ctrlObj->estHandle) == EST_State_OnLine) && (_IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STVELMOVE_getVelocityEnd(stObj->velMoveHandle))) {
// Get the configuration for SpinTAC Move
STVELMOVE_setCurveType(stObj->velMoveHandle, gMotorVars.SpinTAC.VelMoveCurveType);
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setAccelerationLimit(stObj->velMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setJerkLimit(stObj->velMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
// Enable SpinTAC Move
STVELMOVE_setEnable(stObj->velMoveHandle, true);
// If starting from zero speed, enable ForceAngle, otherwise disable ForceAngle
if(_IQabs(STVELMOVE_getVelocityStart(stObj->velMoveHandle)) < _IQ(ST_MIN_ID_SPEED_PU)) {
EST_setFlag_enableForceAngle(ctrlObj->estHandle, true);
gMotorVars.Flag_enableForceAngle = true;
}
else {
EST_setFlag_enableForceAngle(ctrlObj->estHandle, false);
gMotorVars.Flag_enableForceAngle = false;
}
}
STVELMOVE_run(stObj->velMoveHandle);
}
void pll_1p_compute(PLL_1P *v)
{
_iq OMEGA_0 = _IQmpy(6.2831852, v->setFreq);
//------------------------------------//
// OPT Input Notch Filter (2P2Z) //
//------------------------------------//
if(v->Notch_EN == true) {
v->Notch1.In = _IQmpy(v->In, v->out_cosine);
v->Notch1.compute(&v->Notch1);
v->pi1.Ref = v->Notch1.Out;
}
else {
v->pi1.Ref = _IQmpy(v->In, v->out_cosine);
}
//------------------------------------//
// PLL Loop Filter (PI) //
//------------------------------------//
v->pi1.Fdb = 0;
v->pi1.Kp = v->Kp;
v->pi1.Ki_ts = v->Ki_ts;
v->pi1.OutMax = v->OutMax;
v->pi1.OutMin = v->OutMin;
v->pi1.compute(&v->pi1);
OMEGA_0 = OMEGA_0 + v->pi1.Out;
//------------------------------------//
// Integration (1/s) //
//------------------------------------//
v->intg1.In = OMEGA_0;
v->intg1.ClampMax = 6.2831852;
v->intg1.ClampMin = 0;
v->intg1.compute(&v->intg1);
// PLL Outputs
//--------------------------------------
v->out_theta = v->intg1.Out;
v->out_sine = _IQsin(v->out_theta);
v->out_cosine = _IQcos(v->out_theta);
}
void updateGlobalVariables_motor(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *) handle;
#ifdef __TMS320C28XX_FPU32__
int32_t tmp;
#endif
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle), EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// get the magnetizing current
fpu_assign(gMotorVars.MagnCurr_A ,EST_getIdRated( obj->estHandle ));
// get the rotor resistance
fpu_assign(gMotorVars.Rr_Ohm, EST_getRr_Ohm( obj->estHandle ));
// get the stator resistance
fpu_assign(gMotorVars.Rs_Ohm , EST_getRs_Ohm( obj->estHandle ));
// get the stator inductance in the direct coordinate direction
fpu_assign(gMotorVars.Lsd_H, EST_getLs_d_H( obj->estHandle ));
// get the stator inductance in the quadrature coordinate direction
fpu_assign(gMotorVars.Lsq_H , EST_getLs_q_H( obj->estHandle ));
// get the flux in V/Hz in floating point
fpu_assign(gMotorVars.Flux_VpHz , EST_getFlux_VpHz( obj->estHandle ));
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus, _IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
return;
} // end of updateGlobalVariables_motor() function
void updateGlobalVariables_motor(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
int32_t tmp;
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// when calling EST_ functions that return a float, and fpu32 is enabled, an integer is needed as a return
// so that the compiler reads the returned value from the accumulator instead of fpu32 registers
// get the magnetizing current
tmp = EST_getIdRated(obj->estHandle);
gMotorVars.MagnCurr_A = *((float_t *)&tmp);
// get the rotor resistance
tmp = EST_getRr_Ohm(obj->estHandle);
gMotorVars.Rr_Ohm = *((float_t *)&tmp);
// get the stator resistance
tmp = EST_getRs_Ohm(obj->estHandle);
gMotorVars.Rs_Ohm = *((float_t *)&tmp);
// get the stator inductance in the direct coordinate direction
tmp = EST_getLs_d_H(obj->estHandle);
gMotorVars.Lsd_H = *((float_t *)&tmp);
// get the stator inductance in the quadrature coordinate direction
tmp = EST_getLs_q_H(obj->estHandle);
gMotorVars.Lsq_H = *((float_t *)&tmp);
// get the flux in V/Hz in floating point
tmp = EST_getFlux_VpHz(obj->estHandle);
gMotorVars.Flux_VpHz = *((float_t *)&tmp);
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
return;
} // end of updateGlobalVariables_motor() function
//! \brief Computes Torque in Nm
//!
_iq USER_computeTorque_Nm(CTRL_Handle handle, const _iq torque_Flux_sf, const _iq torque_Ls_sf)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq Flux_pu = EST_getFlux_pu(obj->estHandle);
_iq Id_pu = PID_getFbackValue(obj->pidHandle_Id);
_iq Iq_pu = PID_getFbackValue(obj->pidHandle_Iq);
_iq Ld_minus_Lq_pu = _IQ30toIQ(EST_getLs_d_pu(obj->estHandle)-EST_getLs_q_pu(obj->estHandle));
_iq Torque_Flux_Iq_Nm = _IQmpy(_IQmpy(Flux_pu,Iq_pu),torque_Flux_sf);
_iq Torque_Ls_Id_Iq_Nm = _IQmpy(_IQmpy(_IQmpy(Ld_minus_Lq_pu,Id_pu),Iq_pu),torque_Ls_sf);
_iq Torque_Nm = Torque_Flux_Iq_Nm + Torque_Ls_Id_Iq_Nm;
return(Torque_Nm);
} // end of USER_computeTorque_Nm() function
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;
}
//! \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 runRsOnLine(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
// execute Rs OnLine code
if(gMotorVars.Flag_Run_Identify == true)
{
if(EST_getState(obj->estHandle) == EST_State_OnLine)
{
float_t RsError_Ohm = gMotorVars.RsOnLine_Ohm - gMotorVars.Rs_Ohm;
EST_setFlag_enableRsOnLine(obj->estHandle,true);
EST_setRsOnLineId_mag_pu(obj->estHandle,_IQmpy(gMotorVars.RsOnLineCurrent_A,_IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
if(abs(RsError_Ohm) < (gMotorVars.Rs_Ohm * 0.05))
{
EST_setFlag_updateRs(obj->estHandle,true);
}
}
else
{
EST_setRsOnLineId_mag_pu(obj->estHandle,_IQ(0.0));
EST_setRsOnLineId_pu(obj->estHandle,_IQ(0.0));
EST_setRsOnLine_pu(obj->estHandle,_IQ(0.0));
EST_setFlag_enableRsOnLine(obj->estHandle,false);
EST_setFlag_updateRs(obj->estHandle,false);
EST_setRsOnLine_qFmt(obj->estHandle,EST_getRs_qFmt(obj->estHandle));
}
}
return;
} // end of runRsOnLine() function
void CTRL_setSpd_ref_krpm(CTRL_Handle handle,const _iq spd_ref_krpm)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq krpm_to_pu_sf = EST_get_krpm_to_pu_sf(obj->estHandle);
_iq spd_ref_pu = _IQmpy(spd_ref_krpm,krpm_to_pu_sf);
obj->spd_ref = spd_ref_pu;
return;
} // end of CTRL_setSpd_ref_krpm() function
void RFFT_f32_sincostable(RFFT_F32_STRUCT *fft)
{
_iq N, tempPI, temp;
Uint16 i, j, k, l;
tempPI = _IQ(0.7853981633975L); // pi/4
k = 1;
l = 0;
for(i = 3; i <= fft->FFTStages; i++)
{
N = _IQ(1.0);
for(j=1; j <= k; j ++)
{
temp = _IQmpy(N, tempPI);
fft->CosSinBuf[l++] = _IQcos(temp);
fft->CosSinBuf[l++] = _IQsin(temp);
N = N + _IQ(1.0);
}
fft->CosSinBuf[l++] = _IQ(0.0);
fft->CosSinBuf[l++] = _IQ(1.0);
k = (k * 2) + 1;
tempPI = _IQmpy(tempPI, _IQ(0.5));
}
}
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
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);
}
void svgendq_calc(SVGENDQ *v)
{
_iq Va,Vb,Vc,t1,t2;
Uint32 Sector = 0; // Sector is treated as Q0 - independently with global Q
// Inverse clarke transformation
Va = v->Ubeta;
Vb = _IQmpy(_IQ(-0.5),v->Ubeta) + _IQmpy(_IQ(0.8660254),v->Ualpha); // 0.8660254 = sqrt(3)/2
Vc = _IQmpy(_IQ(-0.5),v->Ubeta) - _IQmpy(_IQ(0.8660254),v->Ualpha); // 0.8660254 = sqrt(3)/2
// 60 degree Sector determination
if (Va>_IQ(0))
Sector = 1;
if (Vb>_IQ(0))
Sector = Sector + 2;
if (Vc>_IQ(0))
Sector = Sector + 4;
// X,Y,Z (Va,Vb,Vc) calculations
Va = v->Ubeta; // X = Va
Vb = _IQmpy(_IQ(0.5),v->Ubeta) + _IQmpy(_IQ(0.8660254),v->Ualpha); // Y = Vb
Vc = _IQmpy(_IQ(0.5),v->Ubeta) - _IQmpy(_IQ(0.8660254),v->Ualpha); // Z = Vc
if (Sector==0) // Sector 0: this is special case for (Ualpha,Ubeta) = (0,0)
{
v->Ta = _IQ(0.5);
v->Tb = _IQ(0.5);
v->Tc = _IQ(0.5);
}
if (Sector==1) // Sector 1: t1=Z and t2=Y (abc ---> Tb,Ta,Tc)
{
t1 = Vc;
t2 = Vb;
v->Tb = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // tbon = (1-t1-t2)/2
v->Ta = v->Tb+t1; // taon = tbon+t1
v->Tc = v->Ta+t2; // tcon = taon+t2
}
else if (Sector==2) // Sector 2: t1=Y and t2=-X (abc ---> Ta,Tc,Tb)
{
t1 = Vb;
t2 = -Va;
v->Ta = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // taon = (1-t1-t2)/2
v->Tc = v->Ta+t1; // tcon = taon+t1
v->Tb = v->Tc+t2; // tbon = tcon+t2
}
else if (Sector==3) // Sector 3: t1=-Z and t2=X (abc ---> Ta,Tb,Tc)
{
t1 = -Vc;
t2 = Va;
v->Ta = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // taon = (1-t1-t2)/2
v->Tb = v->Ta+t1; // tbon = taon+t1
v->Tc = v->Tb+t2; // tcon = tbon+t2
}
else if (Sector==4) // Sector 4: t1=-X and t2=Z (abc ---> Tc,Tb,Ta)
{
t1 = -Va;
t2 = Vc;
v->Tc = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // tcon = (1-t1-t2)/2
v->Tb = v->Tc+t1; // tbon = tcon+t1
v->Ta = v->Tb+t2; // taon = tbon+t2
}
else if (Sector==5) // Sector 5: t1=X and t2=-Y (abc ---> Tb,Tc,Ta)
{
t1 = Va;
t2 = -Vb;
v->Tb = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // tbon = (1-t1-t2)/2
v->Tc = v->Tb+t1; // tcon = tbon+t1
v->Ta = v->Tc+t2; // taon = tcon+t2
}
else if (Sector==6) // Sector 6: t1=-Y and t2=-Z (abc ---> Tc,Ta,Tb)
{
t1 = -Vb;
t2 = -Vc;
v->Tc = _IQmpy(_IQ(0.5),(_IQ(1)-t1-t2)); // tcon = (1-t1-t2)/2
v->Ta = v->Tc+t1; // taon = tcon+t1
v->Tb = v->Ta+t2; // tbon = taon+t2
}
// Convert the unsigned GLOBAL_Q format (ranged (0,1)) -> signed GLOBAL_Q format (ranged (-1,1))
v->Ta = _IQmpy(_IQ(2.0),(v->Ta-_IQ(0.5)));
v->Tb = _IQmpy(_IQ(2.0),(v->Tb-_IQ(0.5)));
v->Tc = _IQmpy(_IQ(2.0),(v->Tc-_IQ(0.5)));
}
//! \brief Computes Flux in Wb or V/Hz depending on the scale factor sent as parameter
//!
_iq USER_computeFlux(CTRL_Handle handle, const _iq sf)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
return(_IQmpy(EST_getFlux_pu(obj->estHandle),sf));
} // end of USER_computeFlux() function
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
#endif
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
// do not allow code execution if there is a user parameter error
if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// Initialize field weakening
fwHandle = FW_init(&fw,sizeof(fw));
// Disable field weakening
FW_setFlag_enableFw(fwHandle, false);
// Clear field weakening counter
FW_clearCounter(fwHandle);
// Set the number of ISR per field weakening ticks
FW_setNumIsrTicksPerFwTick(fwHandle, FW_NUM_ISR_TICKS_PER_CTRL_TICK);
// Set the deltas of field weakening
FW_setDeltas(fwHandle, FW_INC_DELTA, FW_DEC_DELTA);
// Set initial output of field weakening to zero
FW_setOutput(fwHandle, _IQ(0.0));
// Set the field weakening controller limits
FW_setMinMax(fwHandle,_IQ(USER_MAX_NEGATIVE_ID_REF_CURRENT_A/USER_IQ_FULL_SCALE_CURRENT_A),_IQ(0.0));
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
Flag_Latch_softwareUpdate = true;
// Enable the Library internal PI. Iq is referenced by the speed PI now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
else
{
// set the current bias
HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));
// set the voltage bias
HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));
}
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
_iq Is_Max_squared_pu = _IQ((USER_MOTOR_MAX_CURRENT*USER_MOTOR_MAX_CURRENT)/ \
(USER_IQ_FULL_SCALE_CURRENT_A*USER_IQ_FULL_SCALE_CURRENT_A));
_iq Id_squared_pu = _IQmpy(CTRL_getId_ref_pu(ctrlHandle),CTRL_getId_ref_pu(ctrlHandle));
// Take into consideration that Iq^2+Id^2 = Is^2
Iq_Max_pu = _IQsqrt(Is_Max_squared_pu-Id_squared_pu);
//Set new max trajectory
CTRL_setSpdMax(ctrlHandle, Iq_Max_pu);
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
// initialize the watch window kp and ki current values with pre-calculated values
gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
}
}
else
{
Flag_Latch_softwareUpdate = true;
// initialize the watch window kp and ki values with pre-calculated values
gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle);
}
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// set field weakening enable flag depending on user's input
FW_setFlag_enableFw(fwHandle,gMotorVars.Flag_enableFieldWeakening);
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
} // end of for(;;) loop
} // end of main() function
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
#endif
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
// do not allow code execution if there is a user parameter error
if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
// initialize the SpinTAC Components
stHandle = ST_init(&st_obj, sizeof(st_obj));
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
ST_setupVelMove(stHandle);
ST_setupVelPlan(stHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// Dis-able the Library internal PI. Iq has no reference now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
ST_Obj *stObj = (ST_Obj *)stHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
else
{
// set the current bias
HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));
// set the voltage bias
HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));
}
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
// enable the SpinTAC Speed Controller
STVELCTL_setEnable(stObj->velCtlHandle, true);
if(EST_getState(obj->estHandle) != EST_State_OnLine)
{
// if the estimator is not running, place SpinTAC into reset
STVELCTL_setEnable(stObj->velCtlHandle, false);
// if the estimator is not running, set SpinTAC Move start & end velocity to 0
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQ(0.0));
STVELMOVE_setVelocityStart(stObj->velMoveHandle, _IQ(0.0));
}
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
// initialize the watch window kp and ki current values with pre-calculated values
gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
// initialize the watch window Bw value with the default value
gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);
// initialize the watch window with maximum and minimum Iq reference
gMotorVars.SpinTAC.VelCtlOutputMax_A = _IQmpy(STVELCTL_getOutputMaximum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
gMotorVars.SpinTAC.VelCtlOutputMin_A = _IQmpy(STVELCTL_getOutputMinimum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle, stHandle);
}
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// set the SpinTAC (ST) bandwidth scale
STVELCTL_setBandwidth_radps(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBw_radps);
// set the maximum and minimum values for Iq reference
STVELCTL_setOutputMaximums(stObj->velCtlHandle, _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
// setup the SpinTAC Components
ST_setupVelCtl(stHandle);
ST_setupVelMove(stHandle);
} // end of for(;;) loop
} // end of main() 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 updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle sthandle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
ST_Obj *stObj = (ST_Obj *)sthandle;
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// get the magnetizing current
gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);
// get the rotor resistance
gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);
// get the stator resistance
gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);
// get the stator inductance in the direct coordinate direction
gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);
// get the stator inductance in the quadrature coordinate direction
gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);
// get the flux in V/Hz in floating point
gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
// get the Iq reference from the speed controller
gMotorVars.IqRef_A = _IQmpy(STVELCTL_getTorqueReference(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
// gets the Velocity Controller status
gMotorVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle);
// get the inertia setting
gMotorVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));
// get the friction setting
gMotorVars.SpinTAC.FrictionEstimate_Aperkrpm = _IQmpy(STVELCTL_getFriction(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));
// get the Velocity Controller error
gMotorVars.SpinTAC.VelCtlErrorID = STVELCTL_getErrorID(stObj->velCtlHandle);
return;
} // end of updateGlobalVariables_motor() function
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
#ifdef F2802xF
//copy .econst to unsecure RAM
if(*econst_end - *econst_start)
{
memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
}
//copy .switch ot unsecure RAM
if(*switch_end - *switch_start)
{
memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
}
#endif
#endif
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
// do not allow code execution if there is a user parameter error
if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// initialize the Clarke modules
clarkeHandle_I = CLARKE_init(&clarke_I,sizeof(clarke_I));
clarkeHandle_V = CLARKE_init(&clarke_V,sizeof(clarke_V));
// set the number of current sensors
setupClarke_I(clarkeHandle_I,gUserParams.numCurrentSensors);
// set the number of voltage sensors
setupClarke_V(clarkeHandle_V,gUserParams.numVoltageSensors);
#ifdef FAST_ROM_V1p6
estHandle = controller_obj->estHandle;
#else
estHandle = ctrl.estHandle;
#endif
// initialize the inverse Park module
iparkHandle = IPARK_init(&ipark,sizeof(ipark));
// initialize the Park module
parkHandle = PARK_init(&park,sizeof(park));
// initialize the space vector generator module
svgenHandle = SVGEN_init(&svgen,sizeof(svgen));
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
// turn on the DRV8305 if present
HAL_enableDrv(halHandle);
// initialize the DRV8305 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
#endif
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// Enable the Library internal PI. Iq is referenced by the speed PI now
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
uint_least16_t cnt;
// update the ADC bias values
HAL_updateAdcBias(halHandle);
// record the current bias
for(cnt=0;cnt<3;cnt++)
gAdcBiasI.value[cnt] = HAL_getBias(halHandle,HAL_SensorType_Current,cnt);
// record the voltage bias
for(cnt=0;cnt<3;cnt++)
gAdcBiasV.value[cnt] = HAL_getBias(halHandle,HAL_SensorType_Voltage,cnt);
gMotorVars.Flag_enableOffsetcalc = false;
}
else
{
uint_least16_t cnt;
// set the current bias
for(cnt=0;cnt<3;cnt++)
HAL_setBias(halHandle,HAL_SensorType_Current,cnt,gAdcBiasI.value[cnt]);
// set the voltage bias
for(cnt=0;cnt<3;cnt++)
HAL_setBias(halHandle,HAL_SensorType_Voltage,cnt,gAdcBiasV.value[cnt]);
}
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
// initialize the watch window kp and ki current values with pre-calculated values
gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
// initialize the watch window kp and ki values with pre-calculated values
gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle);
}
// update Kp and Ki gains
updateKpKiGains(ctrlHandle);
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
} // end of for(;;) loop
} // end of main() function
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
}
}
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);
}
/****************************************************************************
* motorware main stuff
***************************************************************************/
void task_motorware()
{
for (;;)
{
// Waiting for enable system flag to be set
while (!(gMotorVars.Flag_enableSys))
{
Task_yield();
}
// loop while the enable system flag is true
while (gMotorVars.Flag_enableSys)
{
Task_yield(); //TODO: change this so that an outside timer activates the motorware task when needed
CTRL_Obj *obj = (CTRL_Obj *) ctrlHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle, gMotorVars.Flag_enableUserParams);
if (CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle, false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle,
gMotorVars.Flag_Run_Identify);
if (flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if (ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if (ctrlState == CTRL_State_OnLine)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle, HAL_SensorType_Current, 0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle, HAL_SensorType_Current, 1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle, HAL_SensorType_Current, 2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle, HAL_SensorType_Voltage, 0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle, HAL_SensorType_Voltage, 1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle, HAL_SensorType_Voltage, 2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if (ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if ((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true)
&& (ctrlState > CTRL_State_Idle)
&& (gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if (EST_isMotorIdentified(obj->estHandle))
{
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle, gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle, _IQmpy(MAX_ACCEL_KRPMPS_SF, gMotorVars.MaxAccel_krpmps));
if (Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if (gCounter_updateGlobals
>= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle);
}
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle, gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle, gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle, &gDrvSpi8301Vars);
HAL_readDrvData(halHandle, &gDrvSpi8301Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle, &gUserParams);
gMotorVars.Flag_Run_Identify = false;
} // end of for(;;) loop
} // end of task_motorware() function
void ST_runVelId(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback, iqReference;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
if(gMotorVars.SpinTAC.VelIdRun == true) {
// if beginning the SpinTAC Identify process
// set the speed reference to zero
gMotorVars.SpeedRef_krpm = 0;
// wait until the actual speed is zero
if((_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU)) && (STVELID_getEnable(stObj->velIdHandle) == false)) {
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
gMotorVars.Flag_enableForceAngle = true;
EST_setFlag_enableForceAngle(ctrlObj->estHandle, gMotorVars.Flag_enableForceAngle);
// set the GoalSpeed
STVELID_setGoalSpeed(stObj->velIdHandle, _IQmpy(gMotorVars.SpinTAC.VelIdGoalSpeed_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
// set the Torque Ramp Time
STVELID_setTorqueRampTime_sec(stObj->velIdHandle, gMotorVars.SpinTAC.VelIdTorqueRampTime_sec);
// Enable SpinTAC Inertia Identify
STVELID_setEnable(stObj->velIdHandle, true);
// Set a positive speed reference to FAST to provide direction information
gMotorVars.SpeedRef_krpm = _IQ(0.001);
CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
}
}
// Run SpinTAC Inertia Identify
STVELID_setVelocityFeedback(stObj->velIdHandle, speedFeedback);
STVELID_run(stObj->velIdHandle);
if((STVELID_getStatus(stObj->velIdHandle) == ST_VEL_ID_IDLE) && (_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU))) {
// If inertia identification is successful, update the inertia setting of SpinTAC Velocity Controller
// EXAMPLE: STVELCTL_setInertia(stObj->velCtlHandle, STVELID_getInertiaEstimate(stObj->velIdHandle));
gMotorVars.SpinTAC.VelIdRun = false;
// return the speed reference to zero
gMotorVars.SpeedRef_krpm = gMotorVars.StopSpeedRef_krpm;
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
}
else if((STVELID_getErrorID(stObj->velIdHandle) != false) && (STVELID_getErrorID(stObj->velIdHandle) != ST_ID_INCOMPLETE_ERROR)) {
// if not done & in error, wait until speed is less than 1RPM to exit
if(_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU)) {
gMotorVars.SpinTAC.VelIdRun = false;
// return the speed reference to zero
gMotorVars.SpeedRef_krpm = gMotorVars.StopSpeedRef_krpm;
CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
}
}
// select SpinTAC Identify
iqReference = STVELID_getTorqueReference(stObj->velIdHandle);
// Set the Iq reference that came out of SpinTAC Velocity Control
CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
void main(void)
{
uint_least8_t estNumber = 0;
#ifdef FAST_ROM_V1p6
uint_least8_t ctrlNumber = 0;
#endif
// Only used if running from FLASH
// Note that the variable FLASH is defined by the project
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
#ifdef F2802xF
//copy .econst to unsecure RAM
if(*econst_end - *econst_start)
{
memCopy((uint16_t *)&econst_start,(uint16_t *)&econst_end,(uint16_t *)&econst_ram_load);
}
//copy .switch ot unsecure RAM
if(*switch_end - *switch_start)
{
memCopy((uint16_t *)&switch_start,(uint16_t *)&switch_end,(uint16_t *)&switch_ram_load);
}
#endif
#endif
// initialize the hardware abstraction layer
halHandle = HAL_init(&hal,sizeof(hal));
// check for errors in user parameters
USER_checkForErrors(&gUserParams);
// store user parameter error in global variable
gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);
// do not allow code execution if there is a user parameter error
if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
{
for(;;)
{
gMotorVars.Flag_enableSys = false;
}
}
// initialize the user parameters
USER_setParams(&gUserParams);
// set the hardware abstraction layer parameters
HAL_setParams(halHandle,&gUserParams);
// initialize the controller
#ifdef FAST_ROM_V1p6
ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber); //v1p6 format (06xF and 06xM devices)
controller_obj = (CTRL_Obj *)ctrlHandle;
#else
ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif
{
CTRL_Version version;
// get the version number
CTRL_getVersion(ctrlHandle,&version);
gMotorVars.CtrlVersion = version;
}
// set the default controller parameters
CTRL_setParams(ctrlHandle,&gUserParams);
// initialize the frequency of execution monitoring module
femHandle = FEM_init(&fem,sizeof(fem));
FEM_setParams(femHandle,
USER_SYSTEM_FREQ_MHz * 1000000.0, // timer frequency, Hz
(uint32_t)USER_SYSTEM_FREQ_MHz * 1000000, // timer period, cnts
USER_CTRL_FREQ_Hz, // set point frequency, Hz
1000.0); // max frequency error, Hz
// initialize the CPU usage module
cpu_usageHandle = CPU_USAGE_init(&cpu_usage,sizeof(cpu_usage));
CPU_USAGE_setParams(cpu_usageHandle,
(uint32_t)USER_SYSTEM_FREQ_MHz * 1000000, // timer period, cnts
(uint32_t)USER_ISR_FREQ_Hz); // average over 1 second of ISRs
// setup faults
HAL_setupFaults(halHandle);
// initialize the interrupt vector table
HAL_initIntVectorTable(halHandle);
// enable the ADC interrupts
HAL_enableAdcInts(halHandle);
// reload timer to start running frequency of execution monitoring
HAL_reloadTimer(halHandle,0);
// enable global interrupts
HAL_enableGlobalInts(halHandle);
// enable debug interrupts
HAL_enableDebugInt(halHandle);
// disable the PWM
HAL_disablePwm(halHandle);
#ifdef DRV8301_SPI
// turn on the DRV8301 if present
HAL_enableDrv(halHandle);
// initialize the DRV8301 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
// turn on the DRV8305 if present
HAL_enableDrv(halHandle);
// initialize the DRV8305 interface
HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
#endif
// enable DC bus compensation
CTRL_setFlag_enableDcBusComp(ctrlHandle, true);
// compute scaling factors for flux and torque calculations
gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();
// disable offsets recalibration by default
gMotorVars.Flag_enableOffsetcalc = false;
for(;;)
{
// Waiting for enable system flag to be set
while(!(gMotorVars.Flag_enableSys));
// loop while the enable system flag is true
while(gMotorVars.Flag_enableSys)
{
CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
// increment counters
gCounter_updateGlobals++;
// enable/disable the use of motor parameters being loaded from user.h
CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);
// enable/disable Rs recalibration during motor startup
EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);
// enable/disable automatic calculation of bias values
CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);
if(CTRL_isError(ctrlHandle))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(ctrlHandle,false);
// set the enable system flag to false
gMotorVars.Flag_enableSys = false;
// disable the PWM
HAL_disablePwm(halHandle);
}
else
{
// update the controller state
bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);
// enable or disable the control
CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);
if(flag_ctrlStateChanged)
{
CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);
if(ctrlState == CTRL_State_OffLine)
{
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_OnLine)
{
if(gMotorVars.Flag_enableOffsetcalc == true)
{
// update the ADC bias values
HAL_updateAdcBias(halHandle);
}
else
{
// set the current bias
HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));
// set the voltage bias
HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));
}
// Return the bias value for currents
gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);
// Return the bias value for voltages
gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);
// enable the PWM
HAL_enablePwm(halHandle);
}
else if(ctrlState == CTRL_State_Idle)
{
// disable the PWM
HAL_disablePwm(halHandle);
gMotorVars.Flag_Run_Identify = false;
}
if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
(ctrlState > CTRL_State_Idle) &&
(gMotorVars.CtrlVersion.minor == 6))
{
// call this function to fix 1p6
USER_softwareUpdate1p6(ctrlHandle);
}
}
}
if(EST_isMotorIdentified(obj->estHandle))
{
// set the current ramp
EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
gMotorVars.Flag_MotorIdentified = true;
// set the speed reference
CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);
// set the speed acceleration
CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
if(Flag_Latch_softwareUpdate)
{
Flag_Latch_softwareUpdate = false;
USER_calcPIgains(ctrlHandle);
}
}
else
{
Flag_Latch_softwareUpdate = true;
// the estimator sets the maximum current slope during identification
gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
}
// when appropriate, update the global variables
if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
{
// reset the counter
gCounter_updateGlobals = 0;
updateGlobalVariables_motor(ctrlHandle);
}
// get the maximum delta count observed
gMaxDeltaCntObserved = FEM_getMaxDeltaCntObserved(femHandle);
// check for errors
if(FEM_isFreqError(femHandle))
{
gNumFreqErrors = FEM_getErrorCnt(femHandle);
}
// update CPU usage
updateCPUusage();
// enable/disable the forced angle
EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);
// enable or disable power warp
CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);
#ifdef DRV8301_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);
HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif
#ifdef DRV8305_SPI
HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);
HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
#endif
} // end of while(gFlag_enableSys) loop
// disable the PWM
HAL_disablePwm(halHandle);
// set the default controller parameters (Reset the control to re-identify the motor)
CTRL_setParams(ctrlHandle,&gUserParams);
gMotorVars.Flag_Run_Identify = false;
} // end of for(;;) loop
} // end of main() function
void ST_runVelPlan(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
// SpinTAC Velocity Plan
if(gVelPlanRunFlag == ST_PLAN_STOP && gMotorVars.SpinTAC.VelPlanRun == ST_PLAN_START) {
if(gMotorVars.SpeedRef_krpm != 0) {
gMotorVars.SpeedRef_krpm = 0;
}
if(_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU)) {
if(STVELPLAN_getErrorID(stObj->velPlanHandle) != false) {
STVELPLAN_setEnable(stObj->velPlanHandle, false);
STVELPLAN_setReset(stObj->velPlanHandle, true);
gMotorVars.SpinTAC.VelPlanRun = gVelPlanRunFlag;
}
else {
STVELPLAN_setEnable(stObj->velPlanHandle, true);
STVELPLAN_setReset(stObj->velPlanHandle, false);
gVelPlanRunFlag = gMotorVars.SpinTAC.VelPlanRun;
}
}
}
if(gMotorVars.SpinTAC.VelPlanRun == ST_PLAN_STOP) {
STVELPLAN_setReset(stObj->velPlanHandle, true);
gVelPlanRunFlag = gMotorVars.SpinTAC.VelPlanRun;
}
if(gVelPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.VelPlanRun == ST_PLAN_PAUSE) {
STVELPLAN_setEnable(stObj->velPlanHandle, false);
gVelPlanRunFlag = gMotorVars.SpinTAC.VelPlanRun;
}
if(gVelPlanRunFlag == ST_PLAN_PAUSE && gMotorVars.SpinTAC.VelPlanRun == ST_PLAN_START) {
STVELPLAN_setEnable(stObj->velPlanHandle, true);
gVelPlanRunFlag = gMotorVars.SpinTAC.VelPlanRun;
}
// Run SpinTAC Velocity Plan
STVELPLAN_run(stObj->velPlanHandle);
// Update the global variable for the SpinTAC Plan State
gState = (PlanState_e)STVELPLAN_getCurrentState(stObj->velPlanHandle);
if(STVELPLAN_getStatus(stObj->velPlanHandle) != ST_PLAN_IDLE) {
// Send the profile configuration to SpinTAC Velocity Profile Generator
gMotorVars.SpeedRef_krpm = _IQmpy(STVELPLAN_getVelocitySetpoint(stObj->velPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
gMotorVars.MaxAccel_krpmps = _IQmpy(STVELPLAN_getAccelerationLimit(stObj->velPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
gMotorVars.MaxJrk_krpmps2 = _IQ20mpy(STVELPLAN_getJerkLimit(stObj->velPlanHandle), _IQ20(ST_SPEED_KRPM_PER_PU));
}
else
{
if(gVelPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.VelPlanRun == ST_PLAN_START) {
gMotorVars.SpinTAC.VelPlanRun = ST_PLAN_STOP;
gVelPlanRunFlag = gMotorVars.SpinTAC.VelPlanRun;
gMotorVars.SpeedRef_krpm = gMotorVars.StopSpeedRef_krpm;
}
}
}
void speed_frq_calc(SPEED_MEAS_QEP *v)
{
_iq tmp1;
/* Differentiator */
/*
if (v->dir_QEP==0) // T2 is counting down
{
if (v->theta_elec>v->theta_old)
tmp1 = _IQ(1) - v->theta_elec + v->theta_old;
else
tmp1 = v->theta_elec - v->theta_old;
}
else if (v->dir_QEP==1) // T2 is counting up
{
if (v->theta_elec<v->theta_old)
tmp1 = _IQ(1) + v->theta_elec - v->theta_old;
else
tmp1 = v->theta_elec - v->theta_old;
}
tmp1 = _IQmpy(v->K1,tmp1); // Q21 = Q21*GLOBAL_Q
*/
// Differentiator
// Synchronous speed computation
if ((v->theta_elec < _IQ(0.9))&(v->theta_elec > _IQ(0.1)))
// Q21 = Q21*(GLOBAL_Q-GLOBAL_Q)
tmp1 = _IQmpy(v->K1,(v->theta_elec - v->theta_old));
else tmp1 = _IQtoIQ21(v->speed_frq);
// Low-pass filter
// Q21 = GLOBAL_Q*Q21 + GLOBAL_Q*Q21
tmp1 = _IQmpy(v->K2,_IQtoIQ21(v->speed_frq))+_IQmpy(v->K3,tmp1);
if (tmp1>_IQ21(1))
v->speed_frq = _IQ(1);
else if (tmp1<_IQ21(-1))
v->speed_frq = _IQ(-1);
else
v->speed_frq = _IQ21toIQ(tmp1);
/* Update the electrical angle */
v->theta_old = v->theta_elec;
/* Change motor speed from pu value to rpm value (GLOBAL_Q -> Q0) */
/* Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q */
v->speed_rpm = _IQmpy(v->rpm_max,v->speed_frq);
}
