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:
#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.
