OPENMV结合PIX飞控实现定点 循迹
来源:互联网 发布:男士护肤 知乎 编辑:程序博客网 时间:2024/05/23 02:23
自从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 2017import sensor,time,pyb,mathfrom pyb import Pin, Timer, LED, UART#黑色点阈值black_threshold = [(0, 64)]#xy平面误差数据err_x = 0err_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,0xAA,err_x>>8,err_x,err_y>>8,err_y,0xA1]) 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; } }}
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
可以提供能力之内的帮助
- OPENMV结合PIX飞控实现定点 循迹
- mplayer的定点实现
- 定点实现圆形progressbar
- Android实现定点提醒
- PIX上实现VPN步骤
- DH410无人机搭配Pix飞控装机初体验
- APM和PIX飞控日志分析入门贴
- OpenMV图像处理 最大类间方差法的实现
- FPGA定点数截断实现
- 实现PIX需要参考的标准资料
- 在PIX防火墙上实现VPN
- OpenMV 简介
- openmv学习
- OpenMV简介
- PIX学习路径-1-选择PIXHAWK作为飞控学习的起点
- 在定点DSP上实现DCT压缩
- OpenMV中文教程
- OpenMV时间操作
- 正向代理、反向代理
- FCN语义分割——直接加载图像数据
- ios-GCD信号量
- Android超酷的加载效果
- Win10系统如何在右键菜单添加"管理员取得所有权
- OPENMV结合PIX飞控实现定点 循迹
- 进程简述
- python反射
- Class文件中的常量池详解(上)
- Django的基本操作流程总结
- java反射
- 猜数字游戏,水仙花数
- python slice的几个小点总结
- 任务调度SpringTask