EEPW论坛

进度遥遥领先于小伙伴们~大赞~

实验八、通过编码器采样数据得到前进距离试验

实验背景：

一直打算做个小车的行驶记录仪。现在先做好如何将小车集成的编码器的采样结果转换为小车的位移。

个人理解，前段时间已经做了实验三、四，分别通过有线还有无线的方式将小车的采样数据传输到电脑上位机软件进行分析，得到小车的编码器采样结果。可以将其结果理解成电机的转动速度。

 

我们都知道，速度对时间的积分就是位移。

黄色是小车编码器的输出结果。对应着速度，假如在STM32 的5MS中断函数中，将编码器的结果乘以0.005，就得到一个与小车的位移成比例关系的一个参数。

 

也就是编码器的输出1，也并不代表小车移动速度为1mm/s，也就是说这个结果还需要转换。

 

 

实验需要的硬件

实验过程中，使用到了：

上图包括两个Uart转USB模块（其实只需要一个即可，一个用于蓝牙主模块接电脑USB，另外一个用于给STM32 下载程序），BC-04蓝牙主从一体模块，以及EEPW平衡小车

实验过程：

注意本试验中用distance表示小车移动的距离。

实验过程1、对左边的编码器的采样结果对时间进行积分

在void TIM1_UP_TIM16_IRQHandler(void) 中断子程序中，加入以下代码：

if(Encoder_Left>0)

                     distance = distance + (float)Encoder_Left*0.005;    

我加上这个if判断的原意是为了只得到小车的前进距离，而不看小车的后退距离。让小车保持平衡，就会得到下图中的数据。小车的平衡过程中，会前进4cm，后退4cm，总共占了8cm的距离。但是看下图，紫色的阶梯波形，每一个台阶的幅值都达不到80。所以需要乘以一个放大系数。

下面的过程的目的，是得到这个放大系数。

实验过程2、对distance进行校正，得到正确的距离

本次校正，打算使用小车的蓝牙将数据传送到电脑中，具体过程请看实验四。

但是小车的蓝牙是从模块，只能选择连接手机或者是电脑。我先用手机操作小车。

让小车移动一个距离。这个距离我用地面上的瓷砖可以数得到。这种瓷砖是60mm*60mm的。我让小车移动了6个瓷砖的距离。

让后手机断开和小车的蓝牙连接，让小车保持在原地。将BC-04主模块通电。它会自动和小车上的SPP-CA连接。并同时小车会不断将数据发送到电脑中。

通过分析数据，得到这时候左编码器的结果的积分是-1653。而实际距离是3600。所以要将积分结果乘以3600/1653。这个放大系数，就是所要得到的校正系数。

具体校正代码：

在代码中，我试过了不可以直接用distance = distance*2.176542;

这样来校正。而是另外弄一个中间变量。

在void oled_show(void)子函数中

注释掉MPU6050的滤波器的显示代码，而用以下代码替代。

int disSHow;
disSHow = distance*2.176542;
if(disSHow<0)
			OLED_ShowNumber(45,0, (-disSHow),5,12);
		else
			OLED_ShowNumber(45,0, disSHow,5,12);

 

就可以实现小车移动距离的实时显示啦！

记得如果需要oled的实时显示需要将

u8 Way_Angle=1;改为2或者3。笔者是用2的。

 

结论

本实验在Mini Balance V2.5 标准版源码的基础上，进一步分析编码器的数据。统一了蓝牙无线调试、电机编码器采样结果计算、OLED显示这几个小实验，通过积分、校正，得到了小车的真正移动距离。

[开发进程]实验九、移动中避障自平衡小车实验

小车标配版，另外加上超声波测距模块，就可以完成本实验。

超声波模块可以直接接到板子上，而不用拆卸压克力板。如果要安装NRF24L01，就需要拆……

笔者使用了源代码：Mini Balance V2.5 顶配版源码（标准版基础上增加摄像头底层 超声波 NRF24L01）

顶配版源代码的使用：

并使用超声波蔽障需要修改一下蔽障使能的标志位：

Main.c的第18行：

u8 Flag_Bizhang=0;                          //避障标志位0：不避障  1：避障   接入超声波模块 把Flag_Bizhang改成1就可以进入避障模式了

需要改变成：

u8 Flag_Bizhang=1;                          //避障标志位0：不避障  1：避障   接入超声波模块 把Flag_Bizhang改成1就可以进入避障模式了

另外值得注意的是：

Main.c第9行

u8 Way_Angle=1;

改为

u8 Way_Angle=2;

那么在小车运行时候才会更新oled显示哦。因为默认Way_Angle=1，在自平衡启动时候是不更新的oled的。以下是证据：

顶配版程序的实验现象：

