3.人脸跟踪底层固件代码
corvin_zhang
0x00 固件代码介绍
这里制作的人脸跟踪底层硬件使用的控制板是arduino，所以这里的固件代码就都是arduino的代码了。使用arduino是因为现在arduino开发非常的简单，对于初次接触底层硬件程序开发的人来说代码很容易理解。
下面让我们先来回忆下在第一篇文章中提到的arduino代码的实现流程图，这样就方便理解后面提到的代码了，图如下所示：
0x01 arduino代码的主程序
首先来看arduino代码的主程序arduino_face_traker.ino，它就跟我们c程序中的main函数差不多，算是了解整个代码的入口：
/********************************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
使用dfrobot的arduino UNO开发板，再接扩展板。当然其他公司的UNO板也行，这里代码还
兼容其他型号的arduino板，包括arduino Mega2560, arduino DUE等。可以在相应版的扩展板
上接了两个舵机，这样就可以组成二自由度的云台。通过串口可以发送命令控制两个舵机的旋转，在
控制转动前需要先使能指定pwm引脚上的舵机，具体操作命令格式如下：
(0).使能pwm 2号引脚上的舵机,该引脚舵机索引值为0,因为为了兼容UNO,Mega2560,DUE板，就
把所有pwm引脚全都包含了，Mega2560和DUE都有12个PWM引脚，分别为2,3,4,5,6,7,8,9,10,
11,12,13，这样2号引脚上的舵机索引就是0。第2个参数表明使能（非0）或禁用（0）：
e 0 1
(1).控制云台旋转，舵机索引分别为为0（左右旋转），1（上下旋转）:
w 0 90 20:该串口命令中w表示写入，0表示0号舵机，90表示旋转到90度,每次移动延时20ms
w 1 30 50:表示1号舵机移动到30度,每个移动周期延时50ms.
(2).读取指定舵机ID的当前角度：
r 0:串口输入该命令，则返回90,表示0号舵机当前在90度位置.
r 1:输入该命令后，返回30,表示1号舵机当前在30度位置。
(3).获取所有舵机的使能状态命令s:
s:0 0 0 1 0 0 0 0 0 0 0 0：返回的一串数字表明第几个舵机使能。
(4).获取所有舵机的角度p:
p: 0 0 0 90 0 0 0 0 0 0 0 0:第4个舵机当前在90度位置。
History:
20181121: initial this code.
20181128: add delay time param when set servo target position.
20181130: 新增命令v可以获取代码版本号,命令e,d用来启用舵机或禁用指定pwm引脚上的舵机。
在使用w命令控制舵机移动前，需要使用e先使能该舵机才可以。
20181203: 删除d命令-禁用指定舵机,使用e命令的第2个参数来表示使能或禁用。第2个参数为0,
表示禁用,非零以外值都表示使能。
20181204：新增获取所有舵机当前使能状态命令s和获取当前所有舵机的角度p,返回内容之间有空格。
20181207: 将存储串口数据的一些全局零散变量都用一个类serialData来实现了,这样代码更清晰.
********************************************************************************/
#define FIRMWARE_VERSION 1
#define CONNECT_BAUDRATE 57600
#include <Servo.h>
#include "serialData.h"
#include "commands.h"
#include "servos.h"
/* Run a command. Commands char are defined in commands.h */
void runCommand(void)
{
serialObj.arg1 = atoi(serialObj.argv1);
serialObj.arg2 = atoi(serialObj.argv2);
serialObj.arg3 = atoi(serialObj.argv3);
switch (serialObj.cmd_chr)
{
case GET_CONNECT_BAUDRATE: // 'b'
Serial.println(CONNECT_BAUDRATE);
break;
case SET_ONE_SERVO_ENABLE: // 'e'
myServos[serialObj.arg1].setEnable(serialObj.arg2);
Serial.println("OK");
break;
case SET_ONE_SERVO_POS: // 'w'
myServos[serialObj.arg1].setTargetPos(serialObj.arg2, serialObj.arg3);
Serial.println("OK");
break;
case GET_ONE_SERVO_POS: // 'r'
Serial.println(myServos[serialObj.arg1].getServoObj().read());
break;
case GET_ALL_SERVOS_POS: // 'p'
for (byte i = 0; i < N_SERVOS; i++)
{
Serial.print(myServos[i].getCurrentPos());
Serial.print(' ');
}
Serial.println("");
break;
case GET_ALL_SERVOS_ENABLE: // 's'
for (byte i = 0; i < N_SERVOS; i++)
{
Serial.print(myServos[i].isEnabled());
Serial.print(' ');
}
Serial.println("");
break;
case GET_FIRMWARE_VERSION: // 'v'
Serial.println(FIRMWARE_VERSION);
break;
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
void setup()
{
Serial.begin(CONNECT_BAUDRATE);
serialObj.resetCmdParam();
/* when power on init all servos position */
for (byte i = 0; i < N_SERVOS; i++)
{
myServos[i].initServo(myServoPins[i], servoInitPosition[i], 0);
}
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands.
*/
void loop()
{
while (Serial.available() > 0)
{
char tmp_chr = Serial.read(); // Read the next character
if (tmp_chr == 13) // Terminate a command with a CR
{
runCommand();
serialObj.resetCmdParam();
}
else if (tmp_chr == ' ') // Use spaces to delimit parts of the command
{
// Step through the arguments
if (serialObj.argCnt == 0)
{
serialObj.argCnt++;
}
else if (serialObj.argCnt == 1)
{
serialObj.argCnt++;
serialObj.argIndex = 0;
}
else if (serialObj.argCnt == 2)
{
serialObj.argCnt++;
serialObj.argIndex = 0;
}
}
else // get valid param
{
if (serialObj.argCnt == 0) // The first arg is the single-letter command
{
serialObj.cmd_chr = tmp_chr;
}
else if (serialObj.argCnt == 1) // Get after cmd first param
{
serialObj.argv1[serialObj.argIndex] = tmp_chr;
serialObj.argIndex++;
}
else if (serialObj.argCnt == 2)
{
serialObj.argv2[serialObj.argIndex] = tmp_chr;
serialObj.argIndex++;
}
else if (serialObj.argCnt == 3)
{
serialObj.argv3[serialObj.argIndex] = tmp_chr;
serialObj.argIndex++;
}
}
} //end while
// Check everyone servos isEnabled, when true will move servo. Other don't move servo.
for (byte i = 0; i < N_SERVOS; i++)
{
if (myServos[i].isEnabled())
{
myServos[i].moveServo();
}
}
}//end loop下面我们对这部分代码进行简要的解析，帮组大家理解这部分代码的功能。阅读arduino代码，首先要从setup()函数入手，因为这是代码的入口函数，就跟我们最开始学习C代码时候的main()函数一样。这里我们再来看看setup()函数如何编写的：
/* Setup function--runs once at startup. */
void setup()
{
Serial.begin(CONNECT_BAUDRATE);
serialObj.resetCmdParam();
/* when power on init all servos position */
for (byte i = 0; i < N_SERVOS; i++)
{
myServos[i].initServo(myServoPins[i], servoInitPosition[i], 0);
}
}第一行的Serial.begin就是用来配置与arduino连接的串口波特率的，只有配置好该参数，后面使用USB线与上位机主控板连接后才能正常通信。下面的serialObj.resetCmdParam()是为了初始化下自定义的串口接收数据的各参数，我这里使用一个自定义的类来保存从上位机发送过来的串口数据，方便在这里进行命令解析和执行。最后的for循环是为了初始化连接的各舵机，包括初始化舵机连接引脚，初始角度等。这些函数会在后面的源码文件中详细介绍。
接下来分析loop()函数，该函数是一个不断循环执行的函数。在这里实现的功能是通过Serial.available()来判读串口缓冲区中是否接收到数据，如果有数据的话就一个字符一个字符的将其读出。接下来就是来判读字符了（ASCII码），如果是13表明就是“回车符”，说明一个命令已经读取完毕，接下来就是可以来执行该命令了（调用runCommand()函数来执行命令）。下图是一个完整的ASCII码表，帮助大家来复习一下：
当判断读取到的字符是空格时，表明这是命令的一个参数结束了，后续还有参数。举个串口读取命令的例子大家可能就更容易理解了，例如要想控制第0个舵机转到90度，那么就需要上位机向arduino通过串口发送"w 0 90"这样的命令即可。这个字母'w'就是命令符，这是用来通知下位机执行什么样操作。在'w'字符后就有个空格，表明命令还没有结束，后续还有参数需要读取。紧接着读取到'0'字符，表明要控制0号舵机转动。后面又有一个空格，表明命令还没有结束，需要继续读取参数。紧接着读取到'9'这个字符，这里就用到最后一个else分支里的代码了，表明后面的内容跟当前的字符是一个参数，需要存储在一起。然后就读取到'0'这个字符，就会将"90"这两个字符存储在一起，作为一个整体参数来操作了。如下图所示的详细代码解释：
下面来介绍判断舵机是否需要转到的代码，这里使用for循环来依次判断每个舵机。但是舵机进行转动的前提是必须要先使能该舵机，否则就算设置了转动的角度，舵机也不会转动的。代码如下：
for (byte i = 0; i < N_SERVOS; i++)
{
if (myServos[i].isEnabled())
{
myServos[i].moveServo();
}
}最后来介绍下这个runCommand()函数，它就是将loop()函数中读取串口数据并保存到serialObj对象中的命令执行一下。根据serialObj.cmd_chr来区分执行什么样操作，是为了使能舵机还是为了控制舵机转动，还是要想读取舵机当前的角度。
0x02 arduino中串口命令定义
这里的代码文件名为commands.h，它是一个头文件。主要就是定义串口接收的命令字符，算是上位机和下位机的通信协议差不多，发送什么样的命令字符，下位机就执行什么样的操作。具体代码如下：
/**********************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
Define single-letter commands that will be sent by the PC over the
serial link.
History:
20181128: initial this comment.
20181130: 新增了d,e命令,分别可以禁用、使能指定舵机，v获取代码版本号。
20181203: 删除了d命令,使用e命令的第2个参数来表示使能或禁用指定舵机。
20181204: 新增用户获取所有舵机使能状态命令s和所有舵机当前角度p。
*/
#ifndef _COMMANDS_H_
#define _COMMANDS_H_
#define GET_CONNECT_BAUDRATE 'b'
#define SET_ONE_SERVO_ENABLE 'e'
#define SET_ONE_SERVO_POS 'w'
#define GET_ONE_SERVO_POS 'r'
#define GET_ALL_SERVOS_POS 'p'
#define GET_ALL_SERVOS_ENABLE 's'
#define GET_FIRMWARE_VERSION 'v'
#endif这里代码就很简单了，都是一些宏定义。通过宏定义名字就可以得知对应字符要执行什么样操作了，我们就是通过判断读取串口中的命令字符与其中的哪个相符，就执行对应的操作。所以这里的命令字符定义在上位机代码编写中也是要用一样的，不然这里的arduino代码会提示命令错误。
0x03 arduino中存储串口数据的类
首先来看一下serialData.h这个头文件的定义，这里定义了存储串口数据的类，并声明一个对象。下面来看一下完整代码：
/***************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
用于从arduino的串口获得命令的类，包含各种成员变量和函数.
History:
20181207: initial this file.
*************************************************************/
#ifndef _SERIALDATA_H_
#define _SERIALDATA_H_
#define ENTER_CHAR '\r'
class serialData
{
public:
void resetCmdParam();
// A pair of varibles to help parse serial commands
byte argCnt;
byte argIndex;
// Variable to hold the current single-character command
char cmd_chr;
// Character arrays to hold the first, second and third arguments
char argv1[4];
char argv2[4];
char argv3[4];
// The arguments converted to integers
int arg1;
int arg2;
int arg3;
};
serialData serialObj;
#endif这里需要注意的就是arv1-3这三个char数组，为什么数组长度是4呢？这是因为每个参数最多为3位数，最后一位是换行符表明这个参数结束了。然后使用atoi()函数将字符转换为整型，并将其存储到arg1-3中。
接着来看下这个resetCmdParam()函数如何实现的吧，这在serialData.ino代码中实现的，代码如下：
/***************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
用于arduino的串口获得命令类中函数的实现.
History:
20181207: initial this file.
*************************************************************/
void serialData::resetCmdParam()
{
this->cmd_chr = ENTER_CHAR;
memset(this->argv1, ENTER_CHAR, sizeof(this->argv1));
memset(this->argv2, ENTER_CHAR, sizeof(this->argv2));
memset(this->argv3, ENTER_CHAR, sizeof(this->argv3));
this->argCnt = 0;
this->argIndex = 0;
}这里的resetCmdParam()函数其实主要用来初始化一下各成员变量，尤其是要将存储命令参数的缓冲区数组给初始化好。
0x04 控制舵机转动的头文件
这里的头文件名为servos.h，该头文件主要就是定义舵机的连接引脚，初始角度和类的定义，具体代码如下：
/***************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
定义舵机操作的类SweepServo,舵机连接的引脚和初始的舵机角度.在这里连接
的两个舵机分别连接5,6两个引脚,5号引脚连接左右旋转的舵机,6号引脚连接
上下移动的舵机.其中设置左右旋转的舵机初始角度为90度,上下转动舵机的初始
角度为0度.在代码里总共可以连接12个舵机,当然你的舵机可以根据arduino板
的不同来连接不同的引脚,同时修改引脚的初始角度即可.
History:
20181128: initial this comment.
20181130: 新增了舵机使能状态的标识，而且最多可接12个舵机。
20181204: 新增getCurrentPos()函数用户获取当前舵机的角度。
*************************************************************/
#ifndef _SERVOS_H_
#define _SERVOS_H_
#define N_SERVOS 12
#define SERVO_ENABLE 1
#define SERVO_DISABLE 0
//Define All Servos's Pins
byte myServoPins[N_SERVOS] = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13};
// Initial Servo Position [0, 180] degrees
int servoInitPosition[N_SERVOS] = {0, 0, 0, 90, 0, 0, 0, 0, 0, 0, 0, 0};
class SweepServo
{
public:
SweepServo();
void initServo(int servoPin, unsigned int initPosition, unsigned int stepDelayMs);
void setTargetPos(unsigned int targetPos, unsigned int stepDelayMs);
int getCurrentPos(void);
void setEnable(byte flag);
void moveServo(void);
byte isEnabled(void);
Servo getServoObj();
private:
Servo servo;
unsigned int stepDelayMs;
unsigned long lastMoveTime;
int currentPosDegrees;
int targetPosDegrees;
byte enabled;
};
SweepServo myServos[N_SERVOS];
#endif通过上述代码最开头的注释也可以得知该头文件的大概用途了，这里再作简单解释。这里定义了12个舵机，其实主要是为了方便大家可以接不同的引脚。其实这里的人脸跟踪我们只需要两个舵机，生成两个自由度即可。剩下的就是SweepServo类的声明了，包含公有函数和私有函数。
0x05 控制舵机转动的代码实现
这里的代码文件是servos.ino，下面先来看完整的代码，后面再进行代码的简要解析说明：
/**********************************************************************
Copyright: 2016-2019 ROS小课堂 www.corvin.cn
Author: corvin
Description:
Sweep servos one degree step at a time with a user defined
delay in between steps.
History:
20181121: initial this code.
20181130: 新增获取和设置舵机使能状态的函数。
20181204: 删除了舵机的disable函数,使用enable来实现。新增了getCurrentPos()
函数,用户获取当前舵机的角度。
**********************************************************************/
// Constructor function
SweepServo::SweepServo()
{
this->currentPosDegrees = 0;
this->targetPosDegrees = 0;
this->lastMoveTime = 0;
}
// Init servo params, default all servos disabled.
void SweepServo::initServo(int servoPin, unsigned int initPosition, unsigned int stepDelayMs)
{
this->servo.attach(servoPin);
this->stepDelayMs = stepDelayMs;
this->currentPosDegrees = initPosition;
this->targetPosDegrees = initPosition;
this->lastMoveTime = millis();
this->enabled = SERVO_DISABLE;
this->servo.write(initPosition); //when power on, move all servos to initPosition
}
//Servo Perform Sweep
void SweepServo::moveServo(void)
{
// Get ellapsed time from last cmd time to now.
unsigned int delta = millis() - this->lastMoveTime;
// Check if time for a step
if (delta > this->stepDelayMs)
{
// Check step direction
if (this->targetPosDegrees > this->currentPosDegrees)
{
this->currentPosDegrees++;
this->servo.write(this->currentPosDegrees);
}
else if (this->targetPosDegrees < this->currentPosDegrees)
{
this->currentPosDegrees--;
this->servo.write(this->currentPosDegrees);
}
// if target == current position, do nothing
// reset timer, save current time to last cmd time.
this->lastMoveTime = millis();
}
}
// Set a new target position with step delay param.
void SweepServo::setTargetPos(unsigned int targetPos, unsigned int stepDelayMs)
{
this->targetPosDegrees = targetPos;
this->stepDelayMs = stepDelayMs;
}
int SweepServo::getCurrentPos(void)
{
return this->currentPosDegrees;
}
void SweepServo::setEnable(byte flag)
{
if (flag == SERVO_ENABLE)
{
this->enabled = SERVO_ENABLE;
}
else
{
this->enabled = SERVO_DISABLE;
}
}
byte SweepServo::isEnabled(void)
{
return this->enabled;
}
// Accessor for servo object
Servo SweepServo::getServoObj()
{
return this->servo;
}下面来对代码进行简要的解析说明，第一个函数就是SweepServo::SweepServo()，它是类的构造函数。就是在新建类的对象时自动执行的函数，一般都是一些初始化变量的操作。接下来就是初始化舵机的函数initServo(),就是为了给类的对象中各成员变量赋值。接下来介绍这个重要的函数moveServo(),舵机的转动全靠调用这个函数来执行的。下面来详细介绍这个函数，如下图所示：
剩下的函数都比较简单了，下面的setTargetPos()就是为了设置要转动的目的角度，这里有两个参数：一个就是转动的目的角度，另外一个就是转动的延时参数，该参数越大转动时速度越慢，如果设置为0，那么就是以最快的速度转动到指定角度。剩下的函数通过函数名就可以理解函数的功能了，例如getCurrentPos()为了得到舵机当前转动到的角度。setEnable()函数就是为了使能指定的舵机，isEnable()函数为了查看舵机的使能标志。getServoObj()是为了得到当前初始化的舵机对象。
到这里基本上所有的底层固件代码都介绍完了，我们会在下一篇教程中给大家演示如何来调试这些底层代码，保证每个函数都可以正常工作。它可以接收串口发送过来的命令并正确执行即可。
0x06 References
[1].corvin_zhang. 1.人脸跟踪项目概述. http://www.corvin.cn/1112.html
[2]. ros_arduion_bridge. github主页地址. https://github.com/hbrobotics/ros_arduino_bridge/tree/kinetic-devel
0x07 Feedback
大家在按照教程操作过程中有任何问题，可以关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题即可，我基本上每天都会处理公众号中的留言！当然，如果你要是顺便给ROS小课堂打个赏，我更是万分感谢，如果打赏30块还会邀请进ROS小课堂的微信群与更多志同道合的小伙伴一起学习和交流！（还有就是记得在打赏完后给我留言或者发条消息，否则我看到的打赏记录是不全的，可能会遗漏你的打赏哦！）
