|
;//**********************************
;//功 能: 电机控制的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
|
