Go Down

Topic: Estimacion de velocidad con encoders incrementales (SOLUCIONADO) (Read 335 times) previous topic - next topic

Perrary

Hola a todos. Soy nuevo en el foro y espero cumplir las reglas con mi publicación. Paso a comentarles mi situación para ver si me pueden dar una mano. Estoy trabajando con un robot con geometría diferencial, el cual cuenta con un Arduino Mega 2560 para el control de bajo nivel (los motores y sensores) y una RPI para el control de alto nivel (es un proyecto implementado en ROS). La idea es implementar un sistema de navegación en el robot, por lo que para eso necesito tener datos de la odometría con el menor error posible. Las ecuaciones para esta geometría en particular las tengo, el problema es que la estimación de velocidad que estoy haciendo tiene un error muy grande para bajas velocidades (estoy utilizando un algoritmo de tiempo fijo).

Hablando con mi tutor, me propuso utilizar un algoritmo de espacio fijo, en donde la idea sería contar cuantos pulsos de una señal de alta frecuencia entran entre pulsos sucesivos de los encoders (tengo que los motores tienen asociados cada uno un encoder incremental de 1120 pasos por revolución), y conociendo estos pulsos de alta frecuencia estimar la velocidad. Querría saber si me podrían dar una mano con esto, porque estuve haciendo pruebas utilizando el timer1 y timer2 para contar pulsos de alta frecuencia para cada motor, pero no pude obtener ninguna buena estimación. Para los pulsos de los encoders estoy utilizando interrupciones con los flancos de bajada.

Les proporciono algunos datos mas que quizas requieran:
- los motores son los 37D mm Metal gearmotors  (with encoders) de Pololu.
- para la generación de los pwm utilizo el shield Pololu Dual VNH5019.
- las velocidades de referencia provienen de la RPI por medio del nodo rosserial, y las mismas estan en las unidades de radianes por segundo.
- Utilizo dos algoritmos PI para cada uno de los motores (utilizo la biblioteca PID() del arduino).
- La estimación de velocidad la necesito como &Input del PID() y para calcular la odometria , la cual volveria al ROS por medio del rosserial.

Si gustan puedo adjuntar una copia del codigo que tengo implementado hasta ahora (con la medición de velocidad hecha con el algoritmo de tiempo fijo) para que se ubiquen en la idea del programa.

Desde ya gracias y saludos

surbyte

Si claro aporta códigos y enlace o referencia de lo que hablas porque tu estas en el tema y nosotros no.
Pero podemos ayudarte claro.

Pero veamos las cosas por separado.
Enconder midiendo velocidad a espacio fijo.  Usas dos separaciones del enconder y metes una frecuencia alta. Las separaciones son la ventana de tiempo.

Ese método es el que se usa para Medir o bien frecuencias de bajas, cuando el periodo de una seña es lento, se usa una frecuencia alta de digamos 1Mhz y se mira entre flancos cuantos pulsos se cuentan. COmo sabes el periodo de la señal patron, podes estimar con precisión el periodo de la señal de baja frecuencia.

Es exactamente lo mismo. Instrumentos Electronicos I en mi carrera universitaria.

En tu caso.. tomas un flanco, inicias la cuenta y terminas con otro flanco, debes establecer en que punto el error cambia y te conviene medir al reves. Pero ese será otro problema. Ahora nos concentramos en este.

Si ya tienes implementadas las interrupciones, configura tus ISR para flanco de subida o bajada como gustes..
Inicia la cuenta con un flag, cargando el valor de micros(). Le dices a tu ISR que ya estas contando, y cuando vuelve a producirse la segunda ISR (interrupcion) entonces cierras la cuenta.
Es como cuando usamos millis() o micros() para microsegundos.
Sugiero micros para tener mas precisión.

Sabes cómo hacerlo con esta guía o necesitas mas ayuda?

Perrary

Hola Surbyte gracias por la pronta respuesta. En cuanto a la bibliografía que estuve utilizando para ver los algoritmos me base en una tesis de grado de mi universidad que mi tutor me facilito, así que se me complica pasar un enlace de referencia para este tema. En cuanto a la utilización del ROS y la odometría me base pura y exclusivamente en la información disponible dentro de wikiros.

