Arduino ROS communication using dc motor with encoder [closed]

asked 2017-11-06 05:03:28 -0600

Mekateng gravatar image

updated 2017-11-20 03:58:50 -0600

Hi, I obtained 3d pointcloud data from 2d laser scanner using servo motor. But now, I want to use dc motor with encoder.Now the dc motor is turning 360 degrees continuously, And I get 360 degrees angle data from encoder The angle data coming from the encoder and the angle data coming from the ROS do not match.So a ridiculous image is formed.How do I pair this two angles? Please say an easy method to solve this problem. Thanks in advance.

Here the code . I communicated this code with ROS.But when the motor turns, I get a very poorly spaced image.

#if (ARDUINO >= 100)
 #include <Arduino.h>
#else
 #include <WProgram.h>
#endif
#include <math.h>
#include <ros.h>
#include <sensor_msgs/JointState.h>

int enA = 10;
int in1 = 9;
int in2 = 8;

long pulse = 0;
double ang_arduino = 0;
double angle=0;
int encoder_a = 2; // A Pin connect to digital pin 2
int encoder_b = 3; // B Pin connect to digital pin 3
float encoder_resolution = 8400;
ros::NodeHandle  nh;
long new_time = 0;
long old_time = 0;
long old_pulse = 0;
double error = 0;
double error_der = 0;
double old_error = 0;
double sinyal = 0;
int sinyal_int = 0;
double velocity[2] = {0,0};
double velocity_filt[2] = {0,0};
double v_ref = 24;
double Kp = 0.4;
double Kd = 0.0000;
long dt = 0;
int sayac = 0;

void servo_cb(const sensor_msgs::JointState& cmd_msg){
  angle=radiansToDegrees(cmd_msg.position[0]);
  }
ros::Subscriber<sensor_msgs::JointState> sub("joint_states", &servo_cb);


void setup()
{
  nh.initNode();
  nh.subscribe(sub);  
  // delay(100);                                                 // Wait for everything to be powered up
  //Serial.begin(57600);    // Communication baudrate
  // Create interrupt : Function name --> encoder_interrupt_a the function is called on each changing edge
  // note that you should connect A pin of encoder to digital pin 2
  attachInterrupt(1, encoder_kesme_a, CHANGE);
  attachInterrupt(0, encoder_kesme_b, CHANGE);




pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);

  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);

  old_time = micros();

}
void loop()
{
  ang_arduino = ((pulse / encoder_resolution) * 360.0); // calculate angel according to pulse count
  ang_arduino = fmod(ang_arduino, 360.0);    // get mode acoording to a cycle  ( i.e 365 --> 5 degree)
  if (sayac >= 1)
  {
    new_time = micros();
    dt = new_time - old_time;
    if (pulse < old_pulse) velocity[1] = 1000000 * (8400 + pulse - old_pulse) / dt;
    else velocity[1] = 1000000 * (pulse - old_pulse) / dt;
    velocity[1] = velocity[1] * 360 / 8400;

    velocity_filt[1] =  0.9 * velocity_filt[0] + 0.1 * velocity[0];

error = v_ref - velocity_filt[1]; // derece/saniye cinsinden
error_der = 1000000 * (error - old_error) / dt;

sinyal = Kp * error + Kd * error_der; // voltaj cinsinden
if (sinyal < -10) sinyal = -10;
else if (sinyal > 10) sinyal = 10;

if (sinyal < 0)
{
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
}
else
{
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
}
sinyal = 25.5 * abs(sinyal);
analogWrite(enA, sinyal);

    velocity_filt[0] = velocity_filt[1];
    velocity[0] = velocity[1];
    old_error = error;
    old_pulse = pulse;
    old_time = new_time;
    //Serial.println(velocity_filt[1]);
    sayac = 0;
  }
  sayac++;

  nh.spinOnce();
  delay(1);
}

// Interrupt Function is created
void encoder_kesme_a()
{
  if (digitalRead(encoder_a) == digitalRead(encoder_b))
  {
    if ( pulse == 0) pulse = 8400;  // (Counter Clock Wise)
    else pulse--;                                // (Clock Wise)
  }
  else
  {
    if (pulse == 8400) pulse = 0;
    else pulse++;
  }
}

void encoder_kesme_b()
{
  if (digitalRead(encoder_b) == digitalRead(encoder_a))
  {
    if (pulse ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason duplicate question by Mekateng
close date 2017-12-03 11:02:24.756685

Comments

Can you share the part of the code that reads the encoder? Are you using any example code? Without that it would be a bit difficult to debug this issue.

Gary Servin gravatar imageGary Servin ( 2017-11-07 09:13:42 -0600 )edit

First of all thank you for your answer Mr. Servin. Can I send the code to your mail address if it's okay for you? :/

Mekateng gravatar imageMekateng ( 2017-11-07 11:46:12 -0600 )edit
2

You can add it to your original post, there is an icon (with 1 and 0s) to format the code

Gary Servin gravatar imageGary Servin ( 2017-11-07 12:25:25 -0600 )edit

I added Mr. Servin. Please look at it

Mekateng gravatar imageMekateng ( 2017-11-07 13:18:59 -0600 )edit

@Mekateng: I moved the information from your answer to your question. In the future, please only update your question, don't use answers to give more information.

jayess gravatar imagejayess ( 2017-11-14 03:46:57 -0600 )edit

do you have any idea???

Mekateng gravatar imageMekateng ( 2017-11-20 03:59:39 -0600 )edit