电赛备赛前，通过OpenMV加舵机云平台由，做了一个追着球跑的小车，由于疫情，以前录制的视频也删除了，最终呈现的效果和B站一位Up主的相似，大家可以参考参考，链接如下：STM32 颜色识别 自动跟随小车_哔哩哔哩_bilibili，首先把我使用到的硬件的图片给大家看看。
电机的驱动我是用的是两路PWM波控制一个电机，OpenMV板子上面的两路PWM波控制云台的转动，小车跟随云台的转动通过两块板子之间的通信，同时物体与摄像头的距离也通过通信发送给STM32,距离和小车转动都通过PID的调节。
首先我们看Openmv上面的代码：
import sensor, image, time
from pyb import UART
from pid import PID
from pyb import Servo
pan_servo=Servo(1)
tilt_servo=Servo(2)
#pan_servo.calibration(500,2500,500)
#tilt_servo.calibration(500,2500,500)
pan_pid = PID(p=0.15, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.15, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
#pan_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
#tilt_pid = PID(p=0.1, i=0, imax=90)#在线调试使用这个PID
def find_max(blobx):
max_size=0
for blob in blobx:
if blob[2]*blob[3] >max_size:
global max_blob
max_blob=blob
max_size=blob[2]*blob[3]
return max_blob
yellow_threshold = (12, 100, 11, 127, -65, 0)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
#sensor.skip_frames(time = 2000)
sensor.skip_frames(10)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()
K=685
uart = UART(1, 115200)
uart_buf=[]
while(True):
clock.tick()
img = sensor.snapshot()
#img = sensor.snapshot().lens_corr(1.8)
#for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,
#r_min = 2, r_max = 100, r_step = 2):
#area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
##area为识别到的圆的区域，即圆的外接矩形框
#statistics = img.get_statistics(roi=area)#像素颜色统计
#if 45<statistics.l_mode()<100 and -102<statistics.a_mode()<58 and 49<statistics.b_mode()<96:#if the circle is red
#img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))
blobx = img.find_blobs([yellow_threshold])
if blobx:
b = find_max(blobx)
pan_error = b.cx()-img.width()/2
tilt_error = b.cy()-img.height()/2
#img.draw_rectangle(b[0:4]) # rect
#img.draw_cross(b[5], b[6]) # cx, cy
Lm = (b[2]+b[3])/2
length = K//Lm
img.draw_cross(int(b[5]),int(b[6]),color=(0,255,0)) #色块中心坐标CX,CY
print('cx0:'+str(b[5]),'cy0:'+str(b[6])) #将色块中心坐标输出
img.draw_rectangle(b.rect())
img.draw_cross(b.cx(), b.cy())
pan_output=pan_pid.get_pid(pan_error,1)/2
tilt_output=tilt_pid.get_pid(tilt_error,1)
pan_servo.angle(pan_servo.angle()+pan_output)
tilt_servo.angle(tilt_servo.angle()-tilt_output)
print(int(length))
uart_buf =bytearray([0x6B,b[5],b[6],int(length),0x6A])
uart.write(uart_buf)
#uart.writechar(0x6B)这段是摄像头检测物体并计算出物体与摄像头距离的代码，我目前是寻找色卡，本来这是我做的一个自动捡球小车，但是发现，如果我把寻找轮廓和颜色一起加入到代码当中后，云台舵机寻找物体的转动会不丝滑，因此只采用了颜色的追踪。具体可以去参考OpenMV的官方例程，这里主要的是通信 uart_buf =bytearray([0x6B,b[5],b[6],int(length),0x6A]) bytearray将参数转换为一个新的字节数组，b[5],b[6]这两个参数主要是物体在摄像头画面中的x和y轴，即为物体存在的坐标，程序找到物体后会将物体画面居中，当物体移动将会产生一个变化量，我们就通过这个变化量去控
制车与车上云台的偏移角度。uart.write(uart_buf)这一段就是将我们的信息发送给STM32。
int openmv[5]; //stm32接收数据数组
int16_t OpenMV_RX; /*OPENMV X 轴反馈坐标*/
int16_t OpenMV_RY; /*OPENMV X 轴反馈坐标*/
int8_t distan;
int i=0;
void Openmv_Receive_Data(int16_t data) //接收Openmv传过来的数据
{
static u8 state = 0;
if(state==0&&data==0x6B)
{
state=1;
openmv[0]=data;
}
else if(state==1)
{
state=2;
openmv[1]=data;
}
else if(state==2)
{
state=3;
openmv[2]=data;
}
else if(state==3)
{
state = 4;
openmv[3]=data;
}
else if(state==4) //检测是否接受到结束标志
{
if(data == 0x6A)
{
state = 0;
openmv[4]=data;
Openmv_Data();
data=0;
}
else if(data != 0x6A)
{
state = 0;
for(i=0;i<8;i++)
{
openmv[i]=0x00;
}
}
}
else
{
state = 0;
data=0;
for(i=0;i<8;i++)
{
openmv[i]=0x00;
}
}
}这里即为通信的代码。最后重点为PID的运算，云台控制的PID直接是使用的OpenMV官方代码，大家也可以参考官方的例程追小球的云台 · OpenMV中文入门教程。
这里为PID控制小车与物体距离的计算：
int detPID1_PWM_out(void)//前进后退PID
{
float time ; //记录时间，配合millis函数用来计时
float echo_value; //echo返回的值，用来计算距离
float target=12; //目标距离
float error; //当前的误差
float error_pre; //上一次的误差
float kp=15; //pid的参数
float ki=0.08; //pid的参数
float kd=0; //pid的参数
float P; //比例项误差
float I;//积分项误差
float D; //微分项误差
//误差总和，用来驱动马达
int deta_t=50; //50ms计算一次
error = distan - target;
P = kp*error; //P
//积分分离，根据实际情况，防止不断累加而产生震荡
if(error > 0 && error < 0.8) ki = 0;
if(error < 0 && error > -0.8)ki = 0;
if(error == 0 )ki =0;
else ki = 0.08;
if(-10 < error && error < 10) I += ki*error;
else I = 0; //I ,在一定误差内I才作用
//D = kd*((error - error_pre)/deta_t); //D，误差的变化率
D = 0; //这里没有用到D项，因为没有突然的变化可以不需要用D项
PID=P + I + D ; //PID
/*限幅*/
if (PID>100) {
PID=20;
return PID;
}
if (PID<-100)
{
PID=-20;
return PID;
}
if (PID==0)
{
PID=0;
return PID;
}
error_pre = error; //记录此次误差为上一刻误差
}这里为PID控制云台转动与小车转动角度：
int detPID2_PWM_out(void)//对正车位PID
{
float time ; //记录时间，配合millis函数用来计时
float echo_value; //echo返回的值，用来计算距离
float target=70; //目标距离
float error; //当前的误差
float error_pre; //上一次的误差
float kp=15; //pid的参数
float ki=0; //pid的参数
float kd=0; //pid的参数
float P; //比例项误差
float I;//积分项误差
float D; //微分项误差 //误差总和，用来驱动马达
int deta_t=50; //50ms计算一次
error = OpenMV_RX - target;
P = -kp*error; //P
//积分分离，根据实际情况，防止不断累加而产生震荡
if(error > 0 && error < 0.8) ki = 0;
if(error < 0 && error > -0.8)ki = 0;
if(error == 0 )ki =0;
else ki = 0.08;
if(-10 < error && error < 10) I += ki*error;
else I = 0; //I ,在一定误差内I才作用
//D = kd*((error - error_pre)/deta_t); //D，误差的变化率
D = 0; //这里没有用到D项，因为没有突然的变化可以不需要用D项
PID=P + I + D ; //PID
/*限幅*/
if (PID>100) {
PID=40;
return PID;
}
if (PID<-100)
{
PID=-40;
return PID;
}
if (PID==0)
{
PID=0;
return PID;
}
error_pre = error; //记录此次误差为上一刻误差
}我的PID是直接等同于PWM的占空比的，为了大家能清晰理解，小车的控制代码如下：文章来源：https://www.toymoban.com/news/detail-412005.html
u8 stop = 0;
/************************************************
正反待定
TIM3:
PA6:前左轮 TIM3_CH1
PA7: TIM3_CH2
PB0:前右轮 TIM3_CH3
PB1: TIM3_CH4
TIM4:
PA2:后左轮 TIM2_CH3
PA3: TIM2_CH4
PB8:后右轮 TIM4_CH3
PB9: TIM4_CH4
************************************************/
void Car_advance()
{
//前进
TIM_SetCompare1(TIM3, PID);
TIM_SetCompare2(TIM3, stop);
TIM_SetCompare3(TIM3, PID);
TIM_SetCompare4(TIM3, stop);
TIM_SetCompare3(TIM2, PID);
TIM_SetCompare4(TIM2, stop);
TIM_SetCompare3(TIM4, PID);
TIM_SetCompare4(TIM4, stop);
//USART1_Send_String("1");
}
void Car_Back_off()
{
//后退
TIM_SetCompare1(TIM3, stop);
TIM_SetCompare2(TIM3, PID);
TIM_SetCompare3(TIM3, stop);
TIM_SetCompare4(TIM3, PID);
TIM_SetCompare3(TIM2, stop);
TIM_SetCompare4(TIM2, PID);
TIM_SetCompare3(TIM4, stop);
TIM_SetCompare4(TIM4, PID);
//USART1_Send_String("2");
}
void Car_right()
{
//右转
TIM_SetCompare1(TIM3, PID);
TIM_SetCompare2(TIM3, stop);
TIM_SetCompare3(TIM3, stop);
TIM_SetCompare4(TIM3, PID);
TIM_SetCompare3(TIM2, PID);
TIM_SetCompare4(TIM2, stop);
TIM_SetCompare3(TIM4, stop);
TIM_SetCompare4(TIM4, PID);
//USART1_Send_String("3");
}
void Car_left()
{
//左转
TIM_SetCompare1(TIM3, stop);
TIM_SetCompare2(TIM3, PID);
TIM_SetCompare3(TIM3, PID);
TIM_SetCompare4(TIM3, stop);
TIM_SetCompare3(TIM2, stop);
TIM_SetCompare4(TIM2, PID);
TIM_SetCompare3(TIM4, PID);
TIM_SetCompare4(TIM4, stop);
//USART1_Send_String("3");
}
void Car_Stop()
{
//停止
TIM_SetCompare1(TIM3, 0);
TIM_SetCompare2(TIM3, 0);
TIM_SetCompare3(TIM3, 0);
TIM_SetCompare4(TIM3, 0);
TIM_SetCompare3(TIM2, 0);
TIM_SetCompare4(TIM2, 0);
TIM_SetCompare3(TIM4, 0);
TIM_SetCompare4(TIM4, 0);
//USART1_Send_String("3");
}
/************************************************
判断车左偏还是右偏
主要是判断OpenMV_RX的值：范围在（0~150左右），OpenMV_RY代表上下的值，因为球在地下所以不需要判断默认值在（50~55左右），上为增加，下为减少，整体范围一致
默认状态在90左右，左偏为增加，右偏为减少
************************************************/
void Car_azimuth(void)
{
//左偏：这个值需要测量可能每个值不一样
if(OpenMV_RX>80&&OpenMV_RX!=0)
{
detPID2_PWM_out();
Car_left();
}
//右偏
else if(OpenMV_RX<60&&OpenMV_RX!=0)
{
detPID2_PWM_out();
Car_right();
}
else if(OpenMV_RX>60&&OpenMV_RX<80)
{
if(distan>12)
{
detPID1_PWM_out();
Car_advance();
}
else if(distan<12)
{
detPID1_PWM_out();
Car_Back_off();
}
else
{
Car_Stop();
}
}做一个带云台追踪小车的项目就到此为止，作者当初做这个项目的时候也参考了许多的博主，东拼西凑完成了这个项目，还有不理解的部分，欢迎大家在评论区留言，如果有错误的地方，也请各路大佬指点，谢谢大家。文章来源地址https://www.toymoban.com/news/detail-412005.html
到了这里，关于基于OpenMV和正点原子开发的自动追球小车（带云台舵机）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