A continuación les adjunto el código funcional haciendo la medición a tiempo fijo:

Code: [Select]

//Librerías utilizadas en el programa
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include "DualVNH5019MotorShield.h"
#include <nav_msgs/Odometry.h>
#include <ros/time.h>
#include <PID_v1.h>
#include <tf/transform_broadcaster.h>

//Pines para lectura de datos encoders
const byte pinEnc1a = 21;  // Pin encoder 1, señal A
const byte pinEnc2a = 18; // Pin encoder 2, señal A
const byte pinEnc1b = 20;  // Pin encoder 1, señal B
const byte pinEnc2b = 19; // Pin encoder 2, señal B

// Contadores de pasos de cada encoder y tiempos para lograr el espacio de tiempo fijo
volatile long paso1=0;   
volatile long paso2=0;
unsigned long ta1, ta2;

//Constantes del robot
float R = 0.034
; // Radio ruedas
float L = 0.24;  // Longitud entre ruedas

//Velocidades lineal y angular que le vamos a proveer al robot. Velocidades que calculamos para cada motor.
volatile float v=0;
volatile float w=0;
double Vr=0, Vl =0;
double Vrp=0, Vlp=0,Deltavr,Deltavl; //Estos son los valores que le paso a los PID para lograr un arranque suave

//Declaración de objetos PID y motores
double Input1, Output1;
double pulseg1, pulseg2;
double Input2, Output2;
double Kp=.4;
double Ki=0.001;
double Kd=0;
double auxiliar1=0;
double auxiliar2=0;
DualVNH5019MotorShield md;


PID myPID1(&Input1, &Output1, &Vrp, Kp, Ki, Kd, DIRECT);
PID myPID2(&Input2, &Output2, &Vlp, Kp, Ki, Kd, DIRECT);


//Variables para cálculo de la odometría
double x = 0.0;
double y = 0.0;
double th = 0.0;
float deltat=0.025;//cada vez que actualizo la velocidad hago una actualizacion de la posicion
double vx;
double vy;
double vth;

//Frecuencia a la que deseo publicar la odometría
double f=10;

//Variables auxiliares
long int cont1,cont2;
double aux;

char base_link[] = "/base_link";
char odom[] = "/odom";

//Crear el nodo de ros nh. El nodo se usa para publicarle al Arduino
ros::NodeHandle nh;

//Función que llamo cada vez que me llega un dato de velocidad desde el ros.
void Cbmotor( const geometry_msgs::Twist& mensaje_motor){
  v = mensaje_motor.linear.x;
  w = mensaje_motor.angular.z;
  Vl = (v + (w*L)/2)/R;
  Vr = (v - (w*L)/2)/R;   

  if(Vr==0){
    Vrp=0;
  }
  if(Vl==0){
    Vlp=0;
  }
   
  Deltavr=(Vr-Vrp)*0.05;
  Deltavl=(Vl-Vlp)*0.05;
}

//Suscripto al tópico cmd_vel. Cada vez que llegue un mensaje de dicho tópico va a llamar a la función Cbmotor.
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &Cbmotor );

//Crear el nodo que va a publicarle la odometría al ROS
nav_msgs::Odometry odometria;
ros::Publisher pub("nav_msgs/Odometry", &odometria);

//Crear el nodo que va a publicarle la transformada al ROS
geometry_msgs::TransformStamped trans_odom;
tf::TransformBroadcaster broadcaster;

