【算法】电机增量式pid

最新推荐文章于 2024-05-16 18:44:41 发布

孙鹤楼

最新推荐文章于 2024-05-16 18:44:41 发布

阅读量2k

点赞数 1

分类专栏：驱动代码文章标签： k60 增量式pid

本文链接：https://blog.csdn.net/qq_38505399/article/details/103351201

版权

驱动代码专栏收录该内容

6 篇文章 3 订阅

订阅专栏

双电机增量式pid的算法

硬件采用欧姆龙500线编码器采集实时脉冲速度由于ftm资源紧张所以只有采用了一路的正交解码模块而另一路采用的是lptmr模块的脉冲计数功能在软件编写上有少许差别。
本文档记录了在k60dn 144pin 上的

int16 RightDuty = 0,LeftDuty = 0;						//占空比承载参量
int error_0_r=0,d_error_r=0,dd_error_r=0;              	//电机误差过渡参量
int vi_e_r=0,vi_d_r=0; 
int error_0_l=0,d_error_l=0,dd_error_l=0;              
int vi_e_l=0,vi_d_l=0;

float SpeedKp=30;                                      	//电机PID参数35
float SpeedKi=0; //                                    	//10
float SpeedKd=250;

/************************************************** 
** 函数名称: void MotorControl(void)
** 功能描述: 电机速度控制函数
** 输    入:
** 输    出: 
** 说明：正交解码获取编码器读数后 pid进行调节
***************************************************/
void MotorControl(void)
{
	volatile int16 right_pulseval = 0,left_pulseval = 0;
	
    right_pulseval = FTM_QUAD_get(FTM2);                    	//获取FTM 正交解码 的脉冲数(负数表示反方向)
	FTM_QUAD_clean(FTM2);                              			//清空正交解码计数寄存器
	
    left_pulseval = lptmr_pulse_get();          				//获取脉冲计数值	
	lptmr_pulse_clean();                   						//清空脉冲计数器计算值
	
	
	if(right_pulseval < 0)										//脉冲计数这一路依靠另一路的正交解码获取方向
	{
		left_pulseval = -left_pulseval;
	} 
	if(STOP_flag||Endline_flag)                         		//遥控器或者终点线停车
	{
		R_prespeed_speed=0;  
		L_prespeed_speed=0;
	}	
	
    error_0_r = R_prespeed_speed - right_pulseval ;             //期望脉冲数-实际脉冲数
    d_error_r = error_0_r - vi_e_r;                           	//本次误差-上次误差
    dd_error_r = d_error_r - vi_d_r;                          	//本次误差-上次误差-（上次本次误差-上次上次误差）
    vi_e_r = error_0_r;                                     	//保存上次误差的值
    vi_d_r = d_error_r;                                     	//保存上次本次误差-上次误差的值
	
    error_0_l = L_prespeed_speed - left_pulseval ;              //期望脉冲数-实际脉冲数
    d_error_l = error_0_l - vi_e_l;                           	//本次误差-上次误差
    dd_error_l = d_error_l - vi_d_l;                          	//本次误差-上次误差-（上次本次误差-上次上次误差）
    vi_e_l = error_0_l;                                     	//保存上次误差的值
    vi_d_l = d_error_l;                                     	//保存上次本次误差-上次误差的值
	
    RightDuty += (int)(SpeedKp*error_0_r
					   + SpeedKi*d_error_r
						   + SpeedKd*dd_error_r);//增量式pid  
	
	LeftDuty  += (int)(SpeedKp*error_0_l 
					   + SpeedKi*d_error_l 
						   + SpeedKd*dd_error_l);
	
	if(R_prespeed_speed == 0)  //&& L_prespeed_speed == 0                               	//制动轮胎抱死 正反转交替
	{
		if(RightDuty < 0)
        {  
            if(RightDuty<-4000)
            {
                RightDuty = -4000;
            }
            FTM_PWM_Duty(FTM0 ,FTM_CH0 ,-RightDuty);
            FTM_PWM_Duty(FTM0 ,FTM_CH1 , 0);
        }
        else 
        {
            if(RightDuty>9000)
            {
                RightDuty = 9000;
            }
            
            FTM_PWM_Duty(FTM0 ,FTM_CH0 ,0);
            FTM_PWM_Duty(FTM0 ,FTM_CH1 ,RightDuty);
        }
		
		if(LeftDuty < 0)
        {  
            if(LeftDuty<-4000)
            {
                LeftDuty = -4000;
            }
            FTM_PWM_Duty(FTM0 ,FTM_CH3 ,-LeftDuty);
            FTM_PWM_Duty(FTM0 ,FTM_CH2 , 0);
        }
        else 
        {
            if(LeftDuty>9000)
            {
                LeftDuty = 9000;
            }
			
            FTM_PWM_Duty(FTM0 ,FTM_CH3 ,0);
            FTM_PWM_Duty(FTM0 ,FTM_CH2 ,LeftDuty);
            
        }
	}
	else
	{
		if( error_0_r < -10)                                 	//实际脉冲数远大于期望脉冲时电机反转减速
		{     
			FTM_PWM_Duty(FTM0,FTM_CH0,500);                 		//反转
			FTM_PWM_Duty(FTM0,FTM_CH1,0);
		}
		else
		{ 
			if(RightDuty > 9000)                               	//电机占空比限幅
			{
				RightDuty = 9000;
			}     
			else if(RightDuty < 0)
			{
				RightDuty = 0;
			}
			FTM_PWM_Duty(FTM0,FTM_CH0,0);             	//电机正转
			FTM_PWM_Duty(FTM0,FTM_CH1,RightDuty);
		}
		
		if( error_0_l < -10)                                 	//实际脉冲数远大于期望脉冲时电机反转减速
		{     
			FTM_PWM_Duty(FTM0,FTM_CH3,500);                 		//反转
			FTM_PWM_Duty(FTM0,FTM_CH2,0);
		}
		else
		{ 
			if(LeftDuty > 9000)                               	//电机占空比限幅
			{
				LeftDuty = 9000;
			}     
			else if(LeftDuty < 0)
			{
				LeftDuty = 0;
			}
			FTM_PWM_Duty(FTM0,FTM_CH3,0);             	//电机正转
			FTM_PWM_Duty(FTM0,FTM_CH2,LeftDuty);
		}
	}