root / demos / john / projects / libdragonfly / odometry.lst @ 1693
History | View | Annotate | Download (29.5 KB)
1 | 1693 | emullini | 1 .file "odometry.c" |
---|---|---|---|
2 | 2 .arch atmega128 |
||
3 | 3 __SREG__ = 0x3f |
||
4 | 4 __SP_H__ = 0x3e |
||
5 | 5 __SP_L__ = 0x3d |
||
6 | 6 __tmp_reg__ = 0 |
||
7 | 7 __zero_reg__ = 1 |
||
8 | 8 .global __do_copy_data |
||
9 | 9 .global __do_clear_bss |
||
10 | 17 .Ltext0: |
||
11 | 18 .global odometry_dx |
||
12 | 20 odometry_dx: |
||
13 | 21 .LFB3: |
||
14 | 22 .LM1: |
||
15 | 23 /* prologue: frame size=0 */ |
||
16 | 24 /* prologue end (size=0) */ |
||
17 | 25 0000 6091 0000 lds r22,diff_x |
||
18 | 26 0004 7091 0000 lds r23,(diff_x)+1 |
||
19 | 27 0008 8091 0000 lds r24,(diff_x)+2 |
||
20 | 28 000c 9091 0000 lds r25,(diff_x)+3 |
||
21 | 29 /* epilogue: frame size=0 */ |
||
22 | 30 0010 0895 ret |
||
23 | 31 /* epilogue end (size=1) */ |
||
24 | 32 /* function odometry_dx size 9 (8) */ |
||
25 | 33 .LFE3: |
||
26 | 35 .global odometry_dy |
||
27 | 37 odometry_dy: |
||
28 | 38 .LFB4: |
||
29 | 39 .LM2: |
||
30 | 40 /* prologue: frame size=0 */ |
||
31 | 41 /* prologue end (size=0) */ |
||
32 | 42 0012 6091 0000 lds r22,diff_y |
||
33 | 43 0016 7091 0000 lds r23,(diff_y)+1 |
||
34 | 44 001a 8091 0000 lds r24,(diff_y)+2 |
||
35 | 45 001e 9091 0000 lds r25,(diff_y)+3 |
||
36 | 46 /* epilogue: frame size=0 */ |
||
37 | 47 0022 0895 ret |
||
38 | 48 /* epilogue end (size=1) */ |
||
39 | 49 /* function odometry_dy size 9 (8) */ |
||
40 | 50 .LFE4: |
||
41 | 52 .global odometry_angle |
||
42 | 54 odometry_angle: |
||
43 | 55 .LFB5: |
||
44 | 56 .LM3: |
||
45 | 57 /* prologue: frame size=0 */ |
||
46 | 58 /* prologue end (size=0) */ |
||
47 | 59 0024 6091 0000 lds r22,angle |
||
48 | 60 0028 7091 0000 lds r23,(angle)+1 |
||
49 | 61 002c 8091 0000 lds r24,(angle)+2 |
||
50 | 62 0030 9091 0000 lds r25,(angle)+3 |
||
51 | 63 /* epilogue: frame size=0 */ |
||
52 | 64 0034 0895 ret |
||
53 | 65 /* epilogue end (size=1) */ |
||
54 | 66 /* function odometry_angle size 9 (8) */ |
||
55 | 67 .LFE5: |
||
56 | 69 .global odometry_velocity |
||
57 | 71 odometry_velocity: |
||
58 | 72 .LFB6: |
||
59 | 73 .LM4: |
||
60 | 74 /* prologue: frame size=0 */ |
||
61 | 75 /* prologue end (size=0) */ |
||
62 | 76 0036 6091 0000 lds r22,velocity |
||
63 | 77 003a 7091 0000 lds r23,(velocity)+1 |
||
64 | 78 003e 8091 0000 lds r24,(velocity)+2 |
||
65 | 79 0042 9091 0000 lds r25,(velocity)+3 |
||
66 | 80 /* epilogue: frame size=0 */ |
||
67 | 81 0046 0895 ret |
||
68 | 82 /* epilogue end (size=1) */ |
||
69 | 83 /* function odometry_velocity size 9 (8) */ |
||
70 | 84 .LFE6: |
||
71 | 86 .global lround |
||
72 | 88 lround: |
||
73 | 89 .LFB11: |
||
74 | 90 .LM5: |
||
75 | 91 /* prologue: frame size=0 */ |
||
76 | 92 0048 AF92 push r10 |
||
77 | 93 004a BF92 push r11 |
||
78 | 94 004c CF92 push r12 |
||
79 | 95 004e DF92 push r13 |
||
80 | 96 0050 EF92 push r14 |
||
81 | 97 0052 FF92 push r15 |
||
82 | 98 0054 0F93 push r16 |
||
83 | 99 0056 1F93 push r17 |
||
84 | 100 /* prologue end (size=8) */ |
||
85 | 101 .LVL0: |
||
86 | 102 0058 7B01 movw r14,r22 |
||
87 | 103 005a 8C01 movw r16,r24 |
||
88 | 104 .LM6: |
||
89 | 105 005c 0E94 0000 call floor |
||
90 | 106 .LVL1: |
||
91 | 107 0060 5B01 movw r10,r22 |
||
92 | 108 0062 6C01 movw r12,r24 |
||
93 | 109 .LVL2: |
||
94 | 110 .LM7: |
||
95 | 111 0064 9B01 movw r18,r22 |
||
96 | 112 0066 AC01 movw r20,r24 |
||
97 | 113 0068 C801 movw r24,r16 |
||
98 | 114 006a B701 movw r22,r14 |
||
99 | 115 006c 0E94 0000 call __subsf3 |
||
100 | 116 0070 20E0 ldi r18,lo8(0x3f000000) |
||
101 | 117 0072 30E0 ldi r19,hi8(0x3f000000) |
||
102 | 118 0074 40E0 ldi r20,hlo8(0x3f000000) |
||
103 | 119 0076 5FE3 ldi r21,hhi8(0x3f000000) |
||
104 | 120 0078 0E94 0000 call __gtsf2 |
||
105 | 121 007c 1816 cp __zero_reg__,r24 |
||
106 | 122 007e 04F4 brge .L10 |
||
107 | 123 .LM8: |
||
108 | 124 0080 20E0 ldi r18,lo8(0x3f800000) |
||
109 | 125 0082 30E0 ldi r19,hi8(0x3f800000) |
||
110 | 126 0084 40E8 ldi r20,hlo8(0x3f800000) |
||
111 | 127 0086 5FE3 ldi r21,hhi8(0x3f800000) |
||
112 | 128 0088 C601 movw r24,r12 |
||
113 | 129 008a B501 movw r22,r10 |
||
114 | 130 008c 0E94 0000 call __addsf3 |
||
115 | 131 0090 00C0 rjmp .L15 |
||
116 | 132 .L10: |
||
117 | 133 0092 C601 movw r24,r12 |
||
118 | 134 0094 B501 movw r22,r10 |
||
119 | 135 .L15: |
||
120 | 136 0096 0E94 0000 call __fixsfsi |
||
121 | 137 /* epilogue: frame size=0 */ |
||
122 | 138 009a 1F91 pop r17 |
||
123 | 139 009c 0F91 pop r16 |
||
124 | 140 009e FF90 pop r15 |
||
125 | 141 00a0 EF90 pop r14 |
||
126 | 142 00a2 DF90 pop r13 |
||
127 | 143 00a4 CF90 pop r12 |
||
128 | 144 00a6 BF90 pop r11 |
||
129 | 145 00a8 AF90 pop r10 |
||
130 | 146 00aa 0895 ret |
||
131 | 147 /* epilogue end (size=9) */ |
||
132 | 148 /* function lround size 51 (34) */ |
||
133 | 149 .LFE11: |
||
134 | 151 .global odometry_reset |
||
135 | 153 odometry_reset: |
||
136 | 154 .LFB8: |
||
137 | 155 .LM9: |
||
138 | 156 /* prologue: frame size=0 */ |
||
139 | 157 /* prologue end (size=0) */ |
||
140 | 158 .LM10: |
||
141 | 159 00ac 1092 0000 sts diff_x,__zero_reg__ |
||
142 | 160 00b0 1092 0000 sts (diff_x)+1,__zero_reg__ |
||
143 | 161 00b4 1092 0000 sts (diff_x)+2,__zero_reg__ |
||
144 | 162 00b8 1092 0000 sts (diff_x)+3,__zero_reg__ |
||
145 | 163 .LM11: |
||
146 | 164 00bc 1092 0000 sts diff_y,__zero_reg__ |
||
147 | 165 00c0 1092 0000 sts (diff_y)+1,__zero_reg__ |
||
148 | 166 00c4 1092 0000 sts (diff_y)+2,__zero_reg__ |
||
149 | 167 00c8 1092 0000 sts (diff_y)+3,__zero_reg__ |
||
150 | 168 .LM12: |
||
151 | 169 00cc 80E0 ldi r24,lo8(0) |
||
152 | 170 00ce 0E94 0000 call encoder_read |
||
153 | 171 00d2 9093 0000 sts (encoder_ltm1)+1,r25 |
||
154 | 172 00d6 8093 0000 sts encoder_ltm1,r24 |
||
155 | 173 .LM13: |
||
156 | 174 00da 81E0 ldi r24,lo8(1) |
||
157 | 175 00dc 0E94 0000 call encoder_read |
||
158 | 176 00e0 9093 0000 sts (encoder_rtm1)+1,r25 |
||
159 | 177 00e4 8093 0000 sts encoder_rtm1,r24 |
||
160 | 178 .LM14: |
||
161 | 179 00e8 80E0 ldi r24,lo8(0x0) |
||
162 | 180 00ea 90E0 ldi r25,hi8(0x0) |
||
163 | 181 00ec A0E0 ldi r26,hlo8(0x0) |
||
164 | 182 00ee B0E0 ldi r27,hhi8(0x0) |
||
165 | 183 00f0 8093 0000 sts angle,r24 |
||
166 | 184 00f4 9093 0000 sts (angle)+1,r25 |
||
167 | 185 00f8 A093 0000 sts (angle)+2,r26 |
||
168 | 186 00fc B093 0000 sts (angle)+3,r27 |
||
169 | 187 /* epilogue: frame size=0 */ |
||
170 | 188 0100 0895 ret |
||
171 | 189 /* epilogue end (size=1) */ |
||
172 | 190 /* function odometry_reset size 43 (42) */ |
||
173 | 191 .LFE8: |
||
174 | 193 .data |
||
175 | 194 .LC0: |
||
176 | 195 0000 6463 3A20 .string "dc: " |
||
177 | 195 00 |
||
178 | 196 .LC1: |
||
179 | 197 0005 756D 2C20 .string "um, velocity: " |
||
180 | 197 7665 6C6F |
||
181 | 197 6369 7479 |
||
182 | 197 3A20 00 |
||
183 | 198 .LC2: |
||
184 | 199 0014 0D0A 206D .string "\r\n mm/s" |
||
185 | 199 6D2F 7300 |
||
186 | 200 .text |
||
187 | 201 .global odometry_run |
||
188 | 203 odometry_run: |
||
189 | 204 .LFB9: |
||
190 | 205 .LM15: |
||
191 | 206 /* prologue: frame size=0 */ |
||
192 | 207 0102 2F92 push r2 |
||
193 | 208 0104 3F92 push r3 |
||
194 | 209 0106 4F92 push r4 |
||
195 | 210 0108 5F92 push r5 |
||
196 | 211 010a 6F92 push r6 |
||
197 | 212 010c 7F92 push r7 |
||
198 | 213 010e 8F92 push r8 |
||
199 | 214 0110 9F92 push r9 |
||
200 | 215 0112 AF92 push r10 |
||
201 | 216 0114 BF92 push r11 |
||
202 | 217 0116 CF92 push r12 |
||
203 | 218 0118 DF92 push r13 |
||
204 | 219 011a EF92 push r14 |
||
205 | 220 011c FF92 push r15 |
||
206 | 221 011e 0F93 push r16 |
||
207 | 222 0120 1F93 push r17 |
||
208 | 223 /* prologue end (size=16) */ |
||
209 | 224 .LBB2: |
||
210 | 225 .LM16: |
||
211 | 226 0122 81E0 ldi r24,lo8(1) |
||
212 | 227 0124 0E94 0000 call encoder_read |
||
213 | 228 0128 8C01 movw r16,r24 |
||
214 | 229 .LVL3: |
||
215 | 230 .LM17: |
||
216 | 231 012a 80E0 ldi r24,lo8(0) |
||
217 | 232 012c 0E94 0000 call encoder_read |
||
218 | 233 0130 9C01 movw r18,r24 |
||
219 | 234 .LVL4: |
||
220 | 235 .LM18: |
||
221 | 236 0132 8091 0000 lds r24,encoder_ltm1 |
||
222 | 237 0136 9091 0000 lds r25,(encoder_ltm1)+1 |
||
223 | 238 013a B901 movw r22,r18 |
||
224 | 239 013c 681B sub r22,r24 |
||
225 | 240 013e 790B sbc r23,r25 |
||
226 | 241 .LM19: |
||
227 | 242 0140 8091 0000 lds r24,encoder_rtm1 |
||
228 | 243 0144 9091 0000 lds r25,(encoder_rtm1)+1 |
||
229 | 244 0148 A801 movw r20,r16 |
||
230 | 245 014a 481B sub r20,r24 |
||
231 | 246 014c 590B sbc r21,r25 |
||
232 | 247 .LM20: |
||
233 | 248 014e 6115 cp r22,__zero_reg__ |
||
234 | 249 0150 7105 cpc r23,__zero_reg__ |
||
235 | 250 0152 01F4 brne .L19 |
||
236 | 251 .LM21: |
||
237 | 252 0154 4115 cp r20,__zero_reg__ |
||
238 | 253 0156 5105 cpc r21,__zero_reg__ |
||
239 | 254 0158 01F0 breq .+2 |
||
240 | 255 015a 00C0 rjmp .L34 |
||
241 | 256 015c 00C0 rjmp .L33 |
||
242 | 257 .L19: |
||
243 | 258 .LM22: |
||
244 | 259 015e 8827 clr r24 |
||
245 | 260 0160 77FD sbrc r23,7 |
||
246 | 261 0162 8095 com r24 |
||
247 | 262 0164 982F mov r25,r24 |
||
248 | 263 .LVL5: |
||
249 | 264 .LM23: |
||
250 | 265 0166 3093 0000 sts (encoder_ltm1)+1,r19 |
||
251 | 266 016a 2093 0000 sts encoder_ltm1,r18 |
||
252 | 267 .LM24: |
||
253 | 268 016e 1093 0000 sts (encoder_rtm1)+1,r17 |
||
254 | 269 0172 0093 0000 sts encoder_rtm1,r16 |
||
255 | 270 .LM25: |
||
256 | 271 0176 6130 cpi r22,lo8(513) |
||
257 | 272 0178 22E0 ldi r18,hi8(513) |
||
258 | 273 017a 7207 cpc r23,r18 |
||
259 | 274 017c 20E0 ldi r18,hlo8(513) |
||
260 | 275 017e 8207 cpc r24,r18 |
||
261 | 276 0180 20E0 ldi r18,hhi8(513) |
||
262 | 277 0182 9207 cpc r25,r18 |
||
263 | 278 .LVL6: |
||
264 | 279 0184 04F0 brlt .L23 |
||
265 | 280 0186 6050 subi r22,lo8(-(-1024)) |
||
266 | 281 0188 7440 sbci r23,hi8(-(-1024)) |
||
267 | 282 018a 8040 sbci r24,hlo8(-(-1024)) |
||
268 | 283 018c 9040 sbci r25,hhi8(-(-1024)) |
||
269 | 284 .LVL7: |
||
270 | 285 .L23: |
||
271 | 286 .LM26: |
||
272 | 287 018e 6030 cpi r22,lo8(-512) |
||
273 | 288 0190 3EEF ldi r19,hi8(-512) |
||
274 | 289 0192 7307 cpc r23,r19 |
||
275 | 290 0194 3FEF ldi r19,hlo8(-512) |
||
276 | 291 0196 8307 cpc r24,r19 |
||
277 | 292 0198 3FEF ldi r19,hhi8(-512) |
||
278 | 293 019a 9307 cpc r25,r19 |
||
279 | 294 019c 04F4 brge .L25 |
||
280 | 295 019e 6050 subi r22,lo8(-(1024)) |
||
281 | 296 01a0 7C4F sbci r23,hi8(-(1024)) |
||
282 | 297 01a2 8F4F sbci r24,hlo8(-(1024)) |
||
283 | 298 01a4 9F4F sbci r25,hhi8(-(1024)) |
||
284 | 299 .L25: |
||
285 | 300 .LM27: |
||
286 | 301 01a6 7A01 movw r14,r20 |
||
287 | 302 01a8 0027 clr r16 |
||
288 | 303 01aa F7FC sbrc r15,7 |
||
289 | 304 01ac 0095 com r16 |
||
290 | 305 01ae 102F mov r17,r16 |
||
291 | 306 .LVL8: |
||
292 | 307 .LM28: |
||
293 | 308 01b0 41E0 ldi r20,lo8(513) |
||
294 | 309 01b2 E416 cp r14,r20 |
||
295 | 310 01b4 42E0 ldi r20,hi8(513) |
||
296 | 311 01b6 F406 cpc r15,r20 |
||
297 | 312 01b8 40E0 ldi r20,hlo8(513) |
||
298 | 313 01ba 0407 cpc r16,r20 |
||
299 | 314 01bc 40E0 ldi r20,hhi8(513) |
||
300 | 315 01be 1407 cpc r17,r20 |
||
301 | 316 01c0 04F0 brlt .L27 |
||
302 | 317 01c2 20E0 ldi r18,lo8(-1024) |
||
303 | 318 01c4 3CEF ldi r19,hi8(-1024) |
||
304 | 319 01c6 4FEF ldi r20,hlo8(-1024) |
||
305 | 320 01c8 5FEF ldi r21,hhi8(-1024) |
||
306 | 321 .LVL9: |
||
307 | 322 01ca E20E add r14,r18 |
||
308 | 323 01cc F31E adc r15,r19 |
||
309 | 324 01ce 041F adc r16,r20 |
||
310 | 325 01d0 151F adc r17,r21 |
||
311 | 326 .LVL10: |
||
312 | 327 .L27: |
||
313 | 328 .LM29: |
||
314 | 329 01d2 30E0 ldi r19,lo8(-512) |
||
315 | 330 01d4 E316 cp r14,r19 |
||
316 | 331 01d6 3EEF ldi r19,hi8(-512) |
||
317 | 332 01d8 F306 cpc r15,r19 |
||
318 | 333 01da 3FEF ldi r19,hlo8(-512) |
||
319 | 334 01dc 0307 cpc r16,r19 |
||
320 | 335 01de 3FEF ldi r19,hhi8(-512) |
||
321 | 336 01e0 1307 cpc r17,r19 |
||
322 | 337 01e2 04F4 brge .L29 |
||
323 | 338 01e4 20E0 ldi r18,lo8(1024) |
||
324 | 339 01e6 34E0 ldi r19,hi8(1024) |
||
325 | 340 01e8 40E0 ldi r20,hlo8(1024) |
||
326 | 341 01ea 50E0 ldi r21,hhi8(1024) |
||
327 | 342 .LVL11: |
||
328 | 343 01ec E20E add r14,r18 |
||
329 | 344 01ee F31E adc r15,r19 |
||
330 | 345 01f0 041F adc r16,r20 |
||
331 | 346 01f2 151F adc r17,r21 |
||
332 | 347 .LVL12: |
||
333 | 348 .L29: |
||
334 | 349 .LM30: |
||
335 | 350 01f4 2CEC ldi r18,lo8(204) |
||
336 | 351 01f6 30E0 ldi r19,hi8(204) |
||
337 | 352 01f8 40E0 ldi r20,hlo8(204) |
||
338 | 353 01fa 50E0 ldi r21,hhi8(204) |
||
339 | 354 .LVL13: |
||
340 | 355 01fc 0E94 0000 call __mulsi3 |
||
341 | 356 0200 1B01 movw r2,r22 |
||
342 | 357 0202 2C01 movw r4,r24 |
||
343 | 358 .LVL14: |
||
344 | 359 .LM31: |
||
345 | 360 0204 C801 movw r24,r16 |
||
346 | 361 0206 B701 movw r22,r14 |
||
347 | 362 0208 0E94 0000 call __mulsi3 |
||
348 | 363 .LVL15: |
||
349 | 364 .LBE2: |
||
350 | 365 .LM32: |
||
351 | 366 020c 2616 cp r2,r22 |
||
352 | 367 020e 3706 cpc r3,r23 |
||
353 | 368 0210 4806 cpc r4,r24 |
||
354 | 369 0212 5906 cpc r5,r25 |
||
355 | 370 0214 01F0 breq .+2 |
||
356 | 371 0216 00C0 rjmp .L31 |
||
357 | 372 .LM33: |
||
358 | 373 0218 E090 0000 lds r14,diff_x |
||
359 | 374 021c F090 0000 lds r15,(diff_x)+1 |
||
360 | 375 0220 0091 0000 lds r16,(diff_x)+2 |
||
361 | 376 0224 1091 0000 lds r17,(diff_x)+3 |
||
362 | 377 .LVL16: |
||
363 | 378 0228 C201 movw r24,r4 |
||
364 | 379 022a B101 movw r22,r2 |
||
365 | 380 .LVL17: |
||
366 | 381 022c 0E94 0000 call __floatsisf |
||
367 | 382 0230 5B01 movw r10,r22 |
||
368 | 383 0232 6C01 movw r12,r24 |
||
369 | 384 0234 6091 0000 lds r22,angle |
||
370 | 385 0238 7091 0000 lds r23,(angle)+1 |
||
371 | 386 023c 8091 0000 lds r24,(angle)+2 |
||
372 | 387 0240 9091 0000 lds r25,(angle)+3 |
||
373 | 388 0244 0E94 0000 call cos |
||
374 | 389 0248 9B01 movw r18,r22 |
||
375 | 390 024a AC01 movw r20,r24 |
||
376 | 391 024c C601 movw r24,r12 |
||
377 | 392 024e B501 movw r22,r10 |
||
378 | 393 0250 0E94 0000 call __mulsf3 |
||
379 | 394 0254 20E0 ldi r18,lo8(0x447a0000) |
||
380 | 395 0256 30E0 ldi r19,hi8(0x447a0000) |
||
381 | 396 0258 4AE7 ldi r20,hlo8(0x447a0000) |
||
382 | 397 025a 54E4 ldi r21,hhi8(0x447a0000) |
||
383 | 398 025c 0E94 0000 call __divsf3 |
||
384 | 399 0260 0E94 0000 call lround |
||
385 | 400 0264 E60E add r14,r22 |
||
386 | 401 0266 F71E adc r15,r23 |
||
387 | 402 0268 081F adc r16,r24 |
||
388 | 403 026a 191F adc r17,r25 |
||
389 | 404 026c E092 0000 sts diff_x,r14 |
||
390 | 405 0270 F092 0000 sts (diff_x)+1,r15 |
||
391 | 406 0274 0093 0000 sts (diff_x)+2,r16 |
||
392 | 407 0278 1093 0000 sts (diff_x)+3,r17 |
||
393 | 408 .LM34: |
||
394 | 409 027c E090 0000 lds r14,diff_y |
||
395 | 410 0280 F090 0000 lds r15,(diff_y)+1 |
||
396 | 411 0284 0091 0000 lds r16,(diff_y)+2 |
||
397 | 412 0288 1091 0000 lds r17,(diff_y)+3 |
||
398 | 413 028c 6091 0000 lds r22,angle |
||
399 | 414 0290 7091 0000 lds r23,(angle)+1 |
||
400 | 415 0294 8091 0000 lds r24,(angle)+2 |
||
401 | 416 0298 9091 0000 lds r25,(angle)+3 |
||
402 | 417 029c 0E94 0000 call sin |
||
403 | 418 02a0 9B01 movw r18,r22 |
||
404 | 419 02a2 AC01 movw r20,r24 |
||
405 | 420 02a4 C601 movw r24,r12 |
||
406 | 421 02a6 B501 movw r22,r10 |
||
407 | 422 02a8 0E94 0000 call __mulsf3 |
||
408 | 423 02ac 20E0 ldi r18,lo8(0x447a0000) |
||
409 | 424 02ae 30E0 ldi r19,hi8(0x447a0000) |
||
410 | 425 02b0 4AE7 ldi r20,hlo8(0x447a0000) |
||
411 | 426 02b2 54E4 ldi r21,hhi8(0x447a0000) |
||
412 | 427 02b4 0E94 0000 call __divsf3 |
||
413 | 428 02b8 0E94 0000 call lround |
||
414 | 429 02bc E60E add r14,r22 |
||
415 | 430 02be F71E adc r15,r23 |
||
416 | 431 02c0 081F adc r16,r24 |
||
417 | 432 02c2 191F adc r17,r25 |
||
418 | 433 02c4 E092 0000 sts diff_y,r14 |
||
419 | 434 02c8 F092 0000 sts (diff_y)+1,r15 |
||
420 | 435 02cc 0093 0000 sts (diff_y)+2,r16 |
||
421 | 436 02d0 1093 0000 sts (diff_y)+3,r17 |
||
422 | 437 .LM35: |
||
423 | 438 02d4 20E0 ldi r18,lo8(0x447a0000) |
||
424 | 439 02d6 30E0 ldi r19,hi8(0x447a0000) |
||
425 | 440 02d8 4AE7 ldi r20,hlo8(0x447a0000) |
||
426 | 441 02da 54E4 ldi r21,hhi8(0x447a0000) |
||
427 | 442 02dc C601 movw r24,r12 |
||
428 | 443 02de B501 movw r22,r10 |
||
429 | 444 02e0 0E94 0000 call __mulsf3 |
||
430 | 445 02e4 20E0 ldi r18,lo8(0x467f0000) |
||
431 | 446 02e6 30E0 ldi r19,hi8(0x467f0000) |
||
432 | 447 02e8 4FE7 ldi r20,hlo8(0x467f0000) |
||
433 | 448 02ea 56E4 ldi r21,hhi8(0x467f0000) |
||
434 | 449 02ec 0E94 0000 call __divsf3 |
||
435 | 450 02f0 0E94 0000 call lround |
||
436 | 451 02f4 6093 0000 sts velocity,r22 |
||
437 | 452 02f8 7093 0000 sts (velocity)+1,r23 |
||
438 | 453 02fc 8093 0000 sts (velocity)+2,r24 |
||
439 | 454 0300 9093 0000 sts (velocity)+3,r25 |
||
440 | 455 .LM36: |
||
441 | 456 0304 80E0 ldi r24,lo8(.LC0) |
||
442 | 457 0306 90E0 ldi r25,hi8(.LC0) |
||
443 | 458 0308 0E94 0000 call usb_puts |
||
444 | 459 .LM37: |
||
445 | 460 030c C101 movw r24,r2 |
||
446 | 461 030e 0E94 0000 call usb_puti |
||
447 | 462 .LM38: |
||
448 | 463 0312 80E0 ldi r24,lo8(.LC1) |
||
449 | 464 0314 90E0 ldi r25,hi8(.LC1) |
||
450 | 465 0316 0E94 0000 call usb_puts |
||
451 | 466 .LM39: |
||
452 | 467 031a 8091 0000 lds r24,velocity |
||
453 | 468 031e 9091 0000 lds r25,(velocity)+1 |
||
454 | 469 0322 0E94 0000 call usb_puti |
||
455 | 470 .LM40: |
||
456 | 471 0326 80E0 ldi r24,lo8(.LC2) |
||
457 | 472 0328 90E0 ldi r25,hi8(.LC2) |
||
458 | 473 032a 0E94 0000 call usb_puts |
||
459 | 474 032e 00C0 rjmp .L33 |
||
460 | 475 .LVL18: |
||
461 | 476 .L31: |
||
462 | 477 .LM41: |
||
463 | 478 0330 A201 movw r20,r4 |
||
464 | 479 0332 9101 movw r18,r2 |
||
465 | 480 0334 261B sub r18,r22 |
||
466 | 481 0336 370B sbc r19,r23 |
||
467 | 482 0338 480B sbc r20,r24 |
||
468 | 483 033a 590B sbc r21,r25 |
||
469 | 484 033c CA01 movw r24,r20 |
||
470 | 485 033e B901 movw r22,r18 |
||
471 | 486 .LVL19: |
||
472 | 487 0340 0E94 0000 call __floatsisf |
||
473 | 488 0344 7B01 movw r14,r22 |
||
474 | 489 0346 8C01 movw r16,r24 |
||
475 | 490 .LVL20: |
||
476 | 491 .LM42: |
||
477 | 492 0348 20E0 ldi r18,lo8(0x4805ca00) |
||
478 | 493 034a 3AEC ldi r19,hi8(0x4805ca00) |
||
479 | 494 034c 45E0 ldi r20,hlo8(0x4805ca00) |
||
480 | 495 034e 58E4 ldi r21,hhi8(0x4805ca00) |
||
481 | 496 0350 0E94 0000 call __divsf3 |
||
482 | 497 0354 5B01 movw r10,r22 |
||
483 | 498 0356 6C01 movw r12,r24 |
||
484 | 499 .LM43: |
||
485 | 500 0358 C201 movw r24,r4 |
||
486 | 501 035a B101 movw r22,r2 |
||
487 | 502 035c 28E2 ldi r18,lo8(137000) |
||
488 | 503 035e 37E1 ldi r19,hi8(137000) |
||
489 | 504 0360 42E0 ldi r20,hlo8(137000) |
||
490 | 505 0362 50E0 ldi r21,hhi8(137000) |
||
491 | 506 0364 0E94 0000 call __mulsi3 |
||
492 | 507 0368 0E94 0000 call __floatsisf |
||
493 | 508 036c A801 movw r20,r16 |
||
494 | 509 036e 9701 movw r18,r14 |
||
495 | 510 0370 0E94 0000 call __divsf3 |
||
496 | 511 0374 20E0 ldi r18,lo8(0x4805ca00) |
||
497 | 512 0376 3AEC ldi r19,hi8(0x4805ca00) |
||
498 | 513 0378 45E0 ldi r20,hlo8(0x4805ca00) |
||
499 | 514 037a 58E4 ldi r21,hhi8(0x4805ca00) |
||
500 | 515 037c 0E94 0000 call __subsf3 |
||
501 | 516 0380 9B01 movw r18,r22 |
||
502 | 517 0382 AC01 movw r20,r24 |
||
503 | 518 0384 C601 movw r24,r12 |
||
504 | 519 0386 B501 movw r22,r10 |
||
505 | 520 .LVL21: |
||
506 | 521 0388 0E94 0000 call __mulsf3 |
||
507 | 522 .LVL22: |
||
508 | 523 038c 20E0 ldi r18,lo8(0x3f000000) |
||
509 | 524 038e 30E0 ldi r19,hi8(0x3f000000) |
||
510 | 525 0390 40E0 ldi r20,hlo8(0x3f000000) |
||
511 | 526 0392 5FE3 ldi r21,hhi8(0x3f000000) |
||
512 | 527 0394 0E94 0000 call __mulsf3 |
||
513 | 528 0398 3B01 movw r6,r22 |
||
514 | 529 039a 4C01 movw r8,r24 |
||
515 | 530 .LM44: |
||
516 | 531 039c 20E0 ldi r18,lo8(0x447a0000) |
||
517 | 532 039e 30E0 ldi r19,hi8(0x447a0000) |
||
518 | 533 03a0 4AE7 ldi r20,hlo8(0x447a0000) |
||
519 | 534 03a2 54E4 ldi r21,hhi8(0x447a0000) |
||
520 | 535 03a4 0E94 0000 call __mulsf3 |
||
521 | 536 03a8 20E0 ldi r18,lo8(0x467f0000) |
||
522 | 537 03aa 30E0 ldi r19,hi8(0x467f0000) |
||
523 | 538 03ac 4FE7 ldi r20,hlo8(0x467f0000) |
||
524 | 539 03ae 56E4 ldi r21,hhi8(0x467f0000) |
||
525 | 540 03b0 0E94 0000 call __divsf3 |
||
526 | 541 03b4 0E94 0000 call lround |
||
527 | 542 03b8 6093 0000 sts velocity,r22 |
||
528 | 543 03bc 7093 0000 sts (velocity)+1,r23 |
||
529 | 544 03c0 8093 0000 sts (velocity)+2,r24 |
||
530 | 545 03c4 9093 0000 sts (velocity)+3,r25 |
||
531 | 546 .LM45: |
||
532 | 547 03c8 80E0 ldi r24,lo8(.LC0) |
||
533 | 548 03ca 90E0 ldi r25,hi8(.LC0) |
||
534 | 549 03cc 0E94 0000 call usb_puts |
||
535 | 550 .LM46: |
||
536 | 551 03d0 C101 movw r24,r2 |
||
537 | 552 03d2 0E94 0000 call usb_puti |
||
538 | 553 .LM47: |
||
539 | 554 03d6 80E0 ldi r24,lo8(.LC1) |
||
540 | 555 03d8 90E0 ldi r25,hi8(.LC1) |
||
541 | 556 03da 0E94 0000 call usb_puts |
||
542 | 557 .LM48: |
||
543 | 558 03de 8091 0000 lds r24,velocity |
||
544 | 559 03e2 9091 0000 lds r25,(velocity)+1 |
||
545 | 560 03e6 0E94 0000 call usb_puti |
||
546 | 561 .LM49: |
||
547 | 562 03ea 80E0 ldi r24,lo8(.LC2) |
||
548 | 563 03ec 90E0 ldi r25,hi8(.LC2) |
||
549 | 564 03ee 0E94 0000 call usb_puts |
||
550 | 565 .LM50: |
||
551 | 566 03f2 29EE ldi r18,lo8(0xbf9069e9) |
||
552 | 567 03f4 39E6 ldi r19,hi8(0xbf9069e9) |
||
553 | 568 03f6 40E9 ldi r20,hlo8(0xbf9069e9) |
||
554 | 569 03f8 5FEB ldi r21,hhi8(0xbf9069e9) |
||
555 | 570 03fa C601 movw r24,r12 |
||
556 | 571 03fc B501 movw r22,r10 |
||
557 | 572 .LVL23: |
||
558 | 573 03fe 0E94 0000 call __mulsf3 |
||
559 | 574 .LVL24: |
||
560 | 575 0402 2091 0000 lds r18,angle |
||
561 | 576 0406 3091 0000 lds r19,(angle)+1 |
||
562 | 577 040a 4091 0000 lds r20,(angle)+2 |
||
563 | 578 040e 5091 0000 lds r21,(angle)+3 |
||
564 | 579 0412 0E94 0000 call __addsf3 |
||
565 | 580 0416 7B01 movw r14,r22 |
||
566 | 581 0418 8C01 movw r16,r24 |
||
567 | 582 041a 6093 0000 sts angle,r22 |
||
568 | 583 041e 7093 0000 sts (angle)+1,r23 |
||
569 | 584 0422 8093 0000 sts (angle)+2,r24 |
||
570 | 585 0426 9093 0000 sts (angle)+3,r25 |
||
571 | 586 .LM51: |
||
572 | 587 042a A090 0000 lds r10,diff_x |
||
573 | 588 042e B090 0000 lds r11,(diff_x)+1 |
||
574 | 589 0432 C090 0000 lds r12,(diff_x)+2 |
||
575 | 590 0436 D090 0000 lds r13,(diff_x)+3 |
||
576 | 591 043a 22EA ldi r18,lo8(0x4006bca2) |
||
577 | 592 043c 3CEB ldi r19,hi8(0x4006bca2) |
||
578 | 593 043e 46E0 ldi r20,hlo8(0x4006bca2) |
||
579 | 594 0440 50E4 ldi r21,hhi8(0x4006bca2) |
||
580 | 595 0442 C401 movw r24,r8 |
||
581 | 596 0444 B301 movw r22,r6 |
||
582 | 597 .LVL25: |
||
583 | 598 0446 0E94 0000 call __mulsf3 |
||
584 | 599 .LVL26: |
||
585 | 600 044a 3B01 movw r6,r22 |
||
586 | 601 044c 4C01 movw r8,r24 |
||
587 | 602 044e C801 movw r24,r16 |
||
588 | 603 0450 B701 movw r22,r14 |
||
589 | 604 0452 0E94 0000 call cos |
||
590 | 605 0456 9B01 movw r18,r22 |
||
591 | 606 0458 AC01 movw r20,r24 |
||
592 | 607 045a C401 movw r24,r8 |
||
593 | 608 045c B301 movw r22,r6 |
||
594 | 609 045e 0E94 0000 call __mulsf3 |
||
595 | 610 0462 20E0 ldi r18,lo8(0x447a0000) |
||
596 | 611 0464 30E0 ldi r19,hi8(0x447a0000) |
||
597 | 612 0466 4AE7 ldi r20,hlo8(0x447a0000) |
||
598 | 613 0468 54E4 ldi r21,hhi8(0x447a0000) |
||
599 | 614 046a 0E94 0000 call __divsf3 |
||
600 | 615 046e 0E94 0000 call lround |
||
601 | 616 0472 A60E add r10,r22 |
||
602 | 617 0474 B71E adc r11,r23 |
||
603 | 618 0476 C81E adc r12,r24 |
||
604 | 619 0478 D91E adc r13,r25 |
||
605 | 620 047a A092 0000 sts diff_x,r10 |
||
606 | 621 047e B092 0000 sts (diff_x)+1,r11 |
||
607 | 622 0482 C092 0000 sts (diff_x)+2,r12 |
||
608 | 623 0486 D092 0000 sts (diff_x)+3,r13 |
||
609 | 624 .LM52: |
||
610 | 625 048a E090 0000 lds r14,diff_y |
||
611 | 626 048e F090 0000 lds r15,(diff_y)+1 |
||
612 | 627 0492 0091 0000 lds r16,(diff_y)+2 |
||
613 | 628 0496 1091 0000 lds r17,(diff_y)+3 |
||
614 | 629 049a 6091 0000 lds r22,angle |
||
615 | 630 049e 7091 0000 lds r23,(angle)+1 |
||
616 | 631 04a2 8091 0000 lds r24,(angle)+2 |
||
617 | 632 04a6 9091 0000 lds r25,(angle)+3 |
||
618 | 633 04aa 0E94 0000 call sin |
||
619 | 634 04ae 9B01 movw r18,r22 |
||
620 | 635 04b0 AC01 movw r20,r24 |
||
621 | 636 04b2 C401 movw r24,r8 |
||
622 | 637 04b4 B301 movw r22,r6 |
||
623 | 638 04b6 0E94 0000 call __mulsf3 |
||
624 | 639 04ba 20E0 ldi r18,lo8(0x447a0000) |
||
625 | 640 04bc 30E0 ldi r19,hi8(0x447a0000) |
||
626 | 641 04be 4AE7 ldi r20,hlo8(0x447a0000) |
||
627 | 642 04c0 54E4 ldi r21,hhi8(0x447a0000) |
||
628 | 643 04c2 0E94 0000 call __divsf3 |
||
629 | 644 04c6 0E94 0000 call lround |
||
630 | 645 04ca E60E add r14,r22 |
||
631 | 646 04cc F71E adc r15,r23 |
||
632 | 647 04ce 081F adc r16,r24 |
||
633 | 648 04d0 191F adc r17,r25 |
||
634 | 649 04d2 E092 0000 sts diff_y,r14 |
||
635 | 650 04d6 F092 0000 sts (diff_y)+1,r15 |
||
636 | 651 04da 0093 0000 sts (diff_y)+2,r16 |
||
637 | 652 04de 1093 0000 sts (diff_y)+3,r17 |
||
638 | 653 04e2 00C0 rjmp .L33 |
||
639 | 654 .LVL27: |
||
640 | 655 .L34: |
||
641 | 656 .LBB3: |
||
642 | 657 .LM53: |
||
643 | 658 04e4 60E0 ldi r22,lo8(0) |
||
644 | 659 04e6 70E0 ldi r23,hi8(0) |
||
645 | 660 04e8 80E0 ldi r24,hlo8(0) |
||
646 | 661 04ea 90E0 ldi r25,hhi8(0) |
||
647 | 662 .LVL28: |
||
648 | 663 .LM54: |
||
649 | 664 04ec 3093 0000 sts (encoder_ltm1)+1,r19 |
||
650 | 665 04f0 2093 0000 sts encoder_ltm1,r18 |
||
651 | 666 .LM55: |
||
652 | 667 04f4 1093 0000 sts (encoder_rtm1)+1,r17 |
||
653 | 668 04f8 0093 0000 sts encoder_rtm1,r16 |
||
654 | 669 04fc 00C0 rjmp .L23 |
||
655 | 670 .LVL29: |
||
656 | 671 .L33: |
||
657 | 672 .LBE3: |
||
658 | 673 /* epilogue: frame size=0 */ |
||
659 | 674 04fe 1F91 pop r17 |
||
660 | 675 0500 0F91 pop r16 |
||
661 | 676 0502 FF90 pop r15 |
||
662 | 677 0504 EF90 pop r14 |
||
663 | 678 0506 DF90 pop r13 |
||
664 | 679 0508 CF90 pop r12 |
||
665 | 680 050a BF90 pop r11 |
||
666 | 681 050c AF90 pop r10 |
||
667 | 682 050e 9F90 pop r9 |
||
668 | 683 0510 8F90 pop r8 |
||
669 | 684 0512 7F90 pop r7 |
||
670 | 685 0514 6F90 pop r6 |
||
671 | 686 0516 5F90 pop r5 |
||
672 | 687 0518 4F90 pop r4 |
||
673 | 688 051a 3F90 pop r3 |
||
674 | 689 051c 2F90 pop r2 |
||
675 | 690 051e 0895 ret |
||
676 | 691 /* epilogue end (size=17) */ |
||
677 | 692 /* function odometry_run size 527 (494) */ |
||
678 | 693 .LFE9: |
||
679 | 695 .global __vector_9 |
||
680 | 697 __vector_9: |
||
681 | 698 .LFB10: |
||
682 | 699 .LM56: |
||
683 | 700 /* prologue: frame size=0 */ |
||
684 | 701 0520 1F92 push __zero_reg__ |
||
685 | 702 0522 0F92 push __tmp_reg__ |
||
686 | 703 0524 0FB6 in __tmp_reg__,__SREG__ |
||
687 | 704 0526 0F92 push __tmp_reg__ |
||
688 | 705 0528 1124 clr __zero_reg__ |
||
689 | 706 052a 2F93 push r18 |
||
690 | 707 052c 3F93 push r19 |
||
691 | 708 052e 4F93 push r20 |
||
692 | 709 0530 5F93 push r21 |
||
693 | 710 0532 6F93 push r22 |
||
694 | 711 0534 7F93 push r23 |
||
695 | 712 0536 8F93 push r24 |
||
696 | 713 0538 9F93 push r25 |
||
697 | 714 053a AF93 push r26 |
||
698 | 715 053c BF93 push r27 |
||
699 | 716 053e EF93 push r30 |
||
700 | 717 0540 FF93 push r31 |
||
701 | 718 /* prologue end (size=17) */ |
||
702 | 719 .LM57: |
||
703 | 720 0542 0E94 0000 call odometry_run |
||
704 | 721 /* epilogue: frame size=0 */ |
||
705 | 722 0546 FF91 pop r31 |
||
706 | 723 0548 EF91 pop r30 |
||
707 | 724 054a BF91 pop r27 |
||
708 | 725 054c AF91 pop r26 |
||
709 | 726 054e 9F91 pop r25 |
||
710 | 727 0550 8F91 pop r24 |
||
711 | 728 0552 7F91 pop r23 |
||
712 | 729 0554 6F91 pop r22 |
||
713 | 730 0556 5F91 pop r21 |
||
714 | 731 0558 4F91 pop r20 |
||
715 | 732 055a 3F91 pop r19 |
||
716 | 733 055c 2F91 pop r18 |
||
717 | 734 055e 0F90 pop __tmp_reg__ |
||
718 | 735 0560 0FBE out __SREG__,__tmp_reg__ |
||
719 | 736 0562 0F90 pop __tmp_reg__ |
||
720 | 737 0564 1F90 pop __zero_reg__ |
||
721 | 738 0566 1895 reti |
||
722 | 739 /* epilogue end (size=17) */ |
||
723 | 740 /* function __vector_9 size 36 (2) */ |
||
724 | 741 .LFE10: |
||
725 | 743 .global odometry_init |
||
726 | 745 odometry_init: |
||
727 | 746 .LFB7: |
||
728 | 747 .LM58: |
||
729 | 748 /* prologue: frame size=0 */ |
||
730 | 749 /* prologue end (size=0) */ |
||
731 | 750 .LM59: |
||
732 | 751 0568 0E94 0000 call encoders_init |
||
733 | 752 .LM60: |
||
734 | 753 056c 84E6 ldi r24,lo8(100) |
||
735 | 754 056e 90E0 ldi r25,hi8(100) |
||
736 | 755 0570 0E94 0000 call delay_ms |
||
737 | 756 .LM61: |
||
738 | 757 0574 0E94 0000 call odometry_reset |
||
739 | 758 .LM62: |
||
740 | 759 0578 15BC out 69-0x20,__zero_reg__ |
||
741 | 760 .LM63: |
||
742 | 761 057a 8DE0 ldi r24,lo8(13) |
||
743 | 762 057c 85BD out 69-0x20,r24 |
||
744 | 763 .LM64: |
||
745 | 764 057e 87B7 in r24,87-0x20 |
||
746 | 765 0580 8068 ori r24,lo8(-128) |
||
747 | 766 0582 87BF out 87-0x20,r24 |
||
748 | 767 .LM65: |
||
749 | 768 0584 8FEF ldi r24,lo8(-1) |
||
750 | 769 0586 83BD out 67-0x20,r24 |
||
751 | 770 /* epilogue: frame size=0 */ |
||
752 | 771 0588 0895 ret |
||
753 | 772 /* epilogue end (size=1) */ |
||
754 | 773 /* function odometry_init size 17 (16) */ |
||
755 | 774 .LFE7: |
||
756 | 776 .comm encoder_ltm1,2,1 |
||
757 | 777 .comm encoder_rtm1,2,1 |
||
758 | 778 .comm diff_x,4,1 |
||
759 | 779 .comm diff_y,4,1 |
||
760 | 780 .comm velocity,4,1 |
||
761 | 781 .comm angle,4,1 |
||
762 | 870 .Letext0: |
||
763 | DEFINED SYMBOLS |
||
764 | *ABS*:00000000 odometry.c |
||
765 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:3 *ABS*:0000003f __SREG__ |
||
766 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:4 *ABS*:0000003e __SP_H__ |
||
767 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:5 *ABS*:0000003d __SP_L__ |
||
768 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:6 *ABS*:00000000 __tmp_reg__ |
||
769 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:7 *ABS*:00000001 __zero_reg__ |
||
770 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:20 .text:00000000 odometry_dx |
||
771 | *COM*:00000004 diff_x |
||
772 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:37 .text:00000012 odometry_dy |
||
773 | *COM*:00000004 diff_y |
||
774 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:54 .text:00000024 odometry_angle |
||
775 | *COM*:00000004 angle |
||
776 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:71 .text:00000036 odometry_velocity |
||
777 | *COM*:00000004 velocity |
||
778 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:88 .text:00000048 lround |
||
779 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:153 .text:000000ac odometry_reset |
||
780 | *COM*:00000002 encoder_ltm1 |
||
781 | *COM*:00000002 encoder_rtm1 |
||
782 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:203 .text:00000102 odometry_run |
||
783 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:697 .text:00000520 __vector_9 |
||
784 | F:\DOCUME~1\Evan\LOCALS~1\Temp/cc0yS4xv.s:745 .text:00000568 odometry_init |
||
785 | |||
786 | UNDEFINED SYMBOLS |
||
787 | __do_copy_data |
||
788 | __do_clear_bss |
||
789 | floor |
||
790 | __subsf3 |
||
791 | __gtsf2 |
||
792 | __addsf3 |
||
793 | __fixsfsi |
||
794 | encoder_read |
||
795 | __mulsi3 |
||
796 | __floatsisf |
||
797 | cos |
||
798 | __mulsf3 |
||
799 | __divsf3 |
||
800 | sin |
||
801 | usb_puts |
||
802 | usb_puti |
||
803 | encoders_init |
||
804 | delay_ms |