Go Down

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

Perrary

Peter, efectivamente, a veces da positivo y a veces negativo.

Surbyte. Gracias por la confirmación. Estoy haciendo eso justamente, hice un par de ajustes a mi código siguiendo las recomendaciones de Peter, y ahora mismo estoy por probarlo y hacer cálculos. Si el robot mide bien, subo el código final con las modificaciones y doy por cerrado el topico.

Saludos

Perrary

Finalmente obtuve un código que funciona con un error relativamente bajo (a bajas velocidades tengo un error del 6% a lo sumo y a altas velocidades del orden del 3%). Gracias tanto Surtbyte como Peter por la ayuda. El código es el siguiente:

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 = 18;  // Pin encoder 1, señal A
const byte pinEnc2a = 21; // Pin encoder 2, señal A
const byte pinEnc1b = 19;  // Pin encoder 1, señal B
const byte pinEnc2b = 20; // 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.035; // 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, vrpp = 0, vlpp = 0; //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 = 1;
double Kd = 0;
DualVNH5019MotorShield md;

//Variables para cálculo de la odometría
double x = 0.0;
double y = 0.0;
double th = 0.0;
float deltat;//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 = 1;

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

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

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

//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 = vrpp = 0;
  }
  if (Vl == 0) {
    Vlp = vlpp = 0;
  }
  if (Vr != vrpp) {
    Deltavr = (Vr - Vrp) * 0.1;
  }
  if (Vl != vlpp) {
    Deltavl = (Vl - Vlp) * 0.1;
  }
}

//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);

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

  aux = 1000000 / f;

  //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 );

  //Establezco el límite inicial en tiempo para medir velocidad
  ta1 = cont1 = micros();
}

void loop() {
  ta2 = cont2 = micros();
  vrpp = Vr;
  vlpp = Vl;

  if ((ta2 - ta1) > 50000) {
    deltat = (ta2 - ta1) * 0.000001;
   
    noInterrupts();
   
    //Cálculo que me da la velocidad del motor 1
    Input1 = paso1 / 0.05 * PI / 560;
    paso1 = 0;

    //Cálculo que me da la velocidad del motor 2
    Input2 = paso2 / 0.05 * PI / 560;
    paso2 = 0;
   
    interrupts();

    //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
    if (0.01 > abs(Vl - Vlp)) {
      Deltavl = 0;
    }
    Vlp = Vlp + Deltavl;

    if (0.01 > abs(Vr - Vrp)) {
      Deltavr = 0;
    }
    Vrp = Vrp + Deltavr;
  }
 
  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 = x;
  odometria.pose.pose.position.y = y;
  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 (digitalRead(pinEnc1b) == HIGH) {
    paso1--;
  }
  else {
    paso1++;
  }
}

//Cuento la cantidad de pasos del encoder del motor 2
void interrupcion2()
{
  if (digitalRead(pinEnc2b) == HIGH) {
    paso2++;
  }
  else {
    paso2--;
  }
}


Saludos

Go Up