智能追踪补光灯——毫米波雷达的简单应用

merlin

智能追踪补光灯——毫米波雷达的简单应用

前言

新的CH549进阶项目来啦！这次带来的教程项目名是智能追踪补光灯，如上图所示，这次使用到了新的硬件：毫米波雷达；这次将用CH549简单驱动毫米波雷达实现对人脸的追踪捕捉补光。和往常一样，此次依然作为51的进阶练习，基础薄弱的可以看verimake的51相关教程《以 CH549 为例的 51 教程》。这次的项目里面将用到42步进电机，步进电机驱动器A4988，带传动减速器，毫米波雷达，OLED屏。

部件简介

42步进电机

本次项目中的驱动电机，通过接下来要介绍的步进电机驱动器驱动，本身精度较好，可以完成本次项目要求。

步进电机驱动器A4988

基本介绍：

绕组
常用的步进电机有四根线，1A、1B、2A、2B，1A和1B是一个绕组，2A和2B是一个绕组，用万用表测试1A和1B之间是短路的，2A和2B之间是短路的，1A和1B，2A和2B是等效的。
通常状况下，步进电机可以自由转动（用手可以拧动），1A和1B接在一起的时候，用手拧会感到明显阻力，1A和1B，2A和2B分别接在一起，则阻力更大。
步距角
所谓步进电机，就是可以一步一步进动的电机，每一步旋转的角度就是步距角。常用电机步距角1.8°的较多，也就是每次步进1.8°，旋转一圈需要200步，也说这个步进电机的分辨率是200步。
细分
细分的意义就是提高步进电机分辨率，如果没有细分的话，步进电机每次步进的角度就是步距角，比如1.8°，有了细分，比如16细分，就是把1.8°平均分16份，那么电机的分辨率就变成200*16=3200步了，也就是旋转一圈需要3200步。
其作用简而言之就是降低速度，即每次运行的更慢，保证转动的顺滑度，加速器的作用也是如此。
电流
电流越大，电机扭矩越大。

带传动减速器

减速器模型图

减速器实拍
由图中可以看出这个加速器是带传动，传动比1：5，整体设计固定在三脚架上，由于步进电机是倒着挂在减速器带轮上，所以实际转向和电机转向相反。

毫米波雷达

接下来是我们这次项目的重点毫米波雷达。

毫米波雷达，是工作在毫米波波段（millimeter wave）探测的雷达。通常毫米波是指30～300GHz频域(波长为1～10mm)的。毫米波的波长介于微波和厘米波之间，因此毫米波雷达兼有微波雷达和光电雷达的一些优点。
本次项目使用的毫米波雷达是单目标雷达，对动态目标反应明显，上图为该毫米波雷达的正面视图，上面红线是VCC，黑线是GND，蓝线是TX，棕线是RX。

引脚接线

该部分将分为三个部分：

OLED部分

PS：本次项目中OLED的作用仅为将数据显示出来，方便调整；但是由于引脚冲突原因，本次例程中的OLED的接法与之前的接法有所不同。

OLED	CH549	功能
CS	P3.0	片选
RST	P3.1	复位
DC	P3.3	数据/命令控制
D0	P1.7	SCLK时钟
D1	P1.5	MOSI数据

A4988电机驱动器

A4988	CH549	功能
E(ENABLE)	GND	使能，接低电平则模块开始工作，接高电平则模块关机。
D(DIR)	P0.6	方向控制，低电平正转，高电平反转。
S(STEP)	P0.7	脉冲输入，往这个脚输入一个方波，电机转动一步，也就是(1.8/16)°（以1.8°电机，16细分为例），往这个脚持续输入方波，则电机持续转动。
5V	+5V	接CH549正电压
GND	GND	接地

本次的驱动器需要9V的电源，CH549并不能满足要求，所以我们采用外接电源：

A4988电机驱动器	A4988	外接电源12V	功能
	9V	+	接外接电源正电压
	GND	-	接地

其中关于细分的开启，就A4988来说是通过MS1、MS2、MS3高低电平设置细分，在这个板子上有如图所示电位器更方便的改变。

其中具体细分见下表：

1(MS1)	2(MS2)	3(MS3)	细分
Low	Low	Low	1
High	Low	Low	1/2
Low	High	Low	1/4
High	High	Low	1/8
High	High	High	1/16

