📄 pidcalc.lis
字号:
.module PIDCALC.c
.area text(rom, con, rel)
0000 .dbfile E:\PMax\单片机源代码\函数库\PID\PIDCALC.c
0000 .dbfunc e PIDInit _PIDInit fV
0000 .dbstruct 0 16 PID
0000 .dbfield 0 SetValue I
0000 .dbfield 2 P I
0000 .dbfield 4 I I
0000 .dbfield 6 D I
0000 .dbfield 8 LastError I
0000 .dbfield 10 PrevError I
0000 .dbfield 12 SumError L
0000 .dbend
0000 ; pp -> R20,R21
.even
0000 _PIDInit::
0000 00D0 rcall push_gset1
0002 A801 movw R20,R16
0004 2297 sbiw R28,2
0006 .dbline -1
0006 .dbline 23
0006 ; /*****************************************************************
0006 ; * 文件名： PID.c
0006 ; * 版本号：
0006 ; * 创作日期： 2005.7.12
0006 ; * 作者： Wangzq
0006 ; * 功能说明： PID增量式算法
0006 ; * 其它说明：
0006 ;
0006 ; *****************************************************************/
0006 ;
0006 ;
0006 ; /*****************************************************************
0006 ;
0006 ; * 修改日期：
0006 ; * 修改人：
0006 ; * 修改原因：
0006 ;
0006 ; *******************************************************************/
0006 ;
0006 ; #include "PIDCALC.h"
0006 ;
0006 ; void PIDInit (struct PID *pp )
0006 ; {
0006 .dbline 24
0006 ; memset( pp, 0, sizeof(struct PID ) );
0006 80E1 ldi R24,16
0008 90E0 ldi R25,0
000A 9983 std y+1,R25
000C 8883 std y+0,R24
000E 2227 clr R18
0010 3327 clr R19
0012 8A01 movw R16,R20
0014 00D0 rcall _memset
0016 .dbline -2
0016 L1:
0016 2296 adiw R28,2
0018 00D0 rcall pop_gset1
001A .dbline 0 ; func end
001A 0895 ret
001C .dbsym r pp 20 pS[PID]
001C .dbend
001C .dbfunc e PIDCalc _PIDCalc fc
001C ; i -> R10
001C ; dError -> R10,R11
001C ; Ki -> R14
001C ; Error -> R12,R13
001C ; result -> y+0
001C ; Fact_Value -> R10,R11
001C ; pp -> y+14
.even
001C _PIDCalc::
001C 00D0 rcall push_arg4
001E 00D0 rcall push_gset5
0020 5901 movw R10,R18
0022 2497 sbiw R28,4
0024 .dbline -1
0024 .dbline 28
0024 ; }
0024 ;
0024 ; byte PIDCalc(struct PID *pp, word Fact_Value )
0024 ; {
0024 .dbline 34
0024 ; sbyte Ki; // Kp积分阀值
0024 ; sword Error,dError;
0024 ; sdword result;
0024 ; byte i;
0024 ;
0024 ; if (pp->SetValue < 300 ) //低速的PID值
0024 EE85 ldd R30,y+14
0026 FF85 ldd R31,y+15
0028 8081 ldd R24,z+0
002A 9181 ldd R25,z+1
002C EA93 st -y,R30
002E 8C32 cpi R24,44
0030 E1E0 ldi R30,1
0032 9E07 cpc R25,R30
0034 E991 ld R30,y+
0036 84F4 brge L3
0038 .dbline 35
0038 ; {
0038 .dbline 36
0038 ; pp->P = KPL; //低速的p值
0038 82EC ldi R24,450
003A 91E0 ldi R25,1
003C 9383 std z+3,R25
003E 8283 std z+2,R24
0040 .dbline 37
0040 ; pp->I = KIL;
0040 88E3 ldi R24,56
0042 90E0 ldi R25,0
0044 EE85 ldd R30,y+14
0046 FF85 ldd R31,y+15
0048 9583 std z+5,R25
004A 8483 std z+4,R24
004C .dbline 38
004C ; pp->D = KDL;
004C 84E1 ldi R24,20
004E EE85 ldd R30,y+14
0050 FF85 ldd R31,y+15
0052 9783 std z+7,R25
0054 8683 std z+6,R24
0056 .dbline 39
0056 ; }
0056 4BC0 rjmp L4
0058 L3:
0058 .dbline 41
0058 ; else
0058 ; {
0058 .dbline 42
0058 ; if (pp->SetValue > 2500) //高速的PID值
0058 84EC ldi R24,2500
005A 99E0 ldi R25,9
005C EE85 ldd R30,y+14
005E FF85 ldd R31,y+15
0060 2080 ldd R2,z+0
0062 3180 ldd R3,z+1
0064 8215 cp R24,R2
0066 9305 cpc R25,R3
0068 7CF4 brge L5
006A .dbline 43
006A ; {
006A .dbline 44
006A ; pp->P = KPH; //告诉的p值
006A 81EE ldi R24,225
006C 90E0 ldi R25,0
006E 9383 std z+3,R25
0070 8283 std z+2,R24
0072 .dbline 45
0072 ; pp->I = KIH;
0072 8CE1 ldi R24,28
0074 EE85 ldd R30,y+14
0076 FF85 ldd R31,y+15
0078 9583 std z+5,R25
007A 8483 std z+4,R24
007C .dbline 46
007C ; pp->D = KDH;
007C 8AE0 ldi R24,10
007E EE85 ldd R30,y+14
0080 FF85 ldd R31,y+15
0082 9783 std z+7,R25
0084 8683 std z+6,R24
0086 .dbline 47
0086 ; }
0086 33C0 rjmp L6
0088 L5:
0088 .dbline 49
0088 ; else //中速的PID值，p，i，d按一定的斜率计算
0088 ; {
0088 .dbline 50
0088 ; pp->P = ((pp->SetValue-300)*(KPH-KPL)) / (PIM+KPL); //
0088 EE85 ldd R30,y+14
008A FF85 ldd R31,y+15
008C 2081 ldd R18,z+0
008E 3181 ldd R19,z+1
0090 0FE1 ldi R16,-225
0092 1FEF ldi R17,-1
0094 00D0 rcall empy16s
0096 0455 subi R16,84 ; offset = 67500
0098 184F sbci R17,248
009A 2AE5 ldi R18,2650
009C 3AE0 ldi R19,10
009E 00D0 rcall div16s
00A0 1801 movw R2,R16
00A2 EE85 ldd R30,y+14
00A4 FF85 ldd R31,y+15
00A6 3382 std z+3,R3
00A8 2282 std z+2,R2
00AA .dbline 51
00AA ; pp->I = ((pp->SetValue-300)*(KPH-KIL)) / (PIM+KIL); //
00AA EE85 ldd R30,y+14
00AC FF85 ldd R31,y+15
00AE 2081 ldd R18,z+0
00B0 3181 ldd R19,z+1
00B2 09EA ldi R16,169
00B4 10E0 ldi R17,0
00B6 00D0 rcall empy16s
00B8 0C50 subi R16,12
00BA 164C sbci R17,198
00BC 20ED ldi R18,2256
00BE 38E0 ldi R19,8
00C0 00D0 rcall div16s
00C2 1801 movw R2,R16
00C4 EE85 ldd R30,y+14
00C6 FF85 ldd R31,y+15
00C8 3582 std z+5,R3
00CA 2482 std z+4,R2
00CC .dbline 52
00CC ; pp->D = ((pp->SetValue-300)*(KDH-KDL)) / (PIM+KDL);
00CC EE85 ldd R30,y+14
00CE FF85 ldd R31,y+15
00D0 2081 ldd R18,z+0
00D2 3181 ldd R19,z+1
00D4 06EF ldi R16,-10
00D6 1FEF ldi R17,-1
00D8 00D0 rcall empy16s
00DA 0854 subi R16,72 ; offset = 3000
00DC 144F sbci R17,244
00DE 2CEA ldi R18,2220
00E0 38E0 ldi R19,8
00E2 00D0 rcall div16s
00E4 1801 movw R2,R16
00E6 EE85 ldd R30,y+14
00E8 FF85 ldd R31,y+15
00EA 3782 std z+7,R3
00EC 2682 std z+6,R2
00EE .dbline 53
00EE ; }
00EE L6:
00EE .dbline 54
00EE ; }
00EE L4:
00EE .dbline 57
00EE ;
00EE ; //result = 0;// 清零
00EE ; Error = pp->SetValue - Fact_Value;// 偏差
00EE EE85 ldd R30,y+14
00F0 FF85 ldd R31,y+15
00F2 C080 ldd R12,z+0
00F4 D180 ldd R13,z+1
00F6 CA18 sub R12,R10
00F8 DB08 sbc R13,R11
00FA .dbline 58
00FA ; pp->SumError += Error;
00FA CF01 movw R24,R30
00FC 0C96 adiw R24,12
00FE 2601 movw R4,R12
0100 6624 clr R6
0102 57FC sbrc R5,7
0104 6094 com R6
0106 7724 clr R7
0108 67FC sbrc R6,7
010A 7094 com R7
010C FC01 movw R30,R24
010E 4081 ldd R20,z+0
0110 5181 ldd R21,z+1
0112 6281 ldd R22,z+2
0114 7381 ldd R23,z+3
0116 440D add R20,R4
0118 551D adc R21,R5
011A 661D adc R22,R6
011C 771D adc R23,R7
011E 4083 std z+0,R20
0120 5183 std z+1,R21
0122 6283 std z+2,R22
0124 7383 std z+3,R23
0126 .dbline 60
0126 ; // 过度积分的极限处理
0126 ; if( pp->SumError > 500 )
0126 44EF ldi R20,244
0128 51E0 ldi R21,1
012A 60E0 ldi R22,0
012C 70E0 ldi R23,0
012E EE85 ldd R30,y+14
0130 FF85 ldd R31,y+15
0132 2484 ldd R2,z+12
0134 3584 ldd R3,z+13
0136 4684 ldd R4,z+14
0138 5784 ldd R5,z+15
013A 4215 cp R20,R2
013C 5305 cpc R21,R3
013E 6405 cpc R22,R4
0140 7505 cpc R23,R5
0142 54F4 brge L7
0144 .dbline 61
0144 ; {
0144 .dbline 62
0144 ; pp->SumError = 500;
0144 44EF ldi R20,244
0146 51E0 ldi R21,1
0148 60E0 ldi R22,0
014A 70E0 ldi R23,0
014C EE85 ldd R30,y+14
014E FF85 ldd R31,y+15
0150 4487 std z+12,R20
0152 5587 std z+13,R21
0154 6687 std z+14,R22
0156 7787 std z+15,R23
0158 .dbline 63
0158 ; }
0158 L7:
0158 .dbline 64
0158 ; if( pp->SumError < -500 )
0158 4CE0 ldi R20,12
015A 5EEF ldi R21,254
015C 6FEF ldi R22,255
015E 7FEF ldi R23,255
0160 EE85 ldd R30,y+14
0162 FF85 ldd R31,y+15
0164 2484 ldd R2,z+12
0166 3584 ldd R3,z+13
0168 4684 ldd R4,z+14
016A 5784 ldd R5,z+15
016C 2416 cp R2,R20
016E 3506 cpc R3,R21
0170 4606 cpc R4,R22
0172 5706 cpc R5,R23
0174 54F4 brge L9
0176 .dbline 65
0176 ; {
0176 .dbline 66
0176 ; pp->SumError = -500;
0176 4CE0 ldi R20,12
0178 5EEF ldi R21,254
017A 6FEF ldi R22,255
017C 7FEF ldi R23,255
017E EE85 ldd R30,y+14
0180 FF85 ldd R31,y+15
0182 4487 std z+12,R20
0184 5587 std z+13,R21
0186 6687 std z+14,R22
0188 7787 std z+15,R23
018A .dbline 67
018A ; }
018A L9:
018A .dbline 69
018A ; // Ki为积分阈值，目的是采用积分分离的控制算法，既保持积分作用，又减小超调量
018A ; if( Error >= 1500 )// 偏差大于1500取消积分
018A C601 movw R24,R12
018C 8C3D cpi R24,220
018E E5E0 ldi R30,5
0190 9E07 cpc R25,R30
0192 0CF0 brlt L11
0194 .dbline 70
0194 ; {
0194 .dbline 71
0194 ; Ki = 0;
0194 EE24 clr R14
0196 .dbline 72
0196 ; }
0196 L11:
0196 .dbline 74
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -