ros总线舵机机械臂串口协议源码

总线舵机机械臂moveit规划效果

现场

前文有普通模拟舵机diy的舵机手臂.

型号996r普通舵机用在手臂上可以说没有什么精度可言扭力也不行,ros试验无法使用.

所以diy手臂时选择舵机还是非常关键的.

某宝上有zx361s舵机做的手臂看起来还行.

zx361s舵机是总线舵机.

支持位置回读.

串口通信协议

查看文档可以用各种简单字符串命令控制舵机

经测试其扭力,精度和线性度还可以

支持回读可以将其位置实时发布出去供moveit 使用.

以下代码实现了与手臂舵机通信的功能

#include 
#include 
//ROS已经内置了的串口包
#include 
#include 
#include "zzz_arm_2_control_driver/joint_msg.h"
#include                   //包含boost库函数

#include 
#include
#include 
#include 

using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作
using namespace std;
serial::Serial ser; //声明串口对象
//io_service Object
io_service m_ios;

//Serial port Object
serial_port *pSerialPort;

//For save com name
//any_type m_port;

//Serial_port function exception
boost::system::error_code ec;


std::string serial_port_name;
int serial_baudrate = 115200;
unsigned char AA=1;
unsigned char aa;
//回调函数
void write_callback(const zzz_arm_2_control_driver::joint_msg::ConstPtr& msg)
{
    //ROS_INFO_STREAM("Writing to serial port" <id<r);
    unsigned char mid=(char) msg->id;
    double r=msg->r;
    double pi=3.1415926535;
    double per;
    if(mid==1){
      //-90~90
      per=2.0*r/pi;
    }else{
      //-135~135
      per=4.0/3.0*r/pi;
    }

    int posi=1500+int(per*1000.0);
    posi=posi<500?500:posi;
    posi=posi>2500?2500:posi;
    string sendData="#00";
    string c2str(1,'0'+mid);
    sendData+=c2str;
    sendData+="P";
    //string si=itoa(posi);
    ostringstream os;
    os << posi;
    string si = os.str();
    for (char i=0;i<4-si.length();i++){
      sendData+="0";
    }
    sendData+=si;
    sendData+="T0100!";
    //ROS_INFO("-------------%s",sendData.c_str());
    //#000P1500T1000!
    //{#000P1500T1000!#001P1500T1000!}
    //发送串口数据
    try
    {
      size_t len = write( *pSerialPort, buffer( sendData,sendData.length() ), ec );
      ROS_INFO("positon command send: %s",sendData.c_str());
    }catch (boost::system::system_error e){
      ROS_ERROR_STREAM("serail write err ");

    }
}
unsigned char buf[1];                      //定义字符串长度
unsigned char data[255];//
unsigned int dn=0;
unsigned int i=0,j=0;
unsigned char isWaittingPosi=0;
unsigned char waitId=0;
unsigned char waittingId=0;
unsigned char maxId=4;//input a wrong maxid can run test code when time is out.
unsigned char waitDida=0;
unsigned char waitDidaMax=50;//wait dida
int main (int argc, char** argv)
{

    //初始化节点
    ros::init(argc, argv, "arm2_jointserial");
    //声明节点句柄
    ros::NodeHandle nh;

    //订阅主题，并配置回调函数
    ros::Subscriber write_sub = nh.subscribe("/arm_motors", 1000, write_callback);
    //发布主题
    ros::Publisher joint_motor_pub = nh.advertise("/arm_motors", 1000);
    zzz_arm_2_control_driver::joint_msg joint_motor_msg;
    //ros::Publisher read_pub = nh.advertise("read", 1000);
    ros::Publisher joint_pub = nh.advertise("/move_group/fake_controller_joint_states", 1);

    sensor_msgs::JointState joint_state_msg;
    /*
      
      
      
      
      
*/
    string jointnames[5] = {"shoulder_zhuan_joint",
                           "upper_arm_joint",
                           "fore_arm_joint",
                           "hand_wan_joint",
                           "hand_zhuan_joint"};

    ros::NodeHandle nh_private("~");
    nh_private.param("serial_port", serial_port_name, "/dev/ttyUSB0");
    nh_private.param("serial_baudrate", serial_baudrate, 115200);


    pSerialPort = new serial_port( m_ios );
    if ( pSerialPort ){
        //init_port( port_name, 8 );
        //Open Serial port object
        pSerialPort->open( serial_port_name, ec );
        //Set port argument
        pSerialPort->set_option( serial_port::baud_rate( serial_baudrate ), ec );
        pSerialPort->set_option( serial_port::flow_control( serial_port::flow_control::none ), ec );
        pSerialPort->set_option( serial_port::parity( serial_port::parity::none ), ec );
        pSerialPort->set_option( serial_port::stop_bits( serial_port::stop_bits::one ), ec);
        pSerialPort->set_option( serial_port::character_size( 8 ), ec);
        m_ios.run();
    }

    ROS_INFO("-------------zzz joint serail is running .");

    ros::Rate zzzrat(100.0);


    while(nh.ok()){
      //check time is out.
      if(waitDida>waitDidaMax){
        waitDida=0;
        isWaittingPosi=0;
        ROS_INFO("error: time is out when conect motor %d.",waittingId );

        //test random position
        if(0){
          srand(time(0));
          joint_motor_msg.id=3;
          joint_motor_msg.r=3.14* (((float)rand()/RAND_MAX)-0.5)*2.0;
          joint_motor_pub.publish(joint_motor_msg);
        }
      }
      //send new request
      if(isWaittingPosi==0){
        //#000PRAD!
        //send position request ,result is #000P1500!

       /* stringstream ss;
        int n = int (waitId);
        string str;
        ss<>str;
        */
        string sendData="#00";
        string c2str(1,'0'+waitId);//<10
        sendData+=c2str;
        sendData+="PRAD!";
        try
        {
          size_t len = write( *pSerialPort, buffer( sendData,sendData.length() ), ec );
          ROS_INFO("positon request send: %s",sendData.c_str());
          isWaittingPosi=1;
          waittingId=waitId;
          //
          waitId+=1;
          waitId=(waitId>maxId)?0:waitId;
        }catch (boost::system::system_error e){
            ROS_ERROR_STREAM("serail write err ");

        }
      }
      //read
      while(1){
        try
        {
          read (*pSerialPort,buffer(buf));
          data[dn]=buf[0];
          dn++;
          if(dn>=64){break;}
          //ROS_INFO("%d", buf[0]);//打印接受到的字符串
        }catch (boost::system::system_error e){
          //ROS_ERROR_STREAM("read err ");
          break;
        }
      }
      //ROS_INFO("read");
      //parse package//{'#'...'!'}
      vector  result;
      string str="";
      unsigned char findHead=0;
      unsigned char findTail=0;
      unsigned char ti=0;
      for(i=0;i0){
        ROS_INFO("read buffer lenght=%d",i);
      }
      //buffer move to 0
      for(j=0;i

前文:

ros之真实驱动diy6自由度机械臂

ros之真实驱动diy6自由度机械臂（moveit中controller源码）