;//**************************************************************************************************************
        ;//功能: 电机控制的PID(积分分离)PID(void)执行时间少于70uS/932-12MHz
        ;//设 计 者: 牟联树
        ;//日期: 2003.12.28
        ;//版 本 号: 1.0
        ;//申明:
        ;//**************************************************************************************************************
        $NOMOD51
        OCRAH equ 0EFh
        OCRAL equ 0EEh
        OCRBH equ 0FBh
        OCRBL equ 0FAh
        OCRCH equ 0FDh
        OCRCL equ 0FCh
        OCRDH equ 0FFh
        OCRDL equ 0FEh
        TCR21 equ 0f9h
        $include (REG52.INC)
        NAMEPIDWork
        ?PR?PID?PIDWorkSEGMENTCODE
        ?PR?PID_MUL?PIDWork SEGMENTCODE
        ?PR?PID_out?PIDWork SEGMENTCODE
        ?DT?PID_k?PIDWorkSEGMENTDATA
        PUBLICPID_Kp,PID_Ki,PID_Kd,PID_Umax,PID_Emax
        PUBLICPID_U,PID_En1,PID_En2,PID_Temp,PID_TempL
        RSEG?DT?PID_k?PIDWork
PID_Kp: DS2            ;比例系数
        PID_KpLdata PID_Kp+1
PID_Ki: DS2            ;积分系数
        PID_KiLdata PID_Ki+1
PID_Kd: DS2            ;微分系数
        PID_KdLdata PID_Kd+1
PID_Umax:
        
        
        DS2            ;期望最大值设定
        PID_UmaxLdata PID_Umax+1
PID_Emax:
        
        
        DS2            ;积分分离的误差最大值设定
        PID_EmaxLdata PID_Emax+1
PID_U:  DS2            ;当前的采样值
        PID_ULdata PID_U+1
PID_En1:DS2            ;上一次计算的误差值
        PID_En1Ldata PID_En1+1
PID_En2:DS2            ;上一次计算的误差的误差值
        PID_En2Ldata PID_En2+1
PID_Temp:
        
        
        DS4            ;计算过程中的暂存
        PID_TempLdata PID_Temp+1
        PID_Temp1data PID_Temp+2
        PID_Temp1Ldata PID_Temp+3
        PUBLIC_PID
        RSEG?PR?PID?PIDWork
_PID:   push acc
        push psw
        clr c          ;有符号减法
        mov a,PID_UmaxL;计算当前的误差En(PID_U里暂存运算结果)
        subb a,PID_UL
        mov PID_UL,a
        mov a,PID_Umax
        subb a,PID_U
        mov PID_U,a
        jnb acc.7,PID_k
        mov a,PID_UL
        cpl a
        add a,#1
        mov r7,a
        mov a,PID_U
        cpl a
        addc a,#0
        xch a,r7
        sjmp PID_g
PID_k:  mov r7,PID_U
        mov a,PID_UL
PID_g:  clr c
        subb a,PID_EmaxL
        mov a,r7
        subb a,PID_Emax
        jnb acc.7,PID_a;如果En>Emax则转
        mov r7,PID_KiL ;Ki*En
        mov r6,PID_Ki
        mov r5,PID_UL
        mov r4,PID_U
        acall PID_MUL  ;二字节伪有符号乘法(r4-r7里暂存结果)
        mov a,PID_Temp1L       ;PID加法子程序(将结果加入输出)
        add a,r7
        mov PID_Temp1L,a
        mov a,PID_Temp1
        addc a,r6
        mov PID_Temp1,a
        mov a,PID_TempL
        addc a,r5
        mov PID_TempL,a
        mov a,PID_Temp
        addc a,r4
        mov PID_Temp,a
