📄 pidcalc.s
字号:
.module PIDCALC.c
.area text(rom, con, rel)
.dbfile E:\PMax\单片机源代码\函数库\PID\PIDCALC.c
.dbfunc e PIDInit _PIDInit fV
.dbstruct 0 16 PID
.dbfield 0 SetValue I
.dbfield 2 P I
.dbfield 4 I I
.dbfield 6 D I
.dbfield 8 LastError I
.dbfield 10 PrevError I
.dbfield 12 SumError L
.dbend
; pp -> R20,R21
.even
_PIDInit::
rcall push_gset1
movw R20,R16
sbiw R28,2
.dbline -1
.dbline 23
; /*****************************************************************
; * 文件名： PID.c
; * 版本号：
; * 创作日期： 2005.7.12
; * 作者： Wangzq
; * 功能说明： PID增量式算法
; * 其它说明：
;
; *****************************************************************/
;
;
; /*****************************************************************
;
; * 修改日期：
; * 修改人：
; * 修改原因：
;
; *******************************************************************/
;
; #include "PIDCALC.h"
;
; void PIDInit (struct PID *pp )
; {
.dbline 24
; memset( pp, 0, sizeof(struct PID ) );
ldi R24,16
ldi R25,0
std y+1,R25
std y+0,R24
clr R18
clr R19
movw R16,R20
rcall _memset
.dbline -2
L1:
adiw R28,2
rcall pop_gset1
.dbline 0 ; func end
ret
.dbsym r pp 20 pS[PID]
.dbend
.dbfunc e PIDCalc _PIDCalc fc
; i -> R10
; dError -> R10,R11
; Ki -> R14
; Error -> R12,R13
; result -> y+0
; Fact_Value -> R10,R11
; pp -> y+14
.even
_PIDCalc::
rcall push_arg4
rcall push_gset5
movw R10,R18
sbiw R28,4
.dbline -1
.dbline 28
; }
;
; byte PIDCalc(struct PID *pp, word Fact_Value )
; {
.dbline 34
; sbyte Ki; // Kp积分阀值
; sword Error,dError;
; sdword result;
; byte i;
;
; if (pp->SetValue < 300 ) //低速的PID值
ldd R30,y+14
ldd R31,y+15
ldd R24,z+0
ldd R25,z+1
st -y,R30
cpi R24,44
ldi R30,1
cpc R25,R30
ld R30,y+
brge L3
.dbline 35
; {
.dbline 36
; pp->P = KPL; //低速的p值
ldi R24,450
ldi R25,1
std z+3,R25
std z+2,R24
.dbline 37
; pp->I = KIL;
ldi R24,56
ldi R25,0
ldd R30,y+14
ldd R31,y+15
std z+5,R25
std z+4,R24
.dbline 38
; pp->D = KDL;
ldi R24,20
ldd R30,y+14
ldd R31,y+15
std z+7,R25
std z+6,R24
.dbline 39
; }
rjmp L4
L3:
.dbline 41
; else
; {
.dbline 42
; if (pp->SetValue > 2500) //高速的PID值
ldi R24,2500
ldi R25,9
ldd R30,y+14
ldd R31,y+15
ldd R2,z+0
ldd R3,z+1
cp R24,R2
cpc R25,R3
brge L5
.dbline 43
; {
.dbline 44
; pp->P = KPH; //告诉的p值
ldi R24,225
ldi R25,0
std z+3,R25
std z+2,R24
.dbline 45
; pp->I = KIH;
ldi R24,28
ldd R30,y+14
ldd R31,y+15
std z+5,R25
std z+4,R24
.dbline 46
; pp->D = KDH;
ldi R24,10
ldd R30,y+14
ldd R31,y+15
std z+7,R25
std z+6,R24
.dbline 47
; }
rjmp L6
L5:
.dbline 49
; else //中速的PID值，p，i，d按一定的斜率计算
; {
.dbline 50
; pp->P = ((pp->SetValue-300)*(KPH-KPL)) / (PIM+KPL); //
ldd R30,y+14
ldd R31,y+15
ldd R18,z+0
ldd R19,z+1
ldi R16,-225
ldi R17,-1
rcall empy16s
subi R16,84 ; offset = 67500
sbci R17,248
ldi R18,2650
ldi R19,10
rcall div16s
movw R2,R16
ldd R30,y+14
ldd R31,y+15
std z+3,R3
std z+2,R2
.dbline 51
; pp->I = ((pp->SetValue-300)*(KPH-KIL)) / (PIM+KIL); //
ldd R30,y+14
ldd R31,y+15
ldd R18,z+0
ldd R19,z+1
ldi R16,169
ldi R17,0
rcall empy16s
subi R16,12
sbci R17,198
ldi R18,2256
ldi R19,8
rcall div16s
movw R2,R16
ldd R30,y+14
ldd R31,y+15
std z+5,R3
std z+4,R2
.dbline 52
; pp->D = ((pp->SetValue-300)*(KDH-KDL)) / (PIM+KDL);
ldd R30,y+14
ldd R31,y+15
ldd R18,z+0
ldd R19,z+1
ldi R16,-10
ldi R17,-1
rcall empy16s
subi R16,72 ; offset = 3000
sbci R17,244
ldi R18,2220
ldi R19,8
rcall div16s
movw R2,R16
ldd R30,y+14
ldd R31,y+15
std z+7,R3
std z+6,R2
.dbline 53
; }
L6:
.dbline 54
; }
L4:
.dbline 57
;
; //result = 0;// 清零
; Error = pp->SetValue - Fact_Value;// 偏差
ldd R30,y+14
ldd R31,y+15
ldd R12,z+0
ldd R13,z+1
sub R12,R10
sbc R13,R11
.dbline 58
; pp->SumError += Error;
movw R24,R30
adiw R24,12
movw R4,R12
clr R6
sbrc R5,7
com R6
clr R7
sbrc R6,7
com R7
movw R30,R24
ldd R20,z+0
ldd R21,z+1
ldd R22,z+2
ldd R23,z+3
add R20,R4
adc R21,R5
adc R22,R6
adc R23,R7
std z+0,R20
std z+1,R21
std z+2,R22
std z+3,R23
.dbline 60
; // 过度积分的极限处理
; if( pp->SumError > 500 )
ldi R20,244
ldi R21,1
ldi R22,0
ldi R23,0
ldd R30,y+14
ldd R31,y+15
ldd R2,z+12
ldd R3,z+13
ldd R4,z+14
ldd R5,z+15
cp R20,R2
cpc R21,R3
cpc R22,R4
cpc R23,R5
brge L7
.dbline 61
; {
.dbline 62
; pp->SumError = 500;
ldi R20,244
ldi R21,1
ldi R22,0
ldi R23,0
ldd R30,y+14
ldd R31,y+15
std z+12,R20
std z+13,R21
std z+14,R22
std z+15,R23
.dbline 63
; }
L7:
.dbline 64
; if( pp->SumError < -500 )
ldi R20,12
ldi R21,254
ldi R22,255
ldi R23,255
ldd R30,y+14
ldd R31,y+15
ldd R2,z+12
ldd R3,z+13
ldd R4,z+14
ldd R5,z+15
cp R2,R20
cpc R3,R21
cpc R4,R22
cpc R5,R23
brge L9
.dbline 65
; {
.dbline 66
; pp->SumError = -500;
ldi R20,12
ldi R21,254
ldi R22,255
ldi R23,255
ldd R30,y+14
ldd R31,y+15
std z+12,R20
std z+13,R21
std z+14,R22
std z+15,R23
.dbline 67
; }
L9:
.dbline 69
; // Ki为积分阈值，目的是采用积分分离的控制算法，既保持积分作用，又减小超调量
; if( Error >= 1500 )// 偏差大于1500取消积分
movw R24,R12
cpi R24,220
ldi R30,5
cpc R25,R30
brlt L11
.dbline 70
; {
.dbline 71
; Ki = 0;
clr R14
.dbline 72
; }
L11:
.dbline 74
;
; if( 200 < Error < 1000 )// 偏差小于1000线性积分
ldi R24,200
ldi R25,0
cp R24,R12
cpc R25,R13
brge L15
ldi R24,1
movw R10,R24
rjmp L16
L15:
clr R10
clr R11
L16:
movw R24,R10
cpi R24,232
ldi R30,3
cpc R25,R30
brge L13
.dbline 75
; {
.dbline 76
; Ki = 10 - ( Error - 200 ) / 80;
ldi R18,80
ldi R19,0
movw R16,R12
subi R16,200
sbci R17,0
rcall div16s
ldi R24,10
ldi R25,0
mov R14,R24
sub R14,R16
sbc R15,R17
.dbline 78
;
; }
L13:
.dbline 80
;
; if( Error <= 200 )// 全积分
ldi R24,200
ldi R25,0
cp R24,R12
cpc R25,R13
brlt L17
.dbline 81
; {
.dbline 82
; Ki = 10;
ldi R24,10
mov R14,R24
.dbline 83
; }
L17:
.dbline 85
;
; dError = pp->LastError - pp->PrevError;// 当前微分
ldd R30,y+14
ldd R31,y+15
ldd R2,z+10
ldd R3,z+11
ldd R30,y+14
ldd R31,y+15
ldd R10,z+8
ldd R11,z+9
sub R10,R2
sbc R11,R3
.dbline 87
;
; pp->PrevError = pp->LastError;// 当前误差Error[-1]赋给上次误差Error[-2]
ldd R30,y+14
ldd R31,y+15
ldd R2,z+8
ldd R3,z+9
ldd R30,y+14
ldd R31,y+15
std z+11,R3
std z+10,R2
.dbline 88
; pp->LastError = Error;// 取当前误差
ldd R30,y+14
ldd R31,y+15
std z+9,R13
std z+8,R12
.dbline 91
;
; // result=[(pp->P*Error)+ (Kp*pp->I*pp->SumError/10)+(pp->D*dError)]/10
; result = pp->P * Error; // 比例项
ldd R30,y+14
ldd R31,y+15
ldd R16,z+2
ldd R17,z+3
movw R18,R12
rcall empy16s
movw R2,R16
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
movw R30,R28
std z+0,R2
std z+1,R3
std z+2,R4
std z+3,R5
.dbline 92
; result += Ki * pp->I * pp->SumError / 10; // 积分项
ldd R30,y+14
ldd R31,y+15
ldd R18,z+4
ldd R19,z+5
mov R16,R14
clr R17
rcall empy16s
movw R2,R16
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
ldd R30,y+14
ldd R31,y+15
ldd R6,z+12
ldd R7,z+13
ldd R8,z+14
ldd R9,z+15
st -y,R9
st -y,R8
st -y,R7
st -y,R6
movw R16,R2
movw R18,R4
rcall empy32s
ldi R20,10
ldi R21,0
ldi R22,0
ldi R23,0
st -y,R23
st -y,R22
st -y,R21
st -y,R20
rcall div32s
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
add R2,R16
adc R3,R17
adc R4,R18
adc R5,R19
movw R30,R28
std z+0,R2
std z+1,R3
std z+2,R4
std z+3,R5
.dbline 93
; result += pp->D * dError; // 微分项
ldd R30,y+14
ldd R31,y+15
ldd R16,z+6
ldd R17,z+7
movw R18,R10
rcall empy16s
movw R2,R16
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
movw R30,R28
ldd R6,z+0
ldd R7,z+1
ldd R8,z+2
ldd R9,z+3
add R6,R2
adc R7,R3
adc R8,R4
adc R9,R5
movw R30,R28
std z+0,R6
std z+1,R7
std z+2,R8
std z+3,R9
.dbline 94
; result /= 1000;
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
st -y,R23
st -y,R22
st -y,R21
st -y,R20
movw R16,R2
movw R18,R4
rcall div32s
movw R30,R28
std z+0,R16
std z+1,R17
std z+2,R18
std z+3,R19
.dbline 96
;
; if (result <= 0)
ldi R20,0
ldi R21,0
ldi R22,0
ldi R23,0
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
cp R20,R2
cpc R21,R3
cpc R22,R4
cpc R23,R5
brlt L19
.dbline 97
; {
.dbline 98
; result = 0;
ldi R20,0
ldi R21,0
ldi R22,0
ldi R23,0
movw R30,R28
std z+0,R20
std z+1,R21
std z+2,R22
std z+3,R23
.dbline 100
;
; }
rjmp L20
L19:
.dbline 102
; else
; {
.dbline 103
; if (result > 1000)
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
cp R20,R2
cpc R21,R3
cpc R22,R4
cpc R23,R5
brge L21
.dbline 104
; result = 1000;
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
movw R30,R28
std z+0,R20
std z+1,R21
std z+2,R22
std z+3,R23
L21:
.dbline 106
;
; result *= 0xff;
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
ldi R20,255
ldi R21,0
ldi R22,0
ldi R23,0
st -y,R5
st -y,R4
st -y,R3
st -y,R2
movw R16,R20
movw R18,R22
rcall empy32s
movw R30,R28
std z+0,R16
std z+1,R17
std z+2,R18
std z+3,R19
.dbline 107
; result /=1000;
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
movw R30,R28
ldd R2,z+0
ldd R3,z+1
ldd R4,z+2
ldd R5,z+3
st -y,R23
st -y,R22
st -y,R21
st -y,R20
movw R16,R2
movw R18,R4
rcall div32s
movw R30,R28
std z+0,R16
std z+1,R17
std z+2,R18
std z+3,R19
.dbline 108
; }
L20:
.dbline 110
;
; i = result;
movw R30,R28
ldd R10,z+0
.dbline 111
; return i;
mov R16,R10
.dbline -2
L2:
adiw R28,4
rcall pop_gset5
adiw R28,4
.dbline 0 ; func end
ret
.dbsym r i 10 c
.dbsym r dError 10 I
.dbsym r Ki 14 c
.dbsym r Error 12 I
.dbsym l result 0 L
.dbsym r Fact_Value 10 i
.dbsym l pp 14 pS[PID]
.dbend
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -