📄 8.lst
字号:
__text_start:
__start:
0023 E5CF LDI R28,0x5F
0024 E0D4 LDI R29,4
0025 BFCD OUT 0x3D,R28
0026 BFDE OUT 0x3E,R29
0027 51C0 SUBI R28,0x10
0028 40D0 SBCI R29,0
0029 EA0A LDI R16,0xAA
002A 8308 STD Y+0,R16
002B 2400 CLR R0
002C E7EF LDI R30,0x7F
002D E0F0 LDI R31,0
002E E010 LDI R17,0
002F 3AEB CPI R30,0xAB
0030 07F1 CPC R31,R17
0031 F011 BEQ 0x0034
0032 9201 ST R0,Z+
0033 CFFB RJMP 0x002F
0034 8300 STD Z+0,R16
0035 E2E6 LDI R30,0x26
0036 E0F0 LDI R31,0
0037 E6A0 LDI R26,0x60
0038 E0B0 LDI R27,0
0039 E010 LDI R17,0
003A 34E5 CPI R30,0x45
003B 07F1 CPC R31,R17
003C F021 BEQ 0x0041
003D 95C8 LPM
003E 9631 ADIW R30,1
003F 920D ST R0,X+
0040 CFF9 RJMP 0x003A
0041 D6BF RCALL _main
_exit:
0042 CFFF RJMP _exit
_L_PIDInit:
0043 D720 RCALL push_gset2
FILE: E:\李斐机器人\xunxianjiadongzuo\8\L_PID.c
(0001) /*********************************************************************************
(0002) 左电机速度和位置PID控制器参数
(0003) *********************************************************************************/
(0004) #define L_V_Ka 690 //电机旋转速度PID控制器参数
(0005) #define L_V_Kb 90
(0006) #define L_V_Kc 30
(0007)
(0008) #define L_S_Kp 30 //电机旋转位置PID控制器参数
(0009) #define L_S_Ki 10
(0010) #define L_S_Kd 100
(0011)
(0012) /*********************************************************************************/
(0013)
(0014) #define VV_MAX 1023<<10 //1024<<10 使控制信号扩大,以使过渡平顺
(0015) #define VV_MIN -1023<<10
(0016)
(0017) #define SS_MAX 400<<10 //1024<<10 使控制信号扩大,以使过渡平顺
(0018) #define SS_MIN -(400<<10)
(0019)
(0020) #define VV_DEADLINE 0X08 //速度PID，设置死区范围
(0021)
(0022) #define SS_DEADLINE 0 //位置PID，设置死区范围
(0023)
(0024) #define SS_Imax 100000 //位置PID，积分上限
(0025) #define SS_Imin -100000 //位置PID，积分下限
(0026)
(0027) #define SS_ADJUST 500000 //位置PID调节使能域度
(0028)
(0029) #define SS_ACCEL 200 //位置PID，加速度
(0030) #define SS_M_SPEED 1000 //位置PID，最大速度
(0031)
(0032) /*********************************************************************************
(0033) PID 结构体，包含速度和位置数据
(0034) *********************************************************************************/
(0035) typedef struct PID //定义数法核心数据
(0036) {
(0037) signed int vi_Ref; //速度PID，速度设定值
(0038) signed int vi_FeedBack; //速度PID，速度反馈值
(0039)
(0040) signed long si_Ref; //位置PID，位移设定值
(0041) signed long si_FeedBack; //位置PID，位移反馈值,当前位置
(0042)
(0043) signed int vi_PreError; //速度PID，前一次，速度误差,,vi_Ref - vi_FeedBack
(0044) signed int vi_PreDerror; //速度PID，前一次，速度误差之差，d_error-PreDerror;
(0045)
(0046) signed long si_PreError; //位置PID，前一次，位移误差,ui_Ref - FeedBack
(0047) signed long si_PreIntegral; //位置PID，前一次，位移积分项，ui_PreIntegral+ui
(0048)
(0049) signed int v_Ka; //速度PID，Ka = Kp
(0050) signed int v_Kb; //速度PID，Kb = Kp * ( T / Ti )
(0051) signed int v_Kc; //速度PID，
(0052)
(0053) signed int s_Kp; //位置PID，比例系数
(0054) signed int s_Ki; //位置PID，积分系数
(0055) signed int s_Kd; //位置PID，微分系数
(0056)
(0057) signed int s_Accel; //位置PID，加速度
(0058) signed int M_Speed; //位置PID，最大速度
(0059)
(0060) signed long vl_PreU; //电机控制输出值
(0061)
(0062) }PID;
(0063)
(0064) PID L_sPID; // 左右电机PID控制器变量
(0065)
(0066) void L_PIDInit ()
(0067) {
(0068) L_sPID.vi_Ref = 0 ; //速度设定值
(0069) L_sPID.vi_FeedBack = 0 ; //速度反馈值
0044 2422 CLR R2
0045 2433 CLR R3
0046 92300080 STS _L_sPID+1,R3
0048 9220007F STS _L_sPID,R2
004A 92300082 STS _L_sPID+3,R3
004C 92200081 STS _L_sPID+2,R2
(0070)
(0071) L_sPID.si_Ref = 0 ; //位移设定值
004E E040 LDI R20,0
004F E050 LDI R21,0
0050 E060 LDI R22,0
0051 E070 LDI R23,0
0052 93500084 STS 0x84,R21
0054 93400083 STS 0x83,R20
0056 93700086 STS 0x86,R23
0058 93600085 STS 0x85,R22
(0072) L_sPID.si_FeedBack = 0 ; //位移反馈值,当前位置
005A E040 LDI R20,0
005B E050 LDI R21,0
005C E060 LDI R22,0
005D E070 LDI R23,0
005E 93500088 STS 0x88,R21
0060 93400087 STS 0x87,R20
0062 9370008A STS 0x8A,R23
0064 93600089 STS 0x89,R22
(0073)
(0074) L_sPID.vi_PreError = 0 ; //前一次，速度误差,,vi_Ref - vi_FeedBack
0066 9230008C STS 0x8C,R3
0068 9220008B STS 0x8B,R2
(0075) L_sPID.vi_PreDerror = 0 ; //前一次，速度误差之差，d_error-PreDerror;
006A 9230008E STS 0x8E,R3
006C 9220008D STS 0x8D,R2
(0076)
(0077) L_sPID.si_PreError = 0 ; //前一次，位移误差,ui_Ref - FeedBack
006E E040 LDI R20,0
006F E050 LDI R21,0
0070 E060 LDI R22,0
0071 E070 LDI R23,0
0072 93500090 STS 0x90,R21
0074 9340008F STS 0x8F,R20
0076 93700092 STS 0x92,R23
0078 93600091 STS 0x91,R22
(0078) L_sPID.si_PreIntegral = 0 ; //前一次，位移积分项，ui_PreIntegral+ui
007A E040 LDI R20,0
007B E050 LDI R21,0
007C E060 LDI R22,0
007D E070 LDI R23,0
007E 93500094 STS 0x94,R21
0080 93400093 STS 0x93,R20
0082 93700096 STS 0x96,R23
0084 93600095 STS 0x95,R22
(0079)
(0080) L_sPID.v_Ka = L_V_Ka;
0086 EB82 LDI R24,0xB2
0087 E092 LDI R25,2
0088 93900098 STS 0x98,R25
008A 93800097 STS 0x97,R24
(0081) L_sPID.v_Kb = L_V_Kb;
008C E58A LDI R24,0x5A
008D E090 LDI R25,0
008E 9390009A STS 0x9A,R25
0090 93800099 STS 0x99,R24
(0082) L_sPID.v_Kc = L_V_Kc;
0092 E18E LDI R24,0x1E
0093 9390009C STS 0x9C,R25
0095 9380009B STS 0x9B,R24
(0083)
(0084) L_sPID.s_Kp = L_S_Kp;
0097 9390009E STS 0x9E,R25
0099 9380009D STS 0x9D,R24
(0085) L_sPID.s_Ki = L_S_Ki;
009B E08A LDI R24,0xA
009C 939000A0 STS 0xA0,R25
009E 9380009F STS 0x9F,R24
(0086) L_sPID.s_Kd = L_S_Kd;
00A0 E684 LDI R24,0x64
00A1 939000A2 STS 0xA2,R25
00A3 938000A1 STS 0xA1,R24
(0087)
(0088) L_sPID.s_Accel = SS_ACCEL ; //加速度
00A5 EC88 LDI R24,0xC8
00A6 939000A4 STS 0xA4,R25
00A8 938000A3 STS 0xA3,R24
(0089) L_sPID.M_Speed = SS_M_SPEED ; //最大速度
00AA EE88 LDI R24,0xE8
00AB E093 LDI R25,3
00AC 939000A6 STS 0xA6,R25
00AE 938000A5 STS 0xA5,R24
(0090)
(0091) L_sPID.vl_PreU = 0 ; //电机控制输出值
00B0 E040 LDI R20,0
00B1 E050 LDI R21,0
00B2 E060 LDI R22,0
00B3 E070 LDI R23,0
00B4 935000A8 STS 0xA8,R21
00B6 934000A7 STS 0xA7,R20
00B8 937000AA STS 0xAA,R23
00BA 936000A9 STS 0xA9,R22
(0092) }
00BC D68A RCALL pop_gset2
00BD 9508 RET
_v_PIDCalc:
dd_error --> Y+8
d_error --> Y+4
error --> Y+0
pp --> R10
00BE D6A8 RCALL push_gset3
00BF 0158 MOVW R10,R16
00C0 972C SBIW R28,0xC
(0093)
(0094) /*********************************************************************************
(0095) 电机速度PID控制器
(0096) *********************************************************************************/
(0097) signed int v_PIDCalc( PID *pp )
(0098) {
(0099) signed long error,d_error,dd_error;
(0100)
(0101) error = (signed int)(pp->vi_Ref - pp->vi_FeedBack); // 偏差计算
(0102) d_error = error - pp->vi_PreError;
(0103) dd_error = d_error - pp->vi_PreDerror;
(0104)
(0105) pp->vi_PreError = error; //存储当前偏差
00C1 01F5 MOVW R30,R10
00C2 8022 LDD R2,Z+2
00C3 8033 LDD R3,Z+3
00C4 01F5 MOVW R30,R10
00C5 8040 LDD R4,Z+0
00C6 8051 LDD R5,Z+1
00C7 1842 SUB R4,R2
00C8 0853 SBC R5,R3
00C9 0112 MOVW R2,R4
00CA 2444 CLR R4
00CB FC37 SBRC R3,7
00CC 9440 COM R4
00CD 2455 CLR R5
00CE FC47 SBRC R4,7
00CF 9450 COM R5
00D0 01FE MOVW R30,R28
00D1 8220 STD Z+0,R2
00D2 8231 STD Z+1,R3
00D3 8242 STD Z+2,R4
00D4 8253 STD Z+3,R5
00D5 01C5 MOVW R24,R10
00D6 960C ADIW R24,0xC
00D7 01FC MOVW R30,R24
00D8 8040 LDD R4,Z+0
00D9 8051 LDD R5,Z+1
00DA 2466 CLR R6
00DB FC57 SBRC R5,7
00DC 9460 COM R6
00DD 2477 CLR R7
00DE FC67 SBRC R6,7
00DF 9470 COM R7
00E0 01FE MOVW R30,R28
00E1 8140 LDD R20,Z+0
00E2 8151 LDD R21,Z+1
00E3 8162 LDD R22,Z+2
00E4 8173 LDD R23,Z+3
00E5 1944 SUB R20,R4
00E6 0955 SBC R21,R5
00E7 0966 SBC R22,R6
00E8 0977 SBC R23,R7
00E9 01FE MOVW R30,R28
00EA 8344 STD Z+4,R20
00EB 8355 STD Z+5,R21
00EC 8366 STD Z+6,R22
00ED 8377 STD Z+7,R23
00EE 01F5 MOVW R30,R10
00EF 8446 LDD R4,Z+14
00F0 8457 LDD R5,Z+15
00F1 2466 CLR R6
00F2 FC57 SBRC R5,7
00F3 9460 COM R6
00F4 2477 CLR R7
00F5 FC67 SBRC R6,7
00F6 9470 COM R7
00F7 01FE MOVW R30,R28
00F8 8144 LDD R20,Z+4
00F9 8155 LDD R21,Z+5
00FA 8166 LDD R22,Z+6
00FB 8177 LDD R23,Z+7
00FC 1944 SUB R20,R4
00FD 0955 SBC R21,R5
00FE 0966 SBC R22,R6
00FF 0977 SBC R23,R7
0100 01FE MOVW R30,R28
0101 8740 STD Z+8,R20
0102 8751 STD Z+9,R21
0103 8762 STD Z+10,R22
0104 8773 STD Z+11,R23
0105 01FE MOVW R30,R28
0106 8040 LDD R4,Z+0
0107 8051 LDD R5,Z+1
0108 01FC MOVW R30,R24
0109 8251 STD Z+1,R5
010A 8240 STD Z+0,R4
(0106) pp->vi_PreDerror = d_error;
010B 01FE MOVW R30,R28
010C 8024 LDD R2,Z+4
010D 8035 LDD R3,Z+5
010E 01F5 MOVW R30,R10
010F 8637 STD Z+15,R3
0110 8626 STD Z+14,R2
(0107)
(0108) if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) ) //设置调节死区
0111 E048 LDI R20,0x8
0112 E050 LDI R21,0
0113 E060 LDI R22,0
0114 E070 LDI R23,0
0115 01FE MOVW R30,R28
0116 8020 LDD R2,Z+0
0117 8031 LDD R3,Z+1
0118 8042 LDD R4,Z+2
0119 8053 LDD R5,Z+3
011A 1624 CP R2,R20
011B 0635 CPC R3,R21
011C 0646 CPC R4,R22
011D 0657 CPC R5,R23
011E F47C BGE 0x012E
011F EF48 LDI R20,0xF8
0120 EF5F LDI R21,0xFF
0121 EF6F LDI R22,0xFF
0122 EF7F LDI R23,0xFF
0123 01FE MOVW R30,R28
0124 8020 LDD R2,Z+0
0125 8031 LDD R3,Z+1
0126 8042 LDD R4,Z+2
0127 8053 LDD R5,Z+3
0128 1542 CP R20,R2
0129 0553 CPC R21,R3
012A 0564 CPC R22,R4
012B 0575 CPC R23,R5
012C F40C BGE 0x012E
(0109) {
(0110) }
012D C05B RJMP 0x0189
(0111) else //速度PID计算
(0112) pp->vl_PreU += ( ( pp -> v_Ka * d_error + pp -> v_Kb * error ) + (pp->v_Kc*dd_error));
012E 01C5 MOVW R24,R10
012F 9688 ADIW R24,0x28
0130 011C MOVW R2,R24
0131 01FE MOVW R30,R28
0132 8040 LDD R4,Z+0
0133 8051 LDD R5,Z+1
0134 8062 LDD R6,Z+2
0135 8073 LDD R7,Z+3
0136 01F5 MOVW R30,R10
0137 8D42 LDD R20,Z+26
0138 8D53 LDD R21,Z+27
0139 2766 CLR R22
013A FD57 SBRC R21,7
013B 9560 COM R22
013C 2777 CLR R23
013D FD67 SBRC R22,7
013E 9570 COM R23
013F 927A ST R7,-Y
0140 926A ST R6,-Y
0141 925A ST R5,-Y
0142 924A ST R4,-Y
0143 018A MOVW R16,R20
0144 019B MOVW R18,R22
0145 D5D5 RCALL empy32s
0146 0128 MOVW R4,R16
0147 0139 MOVW R6,R18
0148 01FE MOVW R30,R28
0149 8144 LDD R20,Z+4
014A 8155 LDD R21,Z+5
014B 8166 LDD R22,Z+6
014C 8177 LDD R23,Z+7
014D 01F5 MOVW R30,R10
014E 8D80 LDD R24,Z+24
014F 8D91 LDD R25,Z+25
0150 27AA CLR R26
0151 FD97 SBRC R25,7
0152 95A0 COM R26
0153 27BB CLR R27
0154 FDA7 SBRC R26,7
0155 95B0 COM R27
0156 937A ST R23,-Y
0157 936A ST R22,-Y
0158 935A ST R21,-Y
0159 934A ST R20,-Y
015A 018C MOVW R16,R24
015B 019D MOVW R18,R26
015C D5BE RCALL empy32s
015D 01A8 MOVW R20,R16
015E 01B9 MOVW R22,R18
015F 0D44 ADD R20,R4
0160 1D55 ADC R21,R5
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -