I am doing a project with rosserial making the encoder reach the desired angle.I publish the angle from the terminal and the arduino side subscribes it. But the encoder motor keeps rotating without stopping at the desired angle. Why?

asked 2019-03-04 05:57:31 -0600

Sai Htet gravatar image

updated 2019-03-04 06:32:26 -0600

gvdhoorn gravatar image

Copy-paste of entire program:

#if (ARDUINO >= 100)
  #include <Arduino.h>
#else
  #include <WProgram.h>
#endif

#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include <ros.h>
#include <std_msgs/Int64.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <simple_arm/GoToPosition.h>

#define PWM 5
#define CW 7
#define CCW 8
#define encoderPinA 2
#define encoderPinB 3
#define lim_sw 10

ros:: NodeHandle nh;

float count;

float des_angle, act_angle, diff_error, int_error, motorVolt;
float Kp, Ki, Kd, error, prverror, myBit, de, dt, sum_error;
float nowtime, prvtime;

void encoder_cb( const std_msgs::UInt16& cmd_msg){

  nowtime = millis()/1000.0;

  de = error - prverror;
  dt = nowtime - prvtime;
  sum_error = error+prverror;

  Kp = 0.02; 
  Ki = 0.5; //Gains
  Kd = 1.5;

  des_angle =  cmd_msg.data; 
  act_angle = (360.0/3266.5) * count;


  error = des_angle - act_angle;
  diff_error = de/dt;
  int_error = sum_error*dt;

  motorVolt = (Kp*(error))+(Kd*(diff_error))+(Ki*(int_error));

  myBit = (abs(motorVolt)/12.0)*255;
  myBit = constrain (myBit,0,255);

  if (des_angle > act_angle)
  {
    digitalWrite(CW,HIGH);
    digitalWrite(CCW,LOW);
  }
  else if ( des_angle < act_angle )
  {
    digitalWrite(CW,LOW);
    digitalWrite(CCW,HIGH);
  }

  analogWrite(PWM,myBit);

  prverror = error;
  prvtime = nowtime;

}

ros::Subscriber<std_msgs::UInt16> sub("servo", encoder_cb);

void setup() {

  pinMode(PWM,OUTPUT);
  pinMode(CW,OUTPUT);
  pinMode(CCW,OUTPUT);
  pinMode(encoderPinA,INPUT);
  pinMode(encoderPinB,INPUT);
  attachInterrupt(digitalPinToInterrupt(2), readEncoder ,CHANGE);
  Serial.begin(57600);
  digitalWrite(lim_sw,HIGH);
  nh.initNode();
  nh.subscribe(sub);
}

void readEncoder()
{
  if(digitalRead(encoderPinA) == digitalRead(encoderPinB))
  {
    count = count - 1;
  }
  else
  {
    count = count + 1;
  }
}


void loop(){

  nh.spinOnce();

}
edit retag flag offensive close merge delete