void setup() {

  //Declaración de los pines de entrada
  pinMode(pinEnc1a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc1a), interrupcion1, FALLING);
  pinMode(pinEnc2a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc2a), interrupcion2, FALLING);
  pinMode(pinEnc1b, INPUT);
  pinMode(pinEnc2b, INPUT);

  aux=1000000/f;

  //Inicialización de los motores
  md.init();
  md.setM1Speed(0);
  md.setM2Speed(0);

  //Establezco el límite inicial en tiempo para medir velocidad
  ta1=cont1=micros();
 
  //Inicializo la suscripción a ROS
  nh.initNode();
  nh.subscribe(sub);

  //Inicializo la publicación a ROS
  nh.initNode();
  nh.advertise(pub);

  //Inicializo broadcasting a ROS
  nh.initNode();
  broadcaster.init(nh);

  //Comienzo de la comunicación serie
  Serial.begin(57600); 

  myPID1.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);
  myPID1.SetOutputLimits(-15.7,15.7 );
  myPID2.SetOutputLimits(-15.7,15.7 );
 
}

void loop() {
  ta2=cont2=micros();

  if((ta2-ta1)>25000){
    //Cálculo que me da la velocidad del motor 1
    Input1=paso1/0.025*PI/560;
    paso1=0;
   
    //Cálculo que me da la velocidad del motor 2
    Input2=paso2/0.025*PI/560;
    paso2=0;

    //Cálculo de la odometría
    vx= R/2*(Input1+Input2)*cos(th);
    vy= R/2*(Input1+Input2)*sin(th);
    vth= R/L*(Input1-Input2);

    //Calculo posicion
    x=x+vx*deltat;
    y=y+vy*deltat;
    th=th+vth*deltat;

    //Reestablecer contadores
    ta1=ta2;

    //Rampas para el arranque suave
    Vlp=Vlp+Deltavl;
    if(0.001>abs(Vl-Vlp)){
      Deltavl=0;
    }

    Vrp=Vrp+Deltavr;
    if(0.001>abs(Vr-Vrp)){
      Deltavr=0;
    }

  }
 
  myPID1.Compute();
  myPID2.Compute();

  //Alimentación de los motores
  md.setM1Speed((Vrp+Output1)*400/15.7);
  md.setM2Speed(-(Vlp+Output2)*400/15.7);

  //Armo el mensaje de la odometría para enviarlo
  odometria.header.stamp = nh.now();
  odometria.header.frame_id = base_link;
  odometria.pose.pose.position.x = v;
  odometria.pose.pose.position.y = w;
  odometria.pose.pose.position.z = 0;
  odometria.child_frame_id = odom;
  odometria.twist.twist.linear.x = vx;
  odometria.twist.twist.linear.y = vy;
  odometria.twist.twist.angular.z = vth;

  //Armo el mensaje de la transformada para enviarlo
  trans_odom.header.frame_id = base_link;
  trans_odom.child_frame_id = odom;
  trans_odom.transform.translation.x = 0.01;
  trans_odom.transform.translation.y = 0;
  trans_odom.transform.translation.z = 0.1;
  trans_odom.transform.rotation.x = 0.0;
  trans_odom.transform.rotation.y = 0.0;
  trans_odom.transform.rotation.z = 0.0;
  trans_odom.transform.rotation.w = 1.0; 
  trans_odom.header.stamp = nh.now();
 
  //Publico la odometría
  if((cont2-cont1)>aux){
    cont1=cont2;
    pub.publish(&odometria);
    broadcaster.sendTransform(trans_odom);
  }
  nh.spinOnce();
}

//Cuento la cantidad de pasos del encoder del motor 1
void interrupcion1()
{
if(Vr<0){
    paso1--;
}
  if(Vr>0){
    paso1++;
  }
}

//Cuento la cantidad de pasos del encoder del motor 2
void interrupcion2()
{
if(Vl<0){
    paso2--;
  }
  if(Vl>0){
    paso2++;
  }
}


Y por otro lado las pruebas que estuve realizando para hacer el algoritmo a espacio fijo:

Perrary

Tuve que dividir el mensaje en dos partes. El código con el que estuve probando para el espacio fijo es algo como lo siguiente:

Code: [Select]

#include <ros.h>
#include <geometry_msgs/Twist.h>
#include "DualVNH5019MotorShield.h"
#include <nav_msgs/Odometry.h>
#include <PID_v1.h>
#include <TimerThree.h>

