How to Publish Array Data for Multiple Servos on Arduino

asked 2016-05-24 19:58:34 -0600

d7x gravatar image

updated 2016-05-25 12:37:29 -0600

I'm trying to publish an array in Python that is subscribed to by code on an Arduino that has callbacks for an Adafruit PWM servo controller. The code on the Arduino compiles and uploads to an Arduino Uno just fine. I'm not sure how to publish a multiarray for the following example: 3 servos that each have different integer angles between 0 and 180. I'm getting the following error messages when I run: TypeError: Invalid number of arguments, args should be ['layout', 'data'] args are('{data: [20,50,100]}',)

How should I be publishing the multiarray in this example?

Python Publishing Code: print "reco_event_servo_pwm: set up data values for servos" servo_pub1 = rospy.Publisher('servo_pwm', UInt16MultiArray, queue_size=10)

n = 3
while n >= 0:
    servo_pub1.publish('{data: [20,50,100]}')  # THIS IS THE STATEMENT THAT HAS ERRORS
    rate.sleep()
    n = n - 1

Arduino Code:

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

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16MultiArray.h>
#include <std_msgs/String.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>


/////////////////////////////////////////////////////////////////////////////////


ros::NodeHandle  nh;

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Depending on your servo make, the pulse width min and max may vary, you 
// want these to be as small/large as possible without hitting the hard stop
// for max range. You'll have to tweak them as necessary to match the servos you
// have!
#define SERVOMIN  200 // this is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  400 // this is the 'maximum' pulse length count (out of 4096)

// our servo # counter
uint8_t servonum = 0;

void servo_ctlr_cb( const std_msgs::UInt16MultiArray&  cmd_msg) {
//  servo1.write(cmd_msg.data); //set servo angle, should be from 0-180

  for (int i=0; i<3; i++) { // run for all servos for TESTing
    pwm.setPWM(i, 0, cmd_msg.data[i]); //PWM signal, state where it goes high, state where it goes low, 0-4095, deadband 351-362
  }
}

ros::Subscriber<std_msgs::UInt16MultiArray> sub1("servo_pwm", servo_ctlr_cb);

void setup() {
  Serial.begin(9600);

  nh.initNode();
  nh.subscribe(sub1);

  pwm.begin();

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

  for(int j=0;j<8;j++) {  //initialize every thruster via channel (0-7) with a for-loop 
    pwm.setPWM(j,0,351);
  }

}

// you can use this function if you'd like to set the pulse length in seconds
// e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise!
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;

  pulselength = 1000000;   // 1,000,000 us per second
  pulselength /= 60;   // 60 Hz
  pulselength /= 4096;  // 12 bits of resolution
  pulse *= 1000;
  pulse /= pulselength;
  pwm.setPWM(n, 0, pulse);
}

void loop() {
  nh.spinOnce();
  delay(20);
}
edit retag flag offensive close merge delete