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 |