/*
***********************************************************************************
* PID.c
* Description: This file includes some basic calculation function of PID
* (c) Copyright 2006,Zhao Cheng
* All Rights Reserved
*
* By : Zhao Cheng
* Data : 2006_5_6
* Note : Don't change this file if possible.
**********************************************************************************/
#include <mc9s12dg128.h> /* derivative information */
/*
***********************************************************************************
* 宏定义
**********************************************************************************/
#define STABMAX 50
#define SENSORNUM 8
#define SAMPLETIMES 5
/*
***********************************************************************************
* FUNCTION PROTOTYPES
**********************************************************************************/
int CalculateP(void);
float CalculatePID(void);
/********************************** PID控制程序 ********************************/
struct CARSTATE
{
int E0;
int E1;
int E2;
int E3;
float Integral;
}CarState;
/*
***********************************************************************************
* 初始化PID参数
**********************************************************************************/
void Init_PID()
{
CarState.E0 = 0;
CarState.E1 = 0;
CarState.E2 = 0;
CarState.E3 = 0;
CarState.Integral = 0;
}
/*
**********************************************************************************
* 信号处理函数
*
* 程序描述: 对传感器采集过来的数据进行处理，得到一些基本的计算参数
*
* 说明： 无
**********************************************************************************/
int SignalProcess( unsigned int signal )
{
const int BitValue[8] = {43,26,12,6,-6,-12,-26,-43}; //MAX:28
int i,CurrPoint=0,LastPoint=0,BitNum=0;
unsigned char SignalBit[8];
for(i=0;i<8;i++)
{
SignalBit[i] = signal & 0x0001;
BitNum += SignalBit[i];
signal >>= 1;
}
switch(BitNum)
{
case 1:
for(i=0;i<8;i++)
if(SignalBit[i] != 0)
CurrPoint += BitValue[i];
CarState.E0 = CurrPoint;
break;
case 2:
for(i=0;i<8;i++)
if(SignalBit[i] != 0)
CurrPoint += BitValue[i];
CurrPoint >>= 1;
CarState.E0 = CurrPoint;
break;
default:
CarState.E0 = CarState.E1;
break;
}
return CalculateP()*100;
}
/*
**********************************************************************************
*
* PID计算函数
*
* 程序描述: 计算P参数
*
* 说明： 无
**********************************************************************************/
int CalculateP(void)
{
CarState.E1 = CarState.E0;
return((int)CarState.E0);
}
/*
***********************************************************************************
* PID计算函数
*
* 程序描述: 对传感器采集过来的数据进行处理，得到一些基本的计算参数
*
* 说明： 无
**********************************************************************************/
float CalculatePID(void)
{
float P, I = 0, D;
/* parameter const */
float Kp = 1.0, Ki = -0.0002, Kd = -0.0002;
/* P parameter */
P = CarState.E0 * Kp;
/* I parameter */
if(P+I<2)
{
CarState.Integral += Ki * CarState.E0;
I = CarState.Integral;
}
/* D parameter */
D = Kd * ( CarState.E0 + 3*CarState.E1 - 3*CarState.E2 - CarState.E3 )/6.0;
CarState.E3 = CarState.E2;
CarState.E2 = CarState.E1;
CarState.E1 = CarState.E0;
return (P+I+D);
}
