會員中心
會員注冊;//**************************************************************************************************************
;//功 能: 電機控制的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)
NAME PIDWork
?PR?PID?PIDWork SEGMENT CODE
?PR?PID_MUL?PIDWork SEGMENT CODE
?PR?PID_out?PIDWork SEGMENT CODE
?DT?PID_k?PIDWork SEGMENT DATA
PUBLIC PID_Kp,PID_Ki,PID_Kd,PID_Umax,PID_Emax
PUBLIC PID_U,PID_En1,PID_En2,PID_Temp,PID_TempL
RSEG ?DT?PID_k?PIDWork
PID_Kp: DS 2 ;比例系數
PID_KpL data PID_Kp+1
PID_Ki: DS 2 ;積分系數
PID_KiL data PID_Ki+1
PID_Kd: DS 2 ;微分系數
PID_KdL data PID_Kd+1
PID_Umax: DS 2 ;期望最大值設定
PID_UmaxL data PID_Umax+1
PID_Emax: DS 2 ;積分分離的誤差最大值設定
PID_EmaxL data PID_Emax+1
PID_U: DS 2 ;當前的采樣值
PID_UL data PID_U+1
PID_En1: DS 2 ;上一次計算的誤差值
PID_En1L data PID_En1+1
PID_En2: DS 2 ;上一次計算的誤差的誤差值
PID_En2L data PID_En2+1
PID_Temp: DS 4 ;計算過程中的暫存
PID_TempL data PID_Temp+1
PID_Temp1 data PID_Temp+2
PID_Temp1L data 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
;//**************************************************************************************************************
;//功 能: 電機控制電流環的PI PI_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
; mov c,acc.7
;jnb f0,PID_Mf
; cpl c
PID_Mf:; mov f0,c
; jb acc.7,PID_Mb
; sjmp PID_Mc
PID_Mb:; clr c
; mov a,r7
; subb a,#1
; cpl a
; mov r7,a
; mov a,r6
; subb a,#0
; cpl a
; 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
返回頂部
刷新頁面
下到頁底