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

174
0
2021年1月20日 09时16分

创建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 编译即可。依次运行所有节点,即可通过手柄控制真实世界机械臂的运动。

 

发表评论

后才能评论