基于OpenMV和正点原子开发的自动追球小车（带云台舵机）_openmv追小球的云台

作者：我家自动化 | 2024-07-14 17:06:20

踩

openmv追小球的云台

电赛备赛前，通过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的占空比的，为了大家能清晰理解，小车的控制代码如下：


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();
		}	
	}

做一个带云台追踪小车的项目就到此为止，作者当初做这个项目的时候也参考了许多的博主，东拼西凑完成了这个项目，还有不理解的部分，欢迎大家在评论区留言，如果有错误的地方，也请各路大佬指点，谢谢大家。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家自动化/article/detail/825660