代码收藏家 OpenMV识别红色物体，STM32单片机接收坐标，PID控制舵机云台
本人搜索了有关于舵机云台pid控制的代码，但是都没有搜到想要的结果，现在自己写出来了代码，所以就将自己写的代码分享出来，和大家一起学习进步。
1.openmv识别红色物体+返回中心坐标的的代码
import sensor,image,time,math,pyb,lcd
from pyb import UART,LED
import json
import ustruct
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)#QQVGA的分辨率为120*160
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) #关闭白平衡
sensor.set_vflip(True)#垂直方向翻转
red_threshold_01=(18, 56, 103, 35, -57, 116)
clock = time.clock()
uart = UART(3,9600) #定义串口3变量
uart.init(9600, bits=8, parity=None, stop=1) # init with given parameters
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
def sending_data(cx,cy,cw,ch):
global uart;
data = ustruct.pack("<bbhhhhb", #格式为俩个字符俩个短整型(2字节)
0x2C, #帧头1
0x12, #帧头2
int(cx), # up sample by 4 #数据1
int(cy), # up sample by 4 #数据2
int(cw), # up sample by 4 #数据1
int(ch), # up sample by 4 #数据2
0x5B)
uart.write(data); #必须要传入一个字节数组
led = pyb.LED(3)
while(True):
led.on()
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([red_threshold_01])
if blobs:
max_b = find_max(blobs)
cx=max_b[5]
cy=max_b[6]
cw=max_b[2]
ch=max_b[3]
img.draw_rectangle(max_b.rect(),color=(255,0,0)) # rect
img.draw_cross(max_b[5], max_b[6]) # cx, cy
FH = bytearray([0x2C,0x12,int(cx),int(cy),int(cw),int(ch),0x5B])
uart.write(FH)
print(cx,cy,cw,ch)
lcd.display(img)
else:
FH = bytearray([0x2C,0x12,0,0,0,0,0x5B])
uart.write(FH)
img.draw_string(0,0,"no object",color = (120,0,0))
lcd.display(img)
注意：
（1）sensor.set_framesize(sensor.QQVGA)#QQVGA的分辨率为120*160，这行代码是设置分辨率为120*160,QQVGA的分辨率为120*160，QVGA的分辨率为320*240，不同的分辨率对应stm32端的代码也不同，在追踪云台的代码里，分辨率不能设的太高，太高的话，虽然画质会更加清晰，但是画面会出现延时，响应会变慢。
（2）sensor.set_vflip(True)#垂直方向翻转，这行代码是将openmv传回的画面垂直翻转，因为我们的openmv是倒置放的，如果不进行垂直翻转，在电脑端显示的画面就是颠倒的。这行代码是否书写也会影响到后面stm32端代码的书写。
2.stm32通过pid控制舵机云台的代码
（1）usart2.c代码
#include "usart2.h"
#include "usart.h"
#include "led.h"
int Cx,Cy,Cw,Ch;
void uart2_init(u32 bound)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource2,GPIO_AF_USART2);
GPIO_PinAFConfig(GPIOA,GPIO_PinSource3,GPIO_AF_USART2);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOA,&GPIO_InitStructure);
USART_InitStructure.USART_BaudRate = bound;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART2, &USART_InitStructure);
USART_Cmd(USART2, ENABLE);
USART_ClearFlag(USART2, USART_FLAG_TC);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=3;
NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void USART2_IRQHandler(void)
{
u8 com_data;
u8 i;
static u8 RxCounter1=0;
static u16 RxBuffer1[10]={0};
static u8 RxState = 0;
static u8 RxFlag1 = 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)
{
RxState=1;
RxBuffer1[RxCounter1++]=com_data;
}
else if(RxState==1&&com_data==0x12)
{
RxState=2;
RxBuffer1[RxCounter1++]=com_data;
}
else if(RxState==2)
{
RxBuffer1[RxCounter1++]=com_data;
if(RxCounter1>=10||com_data == 0x5B)
{
RxState=3;
RxFlag1=1;
Cx=RxBuffer1[RxCounter1-5];
Cy=RxBuffer1[RxCounter1-4];
Cw=RxBuffer1[RxCounter1-3];
Ch=RxBuffer1[RxCounter1-2];
}
}
else if(RxState==3)
{
if(RxBuffer1[RxCounter1-1] == 0x5B)
{
USART_ITConfig(USART2,USART_IT_RXNE,DISABLE);
if(RxFlag1)
{
//printf("Cx=%d\r,Cy=%d\r,Cw=%d\r,Ch=%d\r\n",Cx,Cy,Cw,Ch);
//printf("xerror=%d, yerror=%d\r\n",80-Cx,60-Cy);
}
RxFlag1 = 0;
RxCounter1 = 0;
RxState = 0;
USART_ITConfig(USART2,USART_IT_RXNE,ENABLE);
}
else
{
RxState = 0;
RxCounter1=0;
for(i=0;i<10;i++)
{
RxBuffer1[i]=0x00;
}
}
}
else
{
RxState = 0;
RxCounter1=0;
for(i=0;i<10;i++)
{
RxBuffer1[i]=0x00;
}
}
}
}
这是接受openmv端传回坐标的代码，此代码是借鉴网上其他博主写的代码，感兴趣的小伙伴也可搜索查阅。
（2）pid.c的代码
#include "pid.h"
//x轴舵机的pid运算
float Kp_x=2, //2
Ki_x=0.15, //0.15
Kd_x=2; //2
float www,zzz;
int pwm_xpid(int xerror)
{
int pid_ActualPwm;
static float pid_Integral,pid_Voltage,error_Last;//注意这里需要使用static关键字
pid_Integral+=xerror;
www=pid_Integral;
if (pid_Integral<-6000) pid_Integral=-6000;//积分限幅
if (pid_Integral>6000) pid_Integral=6000;//积分限幅
pid_Voltage=Kp_x*xerror+Ki_x*pid_Integral+Kd_x*(xerror-error_Last);
error_Last=xerror;
pid_ActualPwm=pid_Voltage*1;
if (pid_ActualPwm<-1000) pid_ActualPwm=-1000;//pwm的范围是500到2500,这里要对pwm进行限幅
if (pid_ActualPwm>1000) pid_ActualPwm=1000;
return pid_ActualPwm;
}
//y轴舵机的pid运算
float Kp_y=1, //1
Ki_y=0.15, //0.15
Kd_y=2; //2
int pwm_ypid(int yerror)
{
int pid_ActualPwm;
static float pid_Integral,pid_Voltage,error_Last;//注意这里需要使用static关键字
pid_Integral+=yerror;
zzz=pid_Integral;
if (pid_Integral<-6000) pid_Integral=-6000;//积分限幅
if (pid_Integral>6000) pid_Integral=6000;//积分限幅
pid_Voltage=Kp_y*yerror+Ki_y*pid_Integral+Kd_y*(yerror-error_Last);
error_Last=yerror;
pid_ActualPwm=pid_Voltage*1;
if (pid_ActualPwm<-1000) pid_ActualPwm=-1000;//pwm的范围是500到2500,这里要对pwm进行限幅
if (pid_ActualPwm>1000) pid_ActualPwm=1000;
return pid_ActualPwm;
}（3）timer.c代码
#include "timer.h"
#include "led.h"
#include "pid.h"
#include "usart.h"
int pid_xerror,pid_yerror,xpwm,ypwm;
extern int Cx,Cy;
void TIM3_Int_Init(u16 arr,u16 psc)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
TIM_TimeBaseInitStructure.TIM_Period = arr;
TIM_TimeBaseInitStructure.TIM_Prescaler=psc;
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);
TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);
TIM_Cmd(TIM3,ENABLE);
NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
{
if (Cx>0&&Cy>0)//Cx、Cy分别为红色物体中心点的横、纵坐标，当Cx、Cy都大于0时代表识别到红色物体
{ pid_xerror=80-Cx;//80为画面中心点的横坐标，这行代码是计算红色物体中心点横坐标离画面中心点横坐标的偏差值
pid_yerror=60-Cy;//60为画面中心点的纵坐标，这行代码是计算红色物体中心点纵坐标离画面中心点纵坐标的偏差值
xpwm=pwm_xpid(pid_xerror);//通过pid计算得到x轴舵机运动的pwm值
ypwm=pwm_ypid(pid_yerror);//通过pid计算得到y轴舵机运动的pwm值
TIM_SetCompare1(TIM4,1500-xpwm);//1500对应x轴舵机转到90度，这行代码是让x轴舵机转到对应的角度
TIM_SetCompare2(TIM4,1500-ypwm);//1500对应y轴舵机转到90度，这行代码是让y轴舵机转到对应的角度
}
else
{
TIM_SetCompare1(TIM4,1500-xpwm);//这两行代码是当没有识别到红色物体时，舵机在当前位置停下
TIM_SetCompare2(TIM4,1500-ypwm);
}
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}
（4）pwm.c代码
#include "pwm.h"
#include "led.h"
#include "usart.h"
void TIM4_PWM_Init(u32 arr,u32 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(GPIOB,&GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Prescaler=psc;
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period=arr;
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM4,&TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
TIM_ARRPreloadConfig(TIM4,ENABLE);
TIM_Cmd(TIM4, ENABLE);
}
（5）main.c代码
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "timer.h"
#include "pwm.h"
#include "usart2.h"
//PA9----TX PA10-----RX
extern float www,zzz;
extern int xpwm;
extern int pid_xerror,pid_yerror,xpwm,ypwm;
extern void TIM4_PWM_Init(u32 arr,u32 psc);
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
delay_init(168); //初始化延时函数
LED_Init(); //初始化LED端口
uart_init(9600);
uart2_init(9600); // openmv stm32
// p4(TX)------A3(RX)
// p5(RX)------A2(TX)
TIM3_Int_Init(100-1,8400-1); //定时器3每10ms中断一次
TIM4_PWM_Init(20000-1,84-1); //舵机 20ms 0度---pwm50 90度---pwm150 180度-pwm250
TIM_SetCompare1(TIM4,1500);//让x轴舵机转到90度
TIM_SetCompare2(TIM4,1500);//让y轴舵机转到90度
while(1)
{
//printf("xpwm=%d\r",xpwm);
//printf("ypwm=%d\r\n",ypwm);
};
}
以上代码可直接复制到自己的工程中运行，openmv端识别的代码以及和32端通信的代码参考其他博主的代码，感兴趣的小伙伴可以自行搜索，欢迎大家在评论区评论，有什么问题可在评论区提问。
以下是完整的代码链接：
链接：https://pan.baidu.com/s/1Ijq4ViOlD4Ca8juhDBmQvw
提取码：ojx1
–来自百度网盘超级会员V4的分享
