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)); }