getting x, y, theta from wheel encoder values for P3DX robot

asked 2015-10-17 06:48:03 -0600

mrobo gravatar image

updated 2015-10-17 11:18:48 -0600

I am using getLeftEncoder() and getRightEnoder() functions from Aria library. i am publishing left and right encoder values using custom message. I calculated x, y and theta. The robot has to follow 2*2 square grid for 5 times. The plot i am getting is not desired and kind of random. can anybody tell me where am i getting wrong?

Below is the code i have used-

#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include "rosaria/custom_encoder.h"
#include <iostream>
#include <fstream>
#include <math.h> 

using namespace std;

float b_actual = 0.3226;
float Cm = 0.0000078125;  /*(units = meters/pulse)*/
float Cl = 1.0000;
float Cr = 1.0000;

float thetaold = 0;
float xold = 0;
float yold = 0;
float _previousEnL = 0;
float _previousEnR = 0;
float dU, EnL, EnR, dUL, dUR, dtheta, x, y, theta, _currentEnL, _currentEnR;


void chatterCallback(const rosaria::custom_encoder::ConstPtr& msg)
{
  _currentEnL = msg->Left_Encoder;
  _currentEnR = msg->Right_Encoder;

  EnL = _currentEnL - _previousEnL;
  EnR = _currentEnR - _previousEnR;

  ROS_INFO("left encoder:[%f]:", EnL);
  ROS_INFO("right encoder:[%f]:", EnR);

  dUL = Cl * Cm * EnL;
  dUR = Cr * Cm * EnR;

  dU = (dUL + dUR)/2;
  dtheta = (dUL - dUR)/b_actual;

  theta = thetaold + dtheta;
  x = xold + dU * cos (theta);
  y = yold + dU * sin (theta);

  thetaold = theta;
  xold = x;
  yold = y;

  _previousEnL = _currentEnL; 
  _previousEnR = _currentEnR;

  ofstream People("mr.txt", std::ios_base::app);
  People <<x<< ", "<<y<< "," <<theta<<std::endl;
  ROS_INFO("Modified wheel Pose: (%f, %f, %f)", x, y, theta);


}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "subs_pose");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/RosAria/custom_Encoder", 1000, chatterCallback);
  ros::spin();
  return 0;
}
edit retag flag offensive close merge delete