Project

General

Profile

Statistics
| Revision:

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