//Pines para lectura de datos encoders
const byte pinEnc1a = 21;  // Pin encoder 1, señal A
const byte pinEnc2a = 18; // Pin encoder 2, señal A
const byte pinEnc1b = 20;  // Pin encoder 1, señal B
const byte pinEnc2b = 19; // Pin encoder 2, señal B

// Contadores de pasos de cada encoder y tiempos para lograr el espacio de tiempo fijo
volatile int flag1=0,flag2=0;
volatile long pulso1=0,pulso2=0;
double ta1=0,ta2=0,deltata=0;
double tb1=0,tb2=0,deltatb=0;

//Constantes del robot
float R = 0.034
; // Radio ruedas
float L = 0.24;  // Longitud entre ruedas

//Velocidades lineal y angular que le vamos a proveer al robot. Velocidades que calculamos para cada motor.
volatile float v=0;
volatile float w=0;
double Vr=0, Vl =0;
double Vrp=0, Vlp=0,Deltavr,Deltavl; //Estos son los valores que le paso a los PID para lograr un arranque suave

//Declaración de objetos PID y motores
double Input1, Output1;
double pulseg1, pulseg2;
double Input2, Output2;
double Kp=.4;
double Ki=0.001;
double Kd=0;
volatile int auxiliar1=0;
volatile int auxiliar2=0;
int contador1=0;
DualVNH5019MotorShield md;


PID myPID1(&Input1, &Output1, &Vrp, Kp, Ki, Kd, DIRECT);
PID myPID2(&Input2, &Output2, &Vlp, Kp, Ki, Kd, DIRECT);

//Variables para cálculo de la odometría
double x = 0.0;
double y = 0.0;
double th = 0.0;
float deltat=0.0003;//cada vez que actualizo la velocidad hago una actualizacion de la posicion
double vx;
double vy;
double vth;

//Frecuencia a la que deseo publicar la odometría
double f=10;

//Crear el nodo de ros nh. El nodo se usa para publicarle al Arduino
ros::NodeHandle nh;

//Función que llamo cada vez que me llega un dato de velocidad desde el ros.
void Cbmotor( const geometry_msgs::Twist& mensaje_motor){
  v = mensaje_motor.linear.x;
  w = mensaje_motor.angular.z;
  Vl = (v + (w*L)/2)/R;
  Vr = (v - (w*L)/2)/R;   

  if(Vr==0){
    Vrp=0;
  }
  if(Vl==0){
    Vlp=0;
  }
   
  Deltavr=(Vr-Vrp)*0.0001;
  Deltavl=(Vl-Vlp)*0.0001;
}

//Suscripto al tópico cmd_vel. Cada vez que llegue un mensaje de dicho tópico va a llamar a la función Cbmotor.
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &Cbmotor );

void setup() {
  //Declaración de los pines de entrada
  pinMode(pinEnc1a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc1a), interrupcion1, FALLING);
  pinMode(pinEnc2a, INPUT);
  attachInterrupt(digitalPinToInterrupt(pinEnc2a), interrupcion2, FALLING);
  pinMode(pinEnc1b, INPUT);
  pinMode(pinEnc2b, INPUT);

  //Inicialización de los motores
  md.init();
  md.setM1Speed(0);
  md.setM2Speed(0);

  //Comienzo de la comunicación serie
  Serial.begin(57600); 

  myPID1.SetMode(AUTOMATIC);
  myPID2.SetMode(AUTOMATIC);
  myPID1.SetOutputLimits(-15.7,15.7 );
  myPID2.SetOutputLimits(-15.7,15.7 );
 

  Timer3.initialize(30);         // Dispara cada 30 us
  Timer3.attachInterrupt(timer_int); // Activa la interrupcion y la asocia a timer_int
 
}

