Windows系统监听键盘通过UDP协议控制树莓派小车

风之物语

树莓派小车硬件从淘宝买到手后已经鼓捣很长时间了，其中最喜欢的应用是控制小车运动了。我的小车控制系统在开发的过程中遇到了很多小问题，都被我一一修正了，将开发经验与大家分享，希望后来人能少走些弯路。

控制小车的方法大体上有以下几种：
1.TCP/UDP控制；
2.网页控制；
3.蓝牙控制；
4.红外遥控器控制；
5.手柄控制。

本次我采用的是【1.TCP/UDP控制】方案。
需要的硬件如下：

1. win7笔记本
2. 树莓派小车
3. 无线wifi网络

控制方案简介：通过监听win7笔记本上的键盘的按键，获得控制信息。然后将控制信息通过UDP协议发送给局域网内的树莓派小车。树莓派小车获得指令后进行动作。

小车控制系统分为三部分：控制协议、客户端设计、服务端设计。

0×01 控制协议

小车控制系统的通讯协议采用面向非连接的UDP协议，相比TCP协议，UDP协议更简单有效。
控制协议格式如下：

协议版本号:包号:命令字

其中协议版本号固定为1；包号为一个数字，每发送一次进行加1处理；命令字用于向小车发送控制命令。

本文用到的命令字有如下几种。

//方向控制
#define CARRUN_REMOTE_FORWARD (0x01)
#define CARRUN_REMOTE_BACK (0x02)
#define CARRUN_REMOTE_LEFT (0x03)
#define CARRUN_REMOTE_RIGHT (0x04)
#define CARRUN_REMOTE_STOP (0x05)
//IP相关命令
#define CARRUN_REMOTE_WHOISONLINE (0x06) // who is online
#define CARRUN_REMOTE_ONLINE (0x07) // I am online

复制代码

0×02 客户端设计

客户端界面如下所示。

可以手动输入树莓派小车的IP地址。为了方面操作，设计了一个自动获得树莓派小车IP的功能。

按下[Find Car]按钮，客户端程序向整个网络广播CARRUN_REMOTE_WHOISONLINE（谁在线）请求。当树莓派小车收到广播通知，立即向客户端发送CARRUN_REMOTE_ONLINE（我在线）的答复。客户端收到答复，自动将树莓派小车的IP地址记录到文本框中显示。

获得树莓派小车IP地址后，按下[Start]按钮，开始监听键盘信息，并根据监听到的特定按键的按下、抬起事件，转换为树莓派小车的控制命令，通过UDP协议与树莓派小车通信。

按键与小车动作的转换表如下：

UDP协议使用的3737端口，发送UDP协议是很简单的事情，本文不详细介绍。

监听键盘是通过安装系统钩子实现的，具体内容不详细介绍，读者可以检索SetWindowsHookEx函数进行详细了解。

0×03 服务器端设计

树莓派小车安装的是RaspbianOS系统，该系统采用linux内核，对熟悉linux的朋友很容易上手。

服务器端使用C++语言和python语言混合编程实现。其中python语言编写的程序用于控制小车的前进、后退、左转、右转、停止动作。C++语言编写的程序用于监听UDP命令，并处理控制命令。

用python控制小车的代码非常简单，我调试好的代码如下：

#!/usr/bin/Python
# -*- coding: UTF-8 -*-
#引入gpio的模块
import RPi.GPIO as GPIO
import time
#设置in1到in4接口
IN1 = 12
IN2 = 16
IN3 = 18
IN4 = 22
#初始化接口
def car_init():
#设置GPIO模式
GPIO.setmode(GPIO.BOARD)
GPIO.setup(IN1,GPIO.OUT)
GPIO.setup(IN2,GPIO.OUT)
GPIO.setup(IN3,GPIO.OUT)
GPIO.setup(IN4,GPIO.OUT)
#前进的代码
def car_forward():
GPIO.output(IN1,GPIO.HIGH)
GPIO.output(IN2,GPIO.LOW)
GPIO.output(IN3,GPIO.HIGH)
GPIO.output(IN4,GPIO.LOW)
time.sleep(0.15)
GPIO.cleanup()
#后退
def car_back():
GPIO.output(IN1,GPIO.LOW)
GPIO.output(IN2,GPIO.HIGH)
GPIO.output(IN3,GPIO.LOW)
GPIO.output(IN4,GPIO.HIGH)
time.sleep(0.15)
GPIO.cleanup()
#左转
def car_left():
GPIO.output(IN1,False)
GPIO.output(IN2,False)
GPIO.output(IN3,GPIO.HIGH)
GPIO.output(IN4,GPIO.LOW)
time.sleep(0.15)
GPIO.cleanup()
#右转
def car_right():
GPIO.output(IN1,GPIO.HIGH)
GPIO.output(IN2,GPIO.LOW)
GPIO.output(IN3,False)
GPIO.output(IN4,False)
time.sleep(0.15)
GPIO.cleanup()
#停止
def car_stop():
GPIO.output(IN1,GPIO.LOW)
GPIO.output(IN2,GPIO.LOW)
GPIO.output(IN3,GPIO.LOW)
GPIO.output(IN4,GPIO.LOW)
GPIO.cleanup()