PID_a:  mov a,PID_En1L ;更新PID_En1
        xch a,PID_UL
        mov PID_En1L,a
        mov a,PID_En1
        xch a,PID_U
        mov PID_En1,a
        clr c          ;有符号减法
        mov a,PID_En1L ;计算当前的误差差(En-En1)
        subb a,PID_UL
        mov PID_UL,a
        mov a,PID_En1
        subb a,PID_U
        mov PID_U,a
        mov r7,PID_KpL ;+Kp*(En-En1)
        mov r6,PID_Kp
        mov r5,PID_UL
        mov r4,PID_U
        acall PID_MUL
        mov a,PID_Temp1L       ;PID加法子程序(将结果加入输出)
        add a,r7
        mov PID_Temp1L,a
        mov a,PID_Temp1
        addc a,r6
        mov PID_Temp1,a
        mov a,PID_TempL
        addc a,r5
        mov PID_TempL,a
        mov a,PID_Temp
        addc a,r4
        mov PID_Temp,a
        clr c          ;有符号减法
        mov a,PID_UL   ;计算误差差的差(En-En1-En2),同时更新En2
        subb a,PID_En2L
        mov PID_En2L,a
        mov a,PID_U
        subb a,PID_En2
        mov PID_En2,a
        mov r7,PID_KdL ;+Kd*(En-En1-En2)
        mov r6,PID_Kd
        mov r5,PID_En2L
        mov r4,PID_En2
        acall PID_MUL
        mov a,PID_Temp1L       ;PID加法子程序(将结果加入输出)
        add a,r7
        mov PID_Temp1L,a
        mov a,PID_Temp1
        addc a,r6
        mov PID_Temp1,a
        mov a,PID_TempL
        addc a,r5
        mov PID_TempL,a
        mov a,PID_Temp
        addc a,r4
        mov PID_Temp,a
        pop psw
        pop acc
        ret
        ret
        
        ;//**************************************************************************************************************
        ;//功能: 电机控制电流环的PIPI_I(void)执行时间少于50uS/932-12MHz
        ;//设 计 者: 牟联树
        ;//日期: 2003.12.28
        ;//版 本 号: 1.0
        ;//申明:
        ;//**************************************************************************************************************
        PUBLIC _PID_out
        RSEG ?PR?PID_out?PIDWork
_PID_out:
        
        
        push acc
        push psw
        clr c
        mov a,r6
        rrc a
        mov r6,a
        mov a,r7
        rrc a
        mov r7,a
out_a:  mov OCRAH,r6   ;PWM更新
        mov OCRAL,r7
        mov a,r7
        add a,#25
        mov OCRBL,a
        mov a,r6
        addc a,#0
        mov OCRBH,a
        orl TCR21,#080h;PWM更新
out_b:  pop psw
        pop acc
        ret
        
        ;//**************************************************************************************************************
        ;//功能: 整形乘法long int PID_MUL(int a,int b)用时13.5uS/932-12MHz
        ;//设 计 者: 牟联树
        ;//日期: 2003.12.28
        ;//版 本 号: 1.0
        ;//申明:
        ;//**************************************************************************************************************
        PUBLIC _PID_MUL
        RSEG ?PR?PID_MUL?PIDWork
_PID_MUL:
        
        
        
PID_MUL:push acc
        push psw
        clr f0         ;符号判断
        mov a,r4
        jb acc.7,PID_Ma
        sjmp PID_Md
PID_Ma: setb f0
        mov a,r5
        cpl a
        add a,#1
        mov r5,a
        mov a,r4
        cpl a
        addc a,#0
        mov r4,a
PID_Md: ;mov a,r6
        ; movc,acc.7
        ;jnbf0,PID_Mf
        ; cplc
PID_Mf: ;movf0,c
        ; jbacc.7,PID_Mb
        ; sjmp PID_Mc
PID_Mb: ;clrc
        ; mov a,r7
        ; subb a,#1
        ; cpla
        ; mov r7,a
        ; mov a,r6
        ; subb a,#0
        ; cpla
        ; mov r6,a
PID_Mc: mov a,r5
        mov b,r7
        mul ab
        xch a,r7
        mov r3,b
        mov b,r4
        mul ab
        add a,r3
        mov r3,a
        mov a,b
        addc a,#0
        xch a,r5
        mov b,r6
        mul ab
        addc a,r3
        xch a,r6
        xch a,r5
        addc a,b
        xch a,r5
        mov b,r4
        mul ab
        addc a,r5
        mov r5,a
        mov a,b
        addc a,#0
        mov r4,a
        jb f0,PID_Me
        pop psw
        pop acc
        ret
PID_Me: mov a,r7
        cpl a
        add a,#1
        mov r7,a
        mov a,r6
        cpl a
        addc a,#0
        mov r6,a
        mov a,r5
        cpl a
        addc a,#0
        mov r5,a
        mov a,r4
        cpl a
        addc a,#0
        mov r4,a
        pop psw
        pop acc
        ret
        END