pid.c.txt

pid算法c语言源码 仅供参考 pid算法c语言源码 仅供参考

DEC R0
DEC R1
MOV A,@R1
MOV @R0,A
DEC R0
DEC R1
MOV A,@R1
MOV @R0,A
RET
; （８.1） 标号： ＦＭＯＶR0 功能：浮点数传送

;入口条件：源操作数在[R0]中，目标地址为[R1]。
;出口信息：[R1]=[R0]，[R0]不变。
;影响资源：A 堆栈需求： ２字节

FMOVR0: INC R1
INC R1
INC R0
INC R0
MOV A,@R0
MOV @R1,A
DEC R1
DEC R0
MOV A,@R0
MOV @R1,A
DEC R1
DEC R0
MOV A,@R0
MOV @R1,A
RET
; （２４）标号： ＤＴＯＦ 功能：双字节十六进制定点数转换成格式化浮点数

;入口条件：双字节定点数的绝对值在[R0]中，数符在位1FH中，整数部分的位数在A中。

;出口信息：转换成格式化浮点数在[R0]中（三字节）。
;影响资源：PSW、A、R2、R3、R4、位1FH 堆栈需求： ６字节

DTOF: MOV R2,A ;按整数的位数初始化阶码
MOV A,@R0 ;将定点数作尾数
MOV R3,A
INC R0
MOV A,@R0
MOV R4,A
DEC R0
LCALL RLN ;进行规格化
LJMP MOV0 ;传送结果到[R0]中

; （２５） 标号： ＦＴＯＤ 功能：格式化浮点数转换成双字节定点数

;入口条件：格式化浮点操作数在[R0]中。
;出口信息：OV=1时溢出，OV=0时转换成功：定点数的绝对值在[R0]中（双字节），数符

;在位1FH中，F0=1 时为整数，CY=1时为一字节整数一字节小数，否则为纯小数。
;影响资源：PSW、A、B、R2、R3、R4、位1FH 堆栈需求： ６字节

FTOD: LCALL MVR0 ;将[R0]传送到第一工作区
MOV A,R2
JZ FTD4 ;阶码为零，纯小数
JB ACC.7,FTD4;阶码为负，纯小数
SETB C
SUBB A,#10H
JC FTD1
SETB OV ;阶码大于１６，溢出
RET
FTD1: SETB C
MOV A,R2
SUBB A,#8 ;阶码大于８否？
JC FTD3
FTD2: MOV B,#10H ;阶码大于８，按双字节整数转换
LCALL FTD8

SETB F0 ;设立双字节整数标志
CLR C
CLR OV
RET
FTD3: MOV B,#8 ;按一字节整数一字节小数转换
LCALL FTD8
SETB C ; 设立一字节整数一字节小数标志
CLR F0
CLR OV
RET
FTD4: MOV B,#0 ;按纯小数转换
LCALL FTD8
CLR OV ;设立纯小数标志
CLR F0
CLR C
RET
FTD8: MOV A,R2 ;按规定的整数位数进行右规***阶码是扩展后的值
CJNE A,B,FTD9
MOV A,R3 ;将双字节结果传送到[R0]中
MOV @R0,A
INC R0
MOV A,R4
MOV @R0,A
DEC R0
RET

FTD9: CLR C
LCALL RR1 ;右规一次
SJMP FTD8

; （２６） 标号： ＢＴＯＦ 功能：浮点ＢＣＤ码转换成格式化浮点数

;入口条件：浮点ＢＣＤ码操作数在[R0]中。
;出口信息：转换成的格式化浮点数仍在[R0]中。
;影响资源：PSW、A、B、R2～R7、位1DH～1FH 堆栈需求：６字节


BTOF: INC R0 ;判断是否为零。
INC R0
MOV A,@R0
MOV R7,A
DEC R0
MOV A,@R0
MOV R6,A
DEC R0
ORL A,R7
JNZ BTF0
MOV @R0,#41H;为零，转换结束。
RET
BTF0: MOV A,@R0