复制代码

C++语言编写的程序是小车控制系统的核心，可以分为4个模块：协议解析模块、UDP端口监听模块、逻辑控制模块、动作执行模块。

协议解析模块用于解析协议。

协议解析模块的核心代码如下：

#define COMMUNICATION_PORT (3737)
//command
#define CARRUN_REMOTE_FORWARD (0x01)
#define CARRUN_REMOTE_BACK (0x02)
#define CARRUN_REMOTE_LEFT (0x03)
#define CARRUN_REMOTE_RIGHT (0x04)
#define CARRUN_REMOTE_STOP (0x05)
#define CARRUN_REMOTE_WHOISONLINE (0x06)
#define CARRUN_REMOTE_ONLINE (0x07)
#define GET_MODE(command) (command & 0x000000ffUL)
struct MsgInfo {
unsigned long ipaddr; // IP Address
int portNo; // port number
std::string version; // version
unsigned long packetNo; // packet number
unsigned long command; // command
};
#define SOCKET_ERROR (-1)
//消息解析函数
bool ResolveMsg(const std::string message, MsgInfo &msg) {
std::vector<std::string> vInfos;
boost::split( vInfos, message, boost::is_any_of( ",:" ), boost::token_compress_on );
if ( vInfos[0] != "1" ) {
std::cout << "protocol is wrong."<< std::endl;
return false;
}
msg.version = vInfos[0];
msg.packetNo = atoi(vInfos[1].c_str());
msg.command = atoi(vInfos[2].c_str());
return true;
}
unsigned long MakePacketNumber() {
static unsigned long packnumber = 0;
packnumber++;
return packnumber;
}
//发送UDP消息函数
bool UDPSend(int localSocket, unsigned long host_addr, unsigned short port_no, unsigned long command )
{
struct sockaddr_in addr;
memset(&addr, 0, sizeof(addr)); /// 网络地址初始化
addr.sin_family = AF_INET;
addr.sin_port = htons(port_no);
addr.sin_addr.s_addr = host_addr;
char message[512] = {0};
unsigned long packnumber = MakePacketNumber();
sprintf(message,"%d:%ld:%ld",0x1,packnumber,command);
if (SOCKET_ERROR == ::sendto(localSocket, message, strlen(message),0, (struct sockaddr *)&addr ,sizeof(addr))) {
return false;
}
return true;
}

复制代码

UDP端口监听模块用于监听从3737端口来的控制信息，并将收到的消息解析后以异步的方式传递给逻辑控制模块。

UDP端口监听模块的核心代码如下：

