/* Set I or V by selected channel */
short I_V_Set(unsigned char channel)
{
short res;
short count = 0;
do
{
res = V_Eval(channel, &PI.Feedback);
if (res != OK)
{
return KO;
}
CalcPI(&PI);
/* TODO set duty cycle */
/* TODO add a delay */
count++;
/* Stop if error is in a dedband or a fix number of steps */
} while (((PI.Error > DEADBAND) || (PI.Error < -1*DEADBAND)) && (count < ERROR_STEP));
return OK;
}
//---------------------------------------------------------------------
// Executes one PI itteration for each of the three loops Id,Iq,Speed
void DoControl( void )
{
short i;
/** check if open loop */
if( eStateControl == CNTRL_OPEN_LOOP )
{
// OPENLOOP: force rotating angle,Vd,Vq
if( uGF.bit.ChangeMode )
{
// just changed to openloop
uGF.bit.ChangeMode = 0;
// synchronize angles
OpenLoopParm.qAngFlux = CurModelParm.qAngFlux;
// VqRef & VdRef not used
CtrlParm.qVqRef = 0;
CtrlParm.qVdRef = 0;
}
OpenLoopParm.qVelMech = CtrlParm.qVelRef;
// calc rotational angle of rotor flux in 1.15 format
// just for reference & sign needed by CorrectPhase
CurModelParm.qVelMech = EncoderParm.qVelMech;
CurModel();
ParkParm.qVq = 0;
if( OpenLoopParm.qVelMech >= 0 )
i = OpenLoopParm.qVelMech;
else
i = -OpenLoopParm.qVelMech;
uWork = i <<2;
if( uWork > 0x5a82 )
uWork = 0x5a82;
if( uWork < 0x1000 )
uWork = 0x1000;
ParkParm.qVd = uWork;
OpenLoop();
ParkParm.qAngle = OpenLoopParm.qAngFlux;
}
else
// Closed Loop Vector Control
{
if( uGF.bit.ChangeMode )
{
// just changed from openloop
uGF.bit.ChangeMode = 0;
// synchronize angles and prep qdImag
CurModelParm.qAngFlux = OpenLoopParm.qAngFlux;
CurModelParm.qdImag = ParkParm.qId;
}
// Current model calculates angle
CurModelParm.qVelMech = EncoderParm.qVelMech;
CurModel();
ParkParm.qAngle = CurModelParm.qAngFlux;
// Calculate qVdRef from field weakening
FdWeakening();
// Check to see if new velocity information is available by comparing
// the number of interrupts per velocity calculation against the
// number of velocity count samples taken. If new velocity info
// is available, calculate the new velocity value and execute
// the speed control loop.
if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc)
{
// Calculate velocity from acumulated encoder counts
CalcVel();
// Execute the velocity control loop
PIParmQref.qInMeas = EncoderParm.qVelMech;
PIParmQref.qInRef = CtrlParm.qVelRef;
CalcPI(&PIParmQref);
CtrlParm.qVqRef = PIParmQref.qOut;
}
#ifdef SNAPSHOT
if(EncoderParm.iVelCntDwn == EncoderParm.iIrpPerCalc)
{
// Log data in the snapshot buffers
if( uGF.bit.DoSnap )
{
SnapBuf1[SnapCount] = SNAP1;
SnapBuf2[SnapCount] = SNAP2;
SnapBuf3[SnapCount] = SNAP3;
SnapCount++;
if(SnapCount == SNAPSIZE)
{
uGF.bit.SnapDone=1;
uGF.bit.DoSnap = 0;
}
}
}
#endif
// If the application is running in torque mode, the velocity
// control loop is bypassed. The velocity reference value, read
// from the potentiometer, is used directly as the torque
// reference, VqRef.
#ifdef TORQUE_MODE
CtrlParm.qVqRef = CtrlParm.qVelRef * 4;
#endif
// PI control for Q
PIParmQ.qInMeas = ParkParm.qIq;
PIParmQ.qInRef = CtrlParm.qVqRef;
CalcPI(&PIParmQ);
ParkParm.qVq = PIParmQ.qOut;
// PI control for D
PIParmD.qInMeas = ParkParm.qId;
PIParmD.qInRef = CtrlParm.qVdRef;
CalcPI(&PIParmD);
ParkParm.qVd = PIParmD.qOut;
}
}
