📄 pidcalc.s
字号:
.module PIDCALC.c
.area text(rom, con, rel)
.dbfile C:\DOCUME~1\niutao\MYDOCU~1\永磁电机\平缝机\新建文件夹\函数库\PID\PIDCALC.c
.dbfunc e PIDInit _PIDInit fV
.dbstruct 0 14 PID
.dbfield 0 SetValue i
.dbfield 2 P i
.dbfield 4 I i
.dbfield 6 D i
.dbfield 8 Error I
.dbfield 10 LastError I
.dbfield 12 PrevError I
.dbend
; pp -> R20,R21
.even
_PIDInit::
rcall push_gset1
movw R20,R16
sbiw R28,2
.dbline -1
.dbline 27
; /*****************************************************************
; * 文件名： PID.c
; * 版本号：
; * 创作日期： 2005.7.12
; * 作者： Wangzq
; * 功能说明： PID增量式算法
; * 其它说明：
;
; *****************************************************************/
;
;
; /*****************************************************************
;
; * 修改日期：
; * 修改人：
; * 修改原因：
;
; *******************************************************************/
;
; #include "PIDCALC.h"
;
; //****************************************************************
; //增量式PID算法
; //
; //***************************************************************
; void PIDInit (struct PID *pp )
; {
.dbline 28
; memset( pp, 0, sizeof(struct PID ) );
ldi R24,14
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
; derror -> <dead>
; perror -> R14,R15
; K -> R10
; result -> y+4
; pidtemp -> y+0
; Fact_Value -> R12,R13
; pp -> R10,R11
.even
_PIDCalc::
rcall push_gset5
movw R12,R18
movw R10,R16
sbiw R28,8
.dbline -1
.dbline 32
; }
;
; byte PIDCalc(struct PID *pp, word Fact_Value )
; {
.dbline 37
; byte K; // PID实际输出值
; sword perror,derror;
; sdword result,pidtemp;
;
; if (pp->SetValue < 300 ) //低速的PID值
movw R30,R10
ldd R24,z+0
ldd R25,z+1
st -y,R30
cpi R24,44
ldi R30,1
cpc R25,R30
ld R30,y+
brsh L3
.dbline 38
; {
.dbline 39
; pp->P = KPL; //低速的p值
ldi R24,45
ldi R25,0
std z+3,R25
std z+2,R24
.dbline 40
; pp->I = KIL;
ldi R24,5
movw R30,R10
std z+5,R25
std z+4,R24
.dbline 41
; pp->D = KDL;
ldi R24,2
movw R30,R10
std z+7,R25
std z+6,R24
.dbline 42
; }
rjmp L4
L3:
.dbline 44
; else
; {
.dbline 45
; if (pp->SetValue > 2500) //高速的PID值
ldi R24,2500
ldi R25,9
movw R30,R10
ldd R2,z+0
ldd R3,z+1
cp R24,R2
cpc R25,R3
brsh L5
.dbline 46
; {
.dbline 47
; pp->P = KPH; //告诉的p值
ldi R24,22
ldi R25,0
std z+3,R25
std z+2,R24
.dbline 48
; pp->I = KIH;
ldi R24,3
movw R30,R10
std z+5,R25
std z+4,R24
.dbline 49
; pp->D = KDH;
ldi R24,1
movw R30,R10
std z+7,R25
std z+6,R24
.dbline 50
; }
rjmp L6
L5:
.dbline 52
; else //中速的PID值，p，i，d按一定的斜率计算
; {
.dbline 55
;
; //*********************计算PID插值常数***************************
; perror = (2500-pp->SetValue); //计算PID插值常数
movw R30,R10
ldd R2,z+0
ldd R3,z+1
ldi R24,2500
ldi R25,9
movw R14,R24
sub R14,R2
sbc R15,R3
.dbline 57
;
; pidtemp = (KPL-KPH);
ldi R20,23
ldi R21,0
ldi R22,0
ldi R23,0
std y+0,R20
std y+1,R21
std y+2,R22
std y+3,R23
.dbline 58
; pidtemp *= perror;
movw R2,R14
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall empy32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 59
; pidtemp /= PIM;
ldi R20,152
ldi R21,8
ldi R22,0
ldi R23,0
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R23
st -y,R22
st -y,R21
st -y,R20
rcall div32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 60
; pidtemp += KPH;
ldi R20,22
ldi R21,0
ldi R22,0
ldi R23,0
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
add R2,R20
adc R3,R21
adc R4,R22
adc R5,R23
std y+0,R2
std y+1,R3
std y+2,R4
std y+3,R5
.dbline 61
; pp->P = pidtemp;
ldd R2,y+0
ldd R3,y+1
movw R30,R10
std z+3,R3
std z+2,R2
.dbline 63
;
; pidtemp = (KIL-KIH);
ldi R20,2
ldi R21,0
ldi R22,0
ldi R23,0
std y+0,R20
std y+1,R21
std y+2,R22
std y+3,R23
.dbline 64
; pidtemp *= perror;
movw R2,R14
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall empy32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 65
; pidtemp /= PIM;
ldi R20,152
ldi R21,8
ldi R22,0
ldi R23,0
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R23
st -y,R22
st -y,R21
st -y,R20
rcall div32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 66
; pidtemp += KIH;
ldi R20,3
ldi R21,0
ldi R22,0
ldi R23,0
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
add R2,R20
adc R3,R21
adc R4,R22
adc R5,R23
std y+0,R2
std y+1,R3
std y+2,R4
std y+3,R5
.dbline 67
; pp->I = pidtemp;
ldd R2,y+0
ldd R3,y+1
movw R30,R10
std z+5,R3
std z+4,R2
.dbline 76
;
; // pidtemp = (KDL-KDH);
; // pidtemp *= perror;
; // pidtemp /= PIM;
; // pidtemp += KDH;
; // pp->D = pidtemp;
; //*********************计算PID插值常数***************************
;
; }
L6:
.dbline 77
; }
L4:
.dbline 80
;
; //result = 0;// 清零
; if (Fact_Value > 7000) //转速上限值设定
ldi R24,7000
ldi R25,27
cp R24,R12
cpc R25,R13
brsh L7
.dbline 81
; {
.dbline 82
; Fact_Value = 7000;
movw R12,R24
.dbline 83
; }
L7:
.dbline 85
;
; pp->Error = pp->SetValue - Fact_Value; // 取当前偏差
movw R30,R10
ldd R2,z+0
ldd R3,z+1
sub R2,R12
sbc R3,R13
std z+9,R3
std z+8,R2
.dbline 87
;
; perror = pp->Error - pp->LastError; //计算比例偏差
movw R30,R10
ldd R2,z+10
ldd R3,z+11
movw R30,R10
ldd R14,z+8
ldd R15,z+9
sub R14,R2
sbc R15,R3
.dbline 96
;
; //*****************************************************************
; // derror = pp->LastError; //计算微分偏差
; // derror += pp->LastError;
; // derror = pp->Error - derror;
; // derror += pp->PrevError;
; //****************************************************************
;
; pp->PrevError = pp->LastError; // 当前误差Error[-1]赋给上次误差Error[-2]
movw R30,R10
ldd R2,z+10
ldd R3,z+11
movw R30,R10
std z+13,R3
std z+12,R2
.dbline 97
; pp->LastError = pp->Error; // 取当前误差
movw R30,R10
ldd R2,z+8
ldd R3,z+9
movw R30,R10
std z+11,R3
std z+10,R2
.dbline 99
;
; pidtemp = perror;
movw R2,R14
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
std y+0,R2
std y+1,R3
std y+2,R4
std y+3,R5
.dbline 100
; pidtemp *= (pp->P);
movw R30,R10
ldd R2,z+2
ldd R3,z+3
clr R4
clr R5
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall empy32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 101
; result = pidtemp; // 比例项
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
std y+4,R2
std y+5,R3
std y+6,R4
std y+7,R5
.dbline 103
;
; pidtemp = pp->Error;
movw R30,R10
ldd R2,z+8
ldd R3,z+9
clr R4
sbrc R3,7
com R4
clr R5
sbrc R4,7
com R5
std y+0,R2
std y+1,R3
std y+2,R4
std y+3,R5
.dbline 104
; pidtemp *= (pp->I);
movw R30,R10
ldd R2,z+4
ldd R3,z+5
clr R4
clr R5
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall empy32s
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 105
; result += pidtemp; // 积分项
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
ldd R6,y+4
ldd R7,y+5
ldd R8,y+6
ldd R9,y+7
add R6,R2
adc R7,R3
adc R8,R4
adc R9,R5
std y+4,R6
std y+5,R7
std y+6,R8
std y+7,R9
.dbline 112
;
;
; // pidtemp = derror;
; // pidtemp *= (pp->D);
; // result += pidtemp; // 微分项
;
; SumDu += result;
ldd R2,y+4
ldd R3,y+5
ldd R4,y+6
ldd R5,y+7
lds R8,_SumDu+2
lds R9,_SumDu+2+1
lds R6,_SumDu
lds R7,_SumDu+1
add R6,R2
adc R7,R3
adc R8,R4
adc R9,R5
sts _SumDu+1,R7
sts _SumDu,R6
sts _SumDu+2+1,R9
sts _SumDu+2,R8
.dbline 114
;
; if (SumDu <= 0)
ldi R20,0
ldi R21,0
ldi R22,0
ldi R23,0
cp R20,R6
cpc R21,R7
cpc R22,R8
cpc R23,R9
brlt L9
.dbline 115
; {
.dbline 116
; K = 0;
clr R10
.dbline 117
; }
rjmp L10
L9:
.dbline 120
;
; else
; {
.dbline 121
; result = SumDu;
lds R4,_SumDu+2
lds R5,_SumDu+2+1
lds R2,_SumDu
lds R3,_SumDu+1
std y+4,R2
std y+5,R3
std y+6,R4
std y+7,R5
.dbline 122
; result /= 3000;
ldi R20,184
ldi R21,11
ldi R22,0
ldi R23,0
ldd R16,y+4
ldd R17,y+5
ldd R18,y+6
ldd R19,y+7
st -y,R23
st -y,R22
st -y,R21
st -y,R20
rcall div32s
std y+4,R16
std y+5,R17
std y+6,R18
std y+7,R19
.dbline 124
;
; if (result > 1000)
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
ldd R2,y+4
ldd R3,y+5
ldd R4,y+6
ldd R5,y+7
cp R20,R2
cpc R21,R3
cpc R22,R4
cpc R23,R5
brge L11
.dbline 125
; result = 1000;
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
std y+4,R20
std y+5,R21
std y+6,R22
std y+7,R23
L11:
.dbline 127
;
; result *= 0xff;
ldd R2,y+4
ldd R3,y+5
ldd R4,y+6
ldd R5,y+7
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
std y+4,R16
std y+5,R17
std y+6,R18
std y+7,R19
.dbline 128
; result /=1000;
ldi R20,232
ldi R21,3
ldi R22,0
ldi R23,0
ldd R16,y+4
ldd R17,y+5
ldd R18,y+6
ldd R19,y+7
st -y,R23
st -y,R22
st -y,R21
st -y,R20
rcall div32s
std y+4,R16
std y+5,R17
std y+6,R18
std y+7,R19
.dbline 130
;
; K = result;
ldd R10,y+4
.dbline 131
; K = PIDBound(K, Fact_Value); //计算控制值上限
movw R18,R12
mov R16,R10
rcall _PIDBound
mov R10,R16
.dbline 132
; }
L10:
.dbline 134
;
; return K;
mov R16,R10
.dbline -2
L2:
adiw R28,8
rcall pop_gset5
.dbline 0 ; func end
ret
.dbsym l derror 1 I
.dbsym r perror 14 I
.dbsym r K 10 c
.dbsym l result 4 L
.dbsym l pidtemp 0 L
.dbsym r Fact_Value 12 i
.dbsym r pp 10 pS[PID]
.dbend
.dbfunc e PIDBound _PIDBound fc
; Fact_Value -> R18,R19
; K -> R16
.even
_PIDBound::
.dbline -1
.dbline 138
; }
;
; byte PIDBound(byte K, word Fact_Value)
; {
.dbline 144
; //if (K >0x7f )
; //{
; // K = 0x7f;
; //}
;
; return K;
.dbline -2
L13:
.dbline 0 ; func end
ret
.dbsym r Fact_Value 18 i
.dbsym r K 16 c
.dbend
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -