四轮驱动(SSMR)移动机器人手柄控制

理论推导

理论推导可以参考下面的文章

四轮驱动（SSMR）移动机器人运动模型及应用分析 (qq.com)

有几个点需要注意一下：

推导假设车的形心与车的质心重合，其实这是不太符合实际的，但出于简化的考虑，暂且可以这样做，想更精细地控制可以查找相关资料
推导将四轮差速模型简化为两轮差速模型，可以认为是模型的退化
模型退化后产生的虚拟轮间距与实际轮间距间的系数需要多次实验获得合适的结果
最后的结果可以简单记忆为同侧车轮转速相同

编程实现

在实际中通常发现理论推导往往是容易的，而编程实现总是困难重重，这次的问题主要有三个

要调试的车是一代移动机器人，完成时间大概是一几年，熟悉的师兄们早就毕业工作了。车的底层是PLC实现的控制，而我们已经很久不接触PLC了…
如何自定义罗技手柄按键遥杆的功能，又如何读取按键的信息以供使用
按键信息读取后又如何下发给PLC，实现期望的运动

是不是已经开始挠头了呢，让我们一起看看要怎么办吧

熟悉底层PLC程序

这部分的程序不方便贴出来，大家自行脑补吧hhh

感谢一打五师兄做出的工作，提供了与上位机交互的接口

我们可以通过下面的变量实现从上到下速度指令的发送以及从下到上传感器数据的读取

自定义手柄按键功能并读取按键信息

从标题也可以看出来，这部分有可以拆成两个子功能

自定义手柄按键功能
读取手柄按键信息

自定义手柄按键功能

Linux下提供了处理手柄的头文件<linux/joystick.h>，里面定义了各个轴的轴号和各个键的键号，实际中我们需要测试自己手柄按键与轴对应的具体值

之前的师兄记录的不是很清楚，所以我又重新记录了一下

<linux/joystick.h>

/**  Copyright (C) 1996-2000 Vojtech Pavlik**  Sponsored by SuSE*/
/** This program is free software; you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation; either version 2 of the License, or * (at your option) any later version.* * This program is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the* GNU General Public License for more details.* * You should have received a copy of the GNU General Public License* along with this program; if not, write to the Free Software* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA* * Should you need to contact me, the author, you can do so either by* e-mail - mail your message to <vojtech@suse.cz>, or by paper mail:* Vojtech Pavlik, Ucitelska 1576, Prague 8, 182 00 Czech Republic*/
#ifndef _LINUX_JOYSTICK_H
#define _LINUX_JOYSTICK_H#include <linux/types.h>
#include <linux/input.h>/** Version*/#define JS_VERSION       0x020100/** Types and constants for reading from /dev/js*/#define JS_EVENT_BUTTON       0x01    /* button pressed/released */
#define JS_EVENT_AXIS       0x02    /* joystick moved */
#define JS_EVENT_INIT       0x80    /* initial state of device */struct js_event {__u32 time;   /* event timestamp in milliseconds */__s16 value;   /* value */__u8 type;   /* event type */__u8 number;    /* axis/button number */
};/** IOCTL commands for joystick driver*/#define JSIOCGVERSION     _IOR('j', 0x01, __u32)                /* get driver version */#define JSIOCGAXES      _IOR('j', 0x11, __u8)             /* get number of axes */
#define JSIOCGBUTTONS       _IOR('j', 0x12, __u8)             /* get number of buttons */
#define JSIOCGNAME(len)     _IOC(_IOC_READ, 'j', 0x13, len)           /* get identifier string */#define JSIOCSCORR       _IOW('j', 0x21, struct js_corr)           /* set correction values */
#define JSIOCGCORR      _IOR('j', 0x22, struct js_corr)           /* get correction values */#define JSIOCSAXMAP      _IOW('j', 0x31, __u8[ABS_CNT])            /* set axis mapping */
#define JSIOCGAXMAP     _IOR('j', 0x32, __u8[ABS_CNT])            /* get axis mapping */
#define JSIOCSBTNMAP        _IOW('j', 0x33, __u16[KEY_MAX - BTN_MISC + 1])   /* set button mapping */
#define JSIOCGBTNMAP        _IOR('j', 0x34, __u16[KEY_MAX - BTN_MISC + 1])   /* get button mapping *//** Types and constants for get/set correction*/#define JS_CORR_NONE        0x00    /* returns raw values */
#define JS_CORR_BROKEN      0x01    /* broken line */struct js_corr {__s32 coef[8];__s16 prec;__u16 type;
};/** v0.x compatibility definitions*/#define JS_RETURN     sizeof(struct JS_DATA_TYPE)
#define JS_TRUE         1
#define JS_FALSE        0
#define JS_X_0          0x01
#define JS_Y_0          0x02
#define JS_X_1          0x04
#define JS_Y_1          0x08
#define JS_MAX          2#define JS_DEF_TIMEOUT     0x1300
#define JS_DEF_CORR     0
#define JS_DEF_TIMELIMIT    10L#define JS_SET_CAL       1
#define JS_GET_CAL      2
#define JS_SET_TIMEOUT      3
#define JS_GET_TIMEOUT      4
#define JS_SET_TIMELIMIT    5
#define JS_GET_TIMELIMIT    6
#define JS_GET_ALL      7
#define JS_SET_ALL      8struct JS_DATA_TYPE {__s32 buttons;__s32 x;__s32 y;
};struct JS_DATA_SAVE_TYPE_32 {__s32 JS_TIMEOUT;__s32 BUSY;__s32 JS_EXPIRETIME;__s32 JS_TIMELIMIT;struct JS_DATA_TYPE JS_SAVE;struct JS_DATA_TYPE JS_CORR;
};struct JS_DATA_SAVE_TYPE_64 {__s32 JS_TIMEOUT;__s32 BUSY;__s64 JS_EXPIRETIME;__s64 JS_TIMELIMIT;struct JS_DATA_TYPE JS_SAVE;struct JS_DATA_TYPE JS_CORR;
};#endif /* _LINUX_JOYSTICK_H */