PS：本次使用的是1/8。

毫米波雷达

毫米波雷达	雷达	CH549	功能
	TX	P2.6	雷达的TX接CH549中UART1的RX引脚
	RX	P2.7	雷达的RX接CH549中UART1的TX引脚

PS：注意TX对应RX接。

具体程序

PS：本次项目中用到的库，基本之前的教程中均有涉及，基本没变，仅对CH549_SPI.h中的引脚设定做了些许调整，不再赘述，直接说明main.c。

#include <CH549_OLED.h>    
#include <CH549_UART.h> 
#include <CH549_SPI.h> 
#include <CH549_ADC.h>
#include <CH549_GPIO.h>
#include <CH549_DEBUG.h>
#include <CH549_sdcc.h>

#define Direction P0_6         //旋转方向
#define Steps P0_7             //转角步长
#define Clockwise 1            //顺时针
#define Counterclockwise 0     //逆时针

int oled_colum;
int oled_row;
void setCursor(int column,int row);       //声明函数


void UART1Init(UINT32 baudrate,UINT8 interrupt)
{
    SCON1 &= ~bU1SM0;                    //选择8位数据通讯
    SCON1 |= bU1SMOD;                    //快速模式
    SCON1 |= bU1REN;                     //使能接收
    SBAUD1 = 0 - FREQ_SYS/16/baudrate; //波特率配置
    SIF1 = bU1TI;                        //清空发送完成标志
	if(interrupt){                   //开启中断使能
		IE_UART1 = 1;
		EA = 1;
	}
}

typedef struct rada
{
	int x1Pos;
	int y1Pos;
	int dist1;
	int ang1;
	int x2Pos;
	int y2Pos;
	int dist2;
	int ang2;
	int x3Pos;
	int y3Pos;
	int dist3;
	int ang3;
	UINT8 checksum;
} RadarData;

//解析 XenP201S 的数据
UINT8 decode_XenP201S( char* buf,RadarData* radar){
	radar->x1Pos = (((int)buf[2]<<8)+buf[1]);
	radar->y1Pos = (((int)buf[4]<<8)+buf[3]);
	radar->dist1 = (((int)buf[6]<<8)+buf[5]);
	radar->ang1 = (((int)buf[8]<<8)+buf[7]);
	radar->ang1 = radar->ang1 / 10.0 - 90;

	radar->x2Pos = (((int)buf[10]<<8)+buf[9]);
	radar->y2Pos = (((int)buf[12]<<8)+buf[11]);
	radar->dist2 = (((int)buf[14]<<8)+buf[13]);
	radar->ang2 = (((int)buf[16]<<8)+buf[15]);
	radar->ang2 = radar->ang2 / 10.0 - 90;

	radar->x3Pos = (((int)buf[18]<<8)+buf[17]);
	radar->y3Pos = (((int)buf[20]<<8)+buf[19]);
	radar->dist3 = (((int)buf[22]<<8)+buf[21]);
	radar->ang3 = (((int)buf[24]<<8)+buf[23]);
	radar->ang3 = radar->ang3 / 10.0 - 90;

	return 0;
}

void LightTurn(BOOL Dir, UINT16 Deg)          //补光灯旋转
{
	GPIO_Init(PORT0,PIN6,MODE1);
	UINT16 STEP;
	STEP = Deg * 5 * 8 / 1.8;       //实际步长为角度/1.8，开的1/8档位，并且1:5减速
	Direction = Dir;
	setCursor(0,4);
	printf_fast_f("D:%d S:%3d G:%2d",Dir,STEP,Deg);
	for (UINT16 i = 0; i < STEP; i++)       //根据1s内调整旋转时间延迟
	{
		Steps = 1;
		mDelayuS(1000000/STEP);
		Steps = 0;
		mDelayuS(1000000/STEP);
	}
	
}

void FaceTrack( int A)           //捕获动作
{
	if ( A < 85)              //由于数据每1s都会不断跳动，保证在一定范围内固定补光
	{
		LightTurn(Counterclockwise, 1+(90-A)*5/9);
	}
	if ( A > 95)
	{
		LightTurn(Clockwise, 1+(A-90)*5/9);
	}	

}

volatile UINT8 recvBuf[30];
volatile UINT8 pBuf = 0;
volatile UINT8 fRecv = 0;

