滚球控制系统详解 —— (附核心代码)

最近练习了17年的国赛题 —— 滚球控制系统

这里展示一下画圆：

观看完整视频点这里

接下来，我来分享一下从搭整体结构到调试完的过程。

这是我搭完的整体结构 （缩小版）

(不管什么题，结构部分还是很重要的，结构搭得好，代码上就可能会轻松很多。)

(第一次搭这种稍大型的结构，对于搭结构小白的我来说还是有点难度的)

 最顶上的板我是用来挡光的，因为白天光线强，晚上弱，openMV跟踪颜色的阈值有点难调。 

底部固定部分的是万向结 + 一些螺丝固定住。其他部分大多都是亚克力板和热熔胶固定住的。

一些要注意的点：

一开始，我把舵机的两个力臂刚刚好固定在了黑板的对角线上,像下图这样子。

 然后发现有的点控制效果很不好，如果单单想控制X或Y方向，两个舵机都得动，这样就比较麻烦，而且在代码上会很费劲，所以就把顶部黑板部分旋转了45°这样，下图所示。

 一个舵机只负责一个轴的控制，这样就简单多了。

另外我们要注意舵机的响应速度，在这道题中，舵机的响应速度快当然是更好些。我在网上看到提高舵机响应速度的方法是提高供电电压，虽说舵机的供电电压范围在5~6.8V之间，但我把供电部分弄到了差不多7V(使用起来没出现什么问题)，个人觉得明显会快些。

这是我的主控和供电驱动部分

我选择的主控是MSP432P401R，先熟悉一下Ti的控制板，以至于23年国赛遇到Ti的题目不会慌。

右边部分其实就是个BUCK电路，另外我加了个TB6612模块的插座上去，这样方便其他场景需要电机使用。

这是我控制平衡的核心部分代码

if(val_1 > 0)   
{
	val_1 = (val_1 > 600) ? 600 : val_1;      
	MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,1900 - val_1);
}
else						
{
	val_1 = -val_1;
	val_1 = (val_1 > 600) ? 600 : val_1;
	MAP_Timer_A_setCompareValue(TIMER_A2_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_1,1900 + val_1);
}

if(val_2 > 0)		
{
	val_2 = (val_2 > 600) ? 600 : val_2;
	MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2,1900 + val_2);
}
else						
{
	val_2 = -val_2;
	val_2 = (val_2 > 600) ? 600 : val_2;
	MAP_Timer_A_setCompareValue(TIMER_A1_BASE,TIMER_A_CAPTURECOMPARE_REGISTER_2,1900 - val_2);
}
	

1900是我使黑板水平时，舵机需要的占空比。限幅600是舵机从最高点到最低点的占空比范围是1300~2500。正负号的话，可以根据实际运动极性改就好。

然后就是调PID的过程了，我这里P只给了2点多，然后D调到30多这样，I没有用上。

到这里我的小球就可以较好的平衡了。

想让小球在哪个点平衡，只需要把修改两个舵机PID的目标值为对应坐标点就好。

接下来，我这里讲一下本题的难点——画圆

(这部分我一开始是想通过很多个点来组成一个圆，但我实际做出来的效果不太好，经师兄指点，换了另外一种方法，效果好多了。)

画圆部分，我这里用到了正弦和余弦两个函数。高中的时候，我们学过圆的参数化方程

 x² + y² = r² ，所以我们让一个舵机做正弦运动，另外一个所余弦运动，两者叠加便是圆周运动。

如果两个舵机都做正弦或者余弦运动，两者叠加便是直线运动。

这里附上我画圆部分的核心代码, （163，123）是我的圆心

 if(draw_round1 == 1)  //正向画圆
 {
	 t++;
		p = k * t;
		set_pid_target_1( 163 + (35 * _sin(p) ));	set_pid_target_2( 123 + (35 * _cos(p)) );
		if( p >= 6.28)
			t = 0; 
 }
 
 if(draw_round2 == 1)  //逆向画圆
 {
	 t++;
		p = k * t;
		set_pid_target_1( 163 + (35 * _sin(p) ));	set_pid_target_2( 123 - (35 * _cos(p)) );
		if( p >= 6.28)
			t = 0; 
 }

t是用来计数的，我这里每5ms中断一次，t也增加一次。

k的定的是浮点数0.13f，增大k可以增加画圆的速度。

这里有一点需要注意：

上面计算正弦和余弦，我这里用的是查找表法。

（可能有小伙伴没听说过这个词，我这里简单说说。查找表就是查表，我们把所有出现的结果的值用数组存起来，这样，在我们进行计算的时候，直接去读这个结果就好，而不用花太多时间去计算，这样既减轻了MCU负担又能得到更好的控制效果）

我的查找表用的是这位博主写的   ———  copy一下就能用，非常方便 0.0

就写到这了，有什么想问的可以在评论区告诉我 。