有两处需要注意，首先是按键、轴、初始化的宏

#define JS_EVENT_BUTTON      0x01    /* button pressed/released */
#define JS_EVENT_AXIS       0x02    /* joystick moved */
#define JS_EVENT_INIT       0x80    /* initial state of device */

其次是定义按键信息的结构体

struct js_event {__u32 time; /* event timestamp in milliseconds */__s16 value;   /* value */__u8 type;   /* event type */__u8 number;    /* axis/button number */
};

后面我们读取按键和轴的信息主要就是根据这两部分

common_tire_joystick.hpp

#ifndef COMMON_TIRE_JOYSTICK_HPP
#define COMMON_TIRE_JOYSTICK_HPP#include <iostream>
#include <stdio.h>
#include <linux/joystick.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <math.h>using namespace std;#define AXES_LLR 0x00 // 左遥杆x轴
#define AXES_LUD 0x01 // 左摇杆y轴
#define AXES_RLR 0x02 // 右摇杆x轴
#define AXES_RUD 0x03 // 右遥杆y轴#define BUTTON_X 0x00
#define BUTTON_A 0x01
#define BUTTON_B 0x02
#define BUTTON_Y 0x03
#define BUTTON_BACK 0x08
#define BUTTON_START 0x09
#define BUTTON_UP_DOWN 0x05const double MAX_NUM = 32767.0;
const double V_MAXSPEED = 0.6;
const double W_MAXSPEED = M_PI/2;class CommonTireJoystick
{private:int m_js_fd;bool m_joy_flag;struct js_event m_js;int m_len, m_joy_type, m_joy_num, m_joy_value;double m_speed[2];public:CommonTireJoystick(){m_js_fd = -1;m_joy_flag = false;memset(&m_js, 0, sizeof(js_event));m_len = m_joy_num = m_joy_type = m_joy_value = 0;memset(&m_speed, 0, sizeof(m_speed));}bool initJoystick();bool listenJoystick();bool getJoyflag();void getSpeed(double* speed);~CommonTireJoystick(){if (m_js_fd > 0){close(m_js_fd);m_js_fd = -1;}}
};bool CommonTireJoystick::initJoystick()
{if (m_js_fd > 0){close(m_js_fd);m_js_fd = -1;}m_len = m_joy_num = m_joy_type = m_joy_value = 0;m_joy_flag = false;memset(&m_speed, 0, sizeof(m_speed));memset(&m_js, 0, sizeof(js_event));if ((m_js_fd = open("/dev/input/js0", O_RDONLY | O_NONBLOCK)) < 0){cout << "joystick connected failed" << endl;return false;}return true;
}bool CommonTireJoystick::listenJoystick()
{fd_set rset;struct timeval time_out;FD_ZERO(&rset);time_out.tv_sec = 1;time_out.tv_usec = 100;FD_SET(m_js_fd, &rset);if (select(m_js_fd + 1, &rset, 0, 0, &time_out) < 0){perror("ERR:read serial timeout!");return false;}m_len = read(m_js_fd, &m_js, sizeof(js_event));if (m_len == sizeof(js_event)){cout << "Read success!" << endl;// cout << "m_len = " << m_len << endl;}m_joy_value = m_js.value;m_joy_type = m_js.type;m_joy_num = m_js.number;//过小的数值当做0处理if (m_joy_value > -600.0 && m_joy_value < 600.0){m_joy_value = 0.0;}else{m_joy_value = -m_joy_value; //常规意义下的正负值cout << "m_joy_value = " << m_joy_value << endl;}if (m_joy_type == JS_EVENT_BUTTON){cout << "Button detached!" << endl << endl;switch (m_joy_num){case BUTTON_A:cout << "button A\t 无动作" << endl;break;case BUTTON_B:cout << "button B\t 无动作" << endl;break;case BUTTON_BACK:cout << "button Back\t 结束控制!" << endl;m_joy_flag = false;memset(&m_speed, 0, sizeof(m_speed));break;case BUTTON_START:cout << "button Start\t 开始控制!" << endl;m_joy_flag = true;break;case BUTTON_X:cout << "button X\t 无动作" << endl;break;case BUTTON_Y:cout << "button Y\t 无动作" << endl;break;default:break;}}else if (m_joy_type == JS_EVENT_AXIS){cout << "Axis detached!" << endl << endl;switch (m_joy_num){case AXES_LLR:cout << "左遥杆X轴" << endl;m_speed[0] = V_MAXSPEED * (m_joy_value / MAX_NUM);break;case AXES_LUD:cout << "左遥杆Y轴" << endl;m_speed[0] = V_MAXSPEED * (m_joy_value / MAX_NUM);break;case AXES_RLR:cout << "右遥杆X轴" << endl;m_speed[1] = W_MAXSPEED * (m_joy_value / MAX_NUM);break;case AXES_RUD:cout << "右遥杆Y轴" << endl;m_speed[1] = W_MAXSPEED * (m_joy_value / MAX_NUM);break;case BUTTON_UP_DOWN:if(m_joy_value > 0){// cout << "button DOWN" << endl;cout << "button UP" << endl;m_speed[0] = V_MAXSPEED * (m_joy_value / MAX_NUM);                }if(m_joy_value < 0){// cout << "button UP" << endl;cout << "button DOWN" << endl;m_speed[0] = V_MAXSPEED * (m_joy_value / MAX_NUM);}break;default:break;}}
}bool CommonTireJoystick::getJoyflag()
{return m_joy_flag;
}void CommonTireJoystick::getSpeed(double* speed)
{memcpy(speed, m_speed, sizeof(m_speed));
}#endif

按键的键号与遥杆的轴号可以从宏定义中看出

#define AXES_LLR 0x00 // 左遥杆x轴
#define AXES_LUD 0x01 // 左摇杆y轴
#define AXES_RLR 0x02 // 右摇杆x轴
#define AXES_RUD 0x03 // 右遥杆y轴#define BUTTON_X 0x00
#define BUTTON_A 0x01
#define BUTTON_B 0x02
#define BUTTON_Y 0x03
#define BUTTON_BACK 0x08
#define BUTTON_START 0x09
#define BUTTON_UP_DOWN 0x05

对应到手柄如下图所示，其中A表示Axis，B表示Button
有几个注意点：

左侧上下左右四个键的type是Axis而不是Button
按键和遥杆Value值的正负与我们常规理解的相反，以左下遥杆为例，遥杆向左时为负，所以在实际应用时要取反
按键和遥杆Value值最大为32767
遥杆值是渐变的，而左上方的按键值则直接为最大值，使用时可以定义为机器人以最高速运动

读取按键信息

可以参考下面的几篇博客

Linux编程控制硬件(5) ---- 操作USB手柄

[joysticke]使用Ubuntu16.04环境下读取USB手柄/方向盘信息

游戏手柄之自定义按钮控制海龟

主要步骤可以简化为以下几步：

手柄的蓝牙插入后，在/dev/input下会出现js0的设备名
定义struct js_event结构体变量，准备接收手柄信息
open( )函数打开/dev/input/js0
read( )函数读取信息到struct js_event结构体变量中
close( )函数关闭/dev/input/js0

具体可以参考上面的common_tire_joystick.hpp文件

读取到手柄信息后，将手柄值作为速度命令发送到/cmd_vel话题上

common_tire_joystick.cpp

#include "common_tire_joystick.hpp"
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <time.h>bool joy_flag;
bool beg_flag;
double speed[2];void *joyThread(void *);pthread_mutex_t s_mutex;int main(int argc, char **argv)
{ros::init(argc, argv, "common_tire_joypub");ros::NodeHandle n;geometry_msgs::Twist twist_msg;joy_flag = false;beg_flag = true;memset(speed, 0, sizeof(speed));pthread_t pth_joy;pthread_create(&pth_joy, NULL, joyThread, NULL);pthread_detach(pth_joy);ros::Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);double dt = 0.1;ros::Rate loop(1 / dt);while (ros::ok()){pthread_mutex_lock(&s_mutex);if (joy_flag){twist_msg.linear.x = speed[0];twist_msg.angular.z = speed[1];}else{twist_msg.linear.x = 0;twist_msg.angular.z = 0;}pthread_mutex_unlock(&s_mutex);pub.publish(twist_msg);ros::spinOnce();loop.sleep();}beg_flag = false;usleep(500);pthread_mutex_destroy(&s_mutex);return 0;
}void *joyThread(void *)
{CommonTireJoystick joy;if (!joy.initJoystick()){cout << "Initialize fail!" << endl;return 0;}while (beg_flag){joy.listenJoystick();pthread_mutex_lock(&s_mutex);joy_flag = joy.getJoyflag();joy.getSpeed(speed);pthread_mutex_unlock(&s_mutex);}
}