int xPos,yPos;
void main()
{
    CfgFsys( );           //CH549时钟选择配置
    mDelaymS(20);
	UART1Init(115200,1);
	// GPIO_Init(PORT0,PIN6,MODE1);
	
	SPIMasterModeSet(3);  //SPI主机模式设置，模式3
    SPI_CK_SET(4);       //设置SPI sclk 时钟信号分频
	OLED_Init();		  //初始化OLED  
	OLED_Clear();         //将OLED屏幕上内容清除
	setFontSize(8);      //设置文字大小

	OLED_Clear();
	setCursor(0,0);
	printf_fast_f("Radar");
	RadarData object;
	while(1){
		if(fRecv){
			fRecv = 0;		
			if (pBuf == 28){
				pBuf = 0;
				IE_UART1 = 0;
				setCursor(0,3);
				if(recvBuf[0]==0XAA && recvBuf[26]==0X55 && recvBuf[27]==0XCC){
//					RadarData object;
					decode_XenP201S(recvBuf,&object);										
					setCursor(0,0);
					printf_fast_f("X :%5d%5d%5d",object.x1Pos,object.x2Pos,object.x3Pos);
					setCursor(0,1);
					printf_fast_f("Y :%5d%5d%5d",object.y1Pos,object.y2Pos,object.y3Pos);
					setCursor(0,2);
					printf_fast_f("D :%5d%5d%5d",object.dist1,object.dist2,object.dist3);
					setCursor(0,3);
					printf_fast_f("A :%5d%5d%5d",object.ang1,object.ang2,object.ang3);
//					FaceTrack(object.ang1);
				}
				else{
					setCursor(0,0);
					printf_fast_f(" %x %x %x ",recvBuf[0],recvBuf[26],recvBuf[27]);
				}
				IE_UART1 = 1;
				FaceTrack(object.ang1);
			}
			if(recvBuf[0]!=0XAA){
				P2_2 = 0;
				pBuf=0;
			}else{
				P2_2 =1;
			}
		}
	}
}


/********************************************************************
* 函 数 名       : putchar
* 函数功能       : 将printf映射到OLED屏幕输出上
* 输    入      : 字符串
* 输    出    	: 字符串
********************************************************************/
int putchar( int a)
{      
    //在光标处显示文字 a
    OLED_ShowChar(oled_colum,oled_row,a);
    //将光标右移一个字的宽度,以显示下一个字
    oled_colum+=6;
    
    /*当此行不足以再显示一个字时，换行.
    同时光标回到最边(列坐标=0).
    */
    if (oled_colum>122){oled_colum=0;oled_row+=1;}
    return(a);
}

/********************************************************************
* 函 数 名       : setCursor
* 函数功能		   : 设置光标（printf到屏幕上的字符串起始位置）
* 输    入       : 行坐标 列坐标(此处一行为8个像素，一列为1个像素,所以屏幕上共有8行128列)
* 输    出    	 : 无
********************************************************************/
void setCursor(int column,int row)
{
    oled_colum = column;
    oled_row = row;
}

void UART1Interrupt(void) __interrupt INT_NO_UART1 __using 1 {
	pBuf%=28;
	recvBuf[pBuf] = CH549UART1RcvByte();
	pBuf++;
	fRecv = 1;
}

分部说明

毫米波雷达数据读取

typedef struct rada
{
	int x1Pos;
	int y1Pos;
	int dist1;
	int ang1;
	int x2Pos;
	int y2Pos;
	int dist2;
	int ang2;
	int x3Pos;
	int y3Pos;
	int dist3;
	int ang3;
	UINT8 checksum;
} RadarData;

//解析 XenP201S 的数据
UINT8 decode_XenP201S( char* buf,RadarData* radar){
	radar->x1Pos = (((int)buf[2]<<8)+buf[1]);
	radar->y1Pos = (((int)buf[4]<<8)+buf[3]);
	radar->dist1 = (((int)buf[6]<<8)+buf[5]);
	radar->ang1 = (((int)buf[8]<<8)+buf[7]);
	radar->ang1 = radar->ang1 / 10.0 - 90;

	radar->x2Pos = (((int)buf[10]<<8)+buf[9]);
	radar->y2Pos = (((int)buf[12]<<8)+buf[11]);
	radar->dist2 = (((int)buf[14]<<8)+buf[13]);
	radar->ang2 = (((int)buf[16]<<8)+buf[15]);
	radar->ang2 = radar->ang2 / 10.0 - 90;

	radar->x3Pos = (((int)buf[18]<<8)+buf[17]);
	radar->y3Pos = (((int)buf[20]<<8)+buf[19]);
	radar->dist3 = (((int)buf[22]<<8)+buf[21]);
	radar->ang3 = (((int)buf[24]<<8)+buf[23]);
	radar->ang3 = radar->ang3 / 10.0 - 90;

	return 0;
}

