3.人脸跟踪底层固件代码

0x00 固件代码介绍

这里制作的人脸跟踪底层硬件使用的控制板是arduino,所以这里的固件代码就都是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码表,帮助大家来复习一下:

ASCII码对照表
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(),舵机的转动全靠调用这个函数来执行的。下面来详细介绍这个函数,如下图所示:

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小课堂的微信群与更多志同道合的小伙伴一起学习和交流!(还有就是记得在打赏完后给我留言或者发条消息,否则我看到的打赏记录是不全的,可能会遗漏你的打赏哦!)

发表评论