📄 8.lst
字号:
0161 1D66 ADC R22,R6
0162 1D77 ADC R23,R7
0163 01FE MOVW R30,R28
0164 8440 LDD R4,Z+8
0165 8451 LDD R5,Z+9
0166 8462 LDD R6,Z+10
0167 8473 LDD R7,Z+11
0168 01F5 MOVW R30,R10
0169 8D84 LDD R24,Z+28
016A 8D95 LDD R25,Z+29
016B 27AA CLR R26
016C FD97 SBRC R25,7
016D 95A0 COM R26
016E 27BB CLR R27
016F FDA7 SBRC R26,7
0170 95B0 COM R27
0171 927A ST R7,-Y
0172 926A ST R6,-Y
0173 925A ST R5,-Y
0174 924A ST R4,-Y
0175 018C MOVW R16,R24
0176 019D MOVW R18,R26
0177 D5A3 RCALL empy32s
0178 0F40 ADD R20,R16
0179 1F51 ADC R21,R17
017A 1F62 ADC R22,R18
017B 1F73 ADC R23,R19
017C 01F1 MOVW R30,R2
017D 8040 LDD R4,Z+0
017E 8051 LDD R5,Z+1
017F 8062 LDD R6,Z+2
0180 8073 LDD R7,Z+3
0181 0E44 ADD R4,R20
0182 1E55 ADC R5,R21
0183 1E66 ADC R6,R22
0184 1E77 ADC R7,R23
0185 8240 STD Z+0,R4
0186 8251 STD Z+1,R5
0187 8262 STD Z+2,R6
0188 8273 STD Z+3,R7
(0113)
(0114) if( pp->vl_PreU >= VV_MAX ) //速度PID，防止调节最高溢出
0189 E040 LDI R20,0
018A EF5C LDI R21,0xFC
018B E06F LDI R22,0xF
018C E070 LDI R23,0
018D 01F5 MOVW R30,R10
018E A420 LDD R2,Z+40
018F A431 LDD R3,Z+41
0190 A442 LDD R4,Z+42
0191 A453 LDD R5,Z+43
0192 1624 CP R2,R20
0193 0635 CPC R3,R21
0194 0646 CPC R4,R22
0195 0657 CPC R5,R23
0196 F054 BLT 0x01A1
(0115) pp->vl_PreU = VV_MAX;
0197 E040 LDI R20,0
0198 EF5C LDI R21,0xFC
0199 E06F LDI R22,0xF
019A E070 LDI R23,0
019B 01F5 MOVW R30,R10
019C A740 STD Z+40,R20
019D A751 STD Z+41,R21
019E A762 STD Z+42,R22
019F A773 STD Z+43,R23
01A0 C017 RJMP 0x01B8
(0116)
(0117) else if( pp->vl_PreU <= VV_MIN ) //速度PID，防止调节最低溢出
01A1 E040 LDI R20,0
01A2 E054 LDI R21,4
01A3 EF60 LDI R22,0xF0
01A4 EF7F LDI R23,0xFF
01A5 01F5 MOVW R30,R10
01A6 A420 LDD R2,Z+40
01A7 A431 LDD R3,Z+41
01A8 A442 LDD R4,Z+42
01A9 A453 LDD R5,Z+43
01AA 1542 CP R20,R2
01AB 0553 CPC R21,R3
01AC 0564 CPC R22,R4
01AD 0575 CPC R23,R5
01AE F04C BLT 0x01B8
(0118) pp->vl_PreU = VV_MIN;
01AF E040 LDI R20,0
01B0 E054 LDI R21,4
01B1 EF60 LDI R22,0xF0
01B2 EF7F LDI R23,0xFF
01B3 01F5 MOVW R30,R10
01B4 A740 STD Z+40,R20
01B5 A751 STD Z+41,R21
01B6 A762 STD Z+42,R22
01B7 A773 STD Z+43,R23
(0119)
(0120) return ( pp->vl_PreU >> 10 ); // 返回预调节占空比
01B8 E08A LDI R24,0xA
01B9 E090 LDI R25,0
01BA 01F5 MOVW R30,R10
01BB A420 LDD R2,Z+40
01BC A431 LDD R3,Z+41
01BD A442 LDD R4,Z+42
01BE A453 LDD R5,Z+43
01BF 938A ST R24,-Y
01C0 0181 MOVW R16,R2
01C1 0192 MOVW R18,R4
01C2 D54C RCALL asr32
01C3 962C ADIW R28,0xC
01C4 D584 RCALL pop_gset3
01C5 9508 RET
_s_PIDCalc:
temp_comp --> Y+8
d_error --> Y+4
error --> Y+0
pp --> R10
01C6 D5A0 RCALL push_gset3
01C7 0158 MOVW R10,R16
01C8 972C SBIW R28,0xC
(0121)
(0122) }
(0123)
(0124) /*********************************************************************************
(0125) 电机旋转位置PID控制器
(0126) *********************************************************************************/
(0127) signed int s_PIDCalc( PID *pp )
(0128) {
(0129) signed long error,d_error;
(0130) signed long temp_comp;
(0131) static unsigned char flag=0;
(0132)
(0133) pp->si_FeedBack += pp->vi_FeedBack; //储存当前速速值
01C9 01C5 MOVW R24,R10
01CA 9608 ADIW R24,0x8
01CB 01F5 MOVW R30,R10
01CC 8042 LDD R4,Z+2
01CD 8053 LDD R5,Z+3
01CE 2466 CLR R6
01CF FC57 SBRC R5,7
01D0 9460 COM R6
01D1 2477 CLR R7
01D2 FC67 SBRC R6,7
01D3 9470 COM R7
01D4 01FC MOVW R30,R24
01D5 8140 LDD R20,Z+0
01D6 8151 LDD R21,Z+1
01D7 8162 LDD R22,Z+2
01D8 8173 LDD R23,Z+3
01D9 0D44 ADD R20,R4
01DA 1D55 ADC R21,R5
01DB 1D66 ADC R22,R6
01DC 1D77 ADC R23,R7
01DD 8340 STD Z+0,R20
01DE 8351 STD Z+1,R21
01DF 8362 STD Z+2,R22
01E0 8373 STD Z+3,R23
(0134) error = pp->si_Ref - pp->si_FeedBack; // 偏差
(0135)
(0136) if( ( error < SS_DEADLINE ) && ( error > -SS_DEADLINE ) ) //设置调节死区
01E1 01F5 MOVW R30,R10
01E2 8420 LDD R2,Z+8
01E3 8431 LDD R3,Z+9
01E4 8442 LDD R4,Z+10
01E5 8453 LDD R5,Z+11
01E6 01F5 MOVW R30,R10
01E7 8064 LDD R6,Z+4
01E8 8075 LDD R7,Z+5
01E9 8086 LDD R8,Z+6
01EA 8097 LDD R9,Z+7
01EB 1862 SUB R6,R2
01EC 0873 SBC R7,R3
01ED 0884 SBC R8,R4
01EE 0895 SBC R9,R5
01EF 01FE MOVW R30,R28
01F0 8260 STD Z+0,R6
01F1 8271 STD Z+1,R7
01F2 8282 STD Z+2,R8
01F3 8293 STD Z+3,R9
01F4 E040 LDI R20,0
01F5 E050 LDI R21,0
01F6 E060 LDI R22,0
01F7 E070 LDI R23,0
01F8 01FE MOVW R30,R28
01F9 8020 LDD R2,Z+0
01FA 8031 LDD R3,Z+1
01FB 8042 LDD R4,Z+2
01FC 8053 LDD R5,Z+3
01FD 1624 CP R2,R20
01FE 0635 CPC R3,R21
01FF 0646 CPC R4,R22
0200 0657 CPC R5,R23
0201 F47C BGE 0x0211
0202 E040 LDI R20,0
0203 E050 LDI R21,0
0204 E060 LDI R22,0
0205 E070 LDI R23,0
0206 01FE MOVW R30,R28
0207 8020 LDD R2,Z+0
0208 8031 LDD R3,Z+1
0209 8042 LDD R4,Z+2
020A 8053 LDD R5,Z+3
020B 1542 CP R20,R2
020C 0553 CPC R21,R3
020D 0564 CPC R22,R4
020E 0575 CPC R23,R5
020F F40C BGE 0x0211
(0137) {
(0138)
(0139) }
0210 C12B RJMP 0x033C
(0140) else //执行位置PID调节
(0141) {
(0142) d_error = error - pp->si_PreError; //计算微分项偏差
(0143)
(0144) pp->si_PreIntegral += error; //存储当前积分偏差
0211 01F5 MOVW R30,R10
0212 8820 LDD R2,Z+16
0213 8831 LDD R3,Z+17
0214 8842 LDD R4,Z+18
0215 8853 LDD R5,Z+19
0216 01FE MOVW R30,R28
0217 8060 LDD R6,Z+0
0218 8071 LDD R7,Z+1
0219 8082 LDD R8,Z+2
021A 8093 LDD R9,Z+3
021B 1862 SUB R6,R2
021C 0873 SBC R7,R3
021D 0884 SBC R8,R4
021E 0895 SBC R9,R5
021F 01FE MOVW R30,R28
0220 8264 STD Z+4,R6
0221 8275 STD Z+5,R7
0222 8286 STD Z+6,R8
0223 8297 STD Z+7,R9
0224 01C5 MOVW R24,R10
0225 9644 ADIW R24,0x14
0226 01FE MOVW R30,R28
0227 8040 LDD R4,Z+0
0228 8051 LDD R5,Z+1
0229 8062 LDD R6,Z+2
022A 8073 LDD R7,Z+3
022B 01FC MOVW R30,R24
022C 8140 LDD R20,Z+0
022D 8151 LDD R21,Z+1
022E 8162 LDD R22,Z+2
022F 8173 LDD R23,Z+3
0230 0D44 ADD R20,R4
0231 1D55 ADC R21,R5
0232 1D66 ADC R22,R6
0233 1D77 ADC R23,R7
0234 8340 STD Z+0,R20
0235 8351 STD Z+1,R21
0236 8362 STD Z+2,R22
0237 8373 STD Z+3,R23
(0145) pp->si_PreError = error; //存储当前偏差
0238 01FE MOVW R30,R28
0239 8020 LDD R2,Z+0
023A 8031 LDD R3,Z+1
023B 8042 LDD R4,Z+2
023C 8053 LDD R5,Z+3
023D 01F5 MOVW R30,R10
023E 8A20 STD Z+16,R2
023F 8A31 STD Z+17,R3
0240 8A42 STD Z+18,R4
0241 8A53 STD Z+19,R5
(0146)
(0147) if(pp->si_PreIntegral > SS_Imax) //积分修正,设定积分上下限，并于正负换向时清零
0242 EA40 LDI R20,0xA0
0243 E856 LDI R21,0x86
0244 E061 LDI R22,1
0245 E070 LDI R23,0
0246 01F5 MOVW R30,R10
0247 8824 LDD R2,Z+20
0248 8835 LDD R3,Z+21
0249 8846 LDD R4,Z+22
024A 8857 LDD R5,Z+23
024B 1542 CP R20,R2
024C 0553 CPC R21,R3
024D 0564 CPC R22,R4
024E 0575 CPC R23,R5
024F F454 BGE 0x025A
(0148) pp->si_PreIntegral = SS_Imax;
0250 EA40 LDI R20,0xA0
0251 E856 LDI R21,0x86
0252 E061 LDI R22,1
0253 E070 LDI R23,0
0254 01F5 MOVW R30,R10
0255 8B44 STD Z+20,R20
0256 8B55 STD Z+21,R21
0257 8B66 STD Z+22,R22
0258 8B77 STD Z+23,R23
0259 C063 RJMP 0x02BD
(0149) else if(pp->si_PreIntegral < SS_Imin)
025A E640 LDI R20,0x60
025B E759 LDI R21,0x79
025C EF6E LDI R22,0xFE
025D EF7F LDI R23,0xFF
025E 01F5 MOVW R30,R10
025F 8824 LDD R2,Z+20
0260 8835 LDD R3,Z+21
0261 8846 LDD R4,Z+22
0262 8857 LDD R5,Z+23
0263 1624 CP R2,R20
0264 0635 CPC R3,R21
0265 0646 CPC R4,R22
0266 0657 CPC R5,R23
0267 F454 BGE 0x0272
(0150) pp->si_PreIntegral = SS_Imin;
0268 E640 LDI R20,0x60
0269 E759 LDI R21,0x79
026A EF6E LDI R22,0xFE
026B EF7F LDI R23,0xFF
026C 01F5 MOVW R30,R10
026D 8B44 STD Z+20,R20
026E 8B55 STD Z+21,R21
026F 8B66 STD Z+22,R22
0270 8B77 STD Z+23,R23
0271 C04B RJMP 0x02BD
(0151) else if( pp->si_PreIntegral>0 && error <0 )
0272 E040 LDI R20,0
0273 E050 LDI R21,0
0274 E060 LDI R22,0
0275 E070 LDI R23,0
0276 01F5 MOVW R30,R10
0277 8824 LDD R2,Z+20
0278 8835 LDD R3,Z+21
0279 8846 LDD R4,Z+22
027A 8857 LDD R5,Z+23
027B 1542 CP R20,R2
027C 0553 CPC R21,R3
027D 0564 CPC R22,R4
027E 0575 CPC R23,R5
027F F4C4 BGE 0x0298
0280 E040 LDI R20,0
0281 E050 LDI R21,0
0282 E060 LDI R22,0
0283 E070 LDI R23,0
0284 01FE MOVW R30,R28
0285 8020 LDD R2,Z+0
0286 8031 LDD R3,Z+1
0287 8042 LDD R4,Z+2
0288 8053 LDD R5,Z+3
0289 1624 CP R2,R20
028A 0635 CPC R3,R21
028B 0646 CPC R4,R22
028C 0657 CPC R5,R23
028D F454 BGE 0x0298
(0152) pp->si_PreIntegral=0;
028E E040 LDI R20,0
028F E050 LDI R21,0
0290 E060 LDI R22,0
0291 E070 LDI R23,0
0292 01F5 MOVW R30,R10
0293 8B44 STD Z+20,R20
0294 8B55 STD Z+21,R21
0295 8B66 STD Z+22,R22
0296 8B77 STD Z+23,R23
0297 C025 RJMP 0x02BD
(0153) else if( pp->si_PreIntegral<0 && error >0 )
0298 E040 LDI R20,0
0299 E050 LDI R21,0
029A E060 LDI R22,0
029B E070 LDI R23,0
029C 01F5 MOVW R30,R10
029D 8824 LDD R2,Z+20
029E 8835 LDD R3,Z+21
029F 8846 LDD R4,Z+22
02A0 8857 LDD R5,Z+23
02A1 1624 CP R2,R20
02A2 0635 CPC R3,R21
02A3 0646 CPC R4,R22
02A4 0657 CPC R5,R23
02A5 F4BC BGE 0x02BD
02A6 E040 LDI R20,0
02A7 E050 LDI R21,0
02A8 E060 LDI R22,0
02A9 E070 LDI R23,0
02AA 01FE MOVW R30,R28
02AB 8020 LDD R2,Z+0
02AC 8031 LDD R3,Z+1
02AD 8042 LDD R4,Z+2
02AE 8053 LDD R5,Z+3
02AF 1542 CP R20,R2
02B0 0553 CPC R21,R3
02B1 0564 CPC R22,R4
02B2 0575 CPC R23,R5
02B3 F44C BGE 0x02BD
(0154) pp->si_PreIntegral=0;
02B4 E040 LDI R20,0
02B5 E050 LDI R21,0
02B6 E060 LDI R22,0
02B7 E070 LDI R23,0
02B8 01F5 MOVW R30,R10
02B9 8B44 STD Z+20,R20
02BA 8B55 STD Z+21,R21
02BB 8B66 STD Z+22,R22
02BC 8B77 STD Z+23,R23
(0155)
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -