Hello there, I'm a ROS beginner and is now trying to build a robot with 18 servos controlled by Moveit! on a Jetson NX2 for graduation project. (Here's the bot) https://drive.google.com/file/d/1iWeBi_G30bHwYX588W1rpUtnrv7GpGnw/view?usp=share_link I made the code base on Ken McElhinney's blog(http://ken.mcelhinney.net.au/servoarm/servoarm-simple-robot-arm-part-2/), and I was able to establish connection between Arduino and ROS. But the publisher(warpwraith) from Arduino sent only zero for each servo no matter how jointstate or the robot in Moveit! changes. Here are the photos of the ROS showing the issue and info: https://drive.google.com/file/d/1doxYpF2Ao4NRxp5QROygH9mxOJTIwSxl/view?usp=share_link https://drive.google.com/file/d/1LATju7W600C5Y0R1YjT-gxGVCCkX41AF/view?usp=share_link And here is the Arduino where is where the problem came (and can't find any): #include "Wire.h" #include

    #define SERVOMIN  102 // This is the 'minimum' pulse length count (o

ut of 4096)
#define SERVOMAX  512 // This is the 'maximum' pulse length count (out of 4096) MID:307
#define MIN_PULSE_WIDTH       500
#define MAX_PULSE_WIDTH       2500
#define FREQUENCY             50

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

#include "Arduino.h"
#include "ArduinoHardware.h"
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Int16MultiArray.h>
#include <sensor_msgs/JointState.h>

// Set ROS - handler, subscribe message, publish message (debugging)
ros::NodeHandle  nh;

std_msgs::Int16MultiArray str_msg2;

ros::Publisher chatter("warp_wraith", &str_msg2);

int servoDegree[18];//joints

// Define Servo Outputs on first PCA9685 board(a,b,c)
int leg_a0 = 0;
int leg_a1 = 1;

int leg_a2 = 2;
int leg_b0 = 3;
int leg_b1 = 4;
int leg_b2 = 5;
int leg_c0 = 6;
int leg_c1 = 7;
int leg_c2 = 8;
// Define Servo Outputs on second PCA9685 board(a,b,c)
int leg_d0 = 9;

int leg_d1 = 10;
int leg_d2 = 11;
int leg_e0 = 12;
int leg_e1 = 13;
int leg_e2 = 14;
int leg_f0 = 15;
int leg_f1 = 16;
int leg_f2 = 17;

// Define Motor position variables on first PCA9685 board(a,b,c)
double mtrDegreeleg_a0;

double mtrDegreeleg_a1;
double mtrDegreeleg_a2;
double mtrDegreeleg_b0;
double mtrDegreeleg_b1;
double mtrDegreeleg_b2;
double mtrDegreeleg_c0;
double mtrDegreeleg_c1;
double mtrDegreeleg_c2;
// Define Motor position variables on second PCA9685 board(a,b,c)
double mtrDegreeleg_d0;

