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 ST_setupVelPlan(ST_Handle handle) {
_iq accMax, jrkMax;
ST_Obj *stObj = (ST_Obj *)handle;
// Pass the configuration array pointer into SpinTAC Velocity Plan
STVELPLAN_setCfgArray(stObj->velPlanHandle, &stVelPlanCfgArray[0], sizeof(stVelPlanCfgArray), 0, 0, 0, NUM_PLAN_TRANS, NUM_PLAN_STATES);
// Establish the Acceleration, and Jerk Maximums
accMax = _IQ24(10.0);
jrkMax = _IQ20(62.5);
// Configure SpinTAC Velocity Plan: Sample Time, LoopENB
STVELPLAN_setCfg(stObj->velPlanHandle, _IQ(ST_SAMPLE_TIME), false);
// Configure halt state: VelEnd, AccMax, JrkMax, Timer
STVELPLAN_setCfgHaltState(stObj->velPlanHandle, 0, accMax, jrkMax, 1000L);
//Example: STVELPLAN_addCfgState(handle, VelSetpoint[pups], StateTimer[ticks]);
STVELPLAN_addCfgState(stObj->velPlanHandle, 0, 200L); // StateA
STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateB
STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(-0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateC
//Example: STVELPLAN_addCfgTran(handle, FromState, ToState, CondOption, CondIdx1, CondiIdx2, AccLim[pups2], JrkLim[pups3]);
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_A, STATE_B, ST_COND_NC, 0, 0, _IQ(0.1), _IQ20(1)); // From StateA to StateB
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_B, STATE_C, ST_COND_NC, 0, 0, _IQ(0.1), _IQ20(1)); // From StateB to StateC
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_C, STATE_A, ST_COND_NC, 0, 0, _IQ(1), _IQ20(1)); // From StateC to StateA
// Note: set CondIdx1 to 0 if CondOption is ST_COND_NC; set CondIdx2 to 0 if CondOption is ST_COND_NC or ST_COND_FC
}
TEST(IQmath_ExtractInteger, IQ20int)
{
LONGS_EQUAL(2047, _IQ20int(_IQ20(2047.999999046))) ;
LONGS_EQUAL(-2048, _IQ20int(_IQ20(-2048.0))) ;
}
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;
}
}
}