Ask Your Question
0

[RESOLVED] Arduino subscribe to joint_states

asked 2016-11-14 07:38:11 -0500

karlhm76 gravatar image

updated 2016-11-14 07:57:56 -0500

Hello,

I have connected my custom built robot arm to my arduino using adafruit PWM. I have tested subscribing to a simple UInt16 topic and manually publishing a message to move a servo. It works.

Now I want to link the arm to rviz. My goal is to use the sliders to move the joints on the screen, and have the real arm move as well.

This does not work at all.

The problem seems to be my callback on the arduino is never called. I have no idea why. It could be because no messages are getting there, but I do echo the joint_states topic in another window and I see the numbers changing as I slide the sliders. I'd expect these to arrive at the arduino as well?

It could be a problem with my arduino code setup, even though it worked with the simple subscription, it may not work with the more complex one. It is below.

Or maybe it is something else entirely.

(I have some problems with joint orientations in my model, with the intention of recalibrating the model properly at some point. So please ignore the dodgy code to convert radians to degrees)

Also I am only attempting to move one servo at this point, which is plugged into the adafruit at position 0, but this relates to joint 5 in the arm.

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <sensor_msgs/JointState.h>

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

ros::NodeHandle nh;
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

#define SERVOMIN  120 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  560 // this is the 'maximum' pulse length count (out of 4096)

#define BASE_SERVO 0
#define SHOULDER_SERVO 1
#define ELBOW_SERVO 2
#define FOREARM_SERVO 3
#define WRIST_SERVO 4
#define WRIST_ROLL_SERVO 5
#define GRIPPER_SERVO 6

#define VELOCITY 0.1

double targets[6] = {90,90,90,90,90,90};
double positions[6] = {90,90,90,90,90};

void servo_cb(const sensor_msgs::JointState& cmd_msg)
{
  digitalWrite(13, HIGH);
  targets[0] = radiansToDegrees(cmd_msg.position[5]);
  digitalWrite(13, LOW);

}

ros::Subscriber<sensor_msgs::JointState> sub("joint_states", servo_cb);

void setup()
{
  pinMode(13, OUTPUT);

  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates

  //nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.subscribe(sub);

  MoveServo(0,90);

}

void loop()
{

  // move towards target at speed velocity
  MoveServo(0, positions[0]);

  if (positions[0] < targets[0])
    positions[0] += VELOCITY;

  if (positions[0] > targets[0])
    positions[0] -= VELOCITY;


  nh.spinOnce();

}

void MoveServo(int servoNumber, double angleInDegrees)
{
  uint16_t pulselength = (uint16_t)map(angleInDegrees, 0, 180, SERVOMIN, SERVOMAX);

  pwm.setPWM(servoNumber, 0, pulselength);
}

double radiansToDegrees(float position_radians)
{

  position_radians = position_radians + 1.6;

  return position_radians * 57.2958;

}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-11-14 07:57:34 -0500

karlhm76 gravatar image

I got it working. seems like my baud rate wasn't fast enough.

the arm is also very slow to respond to changes in position. I suppose this is because of the speed of the connection. I am running at 115200.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-11-14 07:38:11 -0500

Seen: 692 times

Last updated: Nov 14 '16