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

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



现场



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

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

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

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

zx361s舵机是总线舵机.

支持位置回读.

串口通信协议

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

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

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

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

#include <ros/ros.h>
#include <serial/serial.h>
//ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "zzz_arm_2_control_driver/joint_msg.h"
#include <boost/asio.hpp> //包含boost库函数

#include <stdlib.h>
#include<time.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

using namespace boost::asio; //定义一个命名空间，用于
4000
后面的读写操作
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" <<msg->id<<msg->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<zzz_arm_2_control_driver::joint_msg>("/arm_motors", 1000);
zzz_arm_2_control_driver::joint_msg joint_motor_msg;
//ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("/move_group/fake_controller_joint_states", 1);

sensor_msgs::JointState joint_state_msg;
/*
<param name="shoulder_zhuan_joint" value="0" />
<param name="upper_arm_joint" value="1" />
<param name="fore_arm_joint" value="2" />
<param name="hand_wan_joint" value="3" />
<param name="hand_zhuan_joint" value="4" />
*/
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<std::string>("serial_port", serial_port_name, "/dev/ttyUSB0");
nh_private.param<int>("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<<n;
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 <string> result;
string str="";
unsigned char findHead=0;
unsigned char findTail=0;
unsigned char ti=0;
for(i=0;i<dn;i++){
if((data[i])=='#'){
findHead=1;
findTail=0;
str="";
for(ti=i;ti<dn;ti++){
str+=data[ti];
if((data[ti])=='!'){
findTail=1;
break;
}
}
if(findTail){
i=ti;
result.push_back(str);
//here save result

}else{
break;
}
}else{
//ignore
}
}
if(i>0){
ROS_INFO("read buffer lenght=%d",i);
}
//buffer move to 0
for(j=0;i<dn;i++,j++){
data[j]=data[i];
}
dn=j;
//parase strings
int count = result.size();
for (int ri = 0; ri < count;ri++)
{
cout << result[ri] << endl;
string s1,s2,s3;
s1=result[ri];
//#000P1500!
//need check package data here!!!!
size_t iPos = s1.find("P");
if(s1.length()==10&&iPos==4){
s2 = s1.substr(iPos-3, 3);
s3 = s1.substr(iPos+1, 4);
int id=atoi(s2.c_str());
int posi=atoi(s3.c_str());
ROS_INFO("get position result. id= %d ,posi=%d",id,posi );

joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name.resize(1);
joint_state_msg.position.resize(1);
joint_state_msg.name[0] =jointnames[id];
double per=double (posi-1500)/1000.0;
double pi=3.1415926535;
double r;
if(id==1){
//-90~90
r=per*pi/2.0;
}else{
//-135~135
r=per*pi/4.0*3.0;
}
joint_state_msg.position[0] = r ;

joint_pub.publish(joint_state_msg);
ros::spinOnce();
//publishmsg()
//.....to do something
}else{
ROS_INFO("warnning. package data cant be parsed :%s",s1.c_str() );
}

isWaittingPosi=0;
waitDida=0;

}

zzzrat.sleep();
//ROS_INFO("loop---------------");
waitDida+=1;
}

}



前文:

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

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