Unable to properly set up Odometer [closed]

asked 2020-11-21 11:18:53 -0600

Robot_fox gravatar image

Hello, I am using ROS NOETIC on Ubuntu mate 20.04. I am using an arduino mega, rover 5 platform (link text ) that has 2 encoders and motors that i am using. I am trying to send the data collected by the encoders to ros via Rosserial_arduino. I was able to get the data collected but when ran on RVIZ to see how the distance traveled from its starting position, it is going in a circle. It should be going straight because both motors are moving in the same direction at the same speed. Is there something I'm missing? Should i use the nav_msg/Odometry instead? Also for anybody using slam toolbox, do i only need the odometry data to finally start using it properly?

image description image description

image description

#include <Encoder.h>
#include <ros.h>
#include <ros/time.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <math.h>


//DC Motor inputs
const int left_dir=12;
const int left_brake=9;
const int right_dir=13;
const int right_brake=8;

//Setting up Ros
ros::NodeHandle  nh;
geometry_msgs::TransformStamped t;
tf::TransformBroadcaster broadcaster;
char base_link[] = "/base_link";
char odom[] = "/odom";

//Using Encoder library to specify which pins are being used 
Encoder L_Enc(19, 18);
Encoder R_Enc(20,21);

// paramters of robot in mm
float Pi=3.14;
float dia=60;   //diameter 
float ER= 333; // Number of ticks per one full revolution 
float b= 183;
float Dl, Dr, Dc, Ori_ch,Dl_old,Dr_old,x_cor,y_cor;
float Ori  = 0;  //orientation
float x = 0;           
float y = 0; 
float deltaL;
float deltaR;
float L_oldPosition  = 0;
float R_oldPosition  = 0;


//make both dc motors move forward then stop 
void forward_move() {
  digitalWrite(left_brake,LOW);
  digitalWrite(right_brake,LOW);
  digitalWrite(left_dir,HIGH);
  digitalWrite(right_dir,LOW);
  analogWrite(11,255);
  analogWrite(3,255);
  delay(1000);
  digitalWrite(left_brake,HIGH);
  digitalWrite(right_brake,HIGH);
  delay(3000);
  }



void setup() {
  // put your setup code here, to run once:
  nh.initNode();
  broadcaster.init(nh);
  pinMode(left_dir,OUTPUT);
  pinMode(left_brake,OUTPUT);
  pinMode(right_dir,OUTPUT);
  pinMode(right_brake,OUTPUT);


}

void loop() {
  //Left this function a comment when not using motors
  //forward_move();

  //changes in distance traveled by motor set to zero in beginning 
  deltaL=0;
  deltaR=0;

  //Reads the number of ticks  odometer  in each motor
  float R_newPosition = L_Enc.read();
  float L_newPosition = R_Enc.read();

  //If there is a change in ticks for either odometer 
  if (L_newPosition != L_oldPosition) {

    //Converts ticks into distance (Circumference * ticks/resolution) for previous distance
    Dl_old= Pi * dia * (L_oldPosition/ ER);


    L_oldPosition = L_newPosition;

    //Converts ticks into distance (Circumference * ticks/resolution) for previous distance
    Dl= Pi * dia * (L_newPosition/ ER);

    //calculates change in distance 
    deltaL=Dl-Dl_old;

  }

  if (R_newPosition != R_oldPosition) {
    Dr_old= Pi * dia * (R_oldPosition/ ER);
    R_oldPosition = R_newPosition;

    Dr= Pi * dia * (R_newPosition/ ER);

    deltaR=Dr-Dr_old;
  }


  // calculates position
  if ((deltaR!=0)||(deltaL!=0)){

    //Where the center of the rover is 
    Dc=( Dl + Dr) /2 ;

    // Using triangle rule, finds orientation 
    Ori_ch=(Dr - Dl)/b; 

    //if change in orientation, adds it to the previous orientation
    Ori = Ori + Ori_ch;

    // X and y position of rover in mm
    x = x + Dc * cos(Ori);
    y = y + Dc * sin(Ori);

    //Since the distance travel is very small and ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Robot_fox
close date 2020-11-22 20:22:23.295836

Comments

What was the answer? I don't see it.

gvdhoorn gravatar image gvdhoorn  ( 2020-11-23 02:52:21 -0600 )edit