void loop() {
    while((0.001<=abs(Vr-Vrp)) || (0.001<=abs(Vl-Vlp))){
      Vrp=Vrp+Deltavr;
      Vlp=Vlp+Deltavl;
      myPID1.Compute();
      myPID2.Compute();

      //Alimentación de los motores
      md.setM1Speed((Vrp+Output1)*400/15.7);
      md.setM2Speed(-(Vlp+Output2)*400/15.7);
    }
    Deltavr=0;
    Deltavl=0;
   
    while(flag1!=1){
      if(auxiliar1==1){
        contador1++;
      }
      auxiliar1=0;
    }

    if(Vr<0){
      Input1=-2*PI/1120/contador1*100000/3;
    }
    if(Vr>0){
      Input1=2*PI/1120/contador1*100000/3;
      Serial.print(Input1);
      Serial.print("\n");
    }
    flag1=0;
    contador1=0;
   
   
  nh.spinOnce();
}

void timer_int()
{
  auxiliar1=1;
  auxiliar2=1;
}

//Cuento la cantidad de pasos del encoder del motor 1
void interrupcion1()
{
  flag1=1;
}

//Cuento la cantidad de pasos del encoder del motor 2
void interrupcion2()
{
  flag2=1;
}


Ahora que facilité los códigos paso a comentar tu respuesta. Efectivamente, el método que pretendo utilizar es tal cual lo describís, en mi caso la frecuencia que quiero utilizar es como mínimo de unos 30KHz. La velocidad angular del motor a partir de la cual me conviene cambiar el algoritmo de estimación de velocidad lo puedo inferir a partir de las curvas teóricas del error, así que es no es un problema (o al menos eso creo por ahora).

Con respecto a lo último, lo que vos me explicas fue lo primero que probe como método, pero a la hora de hacer la cuenta de los pulsos de alta frecuencia, los valores que me daban eran mucho menores que los que correspondían, hecho que adjudique a que estaba perdiendo pulsos debido a que los tiempos del loop quizás andaban en ese orden. Es por esto que opté por probar con el timer3 ya que este me llama a una interrupción y así no perdería los pulsos (este es el código que adjunte más arriba), pero el mismo sigue perdiendo el 70% de los pulsos.

Espero haber sido lo suficientemente claro con mi explicación. Desde ya gracias.

surbyte

Este es el código que te prometí.
No es mío, es de Nick Gammon una celebridad en el foro Arduino

Code: [Select]

// Frequency timer
// Author: Nick Gammon
// Date: 10th February 2012

// Input: Pin D2

volatile boolean first;
volatile boolean triggered;
volatile unsigned long overflowCount;
volatile unsigned long startTime;
volatile unsigned long finishTime;

// here on rising edge
void isr ()
{
  unsigned int counter = TCNT1;  // quickly save it
 
  // wait until we noticed last one
  if (triggered)
    return;

  if (first)
    {
    startTime = (overflowCount << 16) + counter;
    first = false;
    return; 
    }
   
  finishTime = (overflowCount << 16) + counter;
  triggered = true;
  detachInterrupt(0);   
}  // end of isr

// timer overflows (every 65536 counts)
ISR (TIMER1_OVF_vect)
{
  overflowCount++;
}  // end of TIMER1_OVF_vect


void prepareForInterrupts ()
  {
  // get ready for next time
  EIFR = bit (INTF0);  // clear flag for interrupt 0
  first = true;
  triggered = false;  // re-arm for next time
  attachInterrupt(0, isr, RISING);     
  }  // end of prepareForInterrupts
 

void setup ()
  {
  Serial.begin(9600);       
  Serial.println("Frequency Counter");
 
  // reset Timer 1
  TCCR1A = 0;
  TCCR1B = 0;
  // Timer 1 - interrupt on overflow
  TIMSK1 = bit (TOIE1);   // enable Timer1 Interrupt
  // zero it
  TCNT1 = 0; 
  overflowCount = 0; 
  // start Timer 1
  TCCR1B =  bit (CS10);  //  no prescaling

  // set up for interrupts
  prepareForInterrupts ();   
 
  } // end of setup

void loop ()
  {

  if (!triggered)
    return;
 
  unsigned long elapsedTime = finishTime - startTime;
  float freq = F_CPU / float (elapsedTime);  // each tick is 62.5 nS at 16 MHz
 
  Serial.print ("Took: ");
  Serial.print (elapsedTime);
  Serial.print (" counts. ");

  Serial.print ("Frequency: ");
  Serial.print (freq);
  Serial.println (" Hz. ");

  // so we can read it 
  delay (500);

  prepareForInterrupts ();   
}   // end of loop


