pid1.c

PID的电机控制

#include "spmc75f2413a.h"
#include "unspmacro.h"


int       rOut;                 //  PID Response (Output),PID输出
int       rIn;                  //  PID Feedback (Input)，PID输入

int       num_pul;				// 编码器脉冲数；

struct  PID                     //定义一个结构体，用于PID计算   
{
    int              SetPoint;      // 设定编码器输入值
    float            Proportion;    // 比例常数 Proportional Const
    float            Integral;      // 积分常数 Integral Const
    float            Derivative;    // 微分常数 Derivative Const
    float            LastError;     // 偏差 Error[-1]
    float            PrevError;     // 偏差 Error[-2]
    float            SumError;      // 偏差总和 Sums of Errors
 };

//*======================================================初始化PID部分，把PID参数设为0======================================================*//

void PIDInit (PID *pp)            //无返回值的函数PIDInit, PID *pp是函数double PIDCalc( PID *pp, double NextPoint )中的形参.PID *pp为指向结构体的指针
{
	pp->SetPoint=0;
	
	pp->Proportion=0;
	
	pp->Integral=0;
	
	pp->Derivative=0;
	
	pp->LastError=0;
	
	pp->PrevError=0;
	
	pp->SumError=0;
	
}
//*=================================================PID计算部分,位置型PID计算函数==========================================================*//

float  PIDCalc( PID *pp, int NextPoint )      //有参函数定义的形式：类型标识符 函数名（形参列表）见C语言P144 ，PID *pp是指向结构体指针？？   
{
	int    dError,Error;                       //声明部分定义所用变量dError=E(t)-E(t-1),Error=PSET-P(K)
	                                            //下面是语句部分
	
    Error = pp->SetPoint -  NextPoint;          // 偏差，E(k)=P(k)-P(-1)
    	
    pp->SumError += Error;                      // 积分，复合赋值运算符+=，a+=3---a=a+3,看起来相求所有误差的和,用的位置型PID！！
    
    dError = pp->LastError - pp->PrevError;     // 当前微分，运算E(k)-E(k-1)
    	
    											//下面开始重新写入误差
    pp->PrevError = pp->LastError;              
    
    pp->LastError = Error;
                                      
    
    return (pp->Proportion * Error              // 比例项    有返回值：形如return(Z);
        +   pp->Integral * pp->SumError         // 积分项
        +   pp->Derivative * dError             // 微分项
           );
}
//*======================================================读编码器的脉冲数======================================================*//
int  Encoder  (void)    
{
   P_TMR_Start->W = 0x0001;				 //启动编码器
   
    delay(200);			                 // 读取延时
    
    num_pul=P_TMR0_TCNT->W;
	 
	return(num_pul);		            // 返回脉冲数
}
//*===========================================================采样时长==========================================================*//
void delay (unsigned int t)
{									
	unsigned int i;	
	while (t)	
	{
		i =1000;
		while (i>0) i--;
		t--;
    }
}
//*===========================================================PWM发生===========================================================*//
void PWM_fuc  (float  d_zkb)          //PWM脉冲发生程序

{
    int k=0x0032;                      //设定占空比
    int n=0;
    
	P_TMR2_Ctrl->W =0x20E5;                                   //B13-B10:MODE边沿PWM模式1000；B7-B5：CCLS发生比较匹配事件清除一次111；
	                                                          //B4-B3：CKEGS时钟边沿选择--上升沿00；B2-B0：TMRPS定时器分频，FCK/1024 101

	P_TMR2_TPR->W =0x00FF;                                    //写TPR周期寄存器
	
	P_TMR2_TGRB->W = k;              
	
	P_IOA_SPE->W |= CW_IOA_TMR2_TGRA_SFR_EN;                 //管脚特殊功能使能
	
	P_TMR2_IOCtrl->B.IOAMODE = CB_TMR2_IOAMOD_Output_10;     //PWM比较匹配输出模式
				
	if  (d_zkb > 0)
 	{  
		k-=2;
		for (n=10;n>0;n--)   	                         // 加热一段时间
		{
			P_IOA_Dir->B.bit10=1;                        //写输出PWM波形
		}
 	}
 else
 
     {
		k=0;
		P_IOA_Dir->B.bit10=0; 
	 }
}	

//*==============================================================================================================================*//
main()

{
	P_IOA_SPE->W = 0x7800;
	
	P_IOB_SPE->W = 0x0000;
	
	P_IOC_SPE->W=0x0000;
	
	P_WatchDog_Ctrl->W=0x8000;           //P_WatchDog_Ctrl  B15 位禁止（0）/使能（1）看门狗复位功能
	
	P_TMR0_Ctrl->W=0x1000;               //径向位移编码器,<IOA11,12>,TMR0
	
	P_TMR_Start->W=0x0000;               //关闭计数器
	
	
	P_IOA_Dir->W=0x0000;                 //初始化I/O口。IOA口均为输入口，IOA3--J11<2>镜框开度限位;IOA4--J5<2>右限位;
	                                    //IOA5--J4<2>左限位;IOA6--J1<A2>杆状扫描针上限位为下拉输入
	P_IOA_Attrib->W=0x0000; 
	
	P_IOA_Data->W=0xFFC7;
	
	P_IOB_Dir->W=0xFFFF;                 //IOB口均为输出口, IOB3--STROBE;IOB4--CLOCK;IOB7--旋转电机（IC6）DATA A ;IOB8--旋转电机（IC6）DATA B;
	                                    ///IOB14--TA8192<1>CONT1;IOB15--TA8192<20>CONT2悬空
	P_IOB_Attrib->W=0xFFFF;
	
	P_IOB_Data->W=0x0000;
	
	
    PID   *sPID;                         //定义一个PID结构体sPID
    
    PIDInit (&sPID );                  //初始化PID，然后PID参数应该都为0  
               
    sPID->Proportion = 0.1;              //先设置Kp=0.1,KI=0,KD=0计算PID参数
    
    sPID->Integral   = 0;
    
    sPID->Derivative = 0;
     
    sPID->SetPoint   = 0xEE;          //  先假定PID的设定脉冲数为0x00EE
    
    
    while(1) 
    {                         			//  使用PID控制
        rIn = Encoder ();               //  调用Encoder函数，读编码器脉冲数 
                              
        rOut = PIDCalc ( &sPID,rIn );  //  调用PID计算函数，计算PID输出值
        
        PWM_fuc ( rOut );              //  调用PWM输出函数，输出PID波形
    }
}