 |
 |
| View previous topic :: View next topic |
| Author |
Message |
Warday
Joined: 21 May 2021
Posts: 11
|
| qei configuration to solve overflow encoder |
 Posted: Fri May 21, 2021 2:59 pm |
 |
|
Hi. I’m using an dsPIC30f4012 to control a motor on a robot. Unfortunately, my encoder doesn’t have index and I reach more than 2^16 on counter. I read on the documentation that in that cases is recommended to use QEI_RESET_WHEN_MAXCOUNT and interrupt to increase an auxiliary variable. How can I do that?
I use
| Code: |
setup_qei(QEI_MODE_X2|QEI_RESET_WHEN_MAXCOUNT,QEI_FILTER_DIV_4,1200);
count=qei_get_count();
|
Thanx
full code:
| Code: |
#include <30f4012.h> // Selecciona el PIC
#device ADC=10
#include <math.h>
#fuses HS,NOWDT // Opciones de configuración
#use delay(clock=20000000) // Velocidad del Cristal : 20 Mhz
//#use fast_io(A)
#use fast_io(B)
#use fast_io(e)
#use standard_io(f)
//#use RTOS(timer=0, minor_cycle=10ms) //temporizador Timer0, tiempo mínimo de ejecución de cada tarea 5ms
#use rs232(UART,baud=19200, xmit=PIN_F3, rcv=PIN_F2) // Definición del RS232
#BUILD(STACK = 384)
/*
float referencia=10.0; // Estado Inicial Actuador
float Kp=100.0;
float Kd=10.0;
float Ki=0.2;
float desp_ant=0.0;
float Integral=0.0;
*/
unsigned long count=0;
unsigned long index=0;
unsigned long internal=0;
char dato;
float VelMed=0.0;
unsigned long duty=0;
float dutyf;
float Corrientef;
unsigned int16 Corriente;
char dd[2];
char dir='s'; //stop
//void PID(void);
//Definición de las prototipos de función de las tareas
//#task (rate=20ms, max=10ms) //Ejecutar cada 10ms y consumir como máximo 5ms
//void ControlPID();
void Giro_MOTOR(char g)
{
if(g=='d')
{
output_low(PIN_E1);
output_high(PIN_E2);
}
if(g=='i')
{
output_high(PIN_E1);
output_low(PIN_E2);
}
if(g=='s')
{
output_low(PIN_E1);
output_low(PIN_E2);
}
if(g=='m')
{
output_high(PIN_E1);
output_high(PIN_E2);
}
}
#int_rda
void serial_isr() {
// dato=0x00;
if(kbhit()){
dato=getc();
if(dato=='a')
{
printf("Velocidad Motor= %.1f\r\n",VelMed);
}
if(dato=='b')
{
duty+=5;
if(duty>499)
duty=498;
}
if(dato=='c')
{
duty-=5;
if(duty<5)
duty=5;
}
if(dato=='d')
{
Giro_MOTOR('d');
}
if(dato=='i')
{
Giro_MOTOR('i');
}
if(dato=='s')
{
Giro_MOTOR('s');
}
if(dato=='m')
{
Giro_MOTOR('m');
}
if(dato=='z')
{
memcpy(dd,&count,2);
printf("%c%c",dd[0],dd[1]);
memcpy(dd,&Corriente,2);
printf("%c%c",dd[0],dd[1]);
memcpy(dd,&duty,2);
printf("%c%c",dd[0],dd[1]);
}
if(dato=='q')
{
printf("Cont= %Ld\r\n",count);
printf("Corr= %f\r\n",Corrientef);
}
}
}
unsigned int16 Lee_ADC(int a)
{
SET_ADC_CHANNEL(a);
delay_us(10);
return READ_ADC();
}
/*
void ControlPID() {
float error, desp, Proporcional, Derivativo, Velocidad;
float referencia;
count=qei_get_count();
velMed=(float)count/360.0*(60.0/0.010);
// count=POSCNT;
error=(float)referencia-desp;
// Controlador PID
Proporcional = Kp * error;
Derivativo = error - desp_ant;
Derivativo *= Kd;
desp_ant = error;
Velocidad = fabs(Proporcional + Derivativo + Integral);
if(Velocidad<1023)
Integral += Ki * error;
if(Velocidad > 1023.0)
Velocidad = 1023.0;
qei_set_count(0);
}
*/
void main() {
float Ts=0.02;
SET_TRIS_E(0x00); //Salidas E
output_float(PIN_F2);
output_float(PIN_F3);
setup_qei(QEI_MODE_X2|QEI_RESET_WHEN_MAXCOUNT,QEI_FILTER_DIV_4,1200);
setup_adc(ADC_CLOCK_INTERNAL); // Para el muestreo usamos la señal de reloj del uC.
setup_adc_ports(sAN0|sAN1|sAN2);
SETUP_TIMER2(TMR_INTERNAL|TMR_DIV_BY_1,499); //f =10 (khz)
SETUP_COMPARE(1,COMPARE_PWM | COMPARE_TIMER2);
Giro_Motor('d');
SET_PWM_DUTY(1,0);
qei_set_count(0);
enable_interrupts(INT_RDA); //Habilita interrupcion puerto serie 2
enable_interrupts(INTR_GLOBAL);
printf("Contol Motor Robot 2021\r\n");
printf("Iniciando....\r\n");
//rtos_run(); //A partir de aquí comenzará la ejecución de las tareas
do {
// Encoder
count=qei_get_count();
//qei_set_count(0);
//Lectura Corriente
Corriente=Lee_ADC(0);
Corrientef=(float)Corriente*5.0/1024.0*1500.0/11570.0;
printf("Encoder: %d\r\n",count);
printf("Corriente: %d\r\n",Corriente);
printf("PWM: %d\r\n",duty);
SET_PWM_DUTY(1,duty);
delay_ms(250);
}while(TRUE);
}
|
|
|
 |
