void Timer5ISR(void)
{
static uint8_t NumSpdSet = 0;
static int32_t SpeedLeft, SpeedRight;
ROM_TimerIntClear(TIMER5_BASE, TIMER_TIMA_TIMEOUT);
SpeedLeft = ROM_QEIVelocityGet(QEI0_BASE) * ROM_QEIDirectionGet(QEI0_BASE);
SpeedRight = ROM_QEIVelocityGet(QEI1_BASE) * ROM_QEIDirectionGet(QEI1_BASE);
PosLeftCount = ((int32_t)ROM_QEIPositionGet(QEI0_BASE)) / 2;
PosRightCount = ((int32_t)ROM_QEIPositionGet(QEI1_BASE)) / 2;
#ifdef PID_SPEED
UARTPutn(UART_Bluetooth.PortName, (int32_t)SpeedLeft);
UARTCharPut(UART_Bluetooth.PortName, '\n');
#endif
#ifdef PID_POSITION
UARTPutn(UART_Bluetooth.PortName, (int32_t)PosLeftCount);
UARTCharPut(UART_Bluetooth.PortName, '\n');
#endif
if (PIDVerLeft.Enable)
{
PIDVerCalc(&PIDVerLeft, &SpeedLeft, 90);
SetPWM(MOTOR_LEFT, DEFAULT, (long)PIDVerLeft.PIDResult);
}
if (PIDVerRight.Enable)
{
PIDVerCalc(&PIDVerRight, &SpeedRight, 90);
SetPWM(MOTOR_RIGHT, DEFAULT, (long)PIDVerRight.PIDResult);
}
NumSpdSet++;
if (NumSpdSet == PIDVerLoop) //PID position
{
NumSpdSet = 0;
if (PIDPosLeft.Enable)
{
PIDPosCalc(&PIDPosLeft, PosLeftCount, avrSpeed);
PIDSpeedSet(&PIDVerLeft, (long)PIDPosLeft.PIDResult);
}
if (PIDPosRight.Enable)
{
PIDPosCalc(&PIDPosRight, PosRightCount, avrSpeed);
PIDSpeedSet(&PIDVerRight, (long)PIDPosRight.PIDResult);
}
}
}
//*****************************************************************************
//
// Gets the current speed of the encoder, specified as an unsigned 16.16 fixed-
// point value that represents the speed in revolutions per minute (RPM).
//
//*****************************************************************************
long
EncoderVelocityGet(long lSigned)
{
long lDir;
//
// If the time between edges is not valid, then the speed is zero.
//
if(HWREGBITH(&g_usEncoderFlags, ENCODER_FLAG_VALID) == 0)
{
return(0);
}
//
// See if a signed velocity should be returned.
//
if(lSigned)
{
//
// Get the current direction from the QEI module.
//
lDir = ROM_QEIDirectionGet(QEI0_BASE);
}
else
{
//
// An unsigned velocity is requested, so assume the direction is
// positive.
//
lDir = 1;
}
//
// Convert the time between edges into a speed in RPM and return it.
//
return(MathDiv16x16(lDir * SYSCLK * 60,
g_ulEncoderClocks * g_ulEncoderLines));
}
