通过ROS控制真实机械臂(2)----单轴运动，手柄控制

创建ROS包，包名redwall_arm ，通过自定义的消息，将手柄的数据发布 msg/ joycontrol.msg，内容如下，分别对应罗技手柄的按钮和遥杆轴。

int32 button1
int32 button2
int32 button3
int32 button4
int32 button5
int32 button6
int32 button7
int32 button8
float32 axis0
float32 axis1
float32 axis2
float32 axis3
float32 axis4
float32 axis5

有了消息类型，接下来就是手柄数据处理和发送ros节点，命名为logitech_pub.cpp，导师要求主从模式，即两个手柄，当主手柄控制的时候，从手柄无法操作，当只插入了一个手柄的usb时，默认为主手柄。文件开头的宏定义为手柄按钮和摇杆轴的地址，可以通过jstest-gtk查看。

#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "redwall_arm/joycontrol.h"
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <fcntl.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/input.h>
#include <linux/joystick.h>
#include <errno.h>
#include <pthread.h>
#include <math.h>#define AXES0          0x00
#define AXES1          0x01
#define AXES2          0x02
#define AXES3          0x03
#define AXES4          0x04
#define AXES5          0x05
#define BUTTON1        0x01
#define BUTTON2        0x02
#define BUTTON3        0x03
#define BUTTON4        0x04
#define BUTTON5        0x05
#define BUTTON6        0x06
#define BUTTON7        0x07
#define BUTTON8        0x08/*手柄模式，单主手柄mode=1，双手柄mode=2*/
int mode;/* 主手柄 */
int axes0, axes1, axes2, axes3, axes4, axes5;
int button1, button2, button3, button4, button5, button6, button7, button8;
struct js_event js_master;
int js_fd_master;/* 从手柄 */
int axes0_, axes1_ , axes2_, axes3_, axes4_, axes5_;
int button1_, button2_, button3_, button4_, button5_, button6_, button7_, button8_;
struct js_event js_support;
int js_fd_support;void * read_js_data_master(void *);
void * read_js_data_support(void *);using namespace std;int main(int argc, char **argv)
{ros::init(argc, argv,"joymsg_pub");ros::NodeHandle n1;ros::Publisher number_publisher = n1.advertise<redwall_arm::joycontrol>("/joycontrol_topic",100);ros::Rate loop_rate(100);js_fd_master = open("/dev/input/js0", O_RDONLY);if (js_fd_master < 0){perror("打开js0失败,主手柄不可用");return -1;}else{/* 创建线程并使主线程与子线程分离，子线程结束后，资源自动回收 */pthread_t id0;pthread_create(&id0,NULL,read_js_data_master,NULL);pthread_detach(id0);}js_fd_support = open("/dev/input/js1", O_RDONLY);if (js_fd_support < 0){perror("打开js1失败，从手柄不可用");mode = 1;}else{mode = 2;pthread_t id1;pthread_create(&id1,NULL,read_js_data_support,NULL);pthread_detach(id1);}while (ros::ok()){redwall_arm::joycontrol msg;/* 主从手柄控制 */if(mode==2){if ((button1==0) && (button2==0) && (button3==0) && (button4==0) && (button5==0) && (button6==0) && (button7==0) && (button8==0) && (axes0==0) && (axes1==0)){msg.button1 =  button1_;msg.button2 =  button2_;msg.button3 =  button3_;msg.button4 =  button4_;msg.button5 =  button5_;msg.button6 =  button6_;msg.button7 =  button7_;msg.button8 =  button8_;msg.axis0 = axes0_;msg.axis1 = axes1_;msg.axis2 = axes2_;msg.axis3 = axes3_;msg.axis4 = axes4_;msg.axis5 = axes5_;}else{msg.button1 =  button1;msg.button2 =  button2;msg.button3 =  button3;msg.button4 =  button4;msg.button5 =  button5;msg.button6 =  button6;msg.button7 =  button7;msg.button8 =  button8;msg.axis0 = axes0;msg.axis1 = axes1;msg.axis2 = axes2;msg.axis3 = axes3;msg.axis4 = axes4;msg.axis5 = axes5;}}/* 单主手柄 */else if(mode==1){msg.button1 =  button1;msg.button2 =  button2;msg.button3 =  button3;msg.button4 =  button4;msg.button5 =  button5;msg.button6 =  button6;msg.button7 =  button7;msg.button8 =  button8;msg.axis0 = axes0;msg.axis1 = axes1;msg.axis2 = axes2;msg.axis3 = axes3;msg.axis4 = axes4;msg.axis5 = axes5;}ROS_INFO("%f %f %f %f %f %f",msg.axis0,msg.axis1,msg.axis2,msg.axis3,msg.axis4,msg.axis5);/* 发布消息 */number_publisher.publish(msg);/* 循环所有的操作 */ros::spinOnce();loop_rate.sleep();}return 0;
}void *read_js_data_master(void *)
{while(1){int len = read(js_fd_master, &js_master, sizeof(struct js_event));if (len < 0){perror("读取js失败");return 0 ;}int value = js_master.value;int type = js_master.type;int number = js_master.number;if (type == JS_EVENT_BUTTON){if( number == BUTTON1  ){button1=value;}if( number == BUTTON2  ){button2=value;}if( number == BUTTON3  ){button3=value;}if( number == BUTTON4  ){button4=value;}if( number == BUTTON5  ){button5=value;}if( number == BUTTON6  ){button6=value;}if( number == BUTTON7  ){button7=value;}if( number == BUTTON8  ){button8=value;}}else if (type == JS_EVENT_AXIS){if( number == AXES1 ){axes1 = value;}if( number == AXES0 ){axes0 = value ; // 手柄有误差，可用jstest查看}if( number == AXES2 ){axes2 = value;}if( number == AXES3 ){axes3 = value ; // 手柄有误差，可用jstest查看}if( number == AXES4 ){axes4 = value;}if( number == AXES5 ){axes5 = value ; // 手柄有误差，可用jstest查看}}}
}void *read_js_data_support(void *)
{while(1){int len = read(js_fd_support, &js_support, sizeof(struct js_event));if (len < 0){perror("读取js失败");return 0 ;}int value = js_support.value;int type = js_support.type;int number = js_support.number;if (type == JS_EVENT_BUTTON){if( number == BUTTON1  ){button1_=value;}if( number == BUTTON2  ){button2_=value;}if( number == BUTTON3  ){button3_=value;}if( number == BUTTON4  ){button4_=value;}if( number == BUTTON5  ){button5_=value;}if( number == BUTTON6  ){button6_=value;}if( number == BUTTON7  ){button7_=value;}if( number == BUTTON8  ){button8_=value;}}else if (type == JS_EVENT_AXIS){if( number == AXES1 ){axes1_ = value;}if( number == AXES0 ){axes0_ = value ; // 手柄有误差，可用jstest查看}if( number == AXES2 ){axes2_ = value;}if( number == AXES3 ){axes3_ = value ; // 手柄有误差，可用jstest查看}if( number == AXES4 ){axes4_ = value;}if( number == AXES5 ){axes5_ = value ; // 手柄有误差，可用jstest查看}}}
}

有了手柄数据，接下需要的是 1 .ROS上位机运行服务器，用于将手柄数据转变成机械臂对应轴的运动和转速。2.运行在beaglebone控制板上的客户端，通过tcp套接字连接上述服务器，接受轴的信息和转速信息，然后转化成pwm信息控制步进电机的运动。

服务器命名为 server_redwall_arm.cpp，功能为创建套接字通信服务器端，等待客户端的连接，一旦有客户端连接，就将手柄通过上述自定义消息发布的消息在回调函数中处理，转换成转速信息，然后通过套接字发送给客户端。

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include "redwall_arm/joycontrol.h"#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>  #define PORT 7788
#define MAX_LINE 10
#define MAX_BUF 300// 运动模式
float lumbar_;     // 腰部回转
float big_arm_;    // 大手臂
float small_arm_;  // 小手臂
float wrist_;      // 手腕
float hand_;       // 手抓
float finger_;     // 末端手指执行器// 运动速度
int lumbar;
int big_arm;
int small_arm;
int wrist;
int hand;
int finger;/* 存储的结构体 p2*/
struct vel_data
{char lumbar_vel[9];char big_arm_vel[9];char small_arm_vel[9];char wrist_vel[9];char hand_vel[9];char finger_vel[9];
};/* 客户端套接字文件描述符和地址,端口 */
typedef struct MySocketInfo{int socketCon;char *ipaddr;uint16_t port;
}_MySocketInfo;/* 数据收发结构体 */
struct vel_data p2;
char writebuf[sizeof(p2)];/* 客户端连接所用数据的存储数组 */
struct MySocketInfo arrConSocket[10];
int conClientCount = 0;              // 连接的客户端个数/* 客户端连接所用套接字的存储数组 */
pthread_t arrThrReceiveClient[10];
int thrReceiveClientCount = 0;       // 接受数据线程个数/* 控制信息处理用于运动 */
void joymessageCallback(const redwall_arm::joycontrol::ConstPtr& cmd_)
{lumbar_ = cmd_->axis4;big_arm_ = cmd_->axis1;small_arm_ = cmd_->axis3;wrist_ = cmd_->axis0;hand_ = cmd_->axis5;finger_= cmd_->axis2;lumbar = int(lumbar_/(32767));big_arm = int(big_arm_*10/(-32767));small_arm = int(small_arm_*10/(-32767));wrist = int(wrist_*10/(32767));hand =  int(hand_/(-32767));finger = int(finger_*10/(32767));
}/* SOCKET服务器端处理客户端连接函数 */
void *fun_thrAcceptHandler(void *socketListen)
{while(1){/* accept函数主要用于服务器端，建立好连接后，它返回的一个新的套接字* 此后，服务器端即可使用这个新的套接字与该客户端进行通信，而原本的套接字则继续用于监听其他客户端的连接请求。 */int sockaddr_in_size = sizeof(struct sockaddr_in);struct sockaddr_in client_addr;int _socketListen = *((int *)socketListen);int socketCon = accept(_socketListen, (struct sockaddr *)(&client_addr), (socklen_t *)(&sockaddr_in_size));if(socketCon < 0){printf("连接失败\n");}else{printf("连接成功 ip: %s:%d\r\n",inet_ntoa(client_addr.sin_addr),client_addr.sin_port);}printf("连接套接字为：%d\n",socketCon);/* 开启新的通讯线程，负责同连接上来的客户端进行通讯 */_MySocketInfo socketInfo;                   // 用于保存客户端套接字的信息socketInfo.socketCon = socketCon;socketInfo.ipaddr = inet_ntoa(client_addr.sin_addr);socketInfo.port = client_addr.sin_port;arrConSocket[conClientCount] = socketInfo;conClientCount++;printf("连接了%d个用户\n",conClientCount);//让进程休息1秒sleep(1);}
}/* 判断线程是否被杀死 */
int checkThrIsKill(pthread_t thr)
{/* 传递的pthread_kill的signal参数为0时，用这个保留的信号测试线程是否存在 */int res = 1;int res_kill = pthread_kill(thr,0);if(res_kill == 0){res = 0;}return res;
}/* 主函数主要用于消息订阅和套接字通信 */
int main(int argc, char** argv)
{ros::init(argc, argv, "server_redwall_arm");ros::NodeHandle n;ros::Subscriber sub = n.subscribe("/joycontrol_topic", 100, joymessageCallback); // 订阅imagefeedorder.msg话题,名称为"/img_feedorderprintf("开始socket\n");/* 创建TCP连接的Socket套接字 */int socketListen = socket(AF_INET, SOCK_STREAM, 0);if(socketListen < 0){printf("创建TCP套接字失败\n");exit(-1);}else{printf("创建套接字成功\n");}/* 绑定服务器端口地址信息 */struct sockaddr_in server_addr;                 // struct sockaddr_in是已经声明了的结构名bzero(&server_addr,sizeof(struct sockaddr_in)); // 等价于memset(server_addr,0,sizeof(struct sockaddr_in));清零操作server_addr.sin_family=AF_INET;server_addr.sin_addr.s_addr=htonl(INADDR_ANY);  // 这里地址使用全0，即所有可能的地址server_addr.sin_port=htons(PORT);               // htons一般是转换端口号为整数if(bind(socketListen, (struct sockaddr *)&server_addr,sizeof(struct sockaddr)) != 0){perror("绑定ip地址,端口号失败\n");exit(-1);}else{printf("绑定ip地址,端口号成功\n");}/* 开始监听相应的端口,最大不超过10个连接 */if(listen(socketListen, 10) != 0){printf("开启监听失败\n");exit(-1);}else{printf("开启监听成功\n");}/* 创建一个子线程用于接受连接客户端连接 */pthread_t thrAccept;pthread_create(&thrAccept,NULL,fun_thrAcceptHandler,&socketListen);/* 实时发送数据 */while(ros :: ok()){ros::spinOnce();/***** 获取转速命令信息并保存到结构体p2中 *****/sprintf(p2.lumbar_vel, "%d", lumbar);sprintf(p2.big_arm_vel, "%d", big_arm);sprintf(p2.small_arm_vel, "%d", small_arm);sprintf(p2.wrist_vel, "%d", wrist);sprintf(p2.hand_vel, "%d", hand);sprintf(p2.finger_vel, "%d", finger);/***** 判断线程存活多少 ******/for(int i=0;i<thrReceiveClientCount;i++){if(checkThrIsKill(arrThrReceiveClient[i]) == 1){printf("有个线程被杀了\n");thrReceiveClientCount--;}}// printf("当前有接受数据线程多少个：%d\n",thrReceiveClientCount);/***** 判断客户端连接和数据发送是否成功 *****/if(conClientCount <= 0){printf("没有客户端连接\n");}else{for(int i=0; i<conClientCount; i++){bzero(writebuf,sizeof(writebuf));   // 清零writebufmemcpy(writebuf,&p2,sizeof(p2));    // 复制p2数据到writebufunsigned int sendMsg_len = write(arrConSocket[i].socketCon, writebuf, sizeof(p2)); //返回值：写入文档的字节数（成功）；-1（出错)if(sendMsg_len > 0){// printf("向%s:%d发送成功\n",arrConSocket[i].ipaddr,arrConSocket[i].port);}else{printf("向%s:%d发送失败\n",arrConSocket[i].ipaddr,arrConSocket[i].port);}}}// 测试的时候没加这个延时,导致数据收发不同步usleep(1000);}/* 关闭原始套接字 */close(socketListen);return 0;
}

接下来是客户端，由于bbb板子上一共有3组pwm口，一组有两个，但是这两个之间的频率在配置的时候不能相差太大，而且本次控制的机械臂具有5个自由度，加上一个末端舵机控制的执行器，所以用到了两个beaglebone控制板，分别控制三个自由度。两个beaglebone同时连接主机的时候，ip地址都是192.168.7.2会产生冲突，所以修改其中之一ip地址为192.168.0.2。他们分别运行一个客户端如下：

client_redwall_arm_0.c

#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <sys/time.h>
#include <sys/poll.h>
#include <sys/epoll.h>#define p813 "/sys/devices/ocp.3/pwm_test_P8_13.12/"
#define p914 "/sys/devices/ocp.3/pwm_test_P9_14.13/"
#define p921 "/sys/devices/ocp.3/pwm_test_P9_21.14/"
#define GPIO_DIR "/sys/class/gpio/"
#define pwm_path "/sys/devices/bone_capemgr.9/slots"#define PORT 7788
#define ADDR "192.168.0.1"
#define MAX_LINE 10
#define LINE 5/*  p1<--p2*/
struct vel_data
{char lumbar_vel[9];char big_arm_vel[9];char small_arm_vel[9];char wrist_vel[9];char hand_vel[9];char finger_vel[9];
};struct vel_data p1;
char recvbuf[sizeof(p1)];int lumbar;
int big_arm;
int small_arm;
int wrist;
int hand;
int finger;
int zero = 0;   // 控制电机方向,右转
int one = 1;    // 控制电机方向,左转typedef struct MySocketInfo
{int socketCon;unsigned long ipaddr;unsigned short port;
}_MySocketInfo;/*SOCKET客户端处理接受数据函数*/
void *fun_thrReceiveHandler(void *socketCon)
{while(1){/* 保存目标套接字信息 */int _socketCon = *((int *)socketCon);bzero(recvbuf, sizeof(p1));unsigned int buffer_length = read(_socketCon,recvbuf,sizeof(p1));if(buffer_length == 0){printf("服务器端异常关闭\n");exit(-1);}else if(buffer_length < 0){printf("接受客户端数据失败\n");break;}recvbuf[buffer_length] = '\0';/* 将接收到的速度控制信息以p1结构体格式解码*/memcpy(&p1,recvbuf,sizeof(recvbuf));/* 把字符串转换成整型 */lumbar = atoi(p1.lumbar_vel);big_arm = atoi(p1.big_arm_vel);small_arm = atoi(p1.small_arm_vel);wrist = atoi(p1.wrist_vel);hand = atoi(p1.hand_vel);finger = atoi(p1.finger_vel);printf("lumbar=%d big_arm=%d small_arm =%d wrist=%d,hand=%d finger=%d\n",lumbar,big_arm,small_arm,wrist,hand,finger);}printf("退出接收服务器数据线程\n");return NULL;
}/* 设置gpio函数 */
int set_gpio(void)
{FILE *stream1 = fopen(GPIO_DIR"gpio44/direction","r+");/*如果打开文件失败则先加载*/if(stream1==NULL){stream1=fopen(GPIO_DIR"export","w");fwrite("44",sizeof(int),2,stream1);fclose(stream1);stream1=fopen(GPIO_DIR"gpio44/direction","r+");}/* P8.12端口为输出*/fwrite("out",sizeof(char),3,stream1);fclose(stream1);FILE *stream2=fopen(GPIO_DIR"gpio45/direction","r+");/*如果打开文件失败则打开相应端口*/if (stream2==NULL){stream2=fopen(GPIO_DIR"export","w");fwrite("45",sizeof(int),2,stream2);fclose(stream2);stream2=fopen(GPIO_DIR"gpio45/direction","r+");}/* P8.11端口为输出*/fwrite("out",sizeof(char),3,stream2);fclose(stream2);/* 设置GPIO70-->P8-45为输入模式 */FILE *stream3 = fopen(GPIO_DIR"gpio26/direction","r+");if (stream3==NULL){stream3 = fopen(GPIO_DIR"export","w");fwrite("26",sizeof(int),2,stream3);fclose(stream3);stream3 = fopen(GPIO_DIR"gpio26/direction","r+");}fwrite("out",sizeof(char),3,stream3);fclose(stream3);FILE *stream4= fopen(p921"polarity","r+");if(!stream4){printf("p921的polarity打开失败\n");return ;}char buf3[7];/* 舵机不需要方向信号,所以不必写入gpio26，但是必须给定默认polarity=0才行 */sprintf(buf3,"%d",zero);fwrite(buf3,sizeof(int),3,stream4);fclose(stream4);return 0;
}/* 加载设备树函数 */
int write_tree(void)
{int fd = open(pwm_path,O_WRONLY);if(fd < 0){printf("打开slots失败\n");}write(fd,"am33xx_pwm",10);write(fd,"bone_pwm_P8_13",14);write(fd,"bone_pwm_P9_14",14);write(fd,"bone_pwm_P9_21",14);close(fd);return 0;
}/***************************/
/* p8电机启动函数 */
int wrist_motor_start(int full,int half, int c)
{FILE *x= fopen(p813"period","r+");if(!x){printf("p813的period写入失败!\n");return ;}char buf15[7];sprintf(buf15,"%d",full);fwrite(buf15,sizeof(int),3,x);fclose(x);FILE *y= fopen(p813"duty","r+");if(!y){printf("p813的duty写入失败!\n");return ;}char buf16[7];sprintf(buf16,"%d",half);fwrite(buf16,sizeof(int),3,y);fclose(y);FILE *z= fopen(GPIO_DIR"gpio44/value","r+");if(!z){printf("gpio44的value写入失败\n");return ;}char buf17[7];sprintf(buf17,"%d",c);fwrite(buf17,sizeof(int),3,z);fclose(z);
}
/* p8电机停止函数 */
int wrist_motor_stop(void)
{/* 停止所有电机 */int full = 10000000;FILE *x2=fopen(p813"period","r+");  // 周期char bufh10[7];sprintf(bufh10,"%d",full);fwrite(bufh10,sizeof(int),3,x2);fclose(x2);FILE *y2= fopen(p813"duty","r+");   //占空比char bufh11[7];sprintf(bufh11,"%d",full);fwrite(bufh11,sizeof(int),3,y2);fclose(y2);return 0;
}/* p914电机启动函数 */
int hand_motor_start(int full,int half, int c)
{FILE *x3= fopen(p914"period","r+");if(!x3){printf("p914的period打开失败\n");return ;}char buf111[7];sprintf(buf111,"%d",full);fwrite(buf111,sizeof(int),3,x3);fclose(x3);FILE *y3= fopen(p914"duty","r+");if(!y3){printf("p914的duty打开失败\n");return ;}char buf222[7];sprintf(buf222,"%d",half);fwrite(buf222,sizeof(int),3,y3);fclose(y3);FILE *z3= fopen(GPIO_DIR"gpio45/value","r+");if(!z3){printf("gpio45/value打开失败\n");return ;}char buf3[7];sprintf(buf3,"%d",c);fwrite(buf3,sizeof(int),3,z3);fclose(z3);
}
/* p914电机停止函数 */
int hand_motor_stop(void)
{/* 停止所有电机 */int full = 1000000;FILE *x4=fopen(p914"period","r+");  //周期char bufh12[7];sprintf(bufh12,"%d",full);fwrite(bufh12,sizeof(int),3,x4);fclose(x4);FILE *y4= fopen(p914"duty","r+");   //占空比char bufh13[7];sprintf(bufh13,"%d",full);fwrite(bufh13,sizeof(int),3,y4);fclose(y4);return 0;
}/* p921电机启动函数 */
int finger_motor_start(int full,int half, int c)
{FILE *x5= fopen(p921"period","r+");if(!x5){printf("p921的period打开失败\n");return ;}char buf111[7];sprintf(buf111,"%d",full);fwrite(buf111,sizeof(int),3,x5);fclose(x5);FILE *y5= fopen(p921"duty","r+");if(!y5){printf("p921的duty打开失败\n");return ;}char buf222[7];sprintf(buf222,"%d",half);fwrite(buf222,sizeof(int),3,y5);fclose(y5);
}
/* p921电机停止函数 */
int finger_motor_stop(void)
{/* 停止所有电机 */int full = 20000000;FILE *x6=fopen(p921"period","r+");  //周期char bufh12[7];sprintf(bufh12,"%d",full);fwrite(bufh12,sizeof(int),3,x6);fclose(x6);FILE *y6= fopen(p921"duty","r+");   //占空比char bufh13[7];sprintf(bufh13,"%d",full);fwrite(bufh13,sizeof(int),3,y6);fclose(y6);return 0;
}
/***************************/void wrist_motor(void)
{while(1){if(wrist == 0){/* 电机静止,占空比=周期=500000 *///motor_stop(p813);wrist_motor_stop();}else{int full = 10000000 - 10000 * abs(wrist);int half = full / 2;int c = wrist > 0 ? 1 : 0;    // gpio方向值//motor_start(full, half, c, p813,"gpio44/value");wrist_motor_start(full, half, c);}}
}void hand_motor(void)
{while(1){if(hand == 0){/* 电机静止,占空比=周期=500000 *///motor_stop(p914);hand_motor_stop();}else{int full = 1000000 - 10000 * abs(hand);int half = full / 2;int c = hand > 0 ? 1 : 0;    // gpio方向值//motor_start(full, half, c, p914, "gpio45/value");hand_motor_start(full, half, c);}}
}void finger_motor(void)
{while(1){if(finger == 0){//motor_stop(p921);finger_motor_stop();}else{int full = 20000000;int half = (100000*finger + 1500000);int c = zero;       // 舵机方向确定为0//motor_start(full, half, c, p921, "");finger_motor_start(full, half, c);}}
}/* 主函数完成接受和发送数据 */
int main(int argc, char **argv)
{/***** 加载设备树 *****/write_tree();/***** 配置GPIO用于控制电机转向 *****/set_gpio();/***** 开始套接字通信 *****/printf("开始socket\n");int socketCon = socket(AF_INET, SOCK_STREAM, 0);if(socketCon < 0){printf("创建TCP连接套接字失败\n");exit(-1);}/***** 绑定服务器端口地址信息 *****/struct sockaddr_in server_addr;bzero(&server_addr,sizeof(struct sockaddr_in));server_addr.sin_family=AF_INET;server_addr.sin_port=htons(PORT);if(inet_pton(AF_INET, ADDR, &server_addr.sin_addr) !=1){printf("ipv4地址转换失败");}/***** 连接服务器 *****/int res_con = connect(socketCon,(struct sockaddr *)(&server_addr),sizeof(struct sockaddr));if(res_con != 0){printf("连接服务器失败\n");exit(-1);}printf("连接成功,连接结果为：%d\n",res_con);/***** 开启新的实时接受数据线程 *****/pthread_t thrReceive;pthread_create(&thrReceive,NULL,fun_thrReceiveHandler,&socketCon);/***** 开启处理电机数据线程 *****/pthread_t id1;int ret1=pthread_create(&id1,NULL,(void *) wrist_motor,NULL); // 成功返回0，错误返回错误编号if(ret1!=0){printf("motor1线程创建失败!\n");exit(1);}/***** 开启处理电机数据线程 *****/pthread_t id2;int ret2=pthread_create(&id2,NULL,(void *) hand_motor,NULL); // 成功返回0，错误返回错误编号if(ret2!=0){printf("motor2线程创建失败!\n");exit(1);}/***** 开启处理电机数据线程 *****/pthread_t id3;int ret3=pthread_create(&id3,NULL,(void *) finger_motor,NULL); // 成功返回0，错误返回错误编号if(ret3!=0){printf("motor3线程创建失败!\n");exit(1);}/***** 主线程阻塞等待子线程 *****/pthread_detach(id1);pthread_detach(id2);pthread_detach(id3);pthread_join(thrReceive,NULL);/***** 关闭套接字 *****/close(socketCon);return 0;
}

client_redwall_arm_7.c

#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <sys/time.h>
#include <sys/poll.h>
#include <sys/epoll.h>#define p813 "/sys/devices/ocp.3/pwm_test_P8_13.12/"
#define p914 "/sys/devices/ocp.3/pwm_test_P9_14.13/"
#define p921 "/sys/devices/ocp.3/pwm_test_P9_21.14/"
#define GPIO_DIR "/sys/class/gpio/"
#define pwm_path "/sys/devices/bone_capemgr.9/slots"#define PORT 7788
#define ADDR "192.168.7.1"
#define MAX_LINE 10
#define LINE 5/* 四个轮子的转速和直线速度 p1<--p2*/
struct vel_data
{char lumbar_vel[9];char big_arm_vel[9];char small_arm_vel[9];char wrist_vel[9];char hand_vel[9];char finger_vel[9];
};struct vel_data p1;
char recvbuf[sizeof(p1)];int lumbar;
int big_arm;
int small_arm;
int wrist;
int hand;
int finger;
int zero = 0;   // 控制电机方向,右转
int one = 1;    // 控制电机方向,左转typedef struct MySocketInfo
{int socketCon;unsigned long ipaddr;unsigned short port;
}_MySocketInfo;/*SOCKET客户端处理接受数据函数*/
void *fun_thrReceiveHandler(void *socketCon)
{while(1){/* 保存目标套接字信息 */int _socketCon = *((int *)socketCon);bzero(recvbuf, sizeof(p1));unsigned int buffer_length = read(_socketCon,recvbuf,sizeof(p1));if(buffer_length == 0){printf("服务器端异常关闭\n");exit(-1);}else if(buffer_length < 0){printf("接受客户端数据失败\n");break;}recvbuf[buffer_length] = '\0';/* 将接收到的速度控制信息以p1结构体格式解码*/memcpy(&p1,recvbuf,sizeof(recvbuf));/* 把字符串转换成整型 */lumbar = atoi(p1.lumbar_vel);big_arm = atoi(p1.big_arm_vel);small_arm = atoi(p1.small_arm_vel);wrist = atoi(p1.wrist_vel);hand = atoi(p1.hand_vel);finger = atoi(p1.finger_vel);printf("lumbar=%d big_arm=%d small_arm =%d wrist=%d,hand=%d finger=%d\n",lumbar,big_arm,small_arm,wrist,hand,finger);}printf("退出接收服务器数据线程\n");return NULL;
}/* 设置gpio函数 */
int set_gpio(void)
{FILE *stream1 = fopen(GPIO_DIR"gpio44/direction","r+");/*如果打开文件失败则先加载*/if(stream1==NULL){stream1=fopen(GPIO_DIR"export","w");fwrite("44",sizeof(int),2,stream1);fclose(stream1);stream1=fopen(GPIO_DIR"gpio44/direction","r+");}/* P8.12端口为输出*/fwrite("out",sizeof(char),3,stream1);fclose(stream1);FILE *stream2=fopen(GPIO_DIR"gpio45/direction","r+");/*如果打开文件失败则打开相应端口*/if (stream2==NULL){stream2=fopen(GPIO_DIR"export","w");fwrite("45",sizeof(int),2,stream2);fclose(stream2);stream2=fopen(GPIO_DIR"gpio45/direction","r+");}/* P8.11端口为输出*/fwrite("out",sizeof(char),3,stream2);fclose(stream2);/* 设置GPIO70-->P8-45为输入模式 */FILE *stream3 = fopen(GPIO_DIR"gpio26/direction","r+");if (stream3==NULL){stream3 = fopen(GPIO_DIR"export","w");fwrite("26",sizeof(int),2,stream3);fclose(stream3);stream3 = fopen(GPIO_DIR"gpio26/direction","r+");}fwrite("out",sizeof(char),3,stream3);fclose(stream3);return 0;
}/* 加载设备树函数 */
int write_tree(void)
{int fd = open(pwm_path,O_WRONLY);if(fd < 0){printf("打开slots失败\n");}write(fd,"am33xx_pwm",10);write(fd,"bone_pwm_P8_13",14);write(fd,"bone_pwm_P9_14",14);write(fd,"bone_pwm_P9_21",14);close(fd);return 0;
}/***************************/
/* p8电机启动函数 */
int lumbar_motor_start(int full,int half, int c)
{FILE *x= fopen(p813"period","r+");if(!x){printf("p813的period写入失败!\n");return ;}char buf15[7];sprintf(buf15,"%d",full);fwrite(buf15,sizeof(int),3,x);fclose(x);FILE *y= fopen(p813"duty","r+");if(!y){printf("p813的duty写入失败!\n");return ;}char buf16[7];sprintf(buf16,"%d",half);fwrite(buf16,sizeof(int),3,y);fclose(y);FILE *z= fopen(GPIO_DIR"gpio44/value","r+");if(!z){printf("gpio44的value写入失败\n");return ;}char buf17[7];sprintf(buf17,"%d",c);fwrite(buf17,sizeof(int),3,z);fclose(z);
}
/* p8电机停止函数 */
int lumbar_motor_stop(void)
{/* 停止所有电机 */int full = 100000;FILE *x2=fopen(p813"period","r+");  // 周期char bufh10[7];sprintf(bufh10,"%d",full);fwrite(bufh10,sizeof(int),3,x2);fclose(x2);FILE *y2= fopen(p813"duty","r+");   //占空比char bufh11[7];sprintf(bufh11,"%d",full);fwrite(bufh11,sizeof(int),3,y2);fclose(y2);return 0;
}/* p914电机启动函数 */
int bigarm_motor_start(int full,int half, int c)
{FILE *x3= fopen(p914"period","r+");if(!x3){printf("p914的period打开失败\n");return ;}char buf111[7];sprintf(buf111,"%d",full);fwrite(buf111,sizeof(int),3,x3);fclose(x3);FILE *y3= fopen(p914"duty","r+");if(!y3){printf("p914的duty打开失败\n");return ;}char buf222[7];sprintf(buf222,"%d",half);fwrite(buf222,sizeof(int),3,y3);fclose(y3);FILE *z3= fopen(GPIO_DIR"gpio45/value","r+");if(!z3){printf("gpio45/value打开失败\n");return ;}char buf3[7];sprintf(buf3,"%d",c);fwrite(buf3,sizeof(int),3,z3);fclose(z3);
}
/* p914电机停止函数 */
int bigarm_motor_stop(void)
{/* 停止所有电机 */int full = 5000000;FILE *x4=fopen(p914"period","r+");  //周期char bufh12[7];sprintf(bufh12,"%d",full);fwrite(bufh12,sizeof(int),3,x4);fclose(x4);FILE *y4= fopen(p914"duty","r+");   //占空比char bufh13[7];sprintf(bufh13,"%d",full);fwrite(bufh13,sizeof(int),3,y4);fclose(y4);return 0;
}/* p921电机启动函数 */
int smallarm_motor_start(int full,int half, int c)
{FILE *x5= fopen(p921"period","r+");if(!x5){printf("p921的period打开失败\n");return ;}char buf111[7];sprintf(buf111,"%d",full);fwrite(buf111,sizeof(int),3,x5);fclose(x5);FILE *y5= fopen(p921"duty","r+");if(!y5){printf("p921的duty打开失败\n");return ;}char buf222[7];sprintf(buf222,"%d",half);fwrite(buf222,sizeof(int),3,y5);fclose(y5);FILE *z5= fopen(GPIO_DIR"gpio26/value","r+");if(!z5){printf("gpio26/value打开失败\n");return ;}char buf3[7];sprintf(buf3,"%d",c);fwrite(buf3,sizeof(int),3,z5);fclose(z5);
}
/* p921电机停止函数 */
int smallarm_motor_stop(void)
{/* 停止所有电机 */int full = 1000000;FILE *x6=fopen(p921"period","r+");  //周期char bufh12[7];sprintf(bufh12,"%d",full);fwrite(bufh12,sizeof(int),3,x6);fclose(x6);FILE *y6= fopen(p921"duty","r+");   //占空比char bufh13[7];sprintf(bufh13,"%d",full);fwrite(bufh13,sizeof(int),3,y6);fclose(y6);return 0;
}
/***************************/void lumbar_motor(void)
{while(1){if(lumbar == 0){/* 电机静止,占空比=周期=500000 *///motor_stop(p813);lumbar_motor_stop();}else{int full = 100000;int half = full/2;int c = lumbar > 0 ? 1 : 0;    // gpio方向值//motor_start(full,half,c,p813,"gpio44/value");lumbar_motor_start(full,half,c);}}
}void bigarm_motor(void)
{while(1){if(big_arm == 0){/* 电机静止,占空比=周期=500000 *///motor_stop(p914);bigarm_motor_stop();}else{int full = 5000000 - 10000 * abs(big_arm);int half = full / 2;int c = big_arm > 0 ? 1 : 0;    // gpio方向值//motor_start(full, half, c, p914, "gpio45/value");bigarm_motor_start(full, half, c);}}
}void smallarm_motor(void)
{while(1){if(small_arm == 0){/* 电机静止,占空比=周期=500000 *///motor_stop(p921);smallarm_motor_stop();}else{int full = 1000000 - 1000 * abs(small_arm);int half = full / 2;int c = small_arm > 0 ? 1 : 0;    // gpio方向值//motor_start(full, half, c, p921, "gpio26/value");smallarm_motor_start(full, half, c);}}
}/* 主函数完成接受和发送数据 */
int main(int argc, char **argv)
{/***** 配置GPIO用于控制电机转向 *****/set_gpio();/***** 加载设备树 *****/write_tree();/***** 开始套接字通信 *****/printf("开始socket\n");int socketCon = socket(AF_INET, SOCK_STREAM, 0);if(socketCon < 0){printf("创建TCP连接套接字失败\n");exit(-1);}/***** 绑定服务器端口地址信息 *****/struct sockaddr_in server_addr;bzero(&server_addr,sizeof(struct sockaddr_in));server_addr.sin_family=AF_INET;server_addr.sin_port=htons(PORT);if(inet_pton(AF_INET, ADDR, &server_addr.sin_addr) !=1){printf("ipv4地址转换失败");}/***** 连接服务器 *****/int res_con = connect(socketCon,(struct sockaddr *)(&server_addr),sizeof(struct sockaddr));if(res_con != 0){printf("连接服务器失败\n");exit(-1);}printf("连接成功,连接结果为：%d\n",res_con);/***** 开启新的实时接受数据线程 *****/pthread_t thrReceive;pthread_create(&thrReceive,NULL,fun_thrReceiveHandler,&socketCon);/***** 开启处理电机数据线程 *****/pthread_t id1;int ret1=pthread_create(&id1,NULL,(void *) lumbar_motor,NULL); // 成功返回0，错误返回错误编号if(ret1!=0){printf("motor1线程创建失败!\n");exit(1);}/***** 开启处理电机数据线程 *****/pthread_t id2;int ret2=pthread_create(&id2,NULL,(void *) bigarm_motor,NULL); // 成功返回0，错误返回错误编号if(ret2!=0){printf("motor2线程创建失败!\n");exit(1);}/***** 开启处理电机数据线程 *****/pthread_t id3;int ret3=pthread_create(&id3,NULL,(void *) smallarm_motor,NULL); // 成功返回0，错误返回错误编号if(ret3!=0){printf("motor3线程创建失败!\n");exit(1);}/***** 主线程阻塞等待子线程 *****/pthread_detach(id1);pthread_detach(id2);pthread_detach(id3);pthread_join(thrReceive,NULL);/***** 关闭套接字 *****/close(socketCon);return 0;
}

两个客户端的内容非常相似，仅仅是控制的关节不一样，所以用不同的pwm周期控制不同型号的电机而已。而且此客户端和服务器的可复用性非常高，套接字通信部分的代码几乎不用更改就可以运用到其他地方，仅仅要修改的只是通信内容的不同，实验室的独立驱动转向模块化小车就是如此。

最后一步，修改CMakeList.txt和package.xml ，然后使用catkin_make -DCATKIN_WHITELIST_PACKAGES="redwall_arm"编译包，会生成两个可执行文件，也就是上面的Logitech_pub 和server_redwall_arm节点，最后分别将两个客户端发送到板子上，使用 gcc client_redwall_arm.c -lpthread 编译即可。依次运行所有节点，即可通过手柄控制真实世界机械臂的运动。

通过ROS控制真实机械臂(2)----单轴运动，手柄控制相关推荐

通过ROS控制真实机械臂(15) --- 视觉抓取之手眼标定
通过视觉传感器赋予机械臂"眼睛"的功能,配合ATI力和力矩传感器,就可以完成机械臂"手眼"结合的能力,完成视觉抓取过程.目前测试的视觉传感器为 ZED mini ...
通过ROS控制真实机械臂(9)---上、下位机和PRU程序
上位机的程序redwall_arm_server.cpp 功能是作为ROS的move_group客户端接收ROS规划的机械臂路点信息,进行三次样条插补获得各个关节或自由度的运动PVAT数据,然后通过T ...
通过ROS控制真实机械臂(7)---三次样条插补
在之前的move_group界面中,当点击plan and execute之后,move_group就会帮我们规划出一条通往指定位姿的轨迹,发布在follow_joint_trajectory上,通过 ...
通过ROS控制真实机械臂(8)---延时时间精确控制
根据之前的配置,我们已经可以通过move_group发送出机械臂各关节运动的轨迹,并且通过三次样条插补的方法,赋予各个关节在特定角度时的速度和加速度,通过启动程序节点可以看到,本次运动规划使用了LBK ...
ros melodic控制真实机械臂之获取moveit规划插补点
关于该点可查看前辈博客.本文对其中不一致的地方进行记录,但为了查阅方便,该文也记录了完整的操作步骤. 1.demo.launch文件中参数fake_execution的值改为false <arg ...
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动
ros kinetic-moveit驱动ur3机械臂------控制真实机械臂并且能动本文工作环境配置: ubuntu16.04.6 ros-kinetic ur3 已验证本教程代码在Ubuntu1 ...
ros通过moveit控制真实机械臂
1.demo.launch文件在生成的demo.launch文件中,参数fake_execution的值改为false <include file="$(find tk7arm_mo ...
机器人学习必看系列：如何使用moveit控制真实机械臂？
大家好,我是你们可爱的小鱼.最近关于moveit相关的问题感觉非常多,毕竟机械臂+视觉的应用的确是非常的火爆,小鱼都想直接开课教机械臂运动规划相关的了. 有的同学问小鱼,怎么使用moveit控制真实机 ...
使用ROS控制AUBO机械臂
环境配置: Ubuntu16.04 ROS-kinetic 前提: Ubuntu16.04和 ROS kinetic都需要提前安装好 1.安装依赖 1. sudo apt-get install ro ...

通过ROS控制真实机械臂(2)----单轴运动，手柄控制

通过ROS控制真实机械臂(2)----单轴运动，手柄控制相关推荐

最新文章

热门文章