直流无刷电机的调试

本周对手头的一款大疆M3508直流无刷电机调试的相关内容进行整理及个人的代码进行分享。

一、M3508直流无刷电机

直流无刷电机的工作原理此处不做阐述，相关资料也易查询。

1.1电机结构与连接样式图

1.2电机参数

具体不多加阐述，感兴趣的可到官网下载相关资料和软件驱动，连接如下：

M3508减速电机套装 (robomaster.com)

1.3分电板

电源采用12V，通过一块分电板对单片机主板、电机和电源模块进行连接，这个分电板通过PCB简单设计，实现多少电压输入，多少电压输出。分电板的PCB图如下所示：

2.STM32程序：

控制一个电机，需要1个PWM，2个高低电位

2.1 控制原理图

反馈值（测量值）- 设定值（目标值）= 控制偏差；
控制偏差给到控制器；
从而将偏差给到电机，在经过编码器获得反馈；
实现闭环

相关pid.c代码如下：

#include"pid.h"

#define speed_max 482




struct _pid 
{
	float ExpectedValue;//定义设定值
	float ActualValue;//定义实际值    
	float err;//定义偏差值
	float err_prev;//定义前一个的偏差值
	float integral;//定义积分项
	float Kp, Ki, Kd;//定义比例、积分、微分系数
	float curmax, curmin;//定义电流最值
}pid;

void PID_Init(){//初始化pid各值
	
	pid.ExpectedValue=0.0;
	pid.ActualValue = 0.0;
	pid.err = 0.0;
	pid.err_prev = 0.0;
	pid.integral = 0.0;
	pid.Kp = 1.0;
	pid.Ki = 0.1;
	pid.Kd = 0.2;
	pid.curmax =  3000;
	pid.curmin = -3000;
	
}
//限幅
void abs_limit(float *object, float abs_max)
{
    if(*object > abs_max)  *object =  abs_max;
    if(*object < -abs_max) *object = -abs_max;
}

/*
输入：期望转速
输出：位置型单环PID结果
	
*/
float PID_Realize(float set_speed,float get_speed){//ste_speed期望转速 £»get_speed实际转速
	
	pid.ExpectedValue = set_speed;
	pid.ActualValue = get_speed;
	pid.err = pid.ExpectedValue-pid.ActualValue;//计算误差err
	//计算积分项
	pid.integral += pid.err;
	//位置式PID公式
	float Value = pid.Kp*pid.err + pid.Ki*pid.integral+pid.Kp*(pid.err-pid.err_prev);
	//误差换代
	pid.err_prev=pid.err;
	
	if(Value<pid.curmin)
		return pid.curmin;
	else if(Value>pid.curmax)
		return pid.curmax;
	else
		return Value;

}

/********************************************************************************************************/
void pid_struct_init(pid_t* pid, float kp, float ki, float kd)
{
	
		pid->err[NOW] = 0;
		pid->err[LAST] = 0;
		pid->err[LLAST] = 0;
		pid->pos_out = 0;
	  pid->pout = 0;
		pid->iout = 0;
	  pid->dout = 0;
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
		pid->pos_abs = 0;
		pid->pos_goal = 0;
		
}

//µ¥»·pid

float single_pid_calculate(pid_t * pid_s)
{
	pid_s->err[NOW] = pid_s->target - pid_s->measure;//当前误差
	//p项 比例项
	pid_s->pout = pid_s->p * pid_s->err[NOW];
	//i项 积分项
	pid_s->iout += pid_s->i * pid_s->err[NOW];
	//d项 微分项
	pid_s->dout = pid_s->d * (pid_s->err[NOW] - pid_s->err[LAST]);
	
	pid_s->pos_out = pid_s->pout + pid_s->iout + pid_s->dout;
	//误差更新
	pid_s->err[LAST] = pid_s->err[NOW];
	
	abs_limit(&(pid_s->pos_out),3000);
	return pid_s->pos_out;
}

