创建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_feedorder
printf("开始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)); // 清零writebuf
memcpy(writebuf,&p2,sizeof(p2)); // 复制p2数据到writebuf
unsigned 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)—-单轴运动，手柄控制！
