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, IQ24int)
{
LONGS_EQUAL(127, _IQ24int(_IQ24(127.999999940))) ;
LONGS_EQUAL(-128, _IQ24int(_IQ24(-128.0))) ;
}