OPENMV结合PIX飞控实现四轴定点 循迹 2017电赛

本文章代码已上传Github：
https://github.com/Kevincoooool/2017_Follow
有兴趣的可以加个STAR

自从17年国赛之后，自己做了openmv，加了很多群，也了解到很多人都在想着这个题。
第一版

第二版

第三版


我们做国赛的时候实现了全部功能，找了下题目，这篇文章就以这道题来讲吧。

题目：


看到题相信大家都送了口气，不是巡线，不用去思考怎么识别直线、曲线、直角、起点圆啊这些，因为在赛前我们队一直在想着怎么用OV7670或者OV2640来实现这些东西的识别，那段时间也确实找到了段资料：
http://blog.csdn.net/hello_world12138/article/details/51974092
这位大佬写的相当详细，大家也可以参考下来写自己的识别算法。

而我们采用的方案是OpenMV+PixHawk+STM32F4
这种方案就是最省事，不需要自己写飞控，不需要去调姿态的PID，对于玩过PIX的人来说最方便。

OPENMV负责图像的采集和处理，PIX负责飞机的基础稳定飞行和定高，STM32负责控制PIX怎么飞，也就是用32来模拟了一个遥控器，输出PWM波后经过PPM编码器转换成PPM信号给pix就能用32控制pix啦。

一、OPENMV的设计
当时我们是买的官方代理的OPENMV3，价格388呢，还好能报销哈哈，用openmv实现了对地面黑点的检测，然后通过串口3把黑点的坐标值传回给STM32。

OPENMV寻找黑点串口输出程序

# 寻找黑点串口输出程序 - By: Kevincoooool - 周四 11月 23 2017
import sensor,time,pyb,math
from pyb import Pin, Timer, LED, UART
#黑色点阈值
black_threshold = [(0, 64)]
#xy平面误差数据
err_x = 0
err_y = 0
#发送数据
uart_buf = bytearray([0x55,0xAA,0x00,0x00,0x00,0x00,0xAA])

#串口三配置
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)#设置灰度信息
sensor.set_framesize(sensor.QQVGA)#设置图像大小
sensor.skip_frames(20)#相机自检几张图片
sensor.set_auto_whitebal(False)#关闭白平衡
clock = time.clock()#打开时钟
while(True):
    clock.tick()
    img = sensor.snapshot()
    #寻找blob
    blobs = img.find_blobs(black_threshold)
    if blobs:
        most_pixels = 0
        largest_blob = 0
        for i in range(len(blobs)):
            #目标区域找到的颜色块可能不止一**重点内容**个，找到最大的一个
            if blobs[i].pixels() > most_pixels:
                most_pixels = blobs[i].pixels()
                largest_blob = i
                #位置环用到的变量
                err_x = int(60 - blobs[largest_blob].cy())
                err_y = int(blobs[largest_blob].cx() - 80)
        img.draw_cross(blobs[largest_blob].cx(),blobs[largest_blob].cy())#调试使用
        img.draw_rectangle(blobs[largest_blob].rect())
    else:
       err_x = 0
       err_y = 0
    #数组中数据写入
    uart_buf = bytearray([0x55,err_x>>8,err_x,err_y>>8,err_y,0xAA])
    print(err_x,err_y)
    uart.write(uart_buf)

二、STM32控制端程序设计
既然我们用的是STM32模拟遥控器，那我们就要先初始化两个定时器来输出八路PWM波，电调的频率基本上都是50hz，刚刚把代码贴上来了，但是想了想大家都是有基础的，这些初始化肯定会的。
一个串口用来读取OPENMV的数据，一个串口用来读取超声波模块的高度。
两个定时器用来模拟50hz的PWM波。

恩 ，对，然后就没了，最后还需要个PID控制函数来对OPENMV传回的黑点坐标值进行PID运算，转化为PIX能识别的‘遥控器’控制量即可实现定点。
对于怎么知道模拟出来的PWM波对应的遥控器的哪个通道值，大家只有拿着遥控器一个一个对应调了，记得做好记录。

题目分析:

基础一：把飞机放在黑点上方，需要一键自动起飞到指定高度，我们采用的方法：
按键按下模式1后，先模拟遥控器对PIX解锁、然后开始起飞，油门逐渐增加，增加的同时当高度高于20cm就开启定点，当飞机高度到达指定高度后开启定高模式，因为PIX的气压计定高不是很准，所以我们人为加了定高的修正，高度大于目标值就拉低油门，低于目标值就拉高油门，定高的同时也在定点，然后开始计时，到达指定时间，大幅拉低油门，自动降落。
主函数