//双环pid
float double_pid_calculate(pid_t * pid_I , pid_t* pid_O)//I是速度，o是角度
{
	/****************************角度环**************************************/
	pid_O->err[NOW] = pid_O->target - pid_O->measure;//当前误差
/*
	//p项 比例项
	pid_O->pout = pid_O->p * (pid_O->err[NOW] - pid_O->err[LAST]);
	//i项 积分项
	pid_O->iout += pid_O->i * pid_O->err[NOW];
	//d项 微分项
	pid_O->dout = pid_O->d * (pid_O->err[NOW] - 2*pid_O->err[LAST] + pid_O->err[LLAST]);
*/
	
	if((pid_O->err[NOW]>-50) || (pid_O->err[NOW]<50))
	{
		pid_O->iout = 0;																//设置死区
	}
		
	pid_O->pout = pid_O->p * pid_O->err[NOW];
	
	pid_O->iout += pid_O->i * pid_O->err[NOW];
	
	pid_O->dout = pid_O->d * (pid_O->err[NOW] - pid_O->err[LAST]);
	
	pid_O->pos_out = pid_O->pout + pid_O->iout + pid_O->dout;//算出速度应设置的值
	abs_limit(&(pid_O->pos_out),3000);
	//误差更新
	pid_O->err[LAST] = pid_O->err[NOW];
	pid_O->err[LLAST] = pid_O->err[LAST];
	
	/*******************************速度环************************************/
	pid_I->err[NOW] = pid_O->pos_out - pid_I->measure;//µ±Ç°Îó²î
	//p项 比例项
	pid_I->pout = pid_I->p * pid_I->err[NOW];
	//i项 积分项
	pid_I->iout += pid_I->i * pid_I->err[NOW];
//d项 微分项
	pid_I->dout = pid_I->d * (pid_I->err[NOW] - pid_I->err[LAST]);
	
	pid_I->result =pid_I->pout + pid_I->iout + pid_I->dout;//应设置的电流值
	
	//误差更新
	pid_I->err[LAST] = pid_I->err[NOW];
	abs_limit(&(pid_I->result),8000);
	return pid_I->result;
}

pid.h

enum
{
    LLAST	= 0,
    LAST 	= 1,
    NOW 	= 2,
};
void PID_Init(void);
float PID_Realize(float current,float ActualValue);

typedef struct __pid_t
{
    float p;
    float i;
    float d;
    

		float target;				  //目标值
	  float measure;				  //测量值
    float err[3];				      //误差
		float result;
	
    float pout;							
    float iout;							
    float dout;		
	
		float pos_goal;          //目标角度		
		float pos_abs;			 //绝对角度
    float pos_out;			     //本次位置式输出
}pid_t;

typedef struct
{
	float pos_goal;//目标位置
	float pos_abs;//绝对位置
	float pos_offset;//位置补偿
	float eer;//误差
	float eer_eer;//上次误差
}ANGLE_TypeDef;

void pid_struct_init(pid_t* pid, float kp, float ki, float kd);

float single_pid_calculate(pid_t * pid_s);
float double_pid_calculate(pid_t * pid_I , pid_t* pid_O);

主函数

#include "main.h"
#include "key.h"
#include "led.h"
#include "can.h"
#include "tim.h"
#include "pid.h"

int i;

unsigned char Temp_Value_3508 = { 0 };//CAN接受电机温度
short	Torque_Value_3508 = { 0 };      //CAN实际扭矩电流
short	Velocity_Value_3508 = { 0 };  	//CAN接受实际转速
short	Position_Value_3508 = { 0 };    //CAN接受机械角度
short Velocity_3508_ID1=0;            //CAN实际转速
short Position_3508_ID1=0;						//CAN接受机械角度
short Torque_3508_ID1=0;							//CAN实际扭转电流
unsigned char Temp_Value_3508_ID1=0;	//CAN接受电机温度
pid_t PID,PID_I,PID_O;								//o为角度，i为速度
short set_current1;						        //PID计算后当前需要传输电流值

short Velocity_3508_ID1_Set_Speed=0;	//设定转速

ANGLE_TypeDef motor_angle;


