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

  名稱： 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**************************************************************//