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 position of the encoder, specified as a signed 16.16 fixed-
// point value that represents a number of full revolutions.
//
//*****************************************************************************
long
EncoderPositionGet(void)
{
//
// Convert the encoder position to a number of revolutions and return it.
//
return(MathDiv16x16(ROM_QEIPositionGet(QEI0_BASE), g_ulEncoderLines * 2));
}