MOV C,ACC.7
MOV 1DH,C ;保存数符。
CLR 1FH ;以绝对值进行转换。
MOV C,ACC.6 ;扩充阶码为八位。
MOV ACC.7,C
MOV @R0,A
JNC BTF1
ADD A,#19 ;是否小于１Ｅ－１９？
JC BTF2
MOV @R0,#41H ;小于１Ｅ－１９时以０计。
INC R0
MOV @R0,#0
INC R0
MOV @R0,#0
DEC R0
DEC R0
RET
BTF1: SUBB A,#19
JC BTF2
MOV A,#3FH ;大于１Ｅ１９时封顶。
MOV C,1DH
MOV ACC.7,C
MOV @R0,A
INC R0
MOV @R0,#0FFH
INC R0

MOV @R0,#0FFH
DEC R0
DEC R0
RET
BTF2: CLR A ;准备将ＢＣＤ码尾数转换成十六进制浮点数。
MOV R4,A
MOV R3,A
MOV R2,#10H ;至少两个字节。
BTF3: MOV A,R7
ADD A,R7
DA A
MOV R7,A
MOV A,R6
ADDC A,R6
DA A
MOV R6,A
MOV A,R4
RLC A
MOV R4,A
MOV A,R3
RLC A
MOV R3,A
DEC R2
JNB ACC.7,BTF3;直到尾数规格化。
MOV A,R6 ;四舍五入。
ADD A,#0B0H ; ******加#80H，也可以
CLR A
ADDC A,R4
MOV R4,A
CLR A

ADDC A,R3
MOV R3,A
JNC BTF4
MOV R3,#80H ; ****有进位右规一次
INC R2
BTF4: MOV DPTR,#BTFL;准备查表得到十进制阶码对应的浮点数。
MOV A,@R0
ADD A,#19 ;计算表格偏移量。
MOV B,#3
MUL AB
ADD A,DPL
MOV DPL,A
JNC BTF5
INC DPH
BTF5: CLR A ;查表。
MOVC A,@A+DPTR
MOV C,ACC.6
MOV ACC.7,C
MOV R5,A
MOV A,#1
MOVC A,@A+DPTR
MOV R6,A
MOV A,#2
MOVC A,@A+DPTR
MOV R7,A
LCALL MUL1 ;将阶码对应的浮点数和尾数对应的浮点数相乘。

MOV C,1DH ;取出数符。
MOV 1FH,C
LJMP MOV0 ;传送转换结果。

; （２７） 标号： ＦＴＯＢ 功能：格式化浮点数转换成浮点ＢＣＤ码

;入口条件：格式化浮点操作数在[R0]中。
;出口信息：转换成的浮点ＢＣＤ码仍在[R0]中。
;影响资源：PSW、A、B、R2～R7、位1DH～1FH 堆栈需求：６字节

FTOB: INC R0
MOV A,@R0
INC R0
ORL A,@R0
DEC R0
DEC R0
JNZ FTB0
MOV @R0,#41H
RET
FTB0: MOV A,@R0
MOV C,ACC.7
MOV 1DH,C
CLR ACC.7
MOV @R0,A

LCALL MVR0
MOV DPTR,#BFL0;绝对值大于或等于１时的查表起点。
MOV B,#0 ;十的０次幂。
MOV A,R2
JNB ACC.7,FTB1
MOV DPTR,#BTFL;绝对值小于１Ｅ－６时的查表起点。
MOV B,#0EDH ;十的－１９次幂。
ADD A,#16
JNC FTB1
MOV DPTR,#BFLN;绝对值大于或等于１Ｅ－６时的查表起点。
MOV B,#0FAH ;十的－６次幂。
FTB1: CLR A ;查表，找到一个比待转换浮点数大的整数幂。
MOVC A,@A+DPTR
MOV C,ACC.6
MOV ACC.7,C
MOV R5,A
MOV A,#1
MOVC A,@A+DPTR

MOV R6,A
MOV A,#2
MOVC A,@A+DPTR
MOV R7,A
MOV A,R5 ;和待转换浮点数比较。
CLR C
SUBB A,R2
JB ACC.7,FTB2;差为负数。
JNZ FTB3
MOV A,R6
CLR C
SUBB A,R3
JC FTB2
JNZ FTB3
MOV A,R7
CLR C
SUBB A,R4
JC FTB2
JNZ FTB3
MOV R5,B ;正好是表格中的数。
INC R5 ;幂加一。
MOV R6,#10H ;尾数为０·１０００。
MOV R7,#0
SJMP FTB6 ;传送转换结果。
FTB2: INC DPTR ;准备表格下一项。
INC DPTR

