📄 minid.lst
字号:
0354 9508 RET
_PIDInit:
pp --> R20
0355 D50C RCALL push_gset1
0356 01A8 MOVW R20,R16
0357 9722 SBIW R28,2
FILE: D:\bak\新建文件夹\函数库\PID\PIDCALC.c
(0001) /*****************************************************************
(0002) * 文件名： PID.c
(0003) * 版本号：
(0004) * 创作日期： 2005.7.12
(0005) * 作者： Wangzq
(0006) * 功能说明： PID增量式算法
(0007) * 其它说明：
(0008)
(0009) *****************************************************************/
(0010)
(0011)
(0012) /*****************************************************************
(0013)
(0014) * 修改日期：
(0015) * 修改人：
(0016) * 修改原因：
(0017)
(0018) *******************************************************************/
(0019)
(0020) #include "PIDCALC.h"
(0021)
(0022) //****************************************************************
(0023) //增量式PID算法
(0024) //
(0025) //***************************************************************
(0026) void PIDInit (struct PID *pp )
(0027) {
(0028) memset( pp, 0, sizeof(struct PID ) );
0358 E08E LDI R24,0xE
0359 E090 LDI R25,0
035A 8399 STD Y+1,R25
035B 8388 STD Y+0,R24
035C 2722 CLR R18
035D 2733 CLR R19
035E 018A MOVW R16,R20
035F D43C RCALL _memset
0360 9622 ADIW R28,2
0361 D503 RCALL pop_gset1
0362 9508 RET
_PIDCalc:
derror --> Y+1
perror --> R14
K --> R10
result --> Y+4
pidtemp --> Y+0
Fact_Value --> R12
pp --> R10
0363 D4F6 RCALL push_gset5
0364 0169 MOVW R12,R18
0365 0158 MOVW R10,R16
0366 9728 SBIW R28,0x8
(0029) }
(0030)
(0031) byte PIDCalc(struct PID *pp, word Fact_Value )
(0032) {
(0033) byte K; // PID实际输出值
(0034) sword perror,derror;
(0035) sdword result,pidtemp;
(0036)
(0037) if (pp->SetValue < 300 ) //低速的PID值
0367 01F5 MOVW R30,R10
0368 8180 LDD R24,Z+0
0369 8191 LDD R25,Z+1
036A 93EA ST R30,-Y
036B 328C CPI R24,0x2C
036C E0E1 LDI R30,1
036D 079E CPC R25,R30
036E 91E9 LD R30,Y+
036F F468 BCC 0x037D
(0038) {
(0039) pp->P = KPL; //低速的p值
0370 E28D LDI R24,0x2D
0371 E090 LDI R25,0
0372 8393 STD Z+3,R25
0373 8382 STD Z+2,R24
(0040) pp->I = KIL;
0374 E085 LDI R24,5
0375 01F5 MOVW R30,R10
0376 8395 STD Z+5,R25
0377 8384 STD Z+4,R24
(0041) pp->D = KDL;
0378 E082 LDI R24,2
0379 01F5 MOVW R30,R10
037A 8397 STD Z+7,R25
037B 8386 STD Z+6,R24
(0042) }
037C C0B9 RJMP 0x0436
(0043) else
(0044) {
(0045) if (pp->SetValue > 2500) //高速的PID值
037D EC84 LDI R24,0xC4
037E E099 LDI R25,0x9
037F 01F5 MOVW R30,R10
0380 8020 LDD R2,Z+0
0381 8031 LDD R3,Z+1
0382 1582 CP R24,R2
0383 0593 CPC R25,R3
0384 F468 BCC 0x0392
(0046) {
(0047) pp->P = KPH; //告诉的p值
0385 E186 LDI R24,0x16
0386 E090 LDI R25,0
0387 8393 STD Z+3,R25
0388 8382 STD Z+2,R24
(0048) pp->I = KIH;
0389 E083 LDI R24,3
038A 01F5 MOVW R30,R10
038B 8395 STD Z+5,R25
038C 8384 STD Z+4,R24
(0049) pp->D = KDH;
038D E081 LDI R24,1
038E 01F5 MOVW R30,R10
038F 8397 STD Z+7,R25
0390 8386 STD Z+6,R24
(0050) }
0391 C0A4 RJMP 0x0436
(0051) else //中速的PID值，p，i，d按一定的斜率计算
(0052) {
(0053)
(0054) //*********************计算PID插值常数***************************
(0055) perror = (2500-pp->SetValue); //计算PID插值常数
0392 01F5 MOVW R30,R10
0393 8020 LDD R2,Z+0
0394 8031 LDD R3,Z+1
0395 EC84 LDI R24,0xC4
0396 E099 LDI R25,0x9
0397 017C MOVW R14,R24
0398 18E2 SUB R14,R2
0399 08F3 SBC R15,R3
(0056)
(0057) pidtemp = (KPL-KPH);
039A E147 LDI R20,0x17
039B E050 LDI R21,0
039C E060 LDI R22,0
039D E070 LDI R23,0
039E 01FE MOVW R30,R28
039F 8340 STD Z+0,R20
03A0 8351 STD Z+1,R21
03A1 8362 STD Z+2,R22
03A2 8373 STD Z+3,R23
(0058) pidtemp *= perror;
03A3 0117 MOVW R2,R14
03A4 2444 CLR R4
03A5 FC37 SBRC R3,7
03A6 9440 COM R4
03A7 2455 CLR R5
03A8 FC47 SBRC R4,7
03A9 9450 COM R5
03AA 01FE MOVW R30,R28
03AB 8060 LDD R6,Z+0
03AC 8071 LDD R7,Z+1
03AD 8082 LDD R8,Z+2
03AE 8093 LDD R9,Z+3
03AF 925A ST R5,-Y
03B0 924A ST R4,-Y
03B1 923A ST R3,-Y
03B2 922A ST R2,-Y
03B3 0183 MOVW R16,R6
03B4 0194 MOVW R18,R8
03B5 D472 RCALL empy32s
03B6 01FE MOVW R30,R28
03B7 8300 STD Z+0,R16
03B8 8311 STD Z+1,R17
03B9 8322 STD Z+2,R18
03BA 8333 STD Z+3,R19
(0059) pidtemp /= PIM;
03BB E948 LDI R20,0x98
03BC E058 LDI R21,0x8
03BD E060 LDI R22,0
03BE E070 LDI R23,0
03BF 01FE MOVW R30,R28
03C0 8020 LDD R2,Z+0
03C1 8031 LDD R3,Z+1
03C2 8042 LDD R4,Z+2
03C3 8053 LDD R5,Z+3
03C4 937A ST R23,-Y
03C5 936A ST R22,-Y
03C6 935A ST R21,-Y
03C7 934A ST R20,-Y
03C8 0181 MOVW R16,R2
03C9 0192 MOVW R18,R4
03CA D3E2 RCALL div32s
03CB 01FE MOVW R30,R28
03CC 8300 STD Z+0,R16
03CD 8311 STD Z+1,R17
03CE 8322 STD Z+2,R18
03CF 8333 STD Z+3,R19
(0060) pidtemp += KPH;
03D0 E146 LDI R20,0x16
03D1 E050 LDI R21,0
03D2 E060 LDI R22,0
03D3 E070 LDI R23,0
03D4 01FE MOVW R30,R28
03D5 8020 LDD R2,Z+0
03D6 8031 LDD R3,Z+1
03D7 8042 LDD R4,Z+2
03D8 8053 LDD R5,Z+3
03D9 0E24 ADD R2,R20
03DA 1E35 ADC R3,R21
03DB 1E46 ADC R4,R22
03DC 1E57 ADC R5,R23
03DD 01FE MOVW R30,R28
03DE 8220 STD Z+0,R2
03DF 8231 STD Z+1,R3
03E0 8242 STD Z+2,R4
03E1 8253 STD Z+3,R5
(0061) pp->P = pidtemp;
03E2 01FE MOVW R30,R28
03E3 8020 LDD R2,Z+0
03E4 8031 LDD R3,Z+1
03E5 01F5 MOVW R30,R10
03E6 8233 STD Z+3,R3
03E7 8222 STD Z+2,R2
(0062)
(0063) pidtemp = (KIL-KIH);
03E8 E042 LDI R20,2
03E9 E050 LDI R21,0
03EA E060 LDI R22,0
03EB E070 LDI R23,0
03EC 01FE MOVW R30,R28
03ED 8340 STD Z+0,R20
03EE 8351 STD Z+1,R21
03EF 8362 STD Z+2,R22
03F0 8373 STD Z+3,R23
(0064) pidtemp *= perror;
03F1 0117 MOVW R2,R14
03F2 2444 CLR R4
03F3 FC37 SBRC R3,7
03F4 9440 COM R4
03F5 2455 CLR R5
03F6 FC47 SBRC R4,7
03F7 9450 COM R5
03F8 01FE MOVW R30,R28
03F9 8060 LDD R6,Z+0
03FA 8071 LDD R7,Z+1
03FB 8082 LDD R8,Z+2
03FC 8093 LDD R9,Z+3
03FD 925A ST R5,-Y
03FE 924A ST R4,-Y
03FF 923A ST R3,-Y
0400 922A ST R2,-Y
0401 0183 MOVW R16,R6
0402 0194 MOVW R18,R8
0403 D424 RCALL empy32s
0404 01FE MOVW R30,R28
0405 8300 STD Z+0,R16
0406 8311 STD Z+1,R17
0407 8322 STD Z+2,R18
0408 8333 STD Z+3,R19
(0065) pidtemp /= PIM;
0409 E948 LDI R20,0x98
040A E058 LDI R21,0x8
040B E060 LDI R22,0
040C E070 LDI R23,0
040D 01FE MOVW R30,R28
040E 8020 LDD R2,Z+0
040F 8031 LDD R3,Z+1
0410 8042 LDD R4,Z+2
0411 8053 LDD R5,Z+3
0412 937A ST R23,-Y
0413 936A ST R22,-Y
0414 935A ST R21,-Y
0415 934A ST R20,-Y
0416 0181 MOVW R16,R2
0417 0192 MOVW R18,R4
0418 D394 RCALL div32s
0419 01FE MOVW R30,R28
041A 8300 STD Z+0,R16
041B 8311 STD Z+1,R17
041C 8322 STD Z+2,R18
041D 8333 STD Z+3,R19
(0066) pidtemp += KIH;
041E E043 LDI R20,3
041F E050 LDI R21,0
0420 E060 LDI R22,0
0421 E070 LDI R23,0
0422 01FE MOVW R30,R28
0423 8020 LDD R2,Z+0
0424 8031 LDD R3,Z+1
0425 8042 LDD R4,Z+2
0426 8053 LDD R5,Z+3
0427 0E24 ADD R2,R20
0428 1E35 ADC R3,R21
0429 1E46 ADC R4,R22
042A 1E57 ADC R5,R23
042B 01FE MOVW R30,R28
042C 8220 STD Z+0,R2
042D 8231 STD Z+1,R3
042E 8242 STD Z+2,R4
042F 8253 STD Z+3,R5
(0067) pp->I = pidtemp;
0430 01FE MOVW R30,R28
0431 8020 LDD R2,Z+0
0432 8031 LDD R3,Z+1
0433 01F5 MOVW R30,R10
0434 8235 STD Z+5,R3
0435 8224 STD Z+4,R2
(0068)
(0069) // pidtemp = (KDL-KDH);
(0070) // pidtemp *= perror;
(0071) // pidtemp /= PIM;
(0072) // pidtemp += KDH;
(0073) // pp->D = pidtemp;
(0074) //*********************计算PID插值常数***************************
(0075)
(0076) }
(0077) }
(0078)
(0079) //result = 0;// 清零
(0080) if (Fact_Value > 7000) //转速上限值设定
0436 E588 LDI R24,0x58
0437 E19B LDI R25,0x1B
0438 158C CP R24,R12
0439 059D CPC R25,R13
043A F408 BCC 0x043C
(0081) {
(0082) Fact_Value = 7000;
043B 016C MOVW R12,R24
(0083) }
(0084)
(0085) pp->Error = pp->SetValue - Fact_Value; // 取当前偏差
043C 01F5 MOVW R30,R10
043D 8020 LDD R2,Z+0
043E 8031 LDD R3,Z+1
043F 182C SUB R2,R12
0440 083D SBC R3,R13
0441 8631 STD Z+9,R3
0442 8620 STD Z+8,R2
(0086)
(0087) perror = pp->Error - pp->LastError; //计算比例偏差
0443 01F5 MOVW R30,R10
0444 8422 LDD R2,Z+10
0445 8433 LDD R3,Z+11
0446 01F5 MOVW R30,R10
0447 84E0 LDD R14,Z+8
0448 84F1 LDD R15,Z+9
0449 18E2 SUB R14,R2
044A 08F3 SBC R15,R3
(0088)
(0089) //*****************************************************************
(0090) // derror = pp->LastError; //计算微分偏差
(0091) // derror += pp->LastError;
(0092) // derror = pp->Error - derror;
(0093) // derror += pp->PrevError;
(0094) //****************************************************************
(0095)
(0096) pp->PrevError = pp->LastError; // 当前误差Error[-1]赋给上次误差Error[-2]
044B 01F5 MOVW R30,R10
044C 8422 LDD R2,Z+10
044D 8433 LDD R3,Z+11
044E 01F5 MOVW R30,R10
044F 8635 STD Z+13,R3
0450 8624 STD Z+12,R2
(0097) pp->LastError = pp->Error; // 取当前误差
0451 01F5 MOVW R30,R10
0452 8420 LDD R2,Z+8
0453 8431 LDD R3,Z+9
0454 01F5 MOVW R30,R10
0455 8633 STD Z+11,R3
0456 8622 STD Z+10,R2
(0098)
(0099) pidtemp = perror;
0457 0117 MOVW R2,R14
0458 2444 CLR R4
0459 FC37 SBRC R3,7
045A 9440 COM R4
045B 2455 CLR R5
045C FC47 SBRC R4,7
045D 9450 COM R5
045E 01FE MOVW R30,R28
045F 8220 STD Z+0,R2
0460 8231 STD Z+1,R3
0461 8242 STD Z+2,R4
0462 8253 STD Z+3,R5
(0100) pidtemp *= (pp->P);
0463 01F5 MOVW R30,R10
0464 8022 LDD R2,Z+2
0465 8033 LDD R3,Z+3
0466 2444 CLR R4
0467 2455 CLR R5
0468 01FE MOVW R30,R28
0469 8060 LDD R6,Z+0
046A 8071 LDD R7,Z+1
046B 8082 LDD R8,Z+2
046C 8093 LDD R9,Z+3
046D 925A ST R5,-Y
046E 924A ST R4,-Y
046F 923A ST R3,-Y
0470 922A ST R2,-Y
0471 0183 MOVW R16,R6
0472 0194 MOVW R18,R8
0473 D3B4 RCALL empy32s
0474 01FE MOVW R30,R28
0475 8300 STD Z+0,R16
0476 8311 STD Z+1,R17
0477 8322 STD Z+2,R18
0478 8333 STD Z+3,R19
(0101) result = pidtemp; // 比例项
0479 01FE MOVW R30,R28
047A 8020 LDD R2,Z+0
047B 8031 LDD R3,Z+1
047C 8042 LDD R4,Z+2
047D 8053 LDD R5,Z+3
047E 01FE MOVW R30,R28
047F 8224 STD Z+4,R2
0480 8235 STD Z+5,R3
0481 8246 STD Z+6,R4
0482 8257 STD Z+7,R5
(0102)
(0103) pidtemp = pp->Error;
0483 01F5 MOVW R30,R10
0484 8420 LDD R2,Z+8
0485 8431 LDD R3,Z+9
0486 2444 CLR R4
0487 FC37 SBRC R3,7
0488 9440 COM R4
0489 2455 CLR R5
048A FC47 SBRC R4,7
048B 9450 COM R5
048C 01FE MOVW R30,R28
048D 8220 STD Z+0,R2
048E 8231 STD Z+1,R3
048F 8242 STD Z+2,R4
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -