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

控制小车的方法大体上有以下几种：

1.TCP/UDP控制；

2.网页控制；

3.蓝牙控制；

4.红外遥控器控制；

5.手柄控制。

本次我采用的是【1.TCP/UDP控制】方案。

需要的硬件如下：

1. win7笔记本

2. 树莓派小车

3. 无线wifi网络

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

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

0x01 控制协议

小车控制系统的通讯协议采用面向非连接的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

0x02 客户端设计

客户端界面如下所示。

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

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

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

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

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

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

 0x03 服务器端设计

 树莓派小车安装的是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);
}

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

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

链接: https://pan.baidu.com/s/1jOkGjmTOdWVfoa12ghKjdA 密码: jeyr

 *本文作者：xutiejun，转载请注明来自 FreeBuf.COM