void UpdateSpeed_ISR(){
uint32_t status = TimerIntStatus(WTIMER4_BASE,true);
TimerIntClear(WTIMER4_BASE,status);
if(Target_Dir !=0){
Direction_Dir=QEIDirectionGet(QEI0_BASE);
EncoderDirVel=( (QEIVelocityGet(QEI0_BASE)*13)/48*12) * Direction_Dir;
Error_Dir = Target_Dir - EncoderDirVel;
I_Dir = (I_Dir+Error_Dir) * Ki_Dir;
D_Dir = D_Dir * (Error_Dir - LastError_Dir);
PID_Dir = P_Dir + I_Dir + D_Dir;
if(PID_Dir > 1022)
PID_Dir = 1022;
else if(PID_Dir < -1022)
PID_Dir = -1022;
}
else{
PID_Dir = 0;
I_Dir=0;
Error_Dir=0;
}
if(Target_Esq !=0){
Direction_Esq=QEIDirectionGet(QEI1_BASE);
EncoderEsqVel=( (QEIVelocityGet(QEI1_BASE)*13)/48*12) * Direction_Esq;
Error_Esq = Target_Esq - EncoderEsqVel;
I_Esq = (I_Esq+Error_Esq) * Ki_Esq;
D_Esq = D_Esq * (Error_Esq - LastError_Esq);
PID_Esq = P_Esq + I_Esq + D_Esq ;
if(PID_Esq > 1022)
PID_Esq = 1022;
else if(PID_Esq < -1022)
PID_Esq = -1022;
}
else{
PID_Esq = 0;
I_Esq=0;
Error_Esq=0;
}
DRV8833_MotorB(PID_Dir);
DRV8833_MotorA(PID_Esq);
LastError_Dir = Error_Dir;
LastError_Esq = Error_Esq;
}
// * enc_vel_get **************************************************************
// * Returns the signed velocity of the encoder, in units of encoder pulses. *
// * NOTE: If a change in the direction of rotation occured during the last *
// * velocity sampling period, this value will reflect the magnitude *
// * of displacement and it's sign will refer to the direction of *
// * encoder rotation at the end of the sampling period. *
// * *
// * This yields undesirable results during direction changes, which *
// * may be unacceptable in your application. If this is the case, *
// * you should do your own first derivative calculation by sampling *
// * encoder position on a known timebase, and finding the delta from *
// * the previous sample. *
// * encoder should be ENC_1 or ENC_2. *
// ****************************************************************************
signed long enc_vel_get(unsigned char encoder)
{
signed long retval; // holds value to be returned
if(encoder == ENC_1)
{
retval = (signed long)QEIVelocityGet(ENC_1_BASE); // return encoder 1 reading
retval *= QEIDirectionGet(ENC_1_BASE); // returns direction as -1 or +1
}
else if(encoder == ENC_2)
{
retval = (signed long)QEIVelocityGet(ENC_2_BASE); // return encoder 2 reading
retval *= QEIDirectionGet(ENC_2_BASE); // returns direction as -1 or +1
}
else
{
retval = 0; // return for invalid encoder ID
}
return retval; // return the encoder position
}
int main(void)
{
long dir;
unsigned long vel, pos;
//
// Set the clocking to run directly from the crystal.
//
SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_8MHZ);
//
// Set up low level I/O for printf()
//
llio_init(115200);
//
// Set up GPIO for LED
//
LED_INIT();
//
// Set up QEI peripheral
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_QEI);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeQEI(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_6);
QEIConfigure(QEI0_BASE, (QEI_CONFIG_CAPTURE_A_B | QEI_CONFIG_NO_RESET |
QEI_CONFIG_QUADRATURE | QEI_CONFIG_NO_SWAP), 0xffffffff);
QEIVelocityConfigure(QEI0_BASE, QEI_VELDIV_1, SysCtlClockGet() / 10);
QEIEnable(QEI0_BASE);
QEIVelocityEnable(QEI0_BASE);
//
// Set up GPIO for encoder push switch
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeGPIOInput(GPIO_PORTC_BASE, GPIO_PIN_5);
GPIOPadConfigSet(GPIO_PORTC_BASE, GPIO_PIN_5, GPIO_STRENGTH_2MA, GPIO_PIN_TYPE_STD_WPU);
GPIOIntTypeSet(GPIO_PORTC_BASE, GPIO_PIN_5, GPIO_FALLING_EDGE);
GPIOPortIntRegister(GPIO_PORTC_BASE, SW_IntHandler);
GPIOPinIntEnable(GPIO_PORTC_BASE, GPIO_PIN_5);
//
// Set up the period for the SysTick timer.
//
SysTickPeriodSet(SysCtlClockGet() / 10); // 10Hz SysTick timer
//
// Enable the SysTick Interrupt.
//
SysTickIntEnable();
SysTickIntRegister(SysTickIntHandler);
//
// Enable SysTick.
//
SysTickEnable();
printf("\r\nEncoder example\r\n");
//
// Loop forever.
//
while(1)
{
SysCtlSleep(); // sleep here till interrupt
LED_TOGGLE();
dir = QEIDirectionGet(QEI0_BASE);
vel = QEIVelocityGet(QEI0_BASE);
pos = QEIPositionGet(QEI0_BASE);
printf("%d,%d,%d\r\n", dir, vel, pos);
}
}
// Asservissement en vitesse des moteurs pour qu'il atteignent leur propre consigne
void asservirMoteurs(void){
pos0 = position_m3; //pas inversé
pos1 = pos1 + QEIVelocityGet(QEI1_BASE)*QEIDirectionGet(QEI1_BASE); //pas inversé
pos2 = position_m2;//pas inversé
pos3 = pos3 - QEIVelocityGet(QEI0_BASE)*QEIDirectionGet(QEI0_BASE); //inversé
ajustementVitesse();
//Motor 0
measured_speed0 = (pos0 - previous_pos0)/dt;
if(measured_speed0 < 0){
measured_speed0 = -measured_speed0;
}
previous_pos0 = pos0;
if(consigne0 > 3200){
output0 = PIDHandler0(&consigne0, &measured_speed0, &I0, &previous_error0, dt);
}
else{
output0 = SlowPIDHandler0(&consigne0, &measured_speed0, &I0, &previous_error0, dt);
}
//Motor 1
measured_speed1 = QEIVelocityGet(QEI1_BASE)/dt;//(pos1 - previous_pos1)*10;
if(measured_speed1 < 0){
measured_speed1 = -measured_speed1;
}
previous_pos1 = pos1;
if(consigne1 > 3200){
output1 = PIDHandler1(&consigne1, &measured_speed1, &I1, &previous_error1, dt);
}
else{
output1 = SlowPIDHandler1(&consigne1, &measured_speed1, &I1, &previous_error1, dt);
}
//Motor 2
measured_speed2 = (pos2 - previous_pos2)/dt;
if(measured_speed2 < 0){
measured_speed2 = -measured_speed2;
}
previous_pos2 = pos2;
if(consigne2 > 3200){
output2 = PIDHandler2(&consigne2, &measured_speed2, &I2, &previous_error2, dt);
}
else{
output2 = SlowPIDHandler2(&consigne2, &measured_speed2, &I2, &previous_error2, dt);
}
//Motor 3
measured_speed3 = QEIVelocityGet(QEI0_BASE)/dt;//(pos3 - previous_pos3)*10;
if(measured_speed3 < 0){
measured_speed3 = -measured_speed3;
}
previous_pos3 = pos3;
if(consigne3 > 3200){
output3 = PIDHandler3(&consigne3, &measured_speed3, &I3, &previous_error3, dt);
}
else{
output3 = SlowPIDHandler3(&consigne3, &measured_speed3, &I3, &previous_error3, dt);
}
/*output0 = output0_old +(dt/Tf0)*(output0-output0_old);
output1 = output1_old +(dt/Tf1)*(output1-output1_old);
output2 = output2_old +(dt/Tf2)*(output2-output2_old);
output3 = output3_old +(dt/Tf3)*(output3-output3_old);
output0_old = output0;
output1_old = output1;
output2_old = output2;
output3_old = output3;*/
//output0_table[index%10] = output0;
//output1_table[index%10] = output1;
//output2_table[index%10] = output2;
//output3_table[index%10] = output3;
//Traduction 6400e de tour fraction appliqué au pulse width
float fraction0;
float fraction1;
float fraction2;
float fraction3;
//Une équation linéaire est utilisée x*0.5/7700 = % du duty cycle
fraction0 = ((output0*0.5)/7700);
if(fraction0 > 0.99){
fraction0 = 0.99;
}
else if(fraction0 < 0){
fraction0 = 0;
}
fraction1 = ((output1*0.5)/7700);
if(fraction1 > 0.99){
fraction1 = 0.99;
}else if(fraction1 < 0){
fraction1 = 0;
}
fraction2 = ((output2*0.5)/7700);
if(fraction2 > 0.99){
fraction2 = 0.99;
}else if(fraction2 < 0){
fraction2 = 0;
}
fraction3 = ((output3*0.5)/7700);
if(fraction3 > 0.99){
fraction3 = 0.99;
}else if(fraction3 < 0){
fraction3 = 0;
}
PWMPulseWidthSet(PWM_BASE, PWM_OUT_0, (periodPWM*fraction0));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, (periodPWM*fraction1));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, (periodPWM*fraction2));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, (periodPWM*fraction3));
pos0_table[index%300]=GPIOPinRead(GPIO_PORTD_BASE, GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7);
pos1_table[index%300]=GPIOPinRead(GPIO_PORTE_BASE, GPIO_PIN_4 | GPIO_PIN_5);
pos2_table[index%300]=pos2;
pos3_table[index%300]=pos3;
speed0_table[index%300]=measured_speed0;
speed1_table[index%300]=measured_speed1;
speed2_table[index%300]=measured_speed2;
speed3_table[index%300]=measured_speed3;
output0_table[index%300]=output0;
output1_table[index%300]=output1;
output2_table[index%300]=output2;
output3_table[index%300]=output3;
fraction0_table[index%300]=fraction0;
fraction1_table[index%300]=fraction1;
fraction2_table[index%300]=fraction2;
fraction3_table[index%300]=fraction3;
//Si le robot est immobile
if(a_atteint_consigne && measured_speed0==0 && measured_speed1==0 && measured_speed2==0 && measured_speed3==0){
resetVariables();
resetQEIs();
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_0, 0);//periodPWM / 4);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, 0);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 0);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 0);
est_en_mouvement = false;
}
else{
est_en_mouvement = true;
}
}
//*****************************************************************************
//
// Handles the QEI velocity interrupt.
//
// This function is called when the QEI velocity timer expires. If using the
// edge counting mode for rotor speed determination, the number of edges
// counted during the last velocity period is used as a measure of the rotor
// speed.
//
// \return None.
//
//*****************************************************************************
void
QEIIntHandler(void)
{
unsigned long ulPrev, ulCount;
//
// Clear the QEI interrupt.
//
QEIIntClear(QEI0_BASE, QEI_INTTIMER);
//
// Increment the accumulated time to extend the range of the QEI timer,
// which is used by the edge timing mode.
//
g_ulSpeedTime += SYSTEM_CLOCK / QEI_INT_RATE;
//
// See if edge counting mode is enabled.
//
if(HWREGBITW(&g_ulSpeedFlags, FLAG_COUNT_BIT) == 0)
{
//
// Edge timing mode is currenting operating, so see if an edge was seen
// during this QEI timing period.
//
if(HWREGBITW(&g_ulSpeedFlags, FLAG_EDGE_BIT) == 0)
{
//
// No edge was seen, so set the rotor speed to zero.
//
g_ulRotorSpeed = 0;
//
// Since the amount of time the rotor is stopped is indeterminate,
// skip the first edge when the rotor starts rotating again.
//
HWREGBITW(&g_ulSpeedFlags, FLAG_SKIP_BIT) = 1;
}
else
{
//
// An edge was seen, so clear the flag so the next period can be
// checked as well, and restart the edge reset counter.
//
HWREGBITW(&g_ulSpeedFlags, FLAG_EDGE_BIT) = 0;
}
//
// There is nothing further to do.
//
return;
}
//
// Get the number of edges during the most recent period.
//
ulCount = QEIVelocityGet(QEI0_BASE);
//
// Get the count of edges in the previous timing period.
//
ulPrev = g_ulSpeedPrevious;
//
// Save the count of edges during this timing period.
//
g_ulSpeedPrevious = ulCount;
//
// See if this timing period should be skipped.
//
if(HWREGBITW(&g_ulSpeedFlags, FLAG_SKIP_BIT))
{
//
// This timing period should be skipped, but an edge count from a
// previous timing period now exists so the next timing period should
// not be skipped.
//
HWREGBITW(&g_ulSpeedFlags, FLAG_SKIP_BIT) = 0;
//
// There is nothing further to be done.
//
return;
}
//
// Average the edge count for the previous two timing periods.
//
ulCount = (ulPrev + ulCount) / 2;
//
// Compute the new speed from the number of edges. Note that both
// edges are counted by the QEI block, so the count for a full revolution
// is double the number of encoder lines.
//
SpeedNewValue((ulCount * QEI_INT_RATE * 30) /
(UI_PARAM_NUM_LINES + 1));
//
// See if the number of edges has become too small, meaning that the edge
// time has become large enough.
//
if(ulCount < (((MAX_EDGE_COUNT - EDGE_DELTA) * 2) / QEI_INT_RATE))
{
//
// Edge timing mode should be used instead of edge counting mode.
//
HWREGBITW(&g_ulSpeedFlags, FLAG_COUNT_BIT) = 0;
//
// Indicate that the first edge should be skipped in edge timing mode.
//
HWREGBITW(&g_ulSpeedFlags, FLAG_SKIP_BIT) = 1;
//
// Enable the GPIO interrupt to enable edge timing mode.
//
IntEnable(INT_GPIOC);
}
}