通信参数

串口通信波特率数据格式每帧数据发送时间间隔

UART1 115200 8n1 1000ms
雷达结构体

这部分即雷达读取距离与角度的结构体，需要注意的是就是其波特率和每1s探测一次数据，就探测速度来说不算快，但是对于本次项目的追踪补光灯来说足够了。

电机驱动与人脸捕获追踪

void LightTurn(BOOL Dir, UINT16 Deg)          //补光灯旋转
{
	GPIO_Init(PORT0,PIN6,MODE1);
	UINT16 STEP;
	STEP = Deg * 5 * 8 / 1.8;       //实际步长为角度/1.8，开的1/8档位，并且1:5减速
	Direction = Dir;
	setCursor(0,4);
	printf_fast_f("D:%d S:%3d G:%2d",Dir,STEP,Deg);
	for (UINT16 i = 0; i < STEP; i++)       //根据1s内调整旋转时间延迟
	{
		Steps = 1;
		mDelayuS(1000000/STEP);
		Steps = 0;
		mDelayuS(1000000/STEP);
	}
	
}

void FaceTrack( int A)           //捕获动作
{
	if ( A < 85)              //由于数据每1s都会不断跳动，保证在一定范围内固定补光
	{
		LightTurn(Counterclockwise, 1+(90-A)*5/9);
	}
	if ( A > 95)
	{
		LightTurn(Clockwise, 1+(A-90)*5/9);
	}	

}

这部分基本按照我之前所介绍的：

首先将P0.6设定为推挽输出，这样方便用0和1按位控制电机正反转，即控制补光灯的旋转方向；
由于本身函数输入的是角度，所以转换为步长，为了精确度先乘再除，即1：5减速和1/8细分，然后每步1.8°；
然后由于本身雷达检测是1s一次，那么设计补光灯的旋转速度也得按着这个来，这样才能让补光灯的旋转更加顺滑，而不是经常出现卡顿，也可以随着观测个体的移动距离调整旋转速度，即1000000us去除实际步长，而先给P0.7传先高后低的电压，让他按这个速度旋转；
本身雷达是1s测一次数据，每次数据都会有一定的变化，即使被捕获目标不动，也会产生略微的变化，所以另一个函数规定在一定角度范围内补光灯不再旋转，旋转的角度是自己测试后写成的表达式。

串口1中断

int oled_colum;
int oled_row;
void setCursor(int column,int row);       //声明函数


void UART1Init(UINT32 baudrate,UINT8 interrupt)
{
    SCON1 &= ~bU1SM0;                    //选择8位数据通讯
    SCON1 |= bU1SMOD;                    //快速模式
    SCON1 |= bU1REN;                     //使能接收
    SBAUD1 = 0 - FREQ_SYS/16/baudrate; //波特率配置
    SIF1 = bU1TI;                        //清空发送完成标志
	if(interrupt){                   //开启中断使能
		IE_UART1 = 1;
		EA = 1;
	}
}

volatile UINT8 recvBuf[30];
volatile UINT8 pBuf = 0;
volatile UINT8 fRecv = 0;

