openmv舵机云台 自动追踪色块 STM32源程序

之前做的舵机云台自动瞄色块的装置 现在已经拆掉了，所以没有图片

就是类似19年国赛电磁的发挥部分要求

单片机源程序如下:

#include "control.h"

u8 key = 0;

u8 mode = 0;

u8 sign = 0;

u16 mode_r = 0;

u16 mode_p = 0;

void TIM1_UP_IRQHandler(void)   //TIM1中断

{

        if(TIM_GetITStatus(TIM1,TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源

        {

                        TIM_ClearITPendingBit(TIM1,TIM_IT_Update);  //清除TIMx的中断待处理位:TIM 中断源                 

                        key = KEY_Scan();

                  mode = click_N_Double(50);

                        if(key == 1)

                        {

                                LED0 = 1;

                                mode_r++;

                                if(mode_r <= 6)

                                        roll = 1500 + 110 * mode_r; //9.9度

                                if(mode_r > 6)

                                        roll = 1500 - 110 * (mode_r - 6);

                                if(mode_r > 12)

                                {

                                        roll = 1500;

                                        mode_r = 0;

                                }

                                delay_ms(100);

                                LED0 = 0;

                        }

                        if(key == 2)

                        {

                                LED1 = 1;

                                mode_p++;

                                if(mode_p <= 3)

                                        pitch = 1000 + 110 * mode_p; //9.9度

                                if(mode_p > 3)

                                        pitch = 1000 - 110 * (mode_p - 3); //9.9度

                                if(mode_p > 6)

                                {

                                        pitch = 1000;

                                        mode_p = 0;

                                }

                                delay_ms(100);

                                LED1 = 0;

                        }

                 if(key == 3)

                 {

                         LED0 = 1;

                         LED1 = 1;

                         TIM_SetCompare1(TIM4,roll);

                         TIM_SetCompare2(TIM4,pitch);

                         delay_ms(100);

                         LED0 = 0;

                         LED1 = 0;

                 }

                 if(mode == 2)

                         sign = 1;

                 if(sign == 1)

                        autoaid();

        }

}

void autoaid(void)

{

        delay_ms(100);

        if(OpenMV_Rx_BUF[0] < 140)

        {

                roll += 33;

                if(roll >= 2160)

                        roll -= 33;

                TIM_SetCompare1(TIM4,roll);

        }

        if(OpenMV_Rx_BUF[0] > 180)

        {

                roll -= 33;

                if(roll <= 840)

                        roll += 33;

                TIM_SetCompare1(TIM4,roll);

        }

        if(OpenMV_Rx_BUF[1] < 90)//0x5F 95

        {

                pitch += 33;

                if(pitch >= 1440)

                        pitch -= 33;

                TIM_SetCompare2(TIM4,pitch);

        }

        if(OpenMV_Rx_BUF[1] > 140)//0x87  135

        {

                pitch -= 33;

                if(pitch <= 670)

                        pitch += 33;

                TIM_SetCompare2(TIM4,pitch);

        }

}

import sensor, image, time

from pyb import UART

import json

threshold = [(32, 60, 127, 41, -128, 127)]

sensor.reset()

sensor.set_pixformat(sensor.RGB565)

sensor.set_framesize(sensor.QVGA)

sensor.skip_frames(time = 2000 )

sensor.set_auto_whitebal(False)

#关闭白平衡。白平衡是默认开启的，在颜色识别中，需要关闭白平衡。

clock = time.clock()

uart = UART(3, 115200)

uart.init(115200, bits=8, parity=None, stop=1)  #8位数据位，无校验位，1位停止位、

while(True):

    clock.tick()

    img = sensor.snapshot()

    blob = img.find_blobs(threshold, area_threshold=300)

    if blob: #如果找到了目标颜色

       # print(blob)

       # uart.write("B3 B3 ")    #一帧数据的帧头

        FH = bytearray([0xb3,0xb3])

        uart.write(FH)

        for b in blob:

        #迭代找到的目标颜色区域

            img.draw_rectangle(b[0:4]) # rect

            img.draw_cross(b[5], b[6]) # cx, cy

            x = b.cx()

            y = b.cy()

            #area = b.area()

            if x >= 256:

                x -= 70

            if x < 256:

                print(x, y,'n')

                data = bytearray([x,y])

                uart.write(data)