int localUDPSocket;
void *listenUDPCommunicationThread(void *arg) {
struct sockaddr_in localAddr,remoteAddr;
localUDPSocket = socket(AF_INET, SOCK_DGRAM, 0); // udp
bzero(&localAddr, sizeof(localAddr));
localAddr.sin_family = AF_INET;
localAddr.sin_port = htons(COMMUNICATION_PORT); // port
localAddr.sin_addr.s_addr = INADDR_ANY;
std::cout << "socket returned : " << localUDPSocket << std::endl;
int result = bind(localUDPSocket, (struct sockaddr *)&localAddr, sizeof(localAddr));
std::cout << "bind returned : " << result << std::endl <<std::endl;
if (-1 == result) {
std::cout << "bind error!" << std::endl;
exit(1);
}
char buffer[1024] = {0};
while(1) {
memset(buffer,0,sizeof(buffer));
socklen_t remoteAddrLength = sizeof(remoteAddr);
int nReceiveSize = recvfrom(localUDPSocket, &buffer, sizeof(buffer), 0, (struct sockaddr *)&remoteAddr, &remoteAddrLength);
std::cout << "received UDP data. data is: " << buffer << std::endl;
std::string message(buffer,nReceiveSize);
DoAction(&remoteAddr, message);
}
}
void DoAction(struct sockaddr_in *addr, const std::string message) {
MsgInfo msg;
if (addr) {
msg.ipaddr = addr->sin_addr.s_addr;
msg.portNo = htons(addr->sin_port);
}
bool bResult = ResolveMsg( message, msg );
if ( !bResult ) {
std::cout << "ResolveMsg fail."<< std::endl;
return;
}
// std::cout << "command:0x" << hex << msg.command << std::endl;
switch ( GET_MODE(msg.command) )
{
case CARRUN_REMOTE_FORWARD:
{
std::cout << "command: CARRUN_REMOTE_FORWARD"<< std::endl;
DirectionReq *req = new DirectionReq();
req->setValue(DIRECTION_FORWARD);
ControlManager::instance()->postActionReq(req);
}
break;
case CARRUN_REMOTE_BACK:
{
std::cout << "command: CARRUN_REMOTE_BACK"<< std::endl;
DirectionReq *req = new DirectionReq();
req->setValue(DIRECTION_BACK);
ControlManager::instance()->postActionReq(req);
}
break;
case CARRUN_REMOTE_LEFT:
{
std::cout << "command: CARRUN_REMOTE_LEFT"<< std::endl;
DirectionReq *req = new DirectionReq();
req->setValue(DIRECTION_LEFT);
ControlManager::instance()->postActionReq(req);
}
break;
case CARRUN_REMOTE_RIGHT:
{
std::cout << "command: CARRUN_REMOTE_RIGHT"<< std::endl;
DirectionReq *req = new DirectionReq();
req->setValue(DIRECTION_RIGHT);
ControlManager::instance()->postActionReq(req);
}
break;
case CARRUN_REMOTE_STOP:
{
std::cout << "command: CARRUN_REMOTE_STOP"<< std::endl;
StatusReq *req = new StatusReq();
ControlManager::instance()->postStatusReq(req);
}
break;
case CARRUN_REMOTE_WHOISONLINE:
std::cout << "command: CARRUN_REMOTE_WHOISONLINE"<< std::endl;
UDPSend(localUDPSocket,addr->sin_addr.s_addr,COMMUNICATION_PORT,CARRUN_REMOTE_ONLINE);
break;
default:
{
std::cout << "Unknown command:"<< msg.command << std::endl;
}
break;
}
return;
}

复制代码

逻辑控制模块只有一个线程在处理，但有两个队列。队列1优先级最高，只存放小车的停止命令。队列2优先级最低，用于存放小车的前进、后退、左转、右转命令。

由于键盘按键按下后如果一直不动，那么客户端（PC）会向服务器端（树莓派小车）连续发送大量的命令，由于树莓派小车的处理速度远低于消息发送的速度，此时很容易在树莓派小车产生消息堆积，这样会导致控制失灵。

解决办法：当树莓派小车收到停止命令时，将消息投递到优先级最高的队列1中。当逻辑控制模块每处理一个命令后，会优先检查队列1中是否有停止命令，如果有停止命令的话，会执行让小车停止动作，同时清空队列2中的全部堆积指令。

逻辑控制模块的核心代码如下。

void ReqThread::Run()
{
while (1)
{
if ( m_directionlist.size() > 0)
{
pthread_mutex_lock(&mutex);
DirectionReq* pReq = (DirectionReq*)m_directionlist.front();
pthread_mutex_unlock(&mutex);
if (NULL != pReq) {
pReq->doAction();
delete pReq;
pReq = NULL;
}
pthread_mutex_lock(&mutex);
m_directionlist.erase(m_directionlist.begin());
pthread_mutex_unlock(&mutex);
}
if ( m_statuslist.size() > 0)
{
pthread_mutex_lock(&mutex);
StatusReq* pReq = (StatusReq*)m_statuslist.front();
pthread_mutex_unlock(&mutex);
if (NULL != pReq) {
pReq->doAction();
delete pReq;
pReq = NULL;
}
pthread_mutex_lock(&mutex);
m_statuslist.erase(m_statuslist.begin());
pthread_mutex_unlock(&mutex);
clearAllList();
}
}
}
void ReqThread::clearAllList()
{
pthread_mutex_lock(&mutex);
if (m_statuslist.size() > 0) {
std::cout << "clear statuslist"<< std::endl;
}
while (m_statuslist.size() > 0)
{
RequestBase* pReq = m_statuslist.front();
if (NULL != pReq) {
delete pReq;
pReq = NULL;
}
m_statuslist.erase(m_statuslist.begin());
}
if (m_directionlist.size() > 0) {
std::cout << "clear directionlist"<< std::endl;
}
while (m_directionlist.size() > 0)
{
RequestBase* pReq = m_directionlist.front();
if (NULL != pReq) {
delete pReq;
pReq = NULL;
}
m_directionlist.erase(m_directionlist.begin());
}
pthread_mutex_unlock(&mutex);
}