int xPos,yPos;
void main()
{
    CfgFsys( );           //CH549时钟选择配置
    mDelaymS(20);
	UART1Init(115200,1);
	// GPIO_Init(PORT0,PIN6,MODE1);
	
	SPIMasterModeSet(3);  //SPI主机模式设置，模式3
    SPI_CK_SET(4);       //设置SPI sclk 时钟信号分频
	OLED_Init();		  //初始化OLED  
	OLED_Clear();         //将OLED屏幕上内容清除
	setFontSize(8);      //设置文字大小

	OLED_Clear();
	setCursor(0,0);
	printf_fast_f("Radar");
	RadarData object;
	while(1){
		if(fRecv){
			fRecv = 0;		
			if (pBuf == 28){
				pBuf = 0;
				IE_UART1 = 0;
				setCursor(0,3);
				if(recvBuf[0]==0XAA && recvBuf[26]==0X55 && recvBuf[27]==0XCC){
//					RadarData object;
					decode_XenP201S(recvBuf,&object);										
					setCursor(0,0);
					printf_fast_f("X :%5d%5d%5d",object.x1Pos,object.x2Pos,object.x3Pos);
					setCursor(0,1);
					printf_fast_f("Y :%5d%5d%5d",object.y1Pos,object.y2Pos,object.y3Pos);
					setCursor(0,2);
					printf_fast_f("D :%5d%5d%5d",object.dist1,object.dist2,object.dist3);
					setCursor(0,3);
					printf_fast_f("A :%5d%5d%5d",object.ang1,object.ang2,object.ang3);
//					FaceTrack(object.ang1);
				}
				else{
					setCursor(0,0);
					printf_fast_f(" %x %x %x ",recvBuf[0],recvBuf[26],recvBuf[27]);
				}
				IE_UART1 = 1;
				FaceTrack(object.ang1);
			}
			if(recvBuf[0]!=0XAA){
				P2_2 = 0;
				pBuf=0;
			}else{
				P2_2 =1;
			}
		}
	}
}


/********************************************************************
* 函 数 名       : putchar
* 函数功能       : 将printf映射到OLED屏幕输出上
* 输    入      : 字符串
* 输    出    	: 字符串
********************************************************************/
int putchar( int a)
{      
    //在光标处显示文字 a
    OLED_ShowChar(oled_colum,oled_row,a);
    //将光标右移一个字的宽度,以显示下一个字
    oled_colum+=6;
    
    /*当此行不足以再显示一个字时，换行.
    同时光标回到最边(列坐标=0).
    */
    if (oled_colum>122){oled_colum=0;oled_row+=1;}
    return(a);
}

/********************************************************************
* 函 数 名       : setCursor
* 函数功能		   : 设置光标（printf到屏幕上的字符串起始位置）
* 输    入       : 行坐标 列坐标(此处一行为8个像素，一列为1个像素,所以屏幕上共有8行128列)
* 输    出    	 : 无
********************************************************************/
void setCursor(int column,int row)
{
    oled_colum = column;
    oled_row = row;
}

void UART1Interrupt(void) __interrupt INT_NO_UART1 __using 1 {
	pBuf%=28;
	recvBuf[pBuf] = CH549UART1RcvByte();
	pBuf++;
	fRecv = 1;
}

由于本身毫米波雷达在此次项目中是通过UART1传输的数据，设计了通过串口1中断来将接收数据，接收到之后直接通过OLED屏幕将数据展示出来；

其中X，Y即为坐标系位置，D为目标距离雷达距离，A为被测目标到雷达这条线与+x的夹角，D为电机旋转方向，S为该次步长，G为该次旋转角度，具体如下图：

其中向屏幕发送数据的时候将中断关闭，中断打开的时候再打开之前写好的追踪函数，这样就可以保证检测数据的1s内，补光灯也在转动，减少卡顿，保证顺滑感。

具体演示

本部分通过之前一直用的那种GIF不太好展示，这里直接指路b站verimake发布的视频《基于毫米波传感器的人体定位跟踪补光灯》，这里有较为完整的演示。

后话

至此这次的智能追踪补光灯的解说就到此结束了，有兴趣的同学可以自己试试。毫米波雷达很使用，用途还有很多，这次仅展示他的一部分能力，其他的用途等同学们思考了。这次的例程我将上传至gitee仓库上文件名为：

SDCC-LIGHT

欢迎大家参考！

gym2028

小白交作业请教：
请问毫米波雷达只有三个pin口，Vo探测到移动物体输出高水平可以用吗？接完线显示屏显示radar是没接雷达的缘故吗？

Benue

gym2028 你说的是人体存在感应的雷达吧，那种雷达只能探测有没有人，探测不到人的距离和方向。
实现帖子里这种功能需要一个能探测方向的雷达

gym2028

Benue 多谢

gym2028

