板球控制系统调试过程及代码编写

一只活蹦乱跳的大鲤鱼

已于 2022-03-06 09:10:55 修改

阅读量2k

点赞数 3

分类专栏： STM32 文章标签： stm32 单片机嵌入式硬件 PID

于 2022-03-05 16:36:02 首次发布

本文链接：https://blog.csdn.net/lgyLGY35/article/details/119279615

版权

STM32 专栏收录该内容

30 篇文章 14 订阅

订阅专栏

问题

舵机的速度非常慢，而且只有 Kp的时候，

在这里插入图片描述
异常的振荡，首先板子两边加了围墙，这样掉不下去，所以球就在板子上来回满地图跑来跑去，不停地振荡。根本没有效果。加了D之后也特别抖，用手拨一下，拨得稍微大一点之后，就会反向运动好长，超调很大，没有挡板的话直就掉下去了，舵机反应很慢。静差也不小。

速度慢，有一个问题是每次计算出pid之后，舵机不是在平衡的位置加减的，而是在上一次的位置加减，这样速度会慢很多。（这里没有这么用，但是可以记录一下这个坑）
另外，之前的思路是

openmv程序设成20ms发送一次坐标，串口中断接收存到全局变量里。控制逻辑写在定时器2中断里，每40ms进一次，先接收数据，然后计算pid（是个角度），然后把这个角度给封装好的定时器的函数传进去计算出CCR值，对应占空比，给TIM3赋值。

首先，这里40ms有点长了，22ms可以
第二，直接用PWM就好了，换成角度，再换回来，其实是多次一举了。

下面针对两个问题具体解决

软件——舵机PID计算

初始思路（行不通）

/************************************
0.5ms-------2.5%---------0度；1000 ；
1ms ----------5%-------45度；2000 ；
1.5ms--------7.5%--------90度；3000 ；
2ms ----------10%-------135度；4000 ；
2.5ms --------12.5%-------180度；5000 ；
*************************************************/

封装好一个计算舵机角度的函数，直接给角度就好了。

#define Servo_Period 4000-1 //Servo_Duty*0.1
#define Servo_Static 1000-1 //*0.025


oid Servo_ANGLE_CH12(float angle1,float angle2)
{
	float temp1 = 0,temp2 = 0;
	angle1 = angle1 > 180 ? 180 : angle1; 
	angle2 = angle2 > 180 ? 180 : angle2; 
	temp1 = (angle1 / 180) * Servo_Period + Servo_Static;
	temp2 = (angle2 / 180) * Servo_Period + Servo_Static;
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, temp1);
	__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2, temp2);

}
然后，进行PID计算

/**********************
首先openmv把绝对坐标传回来，放到Servo_pid.x_curr_posi中，
然后在xPID_Realize()函数中，Servo_pid.x_curr_posi减去绝对坐标Servo_pid.x_absolute_posi（）
即可得到相对中心点的绝对坐标
*********************************/
float xPID_Realize(float target , float measure) //num为设定的目标点
{
	float pid_out;
	float error;
	static float error_last;
	float pteam,dteam;
	static float iteam;
	//计算相对坐标
	measure -= x_Servo_pid.absolute_posi;
	error = target - measure;
	pteam = x_Servo_pid.Kp*error;
	iteam += x_Servo_pid.Ki*error;
	dteam = x_Servo_pid.Kd*(error - error_last);	
	//积分限幅
	if(iteam > 30) iteam = 30;
	else if(iteam < -30) iteam = -30;
	pid_out = pteam + iteam + dteam;	
	//误差传递
	error_last = error;	
	return pid_out;
}

之后传值，

PID x_Servo_pid,y_Servo_pid;

float Position_x[9] = {-45,-45,-42,-1,0, 1, 46,49, 46};
float Position_y[9] = { 47, 0,-45,47,0,-46,47, 0,-46};
//控制函数
/************************
首先调用PID计算函数，求出有误差产生的PID的值pid_out
这是一个有正有负的值，pid_out加上balance就是现在需要的角度
那么，为了实现斜坡函数，即如果设定的值与当前的值的误差大于多少，误差就只认为有多少。

****************************/
void Servo_control(u8 x_i,u8 y_i)
{
	float temp[2];//用于斜坡函数、
	float step = 10.0;  //步长
	float x_pid,y_pid;
	temp[0] = x_Servo_pid.angle;
	temp[1] = y_Servo_pid.angle; //先获取当前的角度
	//获取增量
	x_pid = xPID_Realize(Position_x[x_i-1],x_Servo_pid.curr_posi);
	y_pid = yPID_Realize(Position_y[y_i-1],y_Servo_pid.curr_posi);
	//PIDOUT有正有负，加上板子平衡时的角度变成现在需要设定到的角度
	x_pid += x_Servo_pid.balance_angle;
	y_pid += y_Servo_pid.balance_angle;
	//斜坡函数
	    //设定与现在的角度的差值：这次需要设定到x_pid角度，而实际在x_Servo_pid.angle
	temp[0] = x_pid - x_Servo_pid.angle;
	temp[1] = y_pid - y_Servo_pid.angle;
	//   x的斜坡函数
	if(temp[0] > 0)
	{
		if(temp[0] > step) x_Servo_pid.angle += step;
		else 	x_Servo_pid.angle += temp[0];
	}
	else if(temp[0] < 0)
	{
		if(temp[0] < -step) x_Servo_pid.angle -= step ;
		else 	x_Servo_pid.angle += temp[0]; //这里注意，temp本身就是负的，所以用＋，而step为正，所以用－
	}
	
	//   y的斜坡函数
	if(temp[1] > 0)
	{
		if(temp[1] > step) y_Servo_pid.angle += step;
		else 	y_Servo_pid.angle += temp[1];
	}
	else if(temp[1] < 0)
	{
		if(temp[1] < -step) y_Servo_pid.angle -= step ;
		else 	y_Servo_pid.angle += temp[1];
	}

	//输出限幅
	x_Servo_pid.angle = x_Servo_pid.angle > x_Servo_pid.max ? x_Servo_pid.max : x_Servo_pid.angle;
	x_Servo_pid.angle = x_Servo_pid.angle < x_Servo_pid.min ? x_Servo_pid.min : x_Servo_pid.angle;
	y_Servo_pid.angle = y_Servo_pid.angle > y_Servo_pid.max ? y_Servo_pid.max : y_Servo_pid.angle;
	y_Servo_pid.angle = y_Servo_pid.angle < y_Servo_pid.min ? y_Servo_pid.min : y_Servo_pid.angle;
}

在定时器中断中调用以上函数


/**
  * @brief  时间片任务调度函数
  * @param  void
  * @retval void
  * @attention 放入TIM2中使用，定时器中断优先级应高于其他中断，定时器时间应偏长
	OPENMV20ms发送一次，这里22ms进入一次中断
	*/
void Task_Dispatch_tim(void)
{
	system_tick++;
	System_GetValue();//获取openmv的坐标数据		
	//数据进行处理
	Servo_control(5,5);//计算PID并赋值	
	//设定值
	Servo_ANGLE_CH12(x_Servo_pid.angle,y_Servo_pid.angle);//给CCR赋值
	HAL_GPIO_TogglePin(FLAG_GPIO_GPIO_Port,FLAG_GPIO_Pin);//翻转IO口	
	ANO_DT_Data_Exchange();   //匿名上位机
	
}