int main(void)
{ 	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
	delay_init(168);  //初始化延时函数
	uart_init(115200);//初始化串口波特率为115200
	uart2_init(9600);
	uart3_init(115200);
	LED_Init();
	KEY_Init();
	IIC_Init();       //IIC初始化
	OLED_Init();
	OLED_Clear();
	TIM3_PWM_Init(20000-1,84-1);	//84M/84=1Mhz的计数频率,重装载值20000，所以PWM频率为 1M/20000=50hz.    
	TIM4_PWM_Init(20000-1,84-1);	//84M/84=1Mhz的计数频率,重装载值20000，所以PWM频率为 1M/20000=50hz. 	
	TIM2_Int_Init(10-1,8400-1);
	
	while(1)
	{
		Fly_Mode = 0;
		OLED_Clear();
		while(!Fly_Mode)
		{
			Fly_Mode=KEY_Scan();
			OLED_ShowNum(0, 2, Fly_Mode, 1, 16);
			OLED_ShowUnNum(0, 0, hight, 3, 16);
		}
		OLED_ShowNum(0, 2, Fly_Mode, 1, 16);
		LED = 0;
		delay_ms(200);
		LED = 1;
		switch (Fly_Mode)
		{
		case 1:
			Take_off();
			Start_Fixed_high();
			while(1)
			{					
				if(PID_flag == 1)
				{
					PID_flag = 0;
					PositionPID();
					High_fix();
				}	
				SStart_flag = 1;
				if(S_flag == 1)
				{
					S_flag = 0;
					i++;
				}
				if(i == 300)
				{
					i = 0;
					SStart_flag = 0;
					Land_down();
					break;
				}
			}
			break;
		case 2:
			Take_off();
			Start_Fixed_high();
			while(1)
			{					
				if(PID_flag == 1)
				{
					PID_flag = 0;
					PositionPID();
					High_fix();
				}	
				SStart_flag = 1;
				
				//1s开始计时
				if(S_flag == 1)
				{
					S_flag = 0;
					i++;
				}
				//计时盲飞
				if(i == 100)
				{
					mang_flag = 1;
				}
				
				//忙飞结束
				if(i == 120)
				{
					mang_flag = 0;	
				}
				
				//计时降落
				if(i == 200)
				{
					i = 0;
					SStart_flag = 0;
					Land_down();
					break;
				}
			}
			break;
		case 3:
			break;
		case 4:
			break;
		case 5:
			break;
		default:
			Land_down();
			break;		
		}
	}
}

串口解析OPENMV数据函数

void Vision_datadeal(void)
{
	int tmp, tmp1, tmp2;
	
	USART_SendData(USART2, 0x55);  //向串口2发送数据
	PID_flag = 1;
	if(USART3_RX_BUF[0]==0x55 && USART3_RX_BUF[5]==0xAA)
	{		
		/*接收视觉模块发来的信息*/
		if(USART3_Flage)
		{	
			USART3_Flage=0;
			tmp=((int)USART2_RX_BUF[0]<<8) + USART2_RX_BUF[1];
			hight=tmp/10; //除以 1000 转化为单位 M
			{
				/*  Target_Roll */
				tmp1=((int16_t)USART3_RX_BUF[1]<<8) + USART3_RX_BUF[2];
				tmp1 = Median_filer1(tmp1);
				if(tmp1>=32768)
				{
					tmp1 = tmp1-0xffff;
				}					
				else
				{
					tmp1 =tmp1;
				}
				pixX = tmp1;
				/*  Target_Pitch */
				tmp2=((int16_t)USART3_RX_BUF[3]<<8) + USART3_RX_BUF[4];
				tmp2 = Median_filer2(tmp2);
				if(tmp2>=32768)
				{
					tmp2 = tmp2-0xffff;
				}
				else
				{
					tmp2 = tmp2;
				}
				pixY = tmp2;
				
				if(Fly_Mode == 2 && mang_flag == 1)
				{
					if(pixY_last == 0 && pixY - pixY_last < -50)
					{
						mang_flag = 0;
					}
					else
					{
						pixY = -23;
					}
					TIM_SetCompare1(TIM3,1540); 	//Pitch		CH1 PB4 
					TIM_SetCompare3(TIM3,1530);		//Roll		CH3 PB0 
				}
				pixY_last = pixY;
			}							
		}	
	}
}

