OPENMV結合PIX飛控實現定點 循跡

自從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,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
可以提供能力之內的幫助
本人小店：

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