注意退化后的四轮差速移动机器人只有沿X轴的直线速度和绕Z轴的转速，因此Twist消息只有两个分量有值

twist_msg.linear.x = speed[0];
twist_msg.angular.z = speed[1];

snap7与PLC通信

上位机与PLC的通信主要通过snap7实现，师兄们已经做了许多工作，可以看我们的博客

snap7读写西门子plc1200步骤（python）PLC通讯

在VS中配置snap7并用snap7与PLC通信

我只做了很小的改动，代码如下：

control_with_joy.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>#include "snap7.h"#include <iostream>
using namespace std;// plc的IP地址
const char *plc_ip = "192.168.1.33";#define D_WB 0.5
#define PLC_MAX_SPEED 27312.2626
// const double tire_dia = 0.25;
const double fac = 1.5;TS7Client snap7_client;void vel2plc(int *plc_vel, const geometry_msgs::Twist &twist_msg);// void readEnc();void callback(const geometry_msgs::Twist::ConstPtr &msg)
{int plc_speed[4];vel2plc(plc_speed, *msg);byte buff[8] = {0}; //创建一个读写缓存区for (int i = 0; i < 8; i++){if (i % 2){buff[i] = (byte)(0xff & (plc_speed[i / 2]));}else{buff[i] = (byte)(0xff & (plc_speed[i / 2] >> 8));}}// 向PLC写数据（参数分别是DB块，块号，起始地址， 写多少， 写word类型，数据源开始地址）snap7_client.AsWriteArea(0x84, 4, 0, 4, 0x04, buff);
}int main(int argc, char **argv)
{// plc connectsnap7_client.ConnectTo(plc_ip, 0, 0);if (!snap7_client.Connected()){cout << "PLC connect fail!" << endl;cout << "error return in " << __FILE__ << " " << __LINE__ << ":erron type connect failed" << endl;return -1;}else{cout << "PLC connect successQ" << endl;}// rosros::init(argc, argv, "cmd_to_plc");ros::NodeHandle nh;ros::Subscriber sub = nh.subscribe("/cmd_vel", 1, callback);ros::spin();return 0;
}void vel2plc(int *plc_vel, const geometry_msgs::Twist &twist_msg)
{double D_LR = fac * D_WB;double V_L = twist_msg.linear.x + (D_LR / 2) * twist_msg.angular.z;double V_R = twist_msg.linear.x - (D_LR / 2) * twist_msg.angular.z;plc_vel[0] = (int)(PLC_MAX_SPEED * V_L);plc_vel[3] = (int)(PLC_MAX_SPEED * V_L);plc_vel[1] = (int)(PLC_MAX_SPEED * V_R);plc_vel[2] = (int)(PLC_MAX_SPEED * V_R);
}// void readEnc()
// {//     int Enc_data[4];
//     memset(&Enc_data, 0, sizeof(Enc_data));//     cout << "读取编码器数据..." << endl;
//     snap7_client.AsReadArea(0x84, 4, 3, 4, 0x06, Enc_data);//     for (int i = 0; i < 4; ++i)
//     {//         cout << "Encoder " << i + 1 << ":" << Enc_data[i];
//     }
// }

添加了两个函数接口

vel2plc( )实现速度解算，后面不同车的速度解算在这里就可以
readEnc( )实现编码器信息的读取

通信具体的技术细节上面两篇博客已经写得很清楚啦，师兄们真的太强了(迷弟.jpg)

不足之处

一代移动机器人重心偏后，而解算假设重心形心重合
测试时readEnc( )无法读取编码器数据，具体实现有待补充
虚拟轮间距与实际轮间距间的系数未经过多次实验测试