基于大疆RM3508电机的串级PID（角度环＋速度环）

1.前言

最近参加ROBOCON，我负责编写传球机器人，由于传球机构需要一个电机转固定角度来带动球，所以便用大疆3508电机通过串级PID来实现，不得不说3508电机还是真的强，先看一下效果吧。

视频

2.配置程序

关于3508的配置程序以及一些其他部分在我的另外一篇博客里有详细的介绍，本篇主要是详细讲解3508串级pid（转固定角度）。
大疆3508/2006/6020电机使用教程

3.角度位置控制

3508电机的串级PID控制比起其他直流电机要简单一些，不需要过多的处理编码盘的反馈，只需要读取电调反馈的角度。
关于串级PID的知识，csdn上有很多，我就不赘述了。
直流电机角度位置控制详解（附源码）
直流电机角度控制

3.1 先看一下电调反馈的数据

if((rx_message.StdId == (MOTOR_ID_READ+5)) ) //电机5
		{
						
			MOTOR_FEEDBACK[5].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[5].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[5].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[5].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[5].real_angle = MOTOR_FEEDBACK[5].angle_value/8192.0f*360.0f;
			
		}

这里电调ID要对应上，否则回接受不到数据，另外我们主要用的反馈是real.angle这个数据。

3.2角度处理程序

先说一下这个入口参数motor_num是电调ID，T是一个周期的角度值（自设360），通过这个函数可以得到电机的绝对角度POS_ABS。

u8 cnt = 1;
void Motor_Angle_Cal(unsigned short int motor_num,float T)
{
	float  res1, res2;
//	int  res3, res4;
	static float pos[MOTOR_MAX], pos_old[MOTOR_MAX];
	
	if(flag.cnt_calib&&cnt)
	{
		pos_old[motor_num]=MOTOR_FEEDBACK[motor_num].real_angle;
		cnt=0;
	}	
	pos[motor_num] =MOTOR_FEEDBACK[motor_num].real_angle;
	motor.ANGLE[motor_num].eer=pos[motor_num] - pos_old[motor_num];
	
	if(motor.ANGLE[motor_num].eer>0) 	
	{
		res1=motor.ANGLE[motor_num].eer-T;//反转，自减
		res2=motor.ANGLE[motor_num].eer;
	}
	else
	{
		res1=motor.ANGLE[motor_num].eer+T;//正转，自加一个周期的角度值（360）
		res2=motor.ANGLE[motor_num].eer;
	}
	
	if(ABS(res1)<ABS(res2)) //不管正反转，肯定是转的角度小的那个是真的
	{
		motor.ANGLE[motor_num].eer_eer = res1;
	}
	else
	{
		motor.ANGLE[motor_num].eer_eer = res2;
	}
	
	motor.ANGLE[motor_num].POS_ABS += motor.ANGLE[motor_num].eer_eer;
	pos_old[motor_num]  = pos[motor_num];
}

//一些角度控制的结构体变量
typedef struct
{
	float POS_GAOL;//目标位置
	float POS_ABS;//绝对位置0
	float POS_OFFSET;
	float eer;
	float eer_eer;
}ANGLE_TypeDef;


typedef struct
{
	ANGLE_TypeDef ANGLE[8];
	_PID   PID_SPEED[8];
	_PID   PID_ANGLE[8];
	
}MOTOR_TypeDef;

3.3 主要程序

//PID
float PID_Cal_Limt(_PID *PID, float limit, float get, float set)//PID死区修改
{
	PID->err = set - get;
	PID->err_err = PID->err - PID->err_old;
	
	PID->P_OUT  = PID->P * PID->err;
	PID->I_OUT += PID->I * PID->err;
	PID->D_OUT  = PID->D * PID->err_err;
	
	PID->I_OUT = (PID->I_OUT > PID->I_LIMIT)?(PID->I_LIMIT):((PID->I_OUT < -PID->I_LIMIT)?(-PID->I_LIMIT):(PID->I_OUT));
	
	PID->OUT = PID->P_OUT + PID->I_OUT + PID->D_OUT;
	
	PID->OUT = (PID->OUT > PID->OUT_LIMIT)?(PID->OUT_LIMIT):((PID->OUT < -PID->OUT_LIMIT)?(-PID->OUT_LIMIT):(PID->OUT));
	
	if(ABS(PID->err) <= ABS(limit))
	{
	  PID->I_OUT=0;
	  PID->OUT=0;
	}
	
	PID->err_old = PID->err;
	
	return PID->OUT;
}



