passive_encoder.lst
来自「全场地位系统：小车静止或移动过程中码盘进行全场定位,用的是avr单片机」· LST 代码 · 共 1,758 行 · 第 1/5 页
LST
1,758 行
(0121) (float)g_lCounterR * (float)K_R
(0122) - (float)g_lCounterL * (float)K_L
(0123) ) * (1.0 / (float)D_BTW_WHEEL);
(0124) }
(0125)
(0126) /***********************************************************
(0127) * 函数说明：获取当前相对角度函数 *
(0128) * 输入： 左编码器微分量，右编码器微分量 *
(0129) * 输出： 角度值（弧度） *
(0130) * 调用函数：无 *
(0131) ***********************************************************/
(0132) float Get_Relative_Angle(INT32 lDCL,INT32 lDCR)
(0133) {
(0134) if (lDCL == lDCR)
37D 8428 LDD R2,Y+8
37E 8439 LDD R3,Y+9
37F 844A LDD R4,Y+10
380 845B LDD R5,Y+11
381 806C LDD R6,Y+4
382 807D LDD R7,Y+5
383 808E LDD R8,Y+6
384 809F LDD R9,Y+7
385 1462 CP R6,R2
386 0473 CPC R7,R3
387 0484 CPC R8,R4
388 0495 CPC R9,R5
389 F421 BNE 0x038E
(0135) {
(0136) return 0.0;
38A E404 LDI R16,0x44
38B E010 LDI R17,0
38C D316 RCALL lpm32
38D C02F RJMP 0x03BD
(0137) }
(0138)
(0139) return
(0140) (
38E E500 LDI R16,0x50
38F E010 LDI R17,0
390 D312 RCALL lpm32
391 0118 MOVW R2,R16
392 0129 MOVW R4,R18
393 E40C LDI R16,0x4C
394 E010 LDI R17,0
395 D30D RCALL lpm32
396 0138 MOVW R6,R16
397 0149 MOVW R8,R18
398 8508 LDD R16,Y+8
399 8519 LDD R17,Y+9
39A 852A LDD R18,Y+10
39B 853B LDD R19,Y+11
39C D3FC RCALL long2fp
39D 933A ST R19,-Y
39E 932A ST R18,-Y
39F 931A ST R17,-Y
3A0 930A ST R16,-Y
3A1 0183 MOVW R16,R6
3A2 0194 MOVW R18,R8
3A3 D5E2 RCALL fpmule2
3A4 0138 MOVW R6,R16
3A5 0149 MOVW R8,R18
3A6 E408 LDI R16,0x48
3A7 E010 LDI R17,0
3A8 D2FA RCALL lpm32
3A9 01A8 MOVW R20,R16
3AA 01B9 MOVW R22,R18
3AB 810C LDD R16,Y+4
3AC 811D LDD R17,Y+5
3AD 812E LDD R18,Y+6
3AE 813F LDD R19,Y+7
3AF D3E9 RCALL long2fp
3B0 933A ST R19,-Y
3B1 932A ST R18,-Y
3B2 931A ST R17,-Y
3B3 930A ST R16,-Y
3B4 018A MOVW R16,R20
3B5 019B MOVW R18,R22
3B6 D5D9 RCALL fpmule2x
3B7 0183 MOVW R16,R6
3B8 0194 MOVW R18,R8
3B9 D39B RCALL fpsub2x
3BA 0181 MOVW R16,R2
3BB 0192 MOVW R18,R4
3BC D5C9 RCALL fpmule2
3BD D2C9 RCALL pop_xgsetF000
3BE 9624 ADIW R28,4
3BF 9508 RET
_PROC_Difference_Locate:
fR1 --> Y,+20
fR0 --> Y,+16
nDeltaCounterL0 --> R10
fTempAngle0 --> Y,+16
fAbsoluteAngle0 --> Y,+12
fDeltaAngle0 --> Y,+8
fR0 --> Y,+8
nDeltaCounterL0 --> R10
Reg8 --> Y,+4
3C0 D2CB RCALL push_xgsetF00C
3C1 9768 SBIW R28,0x18
(0141) (float)lDCR * (float)K_R
(0142) - (float)lDCL * (float)K_L
(0143) ) * (1.0 / (float)D_BTW_WHEEL);
(0144) }
(0145)
(0146)
(0147)
(0148) /***********************************************************
(0149) * 函数说明：差分定位计算函数 *
(0150) * 输入： 无 *
(0151) * 输出： FALSE *
(0152) * 调用函数：无 *
(0153) ***********************************************************/
(0154) BOOL PROC_Difference_Locate(void)
(0155) {
(0156) static INT32 s_lLastCounterL = 0;
(0157) static INT32 s_lLastCounterR = 0;
(0158)
(0159) if ((g_lCounterLImage - s_lLastCounterL)
3C2 9040 0149 LDS R4,RD_UseDLocate_LIB.c:s_lLastCounterR+2
3C4 9050 014A LDS R5,RD_UseDLocate_LIB.c:s_lLastCounterR+3
3C6 9020 0147 LDS R2,RD_UseDLocate_LIB.c:s_lLastCounterR
3C8 9030 0148 LDS R3,RD_UseDLocate_LIB.c:s_lLastCounterR+1
3CA 9080 0112 LDS R8,g_lCounterRImage+2
3CC 9090 0113 LDS R9,g_lCounterRImage+3
3CE 9060 0110 LDS R6,g_lCounterRImage
3D0 9070 0111 LDS R7,g_lCounterRImage+1
3D2 1862 SUB R6,R2
3D3 0873 SBC R7,R3
3D4 0884 SBC R8,R4
3D5 0895 SBC R9,R5
3D6 9040 0145 LDS R4,RD_UseDLocate_LIB.c:s_lLastCounterL+2
3D8 9050 0146 LDS R5,RD_UseDLocate_LIB.c:s_lLastCounterL+3
3DA 9020 0143 LDS R2,RD_UseDLocate_LIB.c:s_lLastCounterL
3DC 9030 0144 LDS R3,RD_UseDLocate_LIB.c:s_lLastCounterL+1
3DE 9160 010E LDS R22,g_lCounterLImage+2
3E0 9170 010F LDS R23,g_lCounterLImage+3
3E2 9140 010C LDS R20,g_lCounterLImage
3E4 9150 010D LDS R21,g_lCounterLImage+1
3E6 1942 SUB R20,R2
3E7 0953 SBC R21,R3
3E8 0964 SBC R22,R4
3E9 0975 SBC R23,R5
3EA 1546 CP R20,R6
3EB 0557 CPC R21,R7
3EC 0568 CPC R22,R8
3ED 0579 CPC R23,R9
3EE F009 BEQ 0x03F0
3EF C067 RJMP 0x0457
(0160) == (g_lCounterRImage - s_lLastCounterR))
(0161) {
(0162) INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
3F0 90A0 010C LDS R10,g_lCounterLImage
3F2 90B0 010D LDS R11,g_lCounterLImage+1
3F4 18A2 SUB R10,R2
3F5 08B3 SBC R11,R3
(0163) float fR = ((float)nDeltaCounterL * (float)K_L);
3F6 E408 LDI R16,0x48
3F7 E010 LDI R17,0
3F8 D2AA RCALL lpm32
3F9 0118 MOVW R2,R16
3FA 0129 MOVW R4,R18
3FB 0185 MOVW R16,R10
3FC D391 RCALL int2fp
3FD 933A ST R19,-Y
3FE 932A ST R18,-Y
3FF 931A ST R17,-Y
400 930A ST R16,-Y
401 0181 MOVW R16,R2
402 0192 MOVW R18,R4
403 D582 RCALL fpmule2
404 8708 STD Y+8,R16
405 8719 STD Y+9,R17
406 872A STD Y+10,R18
407 873B STD Y+11,R19
(0164)
(0165) g_fX += fR * cos(g_fLastAngle);
408 9120 0139 LDS R18,g_fLastAngle+2
40A 9130 013A LDS R19,g_fLastAngle+3
40C 9100 0137 LDS R16,g_fLastAngle
40E 9110 0138 LDS R17,g_fLastAngle+1
410 D5AC RCALL _cosf
411 0118 MOVW R2,R16
412 0129 MOVW R4,R18
413 9080 013D LDS R8,g_fX+2
415 9090 013E LDS R9,g_fX+3
417 9060 013B LDS R6,g_fX
419 9070 013C LDS R7,g_fX+1
41B 8508 LDD R16,Y+8
41C 8519 LDD R17,Y+9
41D 852A LDD R18,Y+10
41E 853B LDD R19,Y+11
41F 925A ST R5,-Y
420 924A ST R4,-Y
421 923A ST R3,-Y
422 922A ST R2,-Y
423 D56C RCALL fpmule2x
424 0183 MOVW R16,R6
425 0194 MOVW R18,R8
426 D304 RCALL fpadd2
427 9310 013C STS g_fX+1,R17
429 9300 013B STS g_fX,R16
42B 9330 013E STS g_fX+3,R19
42D 9320 013D STS g_fX+2,R18
(0166) g_fY += fR * sin(g_fLastAngle);
42F 9120 0139 LDS R18,g_fLastAngle+2
431 9130 013A LDS R19,g_fLastAngle+3
433 9100 0137 LDS R16,g_fLastAngle
435 9110 0138 LDS R17,g_fLastAngle+1
437 D6F3 RCALL _sinf
438 0118 MOVW R2,R16
439 0129 MOVW R4,R18
43A 9080 0141 LDS R8,g_fY+2
43C 9090 0142 LDS R9,g_fY+3
43E 9060 013F LDS R6,g_fY
440 9070 0140 LDS R7,g_fY+1
442 8508 LDD R16,Y+8
443 8519 LDD R17,Y+9
444 852A LDD R18,Y+10
445 853B LDD R19,Y+11
446 925A ST R5,-Y
447 924A ST R4,-Y
448 923A ST R3,-Y
449 922A ST R2,-Y
44A D545 RCALL fpmule2x
44B 0183 MOVW R16,R6
44C 0194 MOVW R18,R8
44D D2DD RCALL fpadd2
44E 9310 0140 STS g_fY+1,R17
450 9300 013F STS g_fY,R16
452 9330 0142 STS g_fY+3,R19
454 9320 0141 STS g_fY+2,R18
(0167) }
456 C0DE RJMP 0x0535
(0168) else
(0169) {
(0170) float fDeltaAngle,fAbsoluteAngle;
(0171) //计算角度微元
(0172) {
(0173) float fTempAngle = Get_Relative_Angle
457 9040 0112 LDS R4,g_lCounterRImage+2
459 9050 0113 LDS R5,g_lCounterRImage+3
45B 9020 0110 LDS R2,g_lCounterRImage
45D 9030 0111 LDS R3,g_lCounterRImage+1
45F 8228 STD Y+0,R2
460 8239 STD Y+1,R3
461 824A STD Y+2,R4
462 825B STD Y+3,R5
463 9120 010E LDS R18,g_lCounterLImage+2
465 9130 010F LDS R19,g_lCounterLImage+3
467 9100 010C LDS R16,g_lCounterLImage
469 9110 010D LDS R17,g_lCounterLImage+1
46B DF0F RCALL _Get_Relative_Angle
46C 8B08 STD Y+16,R16
46D 8B19 STD Y+17,R17
46E 8B2A STD Y+18,R18
46F 8B3B STD Y+19,R19
(0174) (
(0175) g_lCounterLImage,
(0176) g_lCounterRImage
(0177) );
(0178) fDeltaAngle = (fTempAngle - g_fLastAngle);
470 8908 LDD R16,Y+16
471 8919 LDD R17,Y+17
472 892A LDD R18,Y+18
473 893B LDD R19,Y+19
474 E387 LDI R24,0x37
475 E091 LDI R25,1
476 939A ST R25,-Y
477 938A ST R24,-Y
478 D2C9 RCALL fpsub1
479 8708 STD Y+8,R16
47A 8719 STD Y+9,R17
47B 872A STD Y+10,R18
47C 873B STD Y+11,R19
(0179) fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
47D E400 LDI R16,0x40
47E E010 LDI R17,0
47F D223 RCALL lpm32
480 01CE MOVW R24,R28
481 9608 ADIW R24,0x8
482 939A ST R25,-Y
483 938A ST R24,-Y
484 D4F8 RCALL fpmule1
485 830C STD Y+4,R16
486 831D STD Y+5,R17
487 832E STD Y+6,R18
488 833F STD Y+7,R19
489 810C LDD R16,Y+4
48A 811D LDD R17,Y+5
48B 812E LDD R18,Y+6
48C 813F LDD R19,Y+7
48D E387 LDI R24,0x37
48E E091 LDI R25,1
48F 939A ST R25,-Y
490 938A ST R24,-Y
491 D284 RCALL fpadd1
492 870C STD Y+12,R16
493 871D STD Y+13,R17
494 872E STD Y+14,R18
495 873F STD Y+15,R19
(0180)
(0181) g_fLastAngle = fTempAngle;
496 8828 LDD R2,Y+16
497 8839 LDD R3,Y+17
498 884A LDD R4,Y+18
499 885B LDD R5,Y+19
49A 9230 0138 STS g_fLastAngle+1,R3
49C 9220 0137 STS g_fLastAngle,R2
49E 9250 013A STS g_fLastAngle+3,R5
4A0 9240 0139 STS g_fLastAngle+2,R4
(0182) }
(0183) //计算位置微元
(0184) {
(0185) INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
4A2 9020 0143 LDS R2,RD_UseDLocate_LIB.c:s_lLastCounterL
4A4 9030 0144 LDS R3,RD_UseDLocate_LIB.c:s_lLastCounterL+1
4A6 90A0 010C LDS R10,g_lCounterLImage
4A8 90B0 010D LDS R11,g_lCounterLImage+1
4AA 18A2 SUB R10,R2
4AB 08B3 SBC R11,R3
(0186) float fR = (((float)nDeltaCounterL * (float)K_L) * (1.0 / fDeltaAngle)
4AC E408 LDI R16,0x48
4AD E010 LDI R17,0
4AE D1F4 RCALL lpm32
4AF 0118 MOVW R2,R16
4B0 0129 MOVW R4,R18
4B1 0185 MOVW R16,R10
4B2 D2DB RCALL int2fp
4B3 933A ST R19,-Y
4B4 932A ST R18,-Y
4B5 931A ST R17,-Y
4B6 930A ST R16,-Y
4B7 0181 MOVW R16,R2
4B8 0192 MOVW R18,R4
4B9 D4CC RCALL fpmule2
4BA 0118 MOVW R2,R16
4BB 0129 MOVW R4,R18
4BC E30C LDI R16,0x3C
4BD E010 LDI R17,0
4BE D1E4 RCALL lpm32
4BF 01CE MOVW R24,R28
4C0 9608 ADIW R24,0x8
4C1 939A ST R25,-Y
4C2 938A ST R24,-Y
4C3 D2F9 RCALL fpdiv1x
4C4 0181 MOVW R16,R2
4C5 0192 MOVW R18,R4
4C6 D4BF RCALL fpmule2
4C7 0118 MOVW R2,R16
⌨️ 快捷键说明
复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?