void CAN1_TX_IRQHandler( void )    //CAN发送中断   清楚发送中断标志
{
	if ( CAN_GetITStatus( CAN1, CAN_IT_TME ) != RESET )
	{
		CAN_ClearITPendingBit( CAN1, CAN_IT_TME );
	}
}
void CAN1_RX0_IRQHandler( void )    //CA接受中断 更新当前实际转速 且清楚接受中断标志位
{
	CanRxMsg rx_message;
	if ( CAN_GetITStatus( CAN1, CAN_IT_FMP0 ) != RESET )//判断是否是fifo0消息挂起中断（即收到消息）
	{
		CAN_ClearITPendingBit( CAN1, CAN_IT_FMP0 );
		CAN_Receive( CAN1, CAN_FIFO0, &rx_message);
		if ( (rx_message.IDE == CAN_Id_Standard) && (rx_message.RTR == CAN_RTR_Data) && (rx_message.DLC == 8) ) /* ±ê×¼Ö¡¡¢Êý¾ÝÖ¡¡¢Êý¾Ý³¤¶ÈΪ8 */
		{
			
				Temp_Value_3508 = rx_message.Data[6];    
				Torque_Value_3508	= (rx_message.Data[4] << 8) | (rx_message.Data[5]);
				Velocity_Value_3508	= (rx_message.Data[2] << 8) | (rx_message.Data[3]);
				Position_Value_3508	= (rx_message.Data[0] << 8) | (rx_message.Data[1]);
				
				
				Velocity_3508_ID1=Velocity_Value_3508;	
				Position_3508_ID1=Position_Value_3508;
				Torque_3508_ID1=Torque_Value_3508;
				Temp_Value_3508_ID1=Temp_Value_3508;
			
				PID.measure = Velocity_3508_ID1;//²âÁ¿Öµ
				PID_I.measure = Velocity_Value_3508;
				PID_O.measure = Position_Value_3508;
		}
	}
}




int main(void)
{
	

	delay_init( 180 );     
	delay_ms( 100 );
	KEY_Init();
	Led_Init();

	CAN1_Mode_Init(CAN_SJW_1tq,CAN_BS2_7tq,CAN_BS1_7tq,3,CAN_Mode_Normal);  £¬Ò»°ã²»Óö¯£¬¶¼ÊÇ500kbps
	delay_ms(100);
	PID_Init(); 
	pid_struct_init(&PID,1,0.1,0.2);//³õʼ»¯pidµÄÈý¸ö²ÎÊýÖµp£¬i£¬d
	pid_struct_init(&PID_I,5,0.1,3);
	pid_struct_init(&PID_O,10,0.1,5);
	

	Tick_TIM7_Init(1000);
	
	while (1){	
			
			if(key==0)
				{
						motor_angle.pos_goal = 0;
						LED3=1;
						LED4=0;
					}
			else if(key==1)
					{
						motor_angle.pos_goal = -2160;
						LED3=0;
						LED4=1;
					}
	
	
		delay_ms(10);
	}

}

void Motor_Angle_Cal(float T) 
{
	float res1, res2;
	static float pos,pos_old;
	pos =(PID_O.measure/8192.0f*360.0f);
	motor_angle.eer=pos - pos_old;//½Ç¶ÈÎó²îµÄ¼ÆËã
		if(motor_angle.eer>0) 
	{													
		res1=motor_angle.eer-T;
		res2=motor_angle.eer;
	}
	else
	{
		res1=motor_angle.eer+T;
		res2=motor_angle.eer;
	}
	
	if(fabs(res1)<fabs(res2)) 
	{
		motor_angle.eer_eer = res1;
	}
	else
	{
		motor_angle.eer_eer = res2;
	}

	motor_angle.pos_abs += motor_angle.eer_eer; 
	motor_angle.pos_offset=motor_angle.pos_goal-motor_angle.pos_abs;
	pos_old  = pos;
}

void TIM7_IRQHandler( void ) 
{
	if ( TIM_GetITStatus( TIM7, TIM_IT_Update ) == SET )
	{
		/******************µ¥»·******************/
		
//		set_current1 = single_pid_calculate(&PID);
		
		/******************Ë«»·******************/
		
		Motor_Angle_Cal(360);
		PID_O.target = motor_angle.pos_offset;
		PID_O.measure = 0;
		set_current1 = 0;
		
//		set_current1=PID_Realize(Velocity_3508_ID1_Set_Speed,Velocity_3508_ID1); 
		
		CAN1_Send_Msg(set_current1,set_current1,set_current1,set_current1);//ÉèÖõçÁ÷Öµ
	}
	TIM_ClearITPendingBit( TIM7, TIM_IT_Update );
}