pid_s.c

PIC18F4520控制Maxon电机恒速转动的PID数字调节程序

/********************************************************************************* 

  名称： PID_S.C
  版本:  2.1
  功能:  单极性驱动板；
         内含PID相关参数初值得设定； 
         统一改造后的：
  作者: 谢云  2007-03-13
  修改: XY 2007-03-24  修改了Lvn Rvn的数据类型
  修改: 修改，去掉积分分离
  修改：加入PIDDetect();  //检测障碍
********************************************************************************/
//************************************头文件****************************//
#define PID_GLOBALS
#include "PID_S.h" 
#include "pic18f4520.h"
#include "math.h"
#include "Action.h"
//***************************************************************
#define LKp 0.18         //左轮比例系数  Proportional part
#define LKi 0.10         	//左轮积分系数 Integral part            
#define LKd 0           //左轮微分参数Differdntial parameter for left motor
//#define LKm 1.83         	//左轮PID输出比例系数
#define LKm 0.61        	//左轮PID输出比例系数

#define RKp 0.18         //右轮比例系数    
#define RKi 0.10    		//右轮积分系数          
#define RKd 0           //右轮差分参数Differdntial parameter for left motor
//#define RKm 1.83       		//右轮PID输出比例系数
#define RKm 0.61      		//右轮PID输出比例系数
//***********************************************************

void Init_PID_S(void)    //只有变量的初始化
{   

    PID_Counter=0;
    PID_Begin=5;      		//PID 采样周期(ms)
    CallPID=0; 

    Lvn=0;
    Rvn=0;

    Lvf=0;            		//左反馈速度
    LError[0]=0;      		//速度偏差
    LError[1]=0;
    LError[2]=0;
    LY=0;             		// 左输出

    Rvf=0;            		//右反馈速度
    RError[0]=0;               //速度偏差
    RError[1]=0;
    RError[2]=0;
    RY=0;              		// 右输出   
}


void PID_S(int lvn,int rvn)
{  
 	float L_Error;          //左轮速度偏差
	float Left_P=0;         //左轮比例
	float Left_I=0;         //左轮积分
	float Left_D=0;         //微分
    float LDy=0;            //Delta Y 

	float R_Error;          //右轮速度偏差
	float Right_P=0;
	float Right_I=0;
	float Right_D=0;
	float RDy=0;
    Lvn=lvn;
    Rvn=rvn;
    Lvf=TMR0L;          	//读左轮速度反馈  先低后高    
    Lvf=Lvf+(TMR0H<<8); 	
    TMR0H=0;            	//清TIMOR
    TMR0L=0;
	
 
    Rvf=TMR1L;
    Rvf=Rvf+(TMR1H<<8);
    TMR1H=0;
    TMR1L=0;

    if(!Left_BackDIR)        //方向控制
    Lvf=-Lvf;     
    if(Right_BackDIR)        //左轮反馈为0时取反；右轮反馈为1时取反
    Rvf=-Rvf;

    L_Error=Lvn-Lvf;
    R_Error=Rvn-Rvf;
    LError[2]=LError[1];
    LError[1]=LError[0];
    LError[0]=L_Error;
    RError[2]=RError[1];
    RError[1]=RError[0];
    RError[0]=R_Error;

    //PIDDetect();  //检测障碍
    
    Left_P = LKp*(LError[0] - LError[1]);
    Right_P = RKp*(RError[0] - RError[1]);
    
    Left_I=LKi*LError[0];
    Right_I=RKi*RError[0];

    Left_D=LKd*(LError[0]-2*LError[1]+LError[2]);
    Right_D=RKd*(RError[0]-2*RError[1]+RError[2]);

    LDy=Left_P+Left_I+Left_D;
    RDy=Right_P+Right_I+Right_D;

    LY=LY+LKm*LDy;             //注意极限 
    if(LY<0)
   	Left_ControlDIR=0;
    else 
	Left_ControlDIR=1;        //负数反转，正数正转
   	
    RY=RY+RKm*RDy;            //注意极限
    if(RY<0)
       Right_ControlDIR=0;
    else 
       Right_ControlDIR=1;   //负数反转，正数正转

 	if(fabs(LY)>255)      //极限控制
	{
		CCPR1L=255;
	}
    else
	{
		CCPR1L=(char)fabs(LY);
	}
	if(fabs(RY)>255)        //极限控制
	{
		CCPR2L=255;
	}
    else
	{
		CCPR2L=(char)fabs(RY);
	}
	
}
 //************************************************************END**************************************************************//