double mtrDegreeleg_d1;
double mtrDegreeleg_d2;
double mtrDegreeleg_e0;
double mtrDegreeleg_e1;
double mtrDegreeleg_e2;
double mtrDegreeleg_f0;
double mtrDegreeleg_f1;
double mtrDegreeleg_f2;
// Function move motor to ROS angle
void servo_cb(const sensor_msgs::JointState& cmd_msg)
{



 mtrDegreeleg_a0 = trimLimits(radiansToDegrees(cmd_msg.position[0]));//mtrDegreeleg_a0 = trimLimits(radiansToDegrees(cmd_msg.position[0]) - 90);
  mtrDegreeleg_a1 = trimLimits(radiansToDegrees(cmd_msg.position[1]));
  mtrDegreeleg_a2 = trimLimits(radiansToDegrees(cmd_msg.position[2]));


          mtrDegreeleg_b0 = trimLimits(radiansToDegrees(cmd_msg.position[3]));
      mtrDegreeleg_b1 = trimLimits(radiansToDegrees(cmd_msg.position[4]));

  mtrDegreeleg_b2 = trimLimits(radiansToDegrees(cmd_msg.position[5]));

  mtrDegreeleg_c0 = trimLimits(radiansToDegrees(cmd_msg.position[6]));
  mtrDegreeleg_c1 = trimLimits(radiansToDegrees(cmd_msg.position[7]));
  mtrDegreeleg_c2 = trimLimits(radiansToDegrees(cmd_msg.position[8]));
  mtrDegreeleg_d0 = trimLimits(radiansToDegrees(cmd_msg.position[9]));
  mtrDegreeleg_d1 = trimLimits(radiansToDegrees(cmd_msg.position[10]));
  mtrDegreeleg_d2 = trimLimits(radiansToDegrees(cmd_msg.position[11]));

  mtrDegreeleg_e0 = trimLimits(radiansToDegrees(cmd_msg.position[12]));
  mtrDegreeleg_e1 = trimLimits(radiansToDegrees(cmd_msg.position[13]));
  mtrDegreeleg_e2 = trimLimits(radiansToDegrees(cmd_msg.position[14]));

  mtrDegreeleg_f0 = trimLimits(radiansToDegrees(cmd_msg.position[15]));
  mtrDegreeleg_f1 = trimLimits(radiansToDegrees(cmd_msg.position[16]));

  mtrDegreeleg_f2 = trimLimits(radiansToDegrees(cmd_msg.position[17]));

  // Store motor movements for publishing back to ROS (debugging)
  servoDegree[0] = mtrDegreeleg_a0;
  servoDegree[1] = mtrDegreeleg_a1;
  servoDegree[2] = mtrDegreeleg_a2;
  servoDegree[3] = mtrDegreeleg_b0;
  servoDegree[4] = mtrDegreeleg_b1;
  servoDegree[5] = mtrDegreeleg_b2;
  servoDegree[6] = mtrDegreeleg_c0;
  servoDegree[7] = mtrDegreeleg_c1;



servoDegree[8] = mtrDegreeleg_c2;

      servoDegree[9] = mtrDegreeleg_d0;
      servoDegree[10] = mtrDegreeleg_d1;
      servoDegree[11] = mtrDegreeleg_d2;
      servoDegree[12] = mtrDegreeleg_e0;
      servoDegree[13] = mtrDegreeleg_e1;
      servoDegree[14] = mtrDegreeleg_e2;
      servoDegree[15] = mtrDegreeleg_f0;
      servoDegree[16] = mtrDegreeleg_f1;
      servoDegree[17] = mtrDegreeleg_f2;

  moveMotorDeg(mtrDegreeleg_a0, leg_a0);
  moveMotorDeg(mtrDegreeleg_a1, leg_a1);
  moveMotorDeg(mtrDegreeleg_a2, leg_a2);
  moveMotorDeg(mtrDegreeleg_b0, leg_b0);
  moveMotorDeg(mtrDegreeleg_b1, leg_b1);
  moveMotorDeg(mtrDegreeleg_b2, leg_b2);
  moveMotorDeg(mtrDegreeleg_c0, leg_c0);
  moveMotorDeg(mtrDegreeleg_c1, leg_c1);
  moveMotorDeg(mtrDegreeleg_c2, leg_c2);

  moveMotorDeg(mtrDegreeleg_d0, leg_d0);
  moveMotorDeg(mtrDegreeleg_d1, leg_d1);
  moveMotorDeg(mtrDegreeleg_d2, leg_d2);
  moveMotorDeg(mtrDegreeleg_e0, leg_e0);
  moveMotorDeg(mtrDegreeleg_e1, leg_e1);
  moveMotorDeg(mtrDegreeleg_e2, leg_e2);
  moveMotorDeg(mtrDegreeleg_f0, leg_f0);
  moveMotorDeg(mtrDegreeleg_f1, leg_f1);
  moveMotorDeg(mtrDegreeleg_f2, leg_f2);
}



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

  void setup()
  {
    // Setup ROS fir subscribe and publish
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.subscribe(sub);
    nh.advertise(chatter);

    // Setup PWM Controller object
    pwm1.begin();
pwm1.setPWMFreq(FREQUENCY);
    pwm2.begin();
    pwm2.setPWMFreq(FREQUENCY);
  }

  // Function to move motor to specific position
  void moveMotorDeg(int moveDegree, int motorOut)
  {
    int pulse_wide, pulse_width;

    // Convert to pulse width
    pulse_wide = map(moveDegree, 0, 320, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
    pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);

        //Control Motor 
    //use while two classifi the two board 2022/9/19
   while(motorOut < 9){
    pwm1.setPWM(motorOut, 0, pulse_width);
   }
   while(motorOut >= 9)
   {
    pwm2.setPWM(motorOut-9, 0, pulse_width);
    }
  }

  void loop()
  {
        str_msg2.data = servoDegree;
    str_msg2.data_length = 18;//all servo data message
    chatter.publish( &str_msg2 );
    nh.spinOnce();
  }

  // Convert radians to degreees
  double radiansToDegrees(float position_radians)
  {
        position_radians = position_radians * 57.2958;
    return position_radians;
  }

  // Sometimes servo angle goes just above 320 or just below 0 - trim to 0 or 320
  double trimLimits(double mtr_pos)
  {
    if(mtr_pos > 320) {
      mtr_pos = 320;
        }
        if(mtr_pos < 0) {
          mtr_pos = 0;
    }
    return mtr_pos;
  }  

I personally think the problem could came form void servocb(const sensormsgs::JointState& cmd_msg) nut could not spot any issue. My specification is: Arduino Mega, Jetson NX2, ROS version: Melodic. Using two PCA9685 servo driving board. Any help would be highly appreciated. If this can be solved, a really cool robot can be built.