滚球板球控制系统详解（openmv+stm32实现）

实现球板控制系统——基于openmv视觉处理+stm32主控

最近在着手准备全国大学生电子设计竞赛，于是就找往年的赛题作为练习。这一次我选的是17年电赛的题目——滚球控制系统，要求在一定的时间内对板子上的球位置进行控制，我做出来的效果如下图所示。

接下来就分享我从拿到试题到完成的过程。

1.分析赛题选型

首先阅题要仔细，阅读题目后第一件事就是要如何去实现基本要求。控制量是什么？结果是什么？可以采用什么方案？在这里我第一件事就是把方形板切割出来，我采用的是亚克力材料，也可以用雪费板。质量不要太重也不要太轻。这时候就要思考了，题目无非就是要我们控制小球在板子上面的坐标，这就有了两种方案：1.触摸板定位。2.使用视觉定位。在这里我选用了视觉定位，因为身边有openmv。然后就是我们做控制必不可少的电机选型了。这一次无疑要选用舵机了，因为舵机能精准控制角度。主控的话我选用stm32f103rc系列，因为题目无非就是两个舵机和串口，使用这类芯片不会造成资源浪费，主频有48m完全够用

这里我补充说明一个点就是视觉检测选用白板黑球还是使用黑板白球。经过我的实测，选用白板黑球的话对于识别不太有利的，因为灯光照在黑球上面会反光，以及周围太多黑色的物体对识别产生噪点了（比如影子）。所以下面我搭建机械结构的时候选用了白球，并且用黑色自动喷漆把板子喷成了黑色，这样识别效果非常好。

2.结构搭建

这一个步骤是整道题目最关键的地方了，可以说是控制半壁江山。结构搭得好，代母就很好调很容易完成要求。要是结构搭建不好，软件代码怎么写都没用。

首先给舵机装上舵机臂，让力矩可以传输出去。然后使用轴承把舵机臂与双文螺杆进行连接，然后用金属球头把双头螺杆与板子连接在一起（此处看不懂名字的可以直接去淘宝搜索）。这里结构搭建的要点是：1.舵机能顺利地把力矩转化成板子xy方向的转动，使得通过不哦那个角度的转动控制球的位置（原理和用乒乓球拍控制兵乓球一样）2.舵机上电的时候会瞎转，你要保证做好硬件防护措施，使得机械结构以及舵机不会因为舵机瞎转而破坏掉。

然后就是在板子的上方找位置固定openmv，固定前连接上电脑，使得能够拍摄到整块板子。

最后就是主控板了，我选用了一块我之前话的板子，上面有两个舵机接口引出，一个串口引出以及用于查看运行信息的oled接口。上面白色座子我是用来接无线调试器的，在这里我非常建议做控制的同学拥有一个自己的无线调试器，因为我们做控制要经常进行调参，代码经常编译下载，采用无线调试器能够在下载代码后马上观测现象而不用老是拔插调试器。

3.代码实现与pid调试

首先是openmv的代码，需要实现的功能是识别小球并且把小球的坐标通过串口发送出去，这里再调试的时候可以用ch340模块接到电脑用串口调试助手查看输出数据。

# Blob Detection and uart transport
import sensor, image, time
from pyb import UART
import json
# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
yellow_threshold   = (79, 100, -7, 6, 4, 41)
# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.

uart = UART(3, 115200)
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

while(True):
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([yellow_threshold])
    if blobs:
        max_blob=find_max(blobs)
        print('sum :', len(blobs))
        img.draw_rectangle(max_blob.rect())
        img.draw_cross(max_blob.cx(), max_blob.cy())

        output_str="[%d,%d]" % (max_blob.cx(),max_blob.cy()) #方式1
        #output_str=json.dumps([max_blob.cx(),max_blob.cy()]) #方式2
        print('you send:',output_str)
        uart.write(output_str+'\r\n')
    else:
        print('not found!')

不会使用openmv的建议看一下openmv入门

上面代码的yellow_threshold要填写小球的lab数值，需要通过openmvIDE直接测出来填写。
然后就是数据处理了，因为通过串口传输的数据都是字符串类型的，要进行坐标控制需要的是数值信息，所以需要自己写函数转化一下：