Pruebalo individualmente y observa como mide.

PeterKantTropus

#5
May 18, 2019, 04:58 am Last Edit: May 18, 2019, 12:00 pm by PeterKantTropus
Tu error en el primer código es que utilizas un solo canal y la decisión de sumar o restar pulsos lo tomas por el mensaje que llega desde el ros, esto hace que el error en baja velocidades sea grande, porque estas dependiendo de la velocidad de conexión entre el arduino y el PC para calcularlo, y la suma o resta de pocos pulsos afecta grandemente al calculo. De hecho no es un error de precisión, es un error sistemático, que no pesa cuando tomas un gran cantidad de pulsos para hacer el calculo. Si utilizaras los dos canales de cada encoder puedes obtener directamente los valores reales de desplazamientos, con la detección de dirección y cuádruple resolución (4480 pasos ), mas que suficiente , si el robot no tiene ruedas de varios metros :)

Saludos
"Si no entra como tornillo, entra como clavo"

Perrary

Les respondo a los dos en un mismo mensaje:

Surbyte. Me está costando mucho seguir la idea al código de Nick, y no me gustaría incluir en mi proyecto un código que no comprendo, así que lo voy a seguir analizando más en detalle. La primea duda que me surge del mismo es, me esta dando la frecuencia con la que tengo un nuevo pulso de encoder?

Peter. Pense que no era estrictamente necesario utilizar ambas señales de los encoders, no me había avivado del hecho de la velocidad de conexión PC-Arduino.

Voy a proceder de la siguiente manera:
- Voy a seguir analizando el código de Nick Gammon (las mediciones de frecuencia que estoy obteniendo por el momento son consistentes), para lograr mi propio código de espacio fijo,
- Voy a ver de tener en cuenta la otra señal de los encoders para ver de disminuir el error sistemático en mi código a tiempo fijo.

Ni bien tenga avances los voy a estar comentando.

Saludos y gracias

PeterKantTropus

Prueba un cambio muy simple, en la interrupción cambia de detección de flanco a cambio de señal ( luego divide la velocidad por dos) y debería tener el doble de precisión sin hacer muchas modificaciones.
"Si no entra como tornillo, entra como clavo"

Perrary

#8
May 20, 2019, 10:40 pm Last Edit: May 20, 2019, 10:42 pm by Perrary
EDITADO

Peter, probé el cambio por detección de cambio de señal que me propusiste en el mensaje anterior y la verdad es que no hubo muchos cambios en la precisión de las mediciones, sigo teniendo un error grande. Antes había entendido mal tu explicación y por eso obtenía resultados un tanto extraños.

Por otra parte hice los cambios para utilizar las señales provenientes de ambos encoders y ahora me encuentro con que estoy midiendo velocidades mayores a las que en realidad tengo (estoy contando pulsos de mas). Acá también tengo que tener en cuenta alguna consideración con respecto a dividir por dos o algo por el estilo?

Saludos

surbyte

Quote
Surbyte. Me está costando mucho seguir la idea al código de Nick, y no me gustaría incluir en mi proyecto un código que no comprendo, así que lo voy a seguir analizando más en detalle. La primea duda que me surge del mismo es, me esta dando la frecuencia con la que tengo un nuevo pulso de encoder?
Yo te di el código, supuse que podrias adapatarlo a tus enconders.
La frecuencia es fija y es código cuenta entre detecciones. Pero puedes modificarlo como gustes.
No tengo un enconder de cuadratura como el tuyo asi que no puedo hacerlo.
A y B determinan el sentido de giro.
Asi que para medir velocidad usarás A o B indistintamente.
Las separaciones de A provocan cambios en la interrupcion 1 digamos. Entre dichos cambios mides ese tiempo y ese tiempo va a permitir N ticks del timer. Entonces esos N ticks del timer permiten determinar la velocidad.

Pensé que lo verías con facilidad.

Observa la simpleza del concepto

Ahora fist esta en false y no entra en el if(first)