INC DPTR
INC B ;幂加一。
SJMP FTB1 ;
FTB3: PUSH B ;保存幂值。
LCALL DIV3 ;相除，得到一个二进制浮点数的纯小数。
FTB4: MOV A,R2 ;取阶码。
JZ FTB5 ;为零吗？
CLR C ;
LCALL RR1 ;右规。
SJMP FTB4
FTB5: POP ACC ;取出幂值。
MOV R5,A ;作为十进制浮点数的阶码。
LCALL HB2 ;转换尾数的十分位和百分位。
MOV R6,A
LCALL HB2 ;转换尾数的千分位和万分位。
MOV R7,A
MOV A,R3 ;四舍五入。
RLC A
CLR A
ADDC A,R7

DA A
MOV R7,A
CLR A
ADDC A,R6
DA A
MOV R6,A
JNC FTB6
MOV R6,#10H
INC R5
FTB6: INC R0 ;存放转换结果。
INC R0
MOV A,R7
MOV @R0,A
DEC R0
MOV A,R6
MOV @R0,A
DEC R0
MOV A,R5
MOV C,1DH ;取出数符。
MOV ACC.7,C
MOV @R0,A
RET
HB2: MOV A,R4 ; 尾数扩大１００倍。
MOV B,#100
MUL AB
MOV R4,A
MOV A,B
XCH A,R3
MOV B,#100
MUL AB
ADD A,R3
MOV R3,A
JNC HB21

INC B
HB21: MOV A,B ;将整数部分转换成ＢＣＤ码。
MOV B,#10
DIV AB
SWAP A
ORL A,B
RET
BTFL: DB 41H,0ECH,1EH ;1.0000E-19
DB 45H,93H,93H ;1.0000E-18
DB 48H,0B8H,78H ;1.0000E-17
DB 4BH,0E6H,96H ;1.0000E-16
DB 4FH,90H,1DH ;1.0000E-15
DB 52H,0B4H,25H ;1.0000E-14
DB 55H,0E1H,2EH ;1.0000E-13
DB 59H,8CH,0BDH ;1.0000E-12
DB 5CH,0AFH,0ECH ;1.0000E-11
DB 5FH,0DBH,0E7H ;1.0000E-10
DB 63H,89H,70H ;1.0000E-9

DB 66H,0ABH,0CCH ;1.0000E-8
DB 69H,0D6H,0C0H ;1.0000E-7
BFLN: DB 6DH,86H,38H ;1.0000E-6
DB 70H,0A7H,0C6H ;1.0000E-5
DB 73H,0D1H,0B7H ;1.0000E-4
DB 77H,83H,12H ;1.0000E-3
DB 7AH,0A3H,0D7H ;1.0000E-2
DB 7DH,0CCH,0CDH ;1.0000E-1
BFL0: DB 1,80H,00H ;1.0000
DB 4,0A0H,00H ;；1.0000E1
DB 7,0C8H,00H ;1.0000E2
DB 0AH,0FAH,00H ;1.0000E3
DB 0EH,9CH,40H ;1.0000E4
DB 11H,0C3H,50H ;1.0000E5
DB 14H,0F4H,24H ;1.0000E6

DB 18H,98H,97H ;1.0000E7
DB 1BH,0BEH,0BCH ;1.0000E8
DB 1EH,0EEH,6BH ;1.0000E9
DB 22H,95H,03H ;1.0000E10
DB 25H,0BAH,44H ;1.0000E11
DB 28H,0E8H,0D5H ;1.0000E12
DB 2CH,91H,85H ; 1.0000E13
DB 2FH,0B5H,0E6H ;1.0000E14
DB 32H,0E3H,60H ;1.0000E15
DB 36H,8EH,1CH ;1.0000E16
DB 39H,31H,0A3H ;1.0000E17
DB 3CH,0DEH,0BH ;1.0000E18
DB 40H,8AH,0C7H ;1.0000E19 

  这是从网上找来的一个比较典型的PID处理程序，在使用单片机作为控制cpu时，请稍作简化，具体的PID