Benue 您好，（i am小白）雷达换成了能测距离的但是连接完成之后发现屏幕显示radar，就是读取雷达数据的那段代码好像没运行，然后突然好像是雷达监测到我了，显示了

雷达我用usb连接到电脑上测试了，没问题，想请教一下问题出在哪里了？

gym2028

可能是通讯问题，我买的矽典微毫米波的通讯波特率是256000；疏忽了，改正后但是还是没反应，应该还是通信问题------啊啊啊

Benue

gym2028 文章里用的是 XenP201，XenP202的包长度和包格式都不一样，解析代码要改才行

gym2028

Benue那个，麻烦一下，能补充个XenP202的解析代码吗？

Benue

gym2028
暂时没有例程，贴一些解析代码吧
雷达的数据结构和解析函数：

#include <math.h>
#include <stdint.h>
typedef struct datapos
{
	int xPos;
	int yPos;
	int theta;
	int dist;
} DataPos;
// 解析 XenP202TT01 的数据
UINT8 decode_XenP202TT01(char *buf, DataPos *data)
{
	UINT8 *p = buf + 4;
	uint16_t temp;
	for (UINT8 i = 0; i < 3; i++)
	{
		temp = p[0] + ((uint16_t)p[1] << 8);
		data[i].xPos = temp & 0x8000 ? temp & (~0x8000) : -temp;
		temp = p[2] + ((uint16_t)p[3] << 8);
		data[i].yPos = temp & 0x8000 ? temp & (~0x8000) : -temp;

		data[i].dist = sqrtf((float)((long)data[i].xPos * data[i].xPos + (long)data[i].yPos * data[i].yPos));
		data[i].theta = atan2f((float)data[i].xPos, (float)data[i].yPos) * (180.0f / PI);
		data[i].theta = data[i].theta;
		p += 8;
	}
	return 0;
}

用法：

DataPos dataXenP202TT01[3];
decode_XenP202TT01(recvBuf, dataXenP202TT01);
printf("X :%5d\t", dataXenP202TT01[0].xPos);
printf("Y :%5d\t", dataXenP202TT01[0].yPos);
printf("angle :%5d\t", dataXenP202TT01[0].theta);
printf("Dist :%5d\r\n", dataXenP202TT01[0].dist);

这里 recvBuf 是 XenP202TT01 的 30 字节完整数据帧，所以文中代码的串口接收包长度要改成30。
帧头帧尾可能也要改下，似乎 202 有两种帧尾结构，以你手里的那个为准。

ong

Benue 大佬能麻烦写一个51单片机版本的么，尝试编写过但是失败了诶，也是这个XenP202的

gym2028

哇，太谢谢了，函数有了，然后就是把帧头和帧尾改下是吧，然后那个recvBuf是这个吗？

Benue

gym2028 对，然后中断跟main中还有跟帧长有关的代码,这两个也改掉应该就可以了：

if (pBuf == 28)

pBuf%=28;

gym2028

Benue
奇怪，这两个在原来的代码里也有但是在我修改完帧头帧尾和解析函数后进行编译显示出错，为什么呢，这部分知识我不是很了解，只能和原代码比较，也没看出来所以然

Benue

gym2028 只有 error 那一行是错误，提示在你代码的第14行。
可能是因为没有 uint16_t 类型定义，需要包含 stdint.h
我忘了把它加到我前面的代码里

gym2028

Benue多谢，现在好像编辑ok了，就是还要麻烦问下进行烧录前要不要改串口一的波特率为256000，我在CH549.UART.h里面改了，但是产生的结果就是烧录失败；要是不改波特率能不能通讯？

gym2028

gym2028 我看202TT01用户手册上写的是256000，现在编码应该是完成了的，成功生成hex文件时很兴奋，但是好像还是有问题
这是帧脚和帧头的修改，是这个意思吗？

Benue

gym2028 按手册来看是这样的，但建议你用串口实测一下帧格式

Rango

下载了博主的例程，发现不能直接烧录。重新编译后，才能烧录。编译的时候第一次make提示失败，然后又make了一次才成功。现在运行的时候发现，毫米波雷达和A4988驱动同时接的时候，显示器不会变化，电机也不转。

Rango

Rango 大佬们帮忙look下是什么原因

Benue

Rango 你的雷达是不是 XenP202？文中用的是 XenP201，如果雷达不一样的花应该要改代码。具体看前面的跟帖