电赛：二维云台控制 |
您所在的位置：网站首页 › 云台控制协议怎么设置 › 电赛：二维云台控制 |
电赛：二维云台控制
|
目录 前言： 前期选择： 通信控制方案 （一）串口通信 （二）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 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 效果展示视觉追踪 |
【本文地址】
今日新闻 |
推荐新闻 |