root / demos / john / projects / libdragonfly / odometry.lst @ 1693
History | View | Annotate | Download (29.5 KB)
1 |
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 |