参数必须由具体对象通过实验确定。由于单片机的处理速度和ram资源的限制，一般不采用浮点数运算，
而将所有参数全部用整数，运算到最后再除以一个2的N次方数据（相当于移位），作类似定点数运算，可
大大提高运算速度，根据控制精度的不同要求，当精度要求很高时，注意保留移位引起的“余数”，做好余
数补偿。这个程序只是一般常用pid算法的基本架构，没有包含输入输出处理部分。
=====================================================================================================*/
#include <string.h>
#include <stdio.h>
/*====================================================================================================
    PID Function
    
    The PID (比例、积分、微分) function is used in mainly
    control applications. PIDCalc performs one iteration of the PID
    algorithm.

    While the PID function works, main is just a dummy program showing
    a typical usage.
=====================================================================================================*/

typedef struct PID {

        double  SetPoint;           //  设定目标 Desired Value

        double  Proportion;         //  比例常数 Proportional Const
        double  Integral;           //  积分常数 Integral Const
        double  Derivative;         //  微分常数 Derivative Const

        double  LastError;          //  Error[-1]
        double  PrevError;          //  Error[-2]
        double  SumError;           //  Sums of Errors

} PID;

/*====================================================================================================
   PID计算部分
=====================================================================================================*/

double PIDCalc( PID *pp, double NextPoint )
{
    double  dError,
            Error;

        Error = pp->SetPoint -  NextPoint;          // 偏差
        pp->SumError += Error;                      // 积分
        dError = pp->LastError - pp->PrevError;     // 当前微分
        pp->PrevError = pp->LastError;
        pp->LastError = Error;
        return (pp->Proportion * Error              // 比例项
            +   pp->Integral * pp->SumError         // 积分项
            +   pp->Derivative * dError             // 微分项
        );
}

/*====================================================================================================
   Initialize PID Structure
=====================================================================================================*/

void PIDInit (PID *pp)
{
    memset ( pp,0,sizeof(PID));
}

/*====================================================================================================
    Main Program
=====================================================================================================*/

double sensor (void)                    //  Dummy Sensor Function
{
    return 100.0;
}

void actuator(double rDelta)            //  Dummy Actuator Function
{}

void main(void)
{
    PID         sPID;                   //  PID Control Structure
    double      rOut;                   //  PID Response (Output)
    double      rIn;                    //  PID Feedback (Input)

    PIDInit ( &sPID );                  //  Initialize Structure
    sPID.Proportion = 0.5;              //  Set PID Coefficients
    sPID.Integral   = 0.5;
    sPID.Derivative = 0.0;
    sPID.SetPoint   = 100.0;            //  Set PID Setpoint

    for (;;) {                          //  Mock Up of PID Processing

        rIn = sensor ();                //  Read Input
        rOut = PIDCalc ( &sPID,rIn );   //  Perform PID Interation
        actuator ( rOut );              //  Effect Needed Changes
    }
}

;************************************************************************************************
      本人自己琢磨着弄了一个整型变量来实现了PID算法，由于是用整型数来做的，所以也不是很精确，
但是对于很多的使用场合，这个精度也够了。关于系数和采样电压全部是放大10倍处理的。所以精度不是
很高，但是也不是那么低，大部分的场合都够用了。实在觉得精度不够，可以再放大10倍或者100倍处理，
但是要注意不超出整个数据类型的范围就可以了。
 
      本人做的是带死区控制的PID算法。
 
      具体的参考代码参见下面：
 
typedef struct PIDValue
{
    uint32 Ek_Uint32[3];         //差值保存，给定和反馈的差值
    uint8  EkFlag_Uint8[3];     //符号，1则对应的Ek[i]为负数，0为对应的Ek[i]为正数
    uint8   KP_Uint8;
 uint8   KI_Uint8;
 uint8   KD_Uint8;
 uint8   B_Uint8;     //死区电压
 
 uint8   KP;      //显示修改的时候用
 uint8   KI;      //
 uint8   KD;      //
 uint8   B;       //
 uint16  Uk_Uint16;    //上一时刻的控制电压
}PIDValueStr;
 
