亲，“电路城论坛”已合并升级到更全、更大、更强的「新与非网」。了解「新与非网」

论坛

 注册/登录  
返回列表 发新帖
查看: 4988|回复: 2

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

[复制链接]
风之物语
  • TA的每日心情
    怒
    2020-3-6 09:52

    • 签到天数: 13 天

    连续签到: 1 天

    [LV.3]偶尔看看II
    发表于 2018-10-12 15:01:48 | 显示全部楼层 |阅读模式
    分享到：
    树莓派小车硬件从淘宝买到手后已经鼓捣很长时间了，其中最喜欢的应用是控制小车运动了。我的小车控制系统在开发的过程中遇到了很多小问题，都被我一一修正了，将开发经验与大家分享，希望后来人能少走些弯路。

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


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

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


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

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


    0×01 控制协议

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

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

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

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

    1. //方向控制
    2. #define CARRUN_REMOTE_FORWARD   (0x01)
    3. #define CARRUN_REMOTE_BACK      (0x02)
    4. #define CARRUN_REMOTE_LEFT      (0x03)
    5. #define CARRUN_REMOTE_RIGHT     (0x04)
    6. #define CARRUN_REMOTE_STOP      (0x05)

    8. //IP相关命令
    9. #define CARRUN_REMOTE_WHOISONLINE    (0x06)  // who is online
    10. #define CARRUN_REMOTE_ONLINE         (0x07)  // I am online
    复制代码


    0×02 客户端设计

    客户端界面如下所示。

    1.jpg

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

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

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

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

    2.jpg

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

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


    0×03 服务器端设计

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

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

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

    1. #!/usr/bin/Python
    2. # -*- coding: UTF-8 -*-

    4. #引入gpio的模块
    5. import RPi.GPIO as GPIO
    6. import time


    9. #设置in1到in4接口
    10. IN1 = 12
    11. IN2 = 16
    12. IN3 = 18
    13. IN4 = 22

    15. #初始化接口
    16. def car_init():
    17.     #设置GPIO模式
    18.     GPIO.setmode(GPIO.BOARD)

    20.     GPIO.setup(IN1,GPIO.OUT)
    21.     GPIO.setup(IN2,GPIO.OUT)
    22.     GPIO.setup(IN3,GPIO.OUT)
    23.     GPIO.setup(IN4,GPIO.OUT)

    25. #前进的代码
    26. def car_forward():
    27.     GPIO.output(IN1,GPIO.HIGH)
    28.     GPIO.output(IN2,GPIO.LOW)
    29.     GPIO.output(IN3,GPIO.HIGH)
    30.     GPIO.output(IN4,GPIO.LOW)
    31.     time.sleep(0.15)
    32.     GPIO.cleanup()

    34. #后退
    35. def car_back():
    36.     GPIO.output(IN1,GPIO.LOW)
    37.     GPIO.output(IN2,GPIO.HIGH)
    38.     GPIO.output(IN3,GPIO.LOW)
    39.     GPIO.output(IN4,GPIO.HIGH)
    40.     time.sleep(0.15)
    41.     GPIO.cleanup()

    43. #左转
    44. def car_left():
    45.     GPIO.output(IN1,False)
    46.     GPIO.output(IN2,False)
    47.     GPIO.output(IN3,GPIO.HIGH)
    48.     GPIO.output(IN4,GPIO.LOW)
    49.     time.sleep(0.15)
    50.     GPIO.cleanup()

    52. #右转
    53. def car_right():
    54.     GPIO.output(IN1,GPIO.HIGH)
    55.     GPIO.output(IN2,GPIO.LOW)
    56.     GPIO.output(IN3,False)
    57.     GPIO.output(IN4,False)
    58.     time.sleep(0.15)
    59.     GPIO.cleanup()

    61. #停止
    62. def car_stop():
    63.     GPIO.output(IN1,GPIO.LOW)
    64.     GPIO.output(IN2,GPIO.LOW)
    65.     GPIO.output(IN3,GPIO.LOW)
    66.     GPIO.output(IN4,GPIO.LOW)
    67.     GPIO.cleanup()
    复制代码
    C++语言编写的程序是小车控制系统的核心，可以分为4个模块：协议解析模块、UDP端口监听模块、逻辑控制模块、动作执行模块。

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

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

    1. #define COMMUNICATION_PORT (3737)

    3. //command
    4. #define CARRUN_REMOTE_FORWARD   (0x01)
    5. #define CARRUN_REMOTE_BACK      (0x02)
    6. #define CARRUN_REMOTE_LEFT      (0x03)
    7. #define CARRUN_REMOTE_RIGHT     (0x04)
    8. #define CARRUN_REMOTE_STOP      (0x05)

    10. #define CARRUN_REMOTE_WHOISONLINE    (0x06)
    11. #define CARRUN_REMOTE_ONLINE         (0x07)

    13. #define GET_MODE(command) (command & 0x000000ffUL)

    15. struct MsgInfo {
    16.     unsigned long ipaddr;       // IP Address
    17.     int           portNo;       // port number

    19.     std::string   version;      // version
    20.     unsigned long packetNo;     // packet number
    21.     unsigned long command;      // command
    22. };

    24. #define SOCKET_ERROR            (-1)

    26. //消息解析函数
    27. bool ResolveMsg(const std::string message, MsgInfo &msg) {
    28.     std::vector<std::string> vInfos;  
    29.     boost::split( vInfos, message, boost::is_any_of( ",:" ), boost::token_compress_on );

    31.     if ( vInfos[0] != "1" ) {
    32.         std::cout << "protocol is wrong."<< std::endl;
    33.         return false;
    34.     }

    36.     msg.version  = vInfos[0];
    37.     msg.packetNo = atoi(vInfos[1].c_str());
    38.     msg.command  = atoi(vInfos[2].c_str());

    40.     return true;
    41. }

    43. unsigned long MakePacketNumber() {
    44.     static unsigned long packnumber = 0;
    45.     packnumber++;

    47.     return packnumber;
    48. }

    50. //发送UDP消息函数
    51. bool UDPSend(int localSocket, unsigned long host_addr, unsigned short port_no, unsigned long command )
    52. {
    53.     struct sockaddr_in    addr;

    55.     memset(&addr, 0, sizeof(addr));    /// 网络地址初始化
    56.     addr.sin_family         = AF_INET;
    57.     addr.sin_port           = htons(port_no);
    58.     addr.sin_addr.s_addr    = host_addr;

    60.     char message[512] = {0};
    61.     unsigned long packnumber = MakePacketNumber();

    63.     sprintf(message,"%d:%ld:%ld",0x1,packnumber,command);

    65.     if (SOCKET_ERROR == ::sendto(localSocket, message, strlen(message),0, (struct sockaddr *)&addr ,sizeof(addr))) {
    66.         return false;
    67.     }

    69.     return true;
    70. }
    复制代码


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

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

    1. int localUDPSocket;

    3. void *listenUDPCommunicationThread(void *arg) {
    4.     struct sockaddr_in localAddr,remoteAddr;
    5.     localUDPSocket = socket(AF_INET, SOCK_DGRAM, 0);  // udp
    6.     bzero(&localAddr, sizeof(localAddr));
    7.     localAddr.sin_family = AF_INET;
    8.     localAddr.sin_port = htons(COMMUNICATION_PORT);   // port
    9.     localAddr.sin_addr.s_addr = INADDR_ANY;

    11.     std::cout << "socket returned : " << localUDPSocket << std::endl;

    13.     int result = bind(localUDPSocket, (struct sockaddr *)&localAddr, sizeof(localAddr));
    14.     std::cout << "bind returned : " << result << std::endl <<std::endl;

    16.     if (-1 == result) {
    17.         std::cout << "bind error!" << std::endl;
    18.         exit(1);
    19.     }

    21.     char buffer[1024] = {0};

    23.     while(1) {   
    24.         memset(buffer,0,sizeof(buffer));
    25.         socklen_t remoteAddrLength = sizeof(remoteAddr);
    26.         int nReceiveSize = recvfrom(localUDPSocket, &buffer, sizeof(buffer), 0, (struct sockaddr *)&remoteAddr, &remoteAddrLength);

    28.         std::cout << "received UDP data. data is: " << buffer << std::endl;

    30.         std::string message(buffer,nReceiveSize);

    32.         DoAction(&remoteAddr, message);
    33.     }
    34. }

    36. void DoAction(struct sockaddr_in *addr, const std::string message) {
    37.     MsgInfo msg;
    38.     if (addr) {
    39.         msg.ipaddr = addr->sin_addr.s_addr;
    40.         msg.portNo = htons(addr->sin_port);   
    41.     }
    42.    

    44.     bool bResult = ResolveMsg( message, msg );
    45.     if ( !bResult ) {
    46.         std::cout << "ResolveMsg fail."<< std::endl;

    48.         return;
    49.     }

    51. //    std::cout << "command:0x" << hex << msg.command << std::endl;

    53.     switch ( GET_MODE(msg.command) )
    54.     {
    55.     case CARRUN_REMOTE_FORWARD:
    56.         {
    57.             std::cout << "command: CARRUN_REMOTE_FORWARD"<< std::endl;
    58.         
    59.             DirectionReq *req = new DirectionReq();
    60.             req->setValue(DIRECTION_FORWARD);
    61.             ControlManager::instance()->postActionReq(req);   
    62.         }
    63.         

    65.         break;
    66.     case CARRUN_REMOTE_BACK:
    67.         {
    68.             std::cout << "command: CARRUN_REMOTE_BACK"<< std::endl;
    69.         
    70.             DirectionReq *req = new DirectionReq();
    71.             req->setValue(DIRECTION_BACK);
    72.             ControlManager::instance()->postActionReq(req);
    73.         }
    74.         break;
    75.     case CARRUN_REMOTE_LEFT:
    76.         {
    77.             std::cout << "command: CARRUN_REMOTE_LEFT"<< std::endl;
    78.         
    79.             DirectionReq *req = new DirectionReq();
    80.             req->setValue(DIRECTION_LEFT);
    81.             ControlManager::instance()->postActionReq(req);   
    82.         }
    83.         break;
    84.     case CARRUN_REMOTE_RIGHT:
    85.         {
    86.             std::cout << "command: CARRUN_REMOTE_RIGHT"<< std::endl;
    87.         
    88.             DirectionReq *req = new DirectionReq();
    89.             req->setValue(DIRECTION_RIGHT);
    90.             ControlManager::instance()->postActionReq(req);   
    91.         }
    92.         break;

    94.     case CARRUN_REMOTE_STOP:
    95.         {
    96.             std::cout << "command: CARRUN_REMOTE_STOP"<< std::endl;
    97.         
    98.             StatusReq *req = new StatusReq();
    99.             ControlManager::instance()->postStatusReq(req);   
    100.         }
    101.         break;
    102.         
    103.     case CARRUN_REMOTE_WHOISONLINE:
    104.         std::cout << "command: CARRUN_REMOTE_WHOISONLINE"<< std::endl;
    105.         UDPSend(localUDPSocket,addr->sin_addr.s_addr,COMMUNICATION_PORT,CARRUN_REMOTE_ONLINE);
    106.         break;



    110.     default:
    111.         {
    112.             std::cout << "Unknown command:"<< msg.command << std::endl;
    113.         }
    114.         break;
    115.     }

    117.     return;

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

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

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

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

    1. void ReqThread::Run()
    2. {
    3.         while (1)
    4.         {
    5.                 if ( m_directionlist.size()  > 0)
    6.                 {
    7.                         pthread_mutex_lock(&mutex);
    8.                         DirectionReq* pReq = (DirectionReq*)m_directionlist.front();
    9.                         pthread_mutex_unlock(&mutex);
    10.                         if (NULL != pReq) {
    11.                                 pReq->doAction();
    12.                                 delete pReq;
    13.                                 pReq = NULL;
    14.                         }
    15.             pthread_mutex_lock(&mutex);      
    16.                         m_directionlist.erase(m_directionlist.begin());
    17.             pthread_mutex_unlock(&mutex);
    18.                 }

    20.                 if ( m_statuslist.size()  > 0)
    21.                 {
    22.                         pthread_mutex_lock(&mutex);
    23.                         StatusReq* pReq = (StatusReq*)m_statuslist.front();
    24.                         pthread_mutex_unlock(&mutex);
    25.                         if (NULL != pReq) {
    26.                                 pReq->doAction();
    27.                                 delete pReq;
    28.                                 pReq = NULL;
    29.                         }

    31.                         pthread_mutex_lock(&mutex);
    32.                         m_statuslist.erase(m_statuslist.begin());
    33.             pthread_mutex_unlock(&mutex);

    35.                         clearAllList();
    36.                 }

    38.         }
    39. }

    41. void ReqThread::clearAllList()
    42. {
    43.     pthread_mutex_lock(&mutex);

    45.         if (m_statuslist.size()  > 0) {
    46.                 std::cout << "clear statuslist"<< std::endl;               
    47.         }

    49.         while (m_statuslist.size()  > 0)
    50.         {
    51.                 RequestBase* pReq = m_statuslist.front();
    52.                 if (NULL != pReq) {
    53.                         delete pReq;
    54.                         pReq = NULL;
    55.                 }
    56.         
    57.                 m_statuslist.erase(m_statuslist.begin());
    58.         }

    60.         if (m_directionlist.size()  > 0) {
    61.                 std::cout << "clear directionlist"<< std::endl;               
    62.         }

    64.         while (m_directionlist.size()  > 0)
    65.         {
    66.                 RequestBase* pReq = m_directionlist.front();
    67.                 if (NULL != pReq) {
    68.                         delete pReq;
    69.                         pReq = NULL;
    70.                 }
    71.         
    72.                 m_directionlist.erase(m_directionlist.begin());
    73.         }

    75.     pthread_mutex_unlock(&mutex);
    76. }
    复制代码
    动作执行模块用于调用python模块的接口，实现小车的前进、后退、左转、右转，停止动作。

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

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

    1. #include </usr/include/python2.7/Python.h>   
    2. #include <iostream>
    3. #include "switch.h"

    5. #define DEBUG_TEST 0

    7. void Forward()
    8. {
    9.     std::cout<<"###Forward###"<<std::endl;

    11. #if DEBUG_TEST
    12.     sleep(1);
    13.     return;
    14. #endif

    16.     //初始化python
    17.     Py_Initialize();

    19.     PyRun_SimpleString("import sys");
    20.     PyRun_SimpleString("sys.path.append('./')");

    22.         PyObject * pModule = NULL;
    23.     PyObject * pFunc = NULL;

    25.     //这里是要调用的文件名
    26.     pModule = PyImport_ImportModule("control");
    27.     if (!pModule)
    28.     {
    29.         std::cout<<"Call PyImport_ImportModule("control")  fail."<<std::endl;
    30.         return;
    31.     }

    33.     //这里是要调用的函数名
    34.     pFunc= PyObject_GetAttrString(pModule, "car_init");
    35.    
    36.     //调用函数
    37.     PyEval_CallObject(pFunc, NULL);
    38.     Py_DECREF(pFunc);  


    41.     //这里是要调用的函数名
    42.     pFunc= PyObject_GetAttrString(pModule, "car_forward");

    44.     //调用函数
    45.     PyEval_CallObject(pFunc, NULL);
    46.     Py_DECREF(pFunc);
    47. }

    49. void Back()
    50. {
    51.     std::cout<<"###Back###"<<std::endl;

    53. #if DEBUG_TEST
    54.     sleep(1);
    55.     return;
    56. #endif

    58.     //初始化python
    59.     Py_Initialize();

    61.     PyRun_SimpleString("import sys");
    62.     PyRun_SimpleString("sys.path.append('./')");

    64.         PyObject * pModule = NULL;
    65.     PyObject * pFunc = NULL;

    67.     //这里是要调用的文件名
    68.     pModule = PyImport_ImportModule("control");
    69.     if (!pModule)
    70.     {
    71.         std::cout<<"Call PyImport_ImportModule("control")  fail."<<std::endl;
    72.         return;
    73.     }

    75.     //这里是要调用的函数名
    76.     pFunc= PyObject_GetAttrString(pModule, "car_init");
    77.     //调用函数
    78.     PyEval_CallObject(pFunc, NULL);
    79.     Py_DECREF(pFunc);  

    81.     //这里是要调用的函数名
    82.     pFunc= PyObject_GetAttrString(pModule, "car_back");
    83.     //调用函数
    84.     PyEval_CallObject(pFunc, NULL);
    85.     Py_DECREF(pFunc);  
    86. }

    88. void Left()
    89. {
    90.     std::cout<<"###Left###"<<std::endl;

    92. #if DEBUG_TEST
    93.     sleep(1);
    94.     return;
    95. #endif

    97.     //初始化python
    98.     Py_Initialize();

    100.     PyRun_SimpleString("import sys");
    101.     PyRun_SimpleString("sys.path.append('./')");

    103.         PyObject * pModule = NULL;
    104.     PyObject * pFunc = NULL;

    106.     //这里是要调用的文件名
    107.     pModule = PyImport_ImportModule("control");
    108.     if (!pModule)
    109.     {
    110.         std::cout<<"Call PyImport_ImportModule("control")  fail."<<std::endl;
    111.         return;
    112.     }

    114.     //这里是要调用的函数名
    115.     pFunc= PyObject_GetAttrString(pModule, "car_init");
    116.     //调用函数
    117.     PyEval_CallObject(pFunc, NULL);
    118.     Py_DECREF(pFunc);  

    120.     //这里是要调用的函数名
    121.     pFunc= PyObject_GetAttrString(pModule, "car_left");
    122.     //调用函数
    123.     PyEval_CallObject(pFunc, NULL);
    124.     Py_DECREF(pFunc);  
    125. }

    127. void Right()
    128. {
    129.     std::cout<<"###Right###"<<std::endl;

    131. #if DEBUG_TEST
    132.     sleep(1);
    133.     return;
    134. #endif

    136.     //初始化python
    137.     Py_Initialize();

    139.     PyRun_SimpleString("import sys");
    140.     PyRun_SimpleString("sys.path.append('./')");

    142.         PyObject * pModule = NULL;
    143.     PyObject * pFunc = NULL;

    145.     //这里是要调用的文件名
    146.     pModule = PyImport_ImportModule("control");
    147.     if (!pModule)
    148.     {
    149.         std::cout<<"Call PyImport_ImportModule("control")  fail."<<std::endl;
    150.         return;
    151.     }

    153.     //这里是要调用的函数名
    154.     pFunc= PyObject_GetAttrString(pModule, "car_init");
    155.     //调用函数
    156.     PyEval_CallObject(pFunc, NULL);
    157.     Py_DECREF(pFunc);  

    159.     //这里是要调用的函数名
    160.     pFunc= PyObject_GetAttrString(pModule, "car_right");
    161.     //调用函数
    162.     PyEval_CallObject(pFunc, NULL);
    163.     Py_DECREF(pFunc);

    165. }

    167. void Stop()
    168. {
    169.     std::cout<<"###Stop###"<<std::endl;
    170.    
    171. #if DEBUG_TEST
    172.     sleep(1);
    173.     return;
    174. #endif

    176.     //初始化python
    177.     Py_Initialize();

    179.     PyRun_SimpleString("import sys");
    180.     PyRun_SimpleString("sys.path.append('./')");
    181.    
    182.         PyObject * pModule = NULL;
    183.     PyObject * pFunc = NULL;

    185.     //这里是要调用的文件名
    186.     pModule = PyImport_ImportModule("control");
    187.     if (!pModule)
    188.     {
    189.         std::cout<<"Call PyImport_ImportModule("control")  fail."<<std::endl;
    190.         return;
    191.     }
    192.    
    193.     //这里是要调用的函数名
    194.     pFunc= PyObject_GetAttrString(pModule, "car_init");
    195.     //调用函数
    196.     PyEval_CallObject(pFunc, NULL);
    197.     Py_DECREF(pFunc);  

    199.     //这里是要调用的函数名
    200.     pFunc= PyObject_GetAttrString(pModule, "car_stop");
    201.     //调用函数
    202.     PyEval_CallObject(pFunc, NULL);
    203.     Py_DECREF(pFunc);
    204. }
    复制代码
    到此整个小车控制系统就介绍完了。

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


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


    本文作者xutiejun，转载自freebuf

    UDP协议

    相关帖子

    下载资料失效了？点击此处告知管理员 > >
    回复

    使用道具 举报

    zhang013hao

    该用户从未签到
    发表于 2018-10-15 11:32:19 | 显示全部楼层
    学习学习学习学习学习学习学习学习学习学习学习学习
    回复 支持 反对

    使用道具 举报

    返回列表 发新帖
    高级模式
    B Color Image Link Quote Code Smilies
    您需要登录后才可以回帖 注册/登录

    本版积分规则

    关闭

    站长推荐上一条 /2 下一条



    手机版|小黑屋|与非网

    GMT+8, 2024-12-29 08:05 , Processed in 0.145096 second(s), 23 queries , MemCache On.

    ICP经营许可证 苏B2-20140176  苏ICP备14012660号-2   苏州灵动帧格网络科技有限公司 版权所有.

    苏公网安备 32059002001037号

    Powered by Discuz! X3.4

    Copyright © 2001-2024, Tencent Cloud.