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 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);
}
//----------------------------------------------------------------------------
// 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]);
}
}
// MainISR
interrupt void MainISR(void)
{
// Verifying the ISR
IsrTicker++;
if(RunMotor)
{
// =============================== LEVEL 1 ======================================
// Checks target independent modules, duty cycle waveforms and PWM update
// Keep the motors disconnected at this level
// ==============================================================================
#if (BUILDLEVEL==LEVEL1)
// ------------------------------------------------------------------------------
// Connect inputs of the RMP module and call the ramp control macro
// ------------------------------------------------------------------------------
rc1.TargetValue = VRef1;
RC_MACRO(rc1)
// ------------------------------------------------------------------------------
// Connect inputs of the PWM_DRV module and call the PWM signal generation macro
// ------------------------------------------------------------------------------
pwm1.MfuncC1 = (int16)_IQtoIQ15(_IQabs(rc1.SetpointValue)); // MfuncC1 is in Q15
PWM_MACRO(pwm1) // Calculate the new PWM compare values
if(rc1.SetpointValue < 0)
{
Rotation1 = 0;
}
else
{
Rotation1 = 1;
}
if(Motor==1)
{
if(Rotation1==0)
{
EPwm1Regs.AQCSFRC.bit.CSFB = 0; //Forcing Disabled on EPWM1B
EPwm1Regs.AQCSFRC.bit.CSFA = 1; //EPWM1A forced low
}
else if(Rotation1 == 1)
{
EPwm1Regs.AQCSFRC.bit.CSFA = 0; //Forcing Disabled on EPWM1A
EPwm1Regs.AQCSFRC.bit.CSFB = 1; //EPWM1B forced low
}
else
{
EPwm1Regs.AQCSFRC.bit.CSFA = 1; //EPWM1A forced low
EPwm1Regs.AQCSFRC.bit.CSFB = 1; //EPWM1B forced low
}
EPwm1Regs.CMPA.half.CMPA=pwm1.PWM1out;
}
//else if (Motor==2)
//{
if(Rotation1==0)
{
EPwm2Regs.AQCSFRC.bit.CSFB = 0; //Forcing Disabled on EPWM2B
EPwm2Regs.AQCSFRC.bit.CSFA = 1; //EPWM2A forced low
}
else if(Rotation1 == 1)
{
EPwm2Regs.AQCSFRC.bit.CSFA = 0; //Forcing Disabled on EPWM2A
EPwm2Regs.AQCSFRC.bit.CSFB = 1; //EPWM2B forced low
}
else
{
EPwm2Regs.AQCSFRC.bit.CSFA = 1; //EPWM2A forced low
EPwm2Regs.AQCSFRC.bit.CSFB = 1; //EPWM2B forced low
}
EPwm2Regs.CMPA.half.CMPA=pwm1.PWM1out;
//}
// ------------------------------------------------------------------------------
// Connect inputs of the PWMDAC module
// ------------------------------------------------------------------------------
PwmDacCh1 = (int16)_IQtoIQ15(VRef1);
PwmDacCh2 = (int16)_IQtoIQ15(rc1.SetpointValue);
// ------------------------------------------------------------------------------
// Connect inputs of the DATALOG module
// ------------------------------------------------------------------------------
DlogCh1 = (int16)_IQtoIQ15(VRef1);
DlogCh2 = (int16)_IQtoIQ15(rc1.SetpointValue);
DlogCh3 = (int16)_IQtoIQ15(VRef1);
DlogCh4 = (int16)_IQtoIQ15(rc1.SetpointValue);
#endif // (BUILDLEVEL==LEVEL1)
// =============================== LEVEL 2 ======================================
// Level 2 verifies the analog-to-digital conversion, offset compensation
// ==============================================================================
#if (BUILDLEVEL==LEVEL2)
// ------------------------------------------------------------------------------
// Connect inputs of the RMP module and call the ramp control macro
// ------------------------------------------------------------------------------
rc1.TargetValue = VRef1;
RC_MACRO(rc1)
// ------------------------------------------------------------------------------
// Measure phase currents, subtract the offset and normalize from (-0.5,+0.5) to (-1,+1).
// ------------------------------------------------------------------------------
if(Motor==1)
{
IFdbk1b=_IQ15toIQ((AdcResult.ADCRESULT1<<3)-_IQ15(0.5))<<1;
IFdbk1a=_IQ15toIQ((AdcResult.ADCRESULT0<<3)-_IQ15(0.5))<<1;
IFdbk1 = (IFdbk1a - IFdbk1b) >> 1;
}
else if(Motor==2)
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 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);
}