PIDValueStr xdata PID;
/*******************************
**PID = Uk + (KP*E(k) - KI*E(k-1) + KD*E(k-2));
********************************/
void    PIDProcess(void)
{
 uint32 idata Temp[3];  //
 uint32 idata PostSum;  //正数和
 uint32 idata NegSum;   //负数和
 Temp[0] = 0;
    Temp[1] = 0;
    Temp[2] = 0;
 PostSum = 0;
 NegSum = 0;
 if( ADPool.Value_Uint16[UINADCH] > ADPool.Value_Uint16[UFADCH] )  //给定大于反馈,则EK为正数
 {
     Temp[0] = ADPool.Value_Uint16[UINADCH] - ADPool.Value_Uint16[UFADCH];   //计算Ek[0]
    if( Temp[0] > PID.B_Uint8 )
        {
      //数值移位
            PID.Ek_Uint32[2] = PID.Ek_Uint32[1];
            PID.Ek_Uint32[1] = PID.Ek_Uint32[0];
            PID.Ek_Uint32[0] = Temp[0];
            //符号移位
   PID.EkFlag_Uint8[2] = PID.EkFlag_Uint8[1];
   PID.EkFlag_Uint8[1] = PID.EkFlag_Uint8[0];
   PID.EkFlag_Uint8[0] = 0;                       //当前EK为正数
            Temp[0] = (uint32)PID.KP_Uint8 * PID.Ek_Uint32[0];    // KP*EK0
            Temp[1] = (uint32)PID.KI_Uint8 * PID.Ek_Uint32[1];    // KI*EK1
            Temp[2] = (uint32)PID.KD_Uint8 * PID.Ek_Uint32[2];    // KD*EK2
        }
 }
 else   //反馈大于给定
 {
     Temp[0] = ADPool.Value_Uint16[UFADCH] - ADPool.Value_Uint16[UINADCH];   //计算Ek[0]
        if( Temp[0] > PID.B_Uint8 )
        {
      //数值移位
            PID.Ek_Uint32[2] = PID.Ek_Uint32[1];
            PID.Ek_Uint32[1] = PID.Ek_Uint32[0];
            PID.Ek_Uint32[0] = Temp[0];
            //符号移位
   PID.EkFlag_Uint8[2] = PID.EkFlag_Uint8[1];
   PID.EkFlag_Uint8[1] = PID.EkFlag_Uint8[0];
   PID.EkFlag_Uint8[0] = 1;      &
nbsp;                //当前EK为负数
            Temp[0] = (uint32)PID.KP_Uint8 * PID.Ek_Uint32[0];    // KP*EK0
            Temp[1] = (uint32)PID.KI_Uint8 * PID.Ek_Uint32[1];    // KI*EK1
            Temp[2] = (uint32)PID.KD_Uint8 * PID.Ek_Uint32[2];    // KD*EK2
        }
 }
 
/*以下部分代码是讲所有的正数项叠加，负数项叠加*/
    if(PID.EkFlag_Uint8[0]==0)
    {
        PostSum += Temp[0];   //正数和
 }
    else
 {
        NegSum += Temp[0];    //负数和
 }                         // KP*EK0
    if(PID.EkFlag_Uint8[1]!=0)      
    {
        PostSum += Temp[1];   //正数和
 }
 else
 {
        NegSum += Temp[1];    //负数和
 }                         // - kI * EK1
    if(PID.EkFlag_Uint8[2]==0)
    {
        PostSum += Temp[2];   //正数和
    }
 else
 {
        NegSum += Temp[2];    //负数和
 }                         // KD * EK2
    PostSum += (uint32)PID.Uk_Uint16;        // 
    if( PostSum > NegSum
 
)             // 是否控制量为正数
    {
        Temp[0] = PostSum - NegSum;
        if( Temp[0] < (uint32)ADPool.Value_Uint16[UMAXADCH] )   //小于限幅值则为计算值输出
  {
            PID.Uk_Uint16 = (uint16)Temp[0];
  }
  else
  {
            PID.Uk_Uint16 = ADPool.Value_Uint16[UMAXADCH];    //否则为限幅值输出
     }
    }
    else               //控制量输出为负数，则输出0
    {
        PID.Uk_Uint16 = 0;
    }
} 

;***********************************************************************************************