PIX加锁函数

void lock(void)
{
	TIM_SetCompare1(TIM3,1500); 	//Pitch		CH1 PB4 
	TIM_SetCompare2(TIM3,1000);		//Throttle	CH2 PB5
	TIM_SetCompare3(TIM3,1500);		//Roll		CH3 PB0 
	TIM_SetCompare4(TIM3,1000);		//Yaw		CH4 PB1
	TIM_SetCompare1(TIM4,1000);		//			CH5 PD12
	TIM_SetCompare2(TIM4,1000);		//			CH6 PD13
	TIM_SetCompare3(TIM4,1000);		//			CH7 PD14
	TIM_SetCompare4(TIM4,1000);		//			CH8 PD15
	delay_ms(3000);
}

解锁函数

void Unlock(void)
{
	TIM_SetCompare1(TIM3,1500); 	//Pitch		CH1 PA6 
	TIM_SetCompare2(TIM3,1000);		//Throttle	CH2 PA7
	TIM_SetCompare3(TIM3,1500);		//Roll		CH3 PB0 
	TIM_SetCompare4(TIM3,2000);		//Yaw		CH4 PB1
	TIM_SetCompare1(TIM4,1000);		//			CH5 PB6
	TIM_SetCompare2(TIM4,1000);		//			CH6 PB7
	TIM_SetCompare3(TIM4,1000);		//			CH7 PB8
	TIM_SetCompare4(TIM4,1000);		//			CH8 PB9
	delay_ms(4000);
	TIM_SetCompare4(TIM3,1500);		//Yaw		CH4 PB1
	delay_ms(1000);
	TIM_SetCompare2(TIM3,1300);		//Throttle	CH2 PA7		
	delay_ms(500);
	TIM_SetCompare2(TIM3,1000);		//Throttle	CH2 PA7
}

起飞函数

void Take_off(void)
{
	int Throttle=1000,Throttle_Increase=25,Hight_Last;
	OLED_ShowString(0,4,"Start",16);
	delay_ms(3000);
	OLED_ShowString(0,4,"Unlock",16);
	Unlock();
	
	Hight_Last=hight;
	while (hight<25)
	{
		TIM_SetCompare2(TIM3,Throttle);
		Throttle+=Throttle_Increase;
		if(Throttle>=1800)Throttle=1800;
		delay_ms(100);
		if (hight-Hight_Last>1)
		{
			Throttle_Increase=0;
			Throttle-=10;
		}
		if(hight>25)
		{
			PositionPID();		
		}
		Hight_Last=hight;
		TIM_SetCompare1(TIM3,1540); 	//Pitch		CH1 PB4 
		TIM_SetCompare3(TIM3,1530);		//Roll		CH3 PB0 
	}
//	TIM_SetCompare1(TIM3,1500); 	//Pitch		CH1 PB4 
//	TIM_SetCompare3(TIM3,1500);		//Roll		CH3 PB0 
}

开启定高模式函数

void Start_Fixed_high(void)
{
	OLED_ShowString(0,4,"Highfix",16);
	TIM_SetCompare1(TIM4,1500);		//			CH5 PD12
	delay_ms(5);
	TIM_SetCompare2(TIM3,1500);		//Throttle	CH2 PB5
	TIM_SetCompare2(TIM3,1300);
	delay_ms(15);
	TIM_SetCompare2(TIM3,1500);
}

降落函数

void Land_down(void)
{
	OLED_ShowString(0,4,"Land_down",16);
	int j = 0;
	while(j <= 200)
	{
		j++;
		TIM_SetCompare2(TIM3,1280);		//Throttle	CH2 PB5
		delay_ms(20);
		PositionPID();
	}
	
	lock();
}

PID控制函数：