将小车运行自平衡，发现车子在手机遥控前进的时候，往障碍物走，发现到了障碍物前，车子也没有理性的往后退，直到车子撞到障碍并翻倒在地上。

原因分析：

估计是手机蓝牙的控制优先权比蔽障功能的优先权高，所以造成就算通过超声波模块测量得到前面很近的地方有障碍物，车子也没后退。

在int velocity(int encoder_left,int encoder_right)函数中，可以看到控制小车前后是根据全局变量Flag_Qian和Flag_Hou的。而这两个标志参数，两个函数的影响，第一个void USART3_IRQHandler(void)是在uart3.c内第66行，这个函数内的相关代码代码一：

		if(uart_receive==0x00)	Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//////////////刹车
		if(uart_receive==0x01)	Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//////////////前
		if(uart_receive==0x05)	Flag_Qian=0,Flag_Hou=1,Flag_Left=0,Flag_Right=0;//////////////后
		else if(uart_receive==0x02||uart_receive==0x03||uart_receive==0x04)	
													Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1;
		else if(uart_receive==0x06||uart_receive==0x07||uart_receive==0x08)	
													Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;

第二个函数是void TIM1_UP_TIM16_IRQHandler(void)，在MiniBalance.c内第27行。

相关代码代码二：

		  if(Flag_Bizhang==1)
			{	
		  if(Distance<100)Flag_Hou=1; else Flag_Hou=0; //===简单避障：遇到障碍物就退 } 

这样做的缺点是，假如手机遥控小车前进，那么会使Flag_Qian=1而Flag_Hou=0。所以此时蔽障功能并没有用！

解决方案：

代码一执行完成后也需要执行代码二。原本当手机遥控小车前进，就不管有没有障碍物存在也要前进。现在将代码二复制到代码一的后面，就可以了。

上图中红色矩形内的代码就是加进去才能在遥控前进时候也能正常蔽障的代码。里面的u32 ObstacleDistance=180;我在main.c中定义了全局变量。并在main.h中声明。

extern u32 ObstacleDistance;

这个参数我取了180，可是实际中，由于小车前进的太快了。刹车距离根本不只200mm。实际中还需要增加这个值的大小。

---------------------------------------------------分割线-------------------------------------------------------------------------------------

方案二、

在实验过程中，发现上面的方案不行。因为这样在实际过程中就不能遥控小车后退了。。。

十分蛋疼

于是我再加入两个全局变量，用于协调void USART3_IRQHandler(void)和void TIM1_UP_TIM16_IRQHandler(void)  对前进后退往左往右Flag_Qian、Flag_Hou、Flag_Left、Flag_Right的操作。

u32 Distance,Distance_prev=0;            //Distance_prev的作用是记录前一个Distance的值、用于滤波
u32 ObstacleDistance=240;

u8 Flag_Obstacle=0;
u8 Backward=0;

并在 头文件中加入声明：.

extern u32 Distance,Distance_prev;
extern u32 ObstacleDistance;

extern u8 Flag_Obstacle;
extern u8 Backward;

ObstacleDistance=240是自己定义的，如果障碍物在小车前距离在该参数内，避障功能有效，注意单位是mm。

Backward这个参数，Usart3本身直接控制Flag_Hou，现在改为直接控制Backward，间接控制Flag_Hou。

将usart3.c中void USART3_IRQHandler(void)相关代码只有单纯的前进后退和原地左右旋转的，改为更加灵活的可以转弯的。介绍参数:

Flag_Bizhang是避障使能Flag。ObstacleDistance是笔者定义的，避障功能在该距离内有效。Flag_Obstacle=1的话，说明避障功能有效，暂时忽略蓝牙遥控的控制。backward是蓝牙控制小车往后走。因为避障也控制往后，所以不可以直接用Flag_Hou这个参数。

		if(Flag_Obstacle == 0)
		{
			Flag_Qian=0,Backward=0,Flag_Left=0,Flag_Right=0;			
			if(uart_receive==0x00)	Flag_Qian=0,Backward=0,Flag_Left=0,Flag_Right=0;//////////////刹车
			if(uart_receive==0x01)	Flag_Qian=1,Backward=0,Flag_Left=0,Flag_Right=0;//////////////前
			if(uart_receive==0x02)	Flag_Qian=1,Backward=0,Flag_Left=0,Flag_Right=1;//////////////前进右
			if(uart_receive==0x03)	Flag_Qian=0,Backward=0,Flag_Left=0,Flag_Right=1;//////////////原地右
			if(uart_receive==0x04)	Flag_Qian=0,Backward=1,Flag_Left=0,Flag_Right=1;//////////////后退右
			if(uart_receive==0x05)	Flag_Qian=0,Backward=1,Flag_Left=0,Flag_Right=0;//////////////后
			if(uart_receive==0x06)	Flag_Qian=0,Backward=1,Flag_Left=1,Flag_Right=0;//////////////后退左
			if(uart_receive==0x07)	Flag_Qian=0,Backward=0,Flag_Left=1,Flag_Right=0;//////////////原地左
			if(uart_receive==0x08)	Flag_Qian=1,Backward=0,Flag_Left=1,Flag_Right=0;//////////////前进左
		}
		else
		{
			Flag_Qian=0,Backward=1,Flag_Left=0,Flag_Right=0;
		}

void TIM1_UP_TIM16_IRQHandler(void)  内的代码也需要修改。下面这段代码的几个参数介绍：

Flag_Bizhang是避障使能Flag。ObstacleDistance是笔者定义的，避障功能在该距离内有效。Flag_Obstacle=1的话，说明避障功能有效，暂时忽略蓝牙遥控的控制。backward是蓝牙控制小车往后走。因为避障也控制往后，所以不可以直接用Flag_Hou这个参数。

			if(Flag_Bizhang==1)
			{	
				if(Distance>ObstacleDistance)
					Flag_Obstacle = 0;
				else
					Flag_Obstacle = 1;
				
				if(Flag_Obstacle == 1)
					Flag_Hou = 1;
				else
					if(Backward==1)
						Flag_Hou = 1;
					else
						Flag_Hou = 0;
			}

然后就是在读取超声波距离的函数中，假如了一阶低通滤波器，作用就是距离的结果对距离突然发生变化变得不那么敏感。也有过滤掉测量误差的作用。

void Read_Distane(void)
{   
	 PAout(2)=1;
	 delay_us(15);  
	 PAout(2)=0;	
			if(TIM2CH4_CAPTURE_STA&0X80)//成功捕获到了一次高电平
		{
			Distance=TIM2CH4_CAPTURE_STA&0X3F;
			Distance*=65536;					//溢出时间总和
			Distance+=TIM2CH4_CAPTURE_VAL;		//得到总的高电平时间
			Distance=Distance*170/1000;
			//printf("&d \r\n",Distance);
			TIM2CH4_CAPTURE_STA=0;			//开启下一次捕获
			
			Distance = 0.6*Distance + 0.4*Distance_prev;
			
		}				
}
 

此外，由于插入了超声波模块，小车如果转弯太快也会摔倒，因此转弯速率Turn_Amplitude我改为了600：

int Turn_Amplitude=600;

Movement前进后退速率我也相应的改为600了，别太快。。

if(1==Flag_Qian)    Movement=-600;                 //===如果前进标志位置1 位移为负
else if(1==Flag_Hou)      Movement=600;          //===如果后退标志位置1 位移为正
else  Movement=0;

视频中中万用板上做的是我的蓝牙控制器。不过请忽略板子上面的DIP引脚的IC。。。

板子的硬件上存在I2C  eeprom、还有MPU6050、rgb灯、MCU板子、摇杆、主从一体蓝牙模块。但是暂时有作用的只是将摇杆的结果划分为0x00、0x01 0x02 0x03 0x04 0x05 0x06 0x07 0x08这几个结果发送到蓝牙。

下面放出视频

视频地址：http://player.youku.com/player.php/sid/XMTI3NzAzNzA2OA==/v.swf

结论

本楼的实验在Mini Balance V2.5 顶配版源码（标准版基础上增加摄像头底层 超声波 NRF24L01）（简称顶配版V2.5）的基础上修改，原先的工程程序下载到小车内，小车只有在自平衡的时候才会考虑超声波测量的距离，而在蓝牙遥控小车的过程中，会完全忽略车前是否有障碍物、即完全失去了避障的效果。

本楼的代码解释了假如对顶配版V2.5做一些修改，就可以实现小车在收到手机遥控信息的时候，也可以实现小车的前进避障。实验结果放出了视频，显示有较好的结果。

强啊！楼主，

学习，学习，一个不错的体验，也想搭把手做控制器，这样才有感觉嘛~哇咔咔，

太棒了~

受益匪浅，梗着帖子也用了VS-V2软件，代码也照着作了修改，串口1没问题，用蓝牙串口3就不动了，改完也没有数据波形显示，用串口一带线调太麻烦了，不如蓝牙串口方便，楼主引用了V2内部的CRC校验代码，别的转换代码自己写出来很牛了，向楼主学习

您好 有个问题想问一下 

Encoder_Least =(Encoder_Left+Encoder_Right)-0;  //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） 
Encoder *= 0.8;                         //===一阶低通滤波器       
Encoder += Encoder_Least*0.2;             //===一阶低通滤波器    
Encoder_Integral +=Encoder;                                     //===积分出位移 积分时间：10ms
Encoder_Integral=Encoder_Integral-Movement;    
这里的目标速度指的什么....我理解为应该在这里减movement

学习了