Code: [Select]
if (first)
    {
    startTime = (overflowCount << 16) + counter;
    first = false;
    return; 
    }


Esto esta dentro de la ISR o sea la interrupcion en el pin 2, obviamente dicho pin 2 estará asociada a A por ejemplo. La interrupción cambia con RISING o sea flanco 0 a 1.
Con cada cambio tenmos a first = true y eso inicia la cuenta
Como verás startime toma el valor de counter mas el contador de overflow o sea cada 16 bits lo que te da capacidad de lectura de velocidades lentas espectacular.

Una vez disparado first se pone en false
Y esperará la nueva interrupcion o sea la segunda marca del encoder.

Asi que sigue y calcula

Code: [Select]
finishTime = (overflowCount << 16) + counter;
  triggered = true;
  detachInterrupt(0); 


que hizo.?
Tienes una variable de finalización de las cuenas
cambias un flag como que se disparó, triggered = true y paras interrupciones para procesar y hacer cuentas.

Listo.. tienes el tiempo entre startime y finishtime

Mas precisio imposible.

PeterKantTropus

Cuando utilizas change en vez de Rising en la interrupción, están contando tanto el flanco de bajada como de subida. Es decir tienes el doble de pulsos al recorrer la misma distancia, por eso debes dividirse la velocidad por dos. Si no obtuviste mejoras al hacer esto , tu problema reside en otro lado. Pensandolo un poco ( especulación), no veo la razón de calcular una velocidad a partir de los pulsos, cuando esta es una medida absoluta de la distancia recorrida. Yo convertiría la consigna que viene por el ROS en pulsos y haría el pid con pulsos.
Además sería esclarecedor si describieras el método que utilizas para evaluar el error.
Saludos
"Si no entra como tornillo, entra como clavo"

Perrary

Surbyte, gracias por la explicación ahora tengo más en claro el funcionamiento del código. Voy a ver si logro adaptarlo a mi caso (nunca he trabajado con los registros de Arduino, así que no se que tan complicado será) y luego voy a ver de compararlo con la solución que me propuso Peter, para ver cual tiene un mejor desempeño.
La ultima duda que me queda (espero que no me mates y aprecio mucho tu paciencia para los novatos como yo en lo que es el mundo del arduino) es la siguiente. Como vos bien me dijiste, en la variable elapsedtime tengo la diferencia entre el inicio y el final, pero esa como esta medida? En cuentas de pulsitos de 16MHz (62,5 ns)? Por ejemplo si tengo un valor de elapsedtime de 1000 es equivalente a decir que entre los dos pulsos del encoder transcurrieron 62,5 us?

Saludos

Perrary

Peter. Entiendo tu planteo acerca del hecho de que calcular velocidades carece de sentido cuando ya puedo acceder directamente a la distancia que me desplace. El porque de querer calcular la velocidad reside en el hecho de que mi intención es utilizar el navigation stack de ROS, el cual es un nodo del ROS que permite implementar sistemas de navegación. El mismo para su funcionamiento requiere que una de las cosas con la que se lo debe de proveer es con la odometría del robot, y en el caso de los mensajes de odometría del ROS, uno de los parametros que debo precisarle es justamente el de la velocidad en mts/seg.

Para evaluar el error en la medición de velocidad lo unico que estoy haciendo es calcular el error relativo. A la medición le estoy restando el valor de la referencia y el resultado lo divido por el valor de la referencia. Estoy midiendo cual es el porcentaje de diferencia para con mi referencia.

Saludos y gracias por la ayuda.

PeterKantTropus

El error relativo siempre da positivo? O a veces da negativo?
"Si no entra como tornillo, entra como clavo"

surbyte

Quote
por ejemplo si tengo un valor de elapsedtime de 1000 es equivalente a decir que entre los dos pulsos del encoder transcurrieron 62,5 us?
Correcto.
De todos modos estas en dos variantes.. lo mejor que sigas una y la agotes y si no funcioina prueba la otra.
Ahora sigue la idea de Peter que contempla el movimiento relativo. Yo nada dije de eso.

Go Up