float motor_err[10]; //死区控制
float mode=0; //mode0-发射；mode1-回收
float angle_a=0,angle_b=0;//a-发射角度；b-回收角度

void Motor_Auto_Run(void)
{
	motor_err[5]=50;
	
	float abs_err[10];
	static float abs_err_old[10]; 
	Motor_Angle_Cal(5,360);//得到绝对角度
	//发射
	if(mode == 0)
	{
		
	motor.ANGLE[5].POS_GAOL = angle_a;
	PID_Cal_Limt(&motor.PID_ANGLE[5], motor_err[5], motor.ANGLE[5].POS_ABS,motor.ANGLE[5].POS_GAOL);

	
	abs_err[5] = motor.ANGLE[5].POS_ABS - abs_err_old[5];
	
	
	PID_Cal_Limt( &motor.PID_SPEED[5], 10, abs_err[5], motor.PID_ANGLE[5].OUT);
	
	
	abs_err_old[5] = motor.ANGLE[5].POS_ABS;
	Motor_Set_Current2((int16_t)(motor.PID_SPEED[5].OUT) ,0,0,0);
	}
	//回
	else if (mode == 1)
	{
		
	motor.ANGLE[5].POS_GAOL = angle_b  ;
	PID_Cal_Limt(&motor.PID_ANGLE[6], motor_err[5], motor.ANGLE[5].POS_ABS,motor.ANGLE[5].POS_GAOL);
	
	abs_err[5] = motor.ANGLE[5].POS_ABS - abs_err_old[5];
		
	PID_Cal_Limt( &motor.PID_SPEED[6], 10, abs_err[5], motor.PID_ANGLE[6].OUT);
		
	abs_err_old[5] = motor.ANGLE[5].POS_ABS;
	Motor_Set_Current2((int16_t)(motor.PID_SPEED[6].OUT) ,0,0,0);
		
	}
	else
	

}

这个部分有几个小细节主要注意：
1.角度控制一套PID参数即可，为了不损害机械机构所以我写了两套PID参数来控制电机的发射和回收。
2.motor_err()是电机角度死区控制，目的是防止电机在目标角度附件来回跳动。
3. 角度控制的主要流程是先将目标角度在角度环里计算，将角度环的输出发送给速度环，再将速度环的输出发射给电调即可。
4. 一般串级PID的调参主要是调角度环，如果需要电机的爆发力很大，角度环的输出限制不能设置的很大。
5.若是电机在目标值附近来回震动很大，则需要在角度环里加大d的值。
6.角度环的程序需要放在 投射电机 的can接收中断里。

if((rx_message.StdId == (MOTOR_ID_READ+5)) ) //电机5
		{
			Motor_Auto_Run();	//串级PID  3508 正方向-向上
			
			MOTOR_FEEDBACK[5].angle_value  = rx_message.Data[0] << 8 | rx_message.Data[1];
			MOTOR_FEEDBACK[5].speed_rpm    = rx_message.Data[2] << 8 | rx_message.Data[3];
			MOTOR_FEEDBACK[5].real_current = rx_message.Data[4] << 8 | rx_message.Data[5];
			MOTOR_FEEDBACK[5].temperature = rx_message.Data[6];
			
			MOTOR_FEEDBACK[5].real_angle = MOTOR_FEEDBACK[5].angle_value/8192.0f*360.0f;
			
		}

上文若有错误，请指正。