/*黑点悬停控制*/
void PositionPID(void)
{
	static float lastVxErro,lastVyErro;
	static float pidVx_pOut,pidVx_dOut,pidVx_iOut;
	static float pidVy_pOut,pidVy_dOut,pidVy_iOut;
	static float pidVx_value,pidVy_value;
	static unsigned char flag_Y,flag_X;
	/***************X轴PID参数**ROLL************/
	float Vxkp=0.086f;//
	float Vxki=0.0004f;//0.001f;
	float Vxkd=0.027f;//-0.000531;
	/***************Y轴PID参数*PITCH*************/
	float Vykp=0.086f;
	float Vyki=0.0004f;//0.001f;
	float Vykd= 0.024f;
	
	/*X轴位移速度调整*/
	float vxErro=(float)(0.0f-(-pixX*hight/100));
	float vxErroDelta=(vxErro-lastVxErro)/0.016f;
	lastVxErro=vxErro;
	/*X轴积分分离处理*/
	if(vxErro <= 50.0f&&vxErro >= -50.0f)
	{
		flag_X = 0;
	}
	else
	{
		flag_X = 1;
		
	}
	pidVx_pOut=Vxkp * vxErro;
	pidVx_dOut=Vxkd * vxErroDelta;
	pidVx_iOut+=Vxki * vxErro;
	if(pidVx_iOut>2.5f)//1.5
		pidVx_iOut=2.5f;
	if(pidVx_iOut<-2.5f)
		pidVx_iOut=-2.5f;
	
	pidVx_value=pidVx_pOut+pidVx_dOut+flag_X*pidVx_iOut;
	
	if(pidVx_value>10)
		pidVx_value=10;
	if(pidVx_value<-10)
		pidVx_value=-10;
	
	pidVx_value*=22;
	
	
	/***************Y轴PID调节***************/	
	/*X轴位移速度调整*/
	float vyErro=(float)(0.0f-(-pixY*hight/100));
	float vyErroDelta=(vyErro-lastVyErro)/0.016f;
	lastVyErro=vyErro;
	/*Y轴积分分离处理*/
	if(vyErro <= 50.0f&&vyErro >= -50.0f)
	{
		flag_Y = 0;
	}
	else
	{
		flag_Y = 1;
	}
	pidVy_pOut=Vykp * vyErro;
	pidVy_dOut=Vykd * vyErroDelta;
	pidVy_iOut+=Vyki * vyErro;
	/*Y轴积分限幅处理*/
	if(pidVy_iOut>2.5f)
		pidVy_iOut=2.5f;
	if(pidVy_iOut<-2.5f)
		pidVy_iOut=-2.5f;
	
	pidVy_value=pidVy_pOut+pidVy_dOut+flag_Y*pidVy_iOut;
	/*Y轴输出限幅处理*/
	if(pidVy_value>10)
		pidVy_value=10;
	if(pidVy_value<-10)
		pidVy_value=-10;
	
	pidVy_value*=22;
	
	/************PWM赋值***************/	
	TIM_SetCompare1(TIM3,1500+pidVx_value); 	//Pitch		CH1 PB4 
	TIM_SetCompare3(TIM3,1500+pidVy_value);		//Roll		CH3 PB0 			
	TIM_SetCompare4(TIM3,1505);					//Yaw		CH4 PB1
	
	
}

基础二：这道题是检测两个物体间的空间距离，我们用的ESP8266来做的，它可以读出附近热点的信号值，但是这种做法精度极低。最后到比赛场地时才发现居然有人用超声波测距？？而且居然没扣分，很神奇了。

基础三:对于这道题，和第一道题类似，但是需要往前飞一段时间，那么我们就可以先完成基础一，计时到了一定时间，人为的把OPENMV传回来的黑点值修改为定值，那么就可以往前飞了，但是这样掌握不好要往前飞多久才会到达小车的正上方，对于这种情况，解决办法一是去试那个时间，多试下会试出来的，二是判断坐标值的突变，也就是在往前飞的时候，OPENMV传回来的黑点的水平坐标是大致不会变的，而黑点的垂直方向的值会发生突变，一种情况是飞到小车和黑点中间丢失了黑点，这种情况是垂直坐标从最大值变成0再变为最小值。还有一种情况是飞机有点高，往前飞的时候小车也进入了摄像头视野，因为OPENMV找的是视野中面积最大的色块，那么OPENMV得到的黑点值就发生了突变，这时就可以开始取消人为给值，开始正常的定点计时到一定时间自动降落。

发挥一：这个题和基础三类似，修改一下第三阶段定点的时间就可以了。
发挥二：这个题和基础三类似，修改一下第三阶段定点的时间就可以了。

有问题的可以加Q97354734
可以提供能力之内的帮助
本人小店：

https://shop110563242.taobao.com/index.htm?spm=2013.1.w5002-16371582764.2.fo0MiW