CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to CCS Technical Support

qei configuration to solve overflow encoder

 
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion
View previous topic :: View next topic  
Author Message
Warday



Joined: 21 May 2021
Posts: 11

View user's profile Send private message

qei configuration to solve overflow encoder
PostPosted: Fri May 21, 2021 2:59 pm     Reply with quote

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: 19477

View user's profile Send private message

PostPosted: Sat May 22, 2021 1:05 am     Reply with quote

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: 9215
Location: Greensville,Ontario

View user's profile Send private message

PostPosted: Sat May 22, 2021 4:35 am     Reply with quote

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

View user's profile Send private message

PostPosted: Sat May 22, 2021 12:18 pm     Reply with quote

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

View user's profile Send private message

PostPosted: Tue May 25, 2021 12:55 pm     Reply with quote

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: 19477

View user's profile Send private message

PostPosted: Tue May 25, 2021 11:45 pm     Reply with quote

Yes. Typing error. Smile

Glad I got it 'close' first time though. Very Happy

Good news.
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> General CCS C Discussion All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
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