目录
前言：
前期选择：
通信控制方案
（一）串口通信
（二）K210色块识别
（三）STM32主控舵机
duoji.c
duoji.h
（四）K210控制舵机
效果展示
前言：
暑假经历了本次电赛，收获了很多。在比赛前期也是做了很多准备，最终在四天三夜里完成了自己的作品，现在比赛也落下帷幕，简单做一下总结。
前期选择：
在比赛前夕，准备了一些常用电机，舵机的控制例如步进电机，减速电机和舵机控制，简单准备了一下代码。步进电机实现了定速，定角度的控制。减速电机实现了PID控速，舵机也写了简单的控制。当拿到比赛材料清单的时候，看见二维云台，第一时间就开始着手准备实现二维云台的控制，目的先简单实现一些二维云台的追踪。
在实现云台的控制时，有两个方案。
第一是用市场上常见的舵机控制的二维云台。优点：控制简单，代码实现简单。缺点是：容易受到电池电压的影响，效果不稳定，容易产生误差，对于精准定位会产生问题，但是对于追踪这种实时更新误差的功能影响不大。
第二是用两个步进电机来实现二维云台的控制。优点：精准，误差小。缺点：控制稍微难一点，难判断初始位置，归位定点会出现问题。
两个方案对比之下，我们还是选择了常见的舵机驱动的二维云台，省去了 定位的难度，网上资料都很多。
对于舵机的控制也有两种方案：
第一是用STM32C8T6作为主控，K210进行视觉识别后通过串口通信给主控位置信息来实现二维云台的控制。
第二是直接用K210来控制二维云台，这样可以节省主控的资源，也可以减少反应的延迟。
这两套方案我们都有实现。
通信控制方案
（一）串口通信
通过STM32控制舵机主要的就是K210与STM32C8T6之间的通信问题，为了使通信更加准确，我们需要编写一个通信协议，加两个帧头就可以了，一个帧尾。
K210协议编写：
#消息发送函数
def send_data_wx(wx,wl):
global uart,data;
data = bytearray([0x2C,0x12,wx,wl,0x5B])
uart.write(data);
然后STM32在串口接收的时候进行解析就能直接收到K210发送的两位数据。
//STM32串口解析函数
void USART2_IRQHandler(void)
{ //串口1中断服务程序（固定的函数名不能修改）
u8 com_data;
static u8 RxCounter1=0;
static u16 RxBuffer1[4]={0};
static u8 RxState = 0;
if( USART_GetITStatus(USART2,USART_IT_RXNE)!=RESET) //接收中断
{
USART_ClearITPendingBit(USART2,USART_IT_RXNE); //清除中断标志
com_data = USART_ReceiveData(USART2);
if(RxState==0&&com_data==0x2c) //0x2c帧头
{
RxState=1;
RxBuffer1[RxCounter1++]=com_data;
}
else if(RxState==1)
{
if(RxState==1&&com_data==0x12) //0x12帧头
{
RxState=2;
RxBuffer1[RxCounter1++]=com_data;
}
else
{
RxState=0;
}
}
else if(RxState==2)
{
RxBuffer1[RxCounter1++]=com_data;
RxState=3;
}
else if( RxState== 3)
{
RxBuffer1[RxCounter1++]=com_data;
RxState=4;
}
else if(RxState==4)
{
RxBuffer1[RxCounter1++]=com_data;
RxState=5;
}
else if( RxState == 5)
{
RxState=0;
RxCounter1=0;
k210_x=RxBuffer1[2];
k210_y=RxBuffer1[3];
}
}
}
通过简单的配置就可以得到稳定的串口通信。
（二）K210色块识别
在比赛前期，我们就通过简单的红色色块的识别追踪来验证代码，K210的色块识别可以从星瞳科技的官网资料里面进行自主学习。
（比赛中K210功能的实现我会另外发）
https://book.openmv.cc/image/blob.html
import sensor,lcd,time,image
from machine import UART,Timer
from fpioa_manager import fm
#串口初始化
#fm.register(15, fm.fpioa.UART1_RX, force=True)
#fm.register(17, fm.fpioa.UART1_TX, force=True)
#uart = UART(UART.UART1, 115200, read_buf_len=4096)
#uart.write('Hello world！')
def find_max(blobs):
max_size=0
for blob in blobs:
if blob.pixels() > max_size:
max_blob=blob
max_size = blob.pixels()
return max_blob
#设置摄像头
sensor.reset() #初始化感光元件
sensor.set_pixformat(sensor.RGB565) #初始化感光元件
sensor.set_framesize(sensor.QQVGA) #QVGA(320X240)设置图像的大小
sensor.set_vflip(0) #后置模式，所见即所得
sensor.set_hmirror(0)#镜像反转
sensor.skip_frames(time = 2000) # 等待设置生效.跳过n张照片，在更改设置后，跳过一些帧，等待感光元件变稳定。
clock=time.clock()
# 颜色识别阈值 (L Min, L Max, A Min, A Max, B Min, B Max) LAB 模型
# 下面的阈值元组是用来识别 红、绿、蓝三种颜色，当然你也可以调整让识别变得更好。
thresholds = [
(15, 100, 31, 127, 15, 127), # 膜上红灯
(100, 97, -128, 82, -100, 68), # 纸上红灯
(69, 97, -128, 82, -100, 68)] # 纸上小红灯
lcd.init()
while True:
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([thresholds[1]],x_stride=1,y_stride=1)# 0,1,2 分别表示红，绿，蓝色。
if blobs:
max_blob=find_max(blobs)
tmp=img.draw_rectangle(max_blob[0:4]) #方框框起来
tmp=img.draw_cross(max_blob[5], max_blob[6]) #中心十字标
img.draw_string(max_blob.x() + 2, max_blob.y() + 2, "RED")
lcd.display(img) #LCD 显示图片
print(clock.fps()) #打印 FPS
（三）STM32主控舵机
舵机其实就是用PWM来控制舵机旋转的位置，是用来控制位置的很好选择（关于舵机的文章我会另外发）。
舵机的控制是非常简单的，我们只需要配置好合适的PWM输出就行。
duoji.c
#include "duoji.h"
#include "sys.h"
#include "stm32f10x.h"
#include "stm32f10x_rcc.h"
/*===================================================================
程序功能：串口控制舵机控制
**************************************************************************
函数功能：舵机控制pwm初始化
入口参数：舵机定时器分频
返回 值：无
**************************************************************************/
void Set_Jiao(int jiaodu1,int jiaodu2)
{
if(jiaodu1<0){jiaodu1=-jiaodu1;}
if(jiaodu2<0){jiaodu2=-jiaodu2;}
TIM_SetCompare1(TIM2,jiaodu1);//定时器2通道1 PWMA
TIM_SetCompare2(TIM2,jiaodu2); //定时器2通道2 PWMB
}
//TIM2 PWM初始化 PWM波 PA0 CH1 PA1 CH2
//arr：自动重装值
//psc：时钟预分频数
void TIM2_PWM_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //使能定时器2时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_AFIO, ENABLE); //使能GPIOA外设和AFIO复用功能模块时钟,使能定时器2时钟
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_0|GPIO_Pin_1; //TIM2 CH1 PWMA CH2 PWMB
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO
//初始化TIM2
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
//初始化TIM2
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OCInitStructure.TIM_Pulse=0;
TIM_OC1Init(TIM2, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM2 OC1
TIM_OC2Init(TIM2, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM2 OC2
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable); //使能TIM2在CCR1上的预装载寄存器
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //使能TIM2在CCR2上的预装载寄存器
TIM_CtrlPWMOutputs(TIM2,ENABLE); //MOE 主输出使能
TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的预装载寄存器
TIM_Cmd(TIM2, ENABLE); //使能TIM2
TIM_SetCompare1(TIM2,155); //定时器2通道1输出0
TIM_SetCompare2(TIM2,130); //定时器2通道2输出0
}
duoji.h
#ifndef __DUOJI_H
#define __DUOJI_H
#include "stm32f10x.h"
/************************************************************************
舵机1：TIM2通道1取值范围50-260 上层舵机
舵机2：TIM2通道2取值范围50—260 下层舵机
*************************************************************************/
void Set_Jiao(int jiaodu1,int jiaodu2);
void TIM2_PWM_Init(u16 arr,u16 psc);
#endif
（四）K210控制舵机
为了节省32的外设资源，我们可以采用K210识别后直接控制舵机进行位置修正的方法。因为我们的k210本身就是一个微处理器，可以直接用来作控制器。后续我们电赛过程中也是使用的这种方法。
具体的控制原理可以参考星瞳科技的教程：追小球的云台 · OpenMV中文入门教程
在控制的过程中，使用PID算法可以更好地帮助我们控制云台，使云台在修正追踪的过程中变得更加丝滑，使追踪效果更好。
下面我复制了部分代码，来展示主要的思路和控制方法。
#pwm初始化
pitch_pin_1=15 #竖直舵机
roll_pin_2=14 #水平舵机
tim0= Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM) #定时器0
pitch_pwm = PWM(tim0, freq=50, duty=0, pin=pitch_pin_1)
tim2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM) #定时器1
roll_pwm = PWM(tim2, freq=50, duty=0, pin=roll_pin_2)
#舵机初始化
pitch = SERVO(pitch_pwm, dir=init_pitch)
roll = SERVO(roll_pwm, dir=init_roll)
#pid初始化
pid_pitch = PID(minout=pitch_pid[0],maxout=pitch_pid[1],intergral_limit=pitch_pid[2],kp=pitch_pid[3], ki=pitch_pid[4], kd=pitch_pid[5] )
pid_roll = PID(minout=roll_pid[0],maxout=roll_pid[1],intergral_limit=roll_pid[2],kp=roll_pid[3], ki=roll_pid[4], kd=roll_pid[5])
#云台初始化
gimbal = GIMBAL(pitch, pid_pitch, roll, pid_roll)
"""关于舵机的类"""
class SERVO:
def __init__(self, pwm, dir=50, duty_min=2.5, duty_max=12.5):
self.value = dir #【0~100】
self.pwm = pwm #pwm的类
self.duty_min = duty_min
self.duty_max = duty_max
self.duty_range = duty_max -duty_min+2
self.enable(True)
self.pwm.duty(self.value/100*self.duty_range+self.duty_min) #PWM空比
def enable(self, en): #使能
if en:
self.pwm.enable()
else:
self.pwm.disable()
#dir方法用于手动控制舵机的角度。
#传入参数percentage表示目标角度百分比（0到100），舵机将根据该百分比计算出相应的PWM占空比，并将舵机转动到对应的角度位置。
def dir(self, percentage): #手动控制角度调用 输入的dir的值可以手动调整
if percentage > 100: #限制值
percentage = 100
elif percentage < 0:
percentage = 0
self.pwm.duty(percentage/100*self.duty_range+self.duty_min)
#drive方法用于连续控制舵机的角度。传入参数add表示需要增加或减少的角度值。根据add的正负，舵机将增加或减少相应的角度，并根据最小最大范围进行限制，以确保不超出舵机的可行范围。
#最后，舵机的位置将根据计算出的角度值进行调整。
def drive(self, add): #连续控制角度调用
print("value:",self.value,"add:",add) #打印值
self.value = self.value+add
if self.value > 100:
self.value = 100
elif self.value < 0:
self.value = 0
#print("value:",self.value,"add:",add)
self.pwm.duty(self.value/100*self.duty_range+self.duty_min)
"""位置式PD的类"""
class PID:
def __init__(self,minout,maxout,intergral_limit, kp, ki, kd):#设置PID值
self.p = kp
self.i = ki
self.d = kd
self.Minoutput = minout #设置最大最小输出值
self.MaxOutput = maxout
self.IntegralLimit = intergral_limit #整体限制
self.pout=0 #P输出
self.iout=0 #I输出
self.dout=0 #D输出
self.delta_u=0
self.delta_out=0
self.last_delta_out=0
self._set=[0,0,0]
self._get=[0,0,0]
self._err=[0,0,0]
#这部分代码定义了一个PID（位置式比例-积分-微分）控制器的类。
#在初始化时，通过传入参数进行PID控制器的参数设置，包括P（比例）、I（积分）、D（微分）系数，输出值的最大最小范围，以及积分项的限制。
#这些参数在PID控制中用于计算输出，使得控制系统能够快速响应并稳定在目标值附近。
def pid_calc(self, _get, _set) :
self._err[2] = _set - _get
self.pout =self.p * self._err[2]
self.dout = self.d * (self._err[2] - self._err[1])
self.delta_out = self.pout + self.iout +self.dout
self._err[0] = self._err[1]
self._err[1] = self._err[2]
return self.delta_out
效果展示
视觉追踪