if(USART3_RX_STA&0x8000)
		{
     
                        u8 x=0,y=0,z=0;		
			len=USART3_RX_STA&0x3fff;//µÃµ½´Ë´Î½ÓÊÕµ½µÄÊý¾Ý³¤¶È
			//printf("\r\nÄú·¢Ë͵ÄÏûϢΪ:\r\n");
				for(t =0;t<len;t++)
			{
     
				USART3->DR=USART3_RX_BUF[t];
				while((USART3->SR&0X40)==0);//µÈ´ý·¢ËͽáÊø
				if(USART3_RX_BUF[t]=='[')x=1,y=0;
				if(USART3_RX_BUF[t]==','){
     x=0,y=1;xx[t-1]='\0';}	
				if(USART3_RX_BUF[t]==']'){
     yy[z]='\0';x=0,y=0,z=0;};
				if(x==1&&USART3_RX_BUF[t]!='[')xx[t-1] = USART3_RX_BUF[t];
		  	if(y==1&&USART3_RX_BUF[t]!=','){
     yy[z] = USART3_RX_BUF[t]; z++;};
			}
			for(t=0;t<2;t++)
			{
     
				USART3->DR=USART3LAST[t];
				while((USART3->SR&0X40)==0);//µÈ´ý·¢ËͽáÊø
			}
	//		printf("\r\n\r\n");//²åÈë»»ÐÐ
			Xt=atoi(&xx[0]);
			Yt=atoi(&yy[0]);
			OLED_ShowNum(0,52,Yt,3,12);
			OLED_ShowNum(64,52,Xt,3,12);
  //  	OLED_Refresh_Gram();
			stop=0;
			USART3_RX_STA=0;
		}	
		if(stop>25)
		{
     
			OLED_ShowString(0,0,"NOFOUND",24);
			OLED_Float(40,0,pitch,3);
	                OLED_Refresh_Gram();
			control=0;
			
		}
		else
		{
     
			delay_ms(10);
		        OLED_ShowString(0,0,"FINDTAG",24);
			mpu_dmp_get_data(&pitch,&roll,&yaw);
//			OLED_Float(40,0,pitch,3);
//			OLED_Float(40,64,roll,3);
	                OLED_Refresh_Gram();
			control=1;
		}

这里是我再主函数循环里写的串口3接收函数，功能是接收openmv传输过来的xy坐标信息并且转化成整型类型的数值，把数字传送给舵机控制函数进行pid控制。参数control=1的时候就会进行pid调节，当openmv没有识别到小球的时候control=0。
最后就是pid控制函数了，他实现的功能就是通过小球坐标信息作为反馈，实时控制舵机转动而调节小球位置。

extern float Xt,Yt,pwmva,pwmvb;  //     1000=
float media=1340,medib=1380,max=100; 
float X0=78,Y0=57,set=0;         //×ø±êÖÐÖµ
float kp=450,ki=0,kd=290;    //ˮƽÃæ·½Ïò
float kp2=450,ki2=0,kd2=290;  //ÊúÖ±·½Ïò
//float x1,y1,xdiffer,ydiffer,y1last,x1last;
float dia=0,dib=0;

void pida(void)
{
     
	static float error,ierror,derror,errorlast;
	error=X0-Xt;
	ierror=ierror+error;
	derror=error-errorlast;
	errorlast=error;
	if(ierror>3000) ierror=3000;
	else if(ierror<-3000) ierror=-3000;
	dia=kp*error/100+ki*ierror+kd*derror/10;   //P:
//	if(dia>10) dia=10;
//	else if(dia<-10) dia=-10;
}

void pidb(void)
{
     
	static float error,ierror,derror,errorlast;
	error=Y0-Yt;
	ierror=ierror+error;
	derror=error-errorlast;
	errorlast=error;
	if(ierror>3000) ierror=3000;
	else if(ierror<-3000) ierror=-3000;
	dib=kp2*error/100+ki2*ierror+kd2*derror/10;        
}

int absa(int a) 
{
     	

	if(a<media-max)pwmva=media-max;    //×ó±ß                  //1330
	if(a>media+max)pwmva=media+max;		//ÓÒ±ß
	return pwmva;
}

int absb(int b)        
{
     

	if(b<medib-max)pwmvb=medib-max;   //1380
	if(b>medib+max)pwmvb=medib+max;		
	return pwmvb;
}


void setpwm(void)
{
     
	pwmva=media+dia;
	pwmvb=medib-dib;
	TIM_SetCompare3(TIM3,absb(pwmvb));
    TIM_SetCompare4(TIM3,absa(pwmva));	
}

void contro(void)
{
     
	pida();
	pidb();
	set=!set;
	if(set==1)setpwm();
}

pid参数整定的方法：因为舵机有两个xy方向，我们需要股东一个方向进行调节，一个方向成功后再换另一个方向。首先ki、kd设为0.调节kp使得小球再中间位置震荡，然后调节kd使得小球稳定，最后可以加ki进行修饰，也可以不加。最后实现的效果是再任意点投放小球，系统都能够在短时间内将小球稳定到板子中心。到了这里就大功告成了，题目的其他要求就显得很简单了，只要我把中值坐标X0、Y0进行动态的调节，小球就能按照要求动起来。