theoretical derivation
Theoretical derivation can refer to the following article
Four-wheel drive (SSMR) mobile robot motion model and application analysis (qq.com)
There are a few points to note:
- The derivation assumes that the centroid of the car coincides with the center of mass of the car . In fact, this is not realistic, but for the sake of simplification, it can be done for the time being. If you want to control it more finely, you can find relevant information
- The derivation simplifies the four-wheel differential model to a two-wheel differential model , which can be considered as the degradation of the model
- The coefficient between the virtual wheel spacing and the actual wheel spacing generated after model degradation requires multiple experiments to obtain suitable results
- The final result can be simply memorized as the wheels on the same side rotate at the same speed
programming
In practice, it is usually found that theoretical derivation is often easy, but programming implementation is always difficult. There are three main problems this time.
- The car to be debugged is a first-generation mobile robot, and it will take about a few years to complete. The familiar seniors have already graduated and worked. The bottom layer of the car is controlled by PLC, and we have not touched PLC for a long time...
- How to customize the functions of the Logitech handle buttons and joysticks, and how to read the information of the buttons for use
- After the button information is read, how to send it to the PLC to achieve the desired movement
Are you already scratching your head, let's see what to do together
Familiar with the underlying PLC program
It’s not convenient to post this part of the program, let’s make it up by yourself hhh
Thanks for the work done by Brother Yidawu, providing an interface for interacting with the host computer
We can send speed commands from top to bottom and read sensor data from bottom to top through the following variables
Customize handle button functions and read button information
It can also be seen from the title that this part can be split into two sub-functions
- Customize handle button function
- Read handle button information
Customize handle button function
The header file <linux/joystick.h> for handling handles is provided under Linux, which defines the axis number of each axis and the key number of each key. In practice, we need to test the specific values corresponding to the keys of our handle and the axes.
The previous brothers did not record very clearly, so I recorded it again
<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 <[email protected]>, 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 8
struct 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 */
There are two things to pay attention to, the first is the key, axis, and initialization macro
#define JS_EVENT_BUTTON 0x01 /* button pressed/released */
#define JS_EVENT_AXIS 0x02 /* joystick moved */
#define JS_EVENT_INIT 0x80 /* initial state of device */
The second is to define the structure of the key information
struct js_event {
__u32 time; /* event timestamp in milliseconds */
__s16 value; /* value */
__u8 type; /* event type */
__u8 number; /* axis/button number */
};
Later we read the information of buttons and axes mainly based on these two parts
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 0x05
const 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
The key number of the button and the axis number of the joystick can be seen from the macro definition
#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
Corresponding to the handle as shown in the figure below, where A means Axis and B means Button
insert image description here
There are a few points to note:
- The type of the four keys on the left side is Axis instead of Button
- The positive and negative values of the button and joystick are opposite to our conventional understanding. Taking the lower left joystick as an example, when the joystick is turned to the left, it is negative, so it should be reversed in actual application
- The maximum value of buttons and joysticks is 32767
- The value of the joystick is gradual, while the value of the button on the upper left is directly the maximum value. When using it, it can be defined as the robot moving at the highest speed.
read key information
You can refer to the following blogs
Linux programming control hardware (5) ---- operate USB joystick
[joysticke]Use the Ubuntu16.04 environment to read the USB joystick/steering wheel information
The main steps can be simplified to the following steps:
- After the bluetooth of the controller is plugged in, the device name of js0 will appear under /dev/input
- Define the struct js_event structure variable, ready to receive handle information
- The open() function opens /dev/input/js0
- The read( ) function reads information into the struct js_event structure variable
- The close( ) function closes /dev/input/js0
For details, please refer to the common_tire_joystick.hpp file above
After reading the handle information, send the handle value as a speed command to the /cmd_vel topic
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);
}
}
Note that the degenerated four-wheel differential mobile robot only has the linear velocity along the X axis and the rotational speed around the Z axis, so the Twist message has only two components with values
twist_msg.linear.x = speed[0];
twist_msg.angular.z = speed[1];
snap7 communicates with PLC
The communication between the upper computer and the PLC is mainly realized through snap7. Brothers have done a lot of work, you can read our blog
snap7 read and write Siemens plc1200 steps (python) PLC communication
I only made a small change, the code is as follows:
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 connect
snap7_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;
}
// ros
ros::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];
// }
// }
Added two functional interfaces
- vel2plc( ) realizes the speed calculation, and the speed calculation of different cars later can be done here
- readEnc( ) realizes the reading of encoder information
The specific technical details of the communication have been clearly written in the above two blogs, and the brothers are really too strong (my brother.jpg)
Inadequacies
- The center of gravity of the first-generation mobile robot is backward, and the solution assumes that the centers of gravity and centroids coincide
- readEnc( ) cannot read the encoder data during the test, the specific implementation needs to be supplemented
- The coefficient between the virtual wheel spacing and the actual wheel spacing has not been tested many times