复制代码

动作执行模块用于调用python模块的接口，实现小车的前进、后退、左转、右转，停止动作。

注意：C++代码是可以调用python模块的接口的。

动作执行模块的代码如下。

#include </usr/include/python2.7/Python.h>
#include <iostream>
#include "switch.h"
#define DEBUG_TEST 0
void Forward()
{
std::cout<<"###Forward###"<<std::endl;
#if DEBUG_TEST
sleep(1);
return;
#endif
//初始化python
Py_Initialize();
PyRun_SimpleString("import sys");
PyRun_SimpleString("sys.path.append('./')");
PyObject * pModule = NULL;
PyObject * pFunc = NULL;
//这里是要调用的文件名
pModule = PyImport_ImportModule("control");
if (!pModule)
{
std::cout<<"Call PyImport_ImportModule("control") fail."<<std::endl;
return;
}
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_init");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_forward");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
}
void Back()
{
std::cout<<"###Back###"<<std::endl;
#if DEBUG_TEST
sleep(1);
return;
#endif
//初始化python
Py_Initialize();
PyRun_SimpleString("import sys");
PyRun_SimpleString("sys.path.append('./')");
PyObject * pModule = NULL;
PyObject * pFunc = NULL;
//这里是要调用的文件名
pModule = PyImport_ImportModule("control");
if (!pModule)
{
std::cout<<"Call PyImport_ImportModule("control") fail."<<std::endl;
return;
}
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_init");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_back");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
}
void Left()
{
std::cout<<"###Left###"<<std::endl;
#if DEBUG_TEST
sleep(1);
return;
#endif
//初始化python
Py_Initialize();
PyRun_SimpleString("import sys");
PyRun_SimpleString("sys.path.append('./')");
PyObject * pModule = NULL;
PyObject * pFunc = NULL;
//这里是要调用的文件名
pModule = PyImport_ImportModule("control");
if (!pModule)
{
std::cout<<"Call PyImport_ImportModule("control") fail."<<std::endl;
return;
}
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_init");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_left");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
}
void Right()
{
std::cout<<"###Right###"<<std::endl;
#if DEBUG_TEST
sleep(1);
return;
#endif
//初始化python
Py_Initialize();
PyRun_SimpleString("import sys");
PyRun_SimpleString("sys.path.append('./')");
PyObject * pModule = NULL;
PyObject * pFunc = NULL;
//这里是要调用的文件名
pModule = PyImport_ImportModule("control");
if (!pModule)
{
std::cout<<"Call PyImport_ImportModule("control") fail."<<std::endl;
return;
}
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_init");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_right");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
}
void Stop()
{
std::cout<<"###Stop###"<<std::endl;
#if DEBUG_TEST
sleep(1);
return;
#endif
//初始化python
Py_Initialize();
PyRun_SimpleString("import sys");
PyRun_SimpleString("sys.path.append('./')");
PyObject * pModule = NULL;
PyObject * pFunc = NULL;
//这里是要调用的文件名
pModule = PyImport_ImportModule("control");
if (!pModule)
{
std::cout<<"Call PyImport_ImportModule("control") fail."<<std::endl;
return;
}
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_init");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
//这里是要调用的函数名
pFunc= PyObject_GetAttrString(pModule, "car_stop");
//调用函数
PyEval_CallObject(pFunc, NULL);
Py_DECREF(pFunc);
}

复制代码

到此整个小车控制系统就介绍完了。

最后，整个服务器端代码已经发到了百度网盘上。

游客，如果您要查看本帖隐藏内容请回复

本文作者xutiejun，转载自freebuf

zhang013hao · 发表于 2018-10-15 11:32:19

学习学习学习学习学习学习学习学习学习学习学习学习

vontrigger · 发表于 2019-3-31 20:08:16

学习

Windows系统监听键盘通过UDP协议控制树莓派小车

相关帖子

FPGA千兆网UDP协议实现

风之物语 LV5

站长推荐 /3