Ttelmah
Joined: 11 Mar 2010
Posts: 19229
|
|
| Posted: Sat May 22, 2021 1:05 am |
|
|
If you look at the data sheet it shows QEIF being set when it
resets the counter.
So you would need to change your variable to something like:
| Code: |
union {
unsigned int16 words[2];
unsigned int32 total;
} QEIval;
//Then at the start set QEIval.total to 0 when you start the QEI.
//Then read the 16bit QEI value into the low word as:
QEIval.words[0]=qei_get_count();
#bit UPDN=getenv("BIT:UPDN")
//Then have an INT_QEI handler:
#INT_QEI
void QEI_over(void)
{
if (UPDN)
QEIval.words[1]++;
else
QEIval.words[1]--;
}
//Then when you want the 32bit count, read QEIval.whole
|
I must admit I'm assuming that the interrupt does trigger at zero as
well as at the overflow, and that you then have to use the direction bit
to know which way the counter is going. Will need testing!...
Obviously INT_QEI will need to be enabled. |
|
|
temtronic
Joined: 01 Jul 2010
Posts: 9114
Location: Greensville,Ontario
|
|
| Posted: Sat May 22, 2021 4:35 am |
|
|
general comments...
1) add 'ERRORS' to the #USE RS232(...options...), to avoid HW UART from 'locking up'(>3 incoming chars in a row...)
2) remove 'printf's in the ISR. They take a huge amount of time and will stop program from executing properly.
3) replace all floating point 'math' and use 'scaled integers'. You'll get far better results and a LOT faster response in your PID loop. Program will use less codespace as well. |
|
|
Warday
Joined: 21 May 2021
Posts: 11
|
|
| Posted: Sat May 22, 2021 12:18 pm |
|
|
| Thanx for the reply. I will modify the code and when it is done I will comment the result. Thanx!! |
|
|
Warday
Joined: 21 May 2021
Posts: 11
|
|
| Posted: Tue May 25, 2021 12:55 pm |
|
|
Thanks Ttelmah it works!!!
with QEIval.whole don't you mean QEIval.total? QEIval.whole give me error
Thank temtronic. As it is a test version i have not yet applied your advice. I will apply it on the final version.
I let here the program with the working overflow fix with the comments. Thanks all!
| Code: |
// Robot Control, Encoder test with overflow
// Autor. Warday
#include <30f4012.h> // Select PIC
#device ADC=10
#include <math.h>
#fuses HS,NOWDT // Opciones de configuración
#use delay(clock=20000000) // Cristal : 20 Mhz
#use fast_io(B)
#use fast_io(e)
#use standard_io(f)
#use rs232(UART,baud=19200, xmit=PIN_F3, rcv=PIN_F2) // Comunication
#BUILD(STACK = 384)
#int_rda
// varialbes used to store the information.
unsigned int32 count=0;
unsigned int16 set;
/*
Here we convert the int 16 of the internal counter to int 32 combining
the counter in words[0] with the amount of overflow errors registered on words[1]
words[0]+words[1]*65535
*/
union {
unsigned int16 words[2];
unsigned int32 total;
} QEIval;
//Direction bit
#bit UPDN=getenv("BIT:UPDN")
/*
Here we control what does interrupt INT QEI.
When its activated it increases or decreases words[1] depending on the direction.
*/
#INT_QEI
void QEI_over(void)
{
if (UPDN)
QEIval.words[1]++;
else
QEIval.words[1]--;
}
void main() {
SET_TRIS_E(0x00); //Salidas E
/* Important: QEI_RESET_WHEN_MAXCOUNT controls the INT_QEI, it must be set to
control the interrupt. You can let the max count empty to use 2^16 max value or
set your personal reset as below. In this case I have 600 pulse encoder * 4
it's 2400 pulses per lap.
setup_qei(QEI_MODE_X4|QEI_RESET_WHEN_MAXCOUNT,QEI_FILTER_DIV_4,2400)
but in this case you have to use words[0]+words[1]*2400 and not EIval.total
as in this example.
*/
setup_qei(QEI_MODE_X4|QEI_RESET_WHEN_MAXCOUNT,QEI_FILTER_DIV_4);
qei_set_count(0); // set count to 0
enable_interrupts(INT_RDA); //Enable interrupt
enable_interrupts(INTR_GLOBAL);
enable_interrupts(INT_QEI); //interrupt INT_QEI must be enable
QEIval.total=0; //Then at the start set QEIval.total to 0 when you start the QEI.
do {
QEIval.words[0]=qei_get_count(); // store current count
count=QEIval.total; // read total
set=QEIval.words[1]; // we read the amount of overflow errors or laps.
// and print all
printf("Encoder: %d\r\n",count);
printf("set: %d\r\n",set);
printf("dir: %d\r\n",UPDN);
delay_ms(250);
}while(TRUE);
}
|
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19229
|
|
| Posted: Tue May 25, 2021 11:45 pm |
|
|
Yes. Typing error. 
Glad I got it 'close' first time though. 
Good news. |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
