这是网上找的一PID算法的程序有点看不懂,对PID有好几个疑问求高人指点一下:
(一)作者用 Kp*(En-En1) + Ki*En + Kd*(En-En1-En2) 这好像是增量式pid 算法又好像不是?
(二) PID_Umax - PID_U (期望最大值设定值减去采样值) 计算偏差值时，比较两个数大小为什么用 jnb acc.7, PID_k 而不是jc PID_k
(三) 在标准PID算法中Pout(t)=Kp*e(t)+Ki*Σ(t)+Kd*( e( t)-e(t-1) )这个式子中 Ki*Σ(t) 用两个字节或四个字节存累加和,可是程序运行时可能会能溢出。这个问题要怎样处理???
以个几个疑问求高手赐教
;//**************************************************************************************************************
;//功 能: 电机控制的PID(积分分离) PID(void) 执行时间少于70uS/932-12MHz
;//设 计 者: 牟联树
;//日 期: 2003.12.28
;//版 本 号: 1.0
;//申 明:
;//**************************************************************************************************************
;$NOMOD51
org 00h
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:
;二字节伪有符号乘法(r4-r7里暂存结果)
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 |
楼主
标题置顶
标题高亮
