1 .file "MotorTask.c" 2 .arch atmega128 3 __SREG__ = 0x3f 4 __SP_H__ = 0x3e 5 __SP_L__ = 0x3d 6 __tmp_reg__ = 0 7 __zero_reg__ = 1 8 .global __do_copy_data 9 .global __do_clear_bss 12 .text 13 .Ltext0: 97 .global MotorTaskTcb 98 .section .progmem.data,"a",@progbits 101 MotorTaskTcb: 102 0000 0000 .word MotorTaskStk+74 103 0002 0000 .word pm(MotorTask) 104 0004 0000 .word MotorTaskPid 105 0006 01 .byte 1 106 .global RightEncoder 107 .global RightEncoder 108 .section .bss 111 RightEncoder: 112 0000 00 .skip 1,0 113 .global LeftEncoder 114 .global LeftEncoder 117 LeftEncoder: 118 0001 00 .skip 1,0 119 .global X 120 .global X 123 X: 124 0002 0000 0000 .skip 4,0 125 .global Y 126 .global Y 129 Y: 130 0006 0000 0000 .skip 4,0 131 .global Theta 132 .global Theta 135 Theta: 136 000a 0000 0000 .skip 4,0 137 .global iX 138 .global iX 141 iX: 142 000e 0000 .skip 2,0 143 .global iY 144 .global iY 147 iY: 148 0010 0000 .skip 2,0 149 .global iTheta 150 .global iTheta 153 iTheta: 154 0012 0000 .skip 2,0 155 .text 157 .global MotorTask 159 MotorTask: 1:MotorTask.c **** //#include // for printf 2:MotorTask.c **** //#include // for PSTR() 3:MotorTask.c **** #include 4:MotorTask.c **** #include 5:MotorTask.c **** #include "avrx.h" 6:MotorTask.c **** #include "hardware.h" 7:MotorTask.c **** #include "generalio.h" 8:MotorTask.c **** #include "serialio.h" 9:MotorTask.c **** #include "definitions.h" // to get motor command defines. 10:MotorTask.c **** #include "MotorTask.h" 11:MotorTask.c **** 12:MotorTask.c **** //#define pid_velocity_dump 1 13:MotorTask.c **** //#define pid_plot2 2 14:MotorTask.c **** 15:MotorTask.c **** 16:MotorTask.c **** AVRX_GCC_TASKDEF(MotorTask, 40, MOTOR_PRIORITY); 17:MotorTask.c **** 18:MotorTask.c **** /* 19:MotorTask.c **** Globals/Statics etc. 20:MotorTask.c **** */ 21:MotorTask.c **** 22:MotorTask.c **** MotorInfo Left, Right; 23:MotorTask.c **** 24:MotorTask.c **** volatile signed char RightEncoder=0, LeftEncoder=0; // Wheel encoder counts 25:MotorTask.c **** 26:MotorTask.c **** volatile float X=0, 27:MotorTask.c **** Y=0, 28:MotorTask.c **** Theta=0; 29:MotorTask.c **** volatile int iX=0, 30:MotorTask.c **** iY=0, 31:MotorTask.c **** iTheta=0; 32:MotorTask.c **** 33:MotorTask.c **** volatile unsigned char OdometryResetPending; 34:MotorTask.c **** 35:MotorTask.c **** // Last encoder status. 36:MotorTask.c **** static char encoders_old; 37:MotorTask.c **** 38:MotorTask.c **** static TimerControlBlock MotorTimer; 39:MotorTask.c **** Mutex MotorLoop; 40:MotorTask.c **** extern char velocity_print; 41:MotorTask.c **** 42:MotorTask.c **** 43:MotorTask.c **** /* ------------------------------------------------------------- 44:MotorTask.c **** This is the actual task that takes does the PID at regular 45:MotorTask.c **** intervals. 46:MotorTask.c **** ------------------------------------------------------------- */ 47:MotorTask.c **** NAKEDFUNC(MotorTask) 48:MotorTask.c **** { 161 .LM1: 162 /* prologue: frame size=0 */ 163 /* prologue: naked */ 164 /* prologue end (size=0) */ 49:MotorTask.c **** signed char LeftEnc, RightEnc; 50:MotorTask.c **** float DeltaTheta, 51:MotorTask.c **** dist; 52:MotorTask.c **** 53:MotorTask.c **** // Init PID variables 54:MotorTask.c **** MotorInit(); 166 .LM2: 167 .LBB2: 168 0000 0E94 0000 call MotorInit 169 .L11: 55:MotorTask.c **** 56:MotorTask.c **** while(1) 57:MotorTask.c **** { 58:MotorTask.c **** AvrXStartTimer(&MotorTimer, PID_LOOP_DELAY); 171 .LM3: 172 0004 68E7 ldi r22,lo8(120) 173 0006 70E0 ldi r23,hi8(120) 174 0008 80E0 ldi r24,lo8(MotorTimer) 175 000a 90E0 ldi r25,hi8(MotorTimer) 176 000c 0E94 0000 call AvrXStartTimer 59:MotorTask.c **** AvrXSetSemaphore(&MotorLoop); // Synchronize other motion tasks 178 .LM4: 179 0010 80E0 ldi r24,lo8(MotorLoop) 180 0012 90E0 ldi r25,hi8(MotorLoop) 181 0014 0E94 0000 call AvrXSetSemaphore 60:MotorTask.c **** 61:MotorTask.c **** BeginCritical(); 183 .LM5: 184 /* #APP */ 185 0018 F894 cli 186 62:MotorTask.c **** LeftEnc = LeftEncoder; 188 .LM6: 189 /* #NOAPP */ 190 001a 0091 0000 lds r16,LeftEncoder 63:MotorTask.c **** LeftEncoder = 0; 192 .LM7: 193 001e 1092 0000 sts LeftEncoder,__zero_reg__ 64:MotorTask.c **** RightEnc = RightEncoder; 195 .LM8: 196 0022 E090 0000 lds r14,RightEncoder 65:MotorTask.c **** RightEncoder = 0; 198 .LM9: 199 0026 1092 0000 sts RightEncoder,__zero_reg__ 66:MotorTask.c **** EndCritical(); 201 .LM10: 202 /* #APP */ 203 002a 7894 sei 204 67:MotorTask.c **** 68:MotorTask.c **** // Now update totals. 69:MotorTask.c **** AddToLong(&Left.Encoder, LeftEnc); 206 .LM11: 207 /* #NOAPP */ 208 002c 602F mov r22,r16 209 002e 80E0 ldi r24,lo8(Left+4) 210 0030 90E0 ldi r25,hi8(Left+4) 211 0032 0E94 0000 call AddToLong 70:MotorTask.c **** AddToLong(&Right.Encoder, RightEnc); 213 .LM12: 214 0036 6E2D mov r22,r14 215 0038 80E0 ldi r24,lo8(Right+4) 216 003a 90E0 ldi r25,hi8(Right+4) 217 003c 0E94 0000 call AddToLong 71:MotorTask.c **** 72:MotorTask.c **** #if pid_plot 73:MotorTask.c **** PutCRLF(); 74:MotorTask.c **** PutDecSignedWord(Right.VelocitySetpoint>>8); 75:MotorTask.c **** PutChar(','); 76:MotorTask.c **** PutDecSignedByte(RightEnc); 77:MotorTask.c **** #endif 78:MotorTask.c **** #if pid_plot2 79:MotorTask.c **** if (plot_count < MAX_SAMPLES) 80:MotorTask.c **** { 81:MotorTask.c **** plot_array[plot_count].velocity = RightEnc; 82:MotorTask.c **** } 83:MotorTask.c **** #endif 84:MotorTask.c **** 85:MotorTask.c **** 86:MotorTask.c **** #if pid_velocity_dump 87:MotorTask.c **** if (velocity_print) 88:MotorTask.c **** { 89:MotorTask.c **** PutCRLF(); 90:MotorTask.c **** PutDecSignedByte(LeftEnc); 91:MotorTask.c **** PutChar('='); 92:MotorTask.c **** PutDecSignedByte(RightEnc); 93:MotorTask.c **** } 94:MotorTask.c **** #endif 95:MotorTask.c **** 96:MotorTask.c **** // 97:MotorTask.c **** // Update odometry readings 98:MotorTask.c **** // 99:MotorTask.c **** 100:MotorTask.c **** // Was an odometry reset requested? 101:MotorTask.c **** if (OdometryResetPending == TRUE) 219 .LM13: 220 0040 8091 0000 lds r24,OdometryResetPending 221 0044 8130 cpi r24,lo8(1) 222 0046 F1F4 brne .L5 102:MotorTask.c **** { 103:MotorTask.c **** OdometryResetPending = FALSE; // Do only once! 224 .LM14: 225 0048 1092 0000 sts OdometryResetPending,__zero_reg__ 104:MotorTask.c **** 105:MotorTask.c **** X = 0; 227 .LM15: 228 004c 80E0 ldi r24,lo8(0x0) 229 004e 90E0 ldi r25,hi8(0x0) 230 0050 A0E0 ldi r26,hlo8(0x0) 231 0052 B0E0 ldi r27,hhi8(0x0) 232 0054 8093 0000 sts X,r24 233 0058 9093 0000 sts (X)+1,r25 234 005c A093 0000 sts (X)+2,r26 235 0060 B093 0000 sts (X)+3,r27 106:MotorTask.c **** Y = 0; 237 .LM16: 238 0064 8093 0000 sts Y,r24 239 0068 9093 0000 sts (Y)+1,r25 240 006c A093 0000 sts (Y)+2,r26 241 0070 B093 0000 sts (Y)+3,r27 107:MotorTask.c **** Theta = 0; 243 .LM17: 244 0074 8093 0000 sts Theta,r24 245 0078 9093 0000 sts (Theta)+1,r25 246 007c A093 0000 sts (Theta)+2,r26 247 0080 B093 0000 sts (Theta)+3,r27 248 .L5: 108:MotorTask.c **** } 109:MotorTask.c **** 110:MotorTask.c **** DeltaTheta = (LeftEnc-RightEnc) * (DEG_PER_TICK/2.0); 250 .LM18: 251 0084 1127 clr r17 252 0086 07FD sbrc r16,7 253 0088 1095 com r17 254 008a FF24 clr r15 255 008c E7FC sbrc r14,7 256 008e F094 com r15 257 0090 C801 movw r24,r16 258 0092 8E19 sub r24,r14 259 0094 9F09 sbc r25,r15 260 0096 AA27 clr r26 261 0098 97FD sbrc r25,7 262 009a A095 com r26 263 009c BA2F mov r27,r26 264 009e BC01 movw r22,r24 265 00a0 CD01 movw r24,r26 266 00a2 0E94 0000 call __floatsisf 267 00a6 DC01 movw r26,r24 268 00a8 CB01 movw r24,r22 269 00aa 2EE2 ldi r18,lo8(0x39ae012e) 270 00ac 31E0 ldi r19,hi8(0x39ae012e) 271 00ae 4EEA ldi r20,hlo8(0x39ae012e) 272 00b0 59E3 ldi r21,hhi8(0x39ae012e) 273 00b2 BC01 movw r22,r24 274 00b4 CD01 movw r24,r26 275 00b6 0E94 0000 call __mulsf3 276 00ba 3B01 movw r6,r22 277 00bc 4C01 movw r8,r24 111:MotorTask.c **** dist = (LeftEnc+RightEnc) * (DIST_PER_TICK/2.0); 279 .LM19: 280 00be 0E0D add r16,r14 281 00c0 1F1D adc r17,r15 282 00c2 C801 movw r24,r16 283 00c4 AA27 clr r26 284 00c6 97FD sbrc r25,7 285 00c8 A095 com r26 286 00ca BA2F mov r27,r26 287 00cc BC01 movw r22,r24 288 00ce CD01 movw r24,r26 289 00d0 0E94 0000 call __floatsisf 290 00d4 DC01 movw r26,r24 291 00d6 CB01 movw r24,r22 292 00d8 2CEB ldi r18,lo8(0x3c0a1dbc) 293 00da 3DE1 ldi r19,hi8(0x3c0a1dbc) 294 00dc 4AE0 ldi r20,hlo8(0x3c0a1dbc) 295 00de 5CE3 ldi r21,hhi8(0x3c0a1dbc) 296 00e0 BC01 movw r22,r24 297 00e2 CD01 movw r24,r26 298 00e4 0E94 0000 call __mulsf3 299 00e8 5B01 movw r10,r22 300 00ea 6C01 movw r12,r24 112:MotorTask.c **** 113:MotorTask.c **** Theta += DeltaTheta; 302 .LM20: 303 00ec 8091 0000 lds r24,Theta 304 00f0 9091 0000 lds r25,(Theta)+1 305 00f4 A091 0000 lds r26,(Theta)+2 306 00f8 B091 0000 lds r27,(Theta)+3 307 00fc A401 movw r20,r8 308 00fe 9301 movw r18,r6 309 0100 BC01 movw r22,r24 310 0102 CD01 movw r24,r26 311 0104 0E94 0000 call __addsf3 312 0108 DC01 movw r26,r24 313 010a CB01 movw r24,r22 314 010c 8093 0000 sts Theta,r24 315 0110 9093 0000 sts (Theta)+1,r25 316 0114 A093 0000 sts (Theta)+2,r26 317 0118 B093 0000 sts (Theta)+3,r27 114:MotorTask.c **** X += dist * cos(Theta); 319 .LM21: 320 011c 8091 0000 lds r24,Theta 321 0120 9091 0000 lds r25,(Theta)+1 322 0124 A091 0000 lds r26,(Theta)+2 323 0128 B091 0000 lds r27,(Theta)+3 324 012c BC01 movw r22,r24 325 012e CD01 movw r24,r26 326 0130 0E94 0000 call cos 327 0134 DC01 movw r26,r24 328 0136 CB01 movw r24,r22 329 0138 9C01 movw r18,r24 330 013a AD01 movw r20,r26 331 013c C601 movw r24,r12 332 013e B501 movw r22,r10 333 0140 0E94 0000 call __mulsf3 334 0144 DC01 movw r26,r24 335 0146 CB01 movw r24,r22 336 0148 E090 0000 lds r14,X 337 014c F090 0000 lds r15,(X)+1 338 0150 0091 0000 lds r16,(X)+2 339 0154 1091 0000 lds r17,(X)+3 340 0158 9C01 movw r18,r24 341 015a AD01 movw r20,r26 342 015c C801 movw r24,r16 343 015e B701 movw r22,r14 344 0160 0E94 0000 call __addsf3 345 0164 DC01 movw r26,r24 346 0166 CB01 movw r24,r22 347 0168 8093 0000 sts X,r24 348 016c 9093 0000 sts (X)+1,r25 349 0170 A093 0000 sts (X)+2,r26 350 0174 B093 0000 sts (X)+3,r27 115:MotorTask.c **** Y -= dist * sin(Theta); 352 .LM22: 353 0178 8091 0000 lds r24,Theta 354 017c 9091 0000 lds r25,(Theta)+1 355 0180 A091 0000 lds r26,(Theta)+2 356 0184 B091 0000 lds r27,(Theta)+3 357 0188 BC01 movw r22,r24 358 018a CD01 movw r24,r26 359 018c 0E94 0000 call sin 360 0190 DC01 movw r26,r24 361 0192 CB01 movw r24,r22 362 0194 9C01 movw r18,r24 363 0196 AD01 movw r20,r26 364 0198 C601 movw r24,r12 365 019a B501 movw r22,r10 366 019c 0E94 0000 call __mulsf3 367 01a0 DC01 movw r26,r24 368 01a2 CB01 movw r24,r22 369 01a4 E090 0000 lds r14,Y 370 01a8 F090 0000 lds r15,(Y)+1 371 01ac 0091 0000 lds r16,(Y)+2 372 01b0 1091 0000 lds r17,(Y)+3 373 01b4 9C01 movw r18,r24 374 01b6 AD01 movw r20,r26 375 01b8 C801 movw r24,r16 376 01ba B701 movw r22,r14 377 01bc 0E94 0000 call __subsf3 378 01c0 DC01 movw r26,r24 379 01c2 CB01 movw r24,r22 380 01c4 8093 0000 sts Y,r24 381 01c8 9093 0000 sts (Y)+1,r25 382 01cc A093 0000 sts (Y)+2,r26 383 01d0 B093 0000 sts (Y)+3,r27 116:MotorTask.c **** Theta += DeltaTheta; 385 .LM23: 386 01d4 8091 0000 lds r24,Theta 387 01d8 9091 0000 lds r25,(Theta)+1 388 01dc A091 0000 lds r26,(Theta)+2 389 01e0 B091 0000 lds r27,(Theta)+3 390 01e4 A401 movw r20,r8 391 01e6 9301 movw r18,r6 392 01e8 BC01 movw r22,r24 393 01ea CD01 movw r24,r26 394 01ec 0E94 0000 call __addsf3 395 01f0 DC01 movw r26,r24 396 01f2 CB01 movw r24,r22 397 01f4 8093 0000 sts Theta,r24 398 01f8 9093 0000 sts (Theta)+1,r25 399 01fc A093 0000 sts (Theta)+2,r26 400 0200 B093 0000 sts (Theta)+3,r27 117:MotorTask.c **** 118:MotorTask.c **** if (Theta >= 2.0*M_PI) 402 .LM24: 403 0204 8091 0000 lds r24,Theta 404 0208 9091 0000 lds r25,(Theta)+1 405 020c A091 0000 lds r26,(Theta)+2 406 0210 B091 0000 lds r27,(Theta)+3 407 0214 2BED ldi r18,lo8(0x40c90fdb) 408 0216 3FE0 ldi r19,hi8(0x40c90fdb) 409 0218 49EC ldi r20,hlo8(0x40c90fdb) 410 021a 50E4 ldi r21,hhi8(0x40c90fdb) 411 021c BC01 movw r22,r24 412 021e CD01 movw r24,r26 413 0220 0E94 0000 call __gesf2 414 0224 8823 tst r24 415 0226 8CF0 brlt .L6 119:MotorTask.c **** Theta -= 2.0*M_PI; // Keep theta between 0-360 417 .LM25: 418 0228 8091 0000 lds r24,Theta 419 022c 9091 0000 lds r25,(Theta)+1 420 0230 A091 0000 lds r26,(Theta)+2 421 0234 B091 0000 lds r27,(Theta)+3 422 0238 2BED ldi r18,lo8(0x40c90fdb) 423 023a 3FE0 ldi r19,hi8(0x40c90fdb) 424 023c 49EC ldi r20,hlo8(0x40c90fdb) 425 023e 50E4 ldi r21,hhi8(0x40c90fdb) 426 0240 BC01 movw r22,r24 427 0242 CD01 movw r24,r26 428 0244 0E94 0000 call __subsf3 429 0248 22C0 rjmp .L12 430 .L6: 120:MotorTask.c **** else if (Theta < 0.0) 432 .LM26: 433 024a 8091 0000 lds r24,Theta 434 024e 9091 0000 lds r25,(Theta)+1 435 0252 A091 0000 lds r26,(Theta)+2 436 0256 B091 0000 lds r27,(Theta)+3 437 025a 20E0 ldi r18,lo8(0x0) 438 025c 30E0 ldi r19,hi8(0x0) 439 025e 40E0 ldi r20,hlo8(0x0) 440 0260 50E0 ldi r21,hhi8(0x0) 441 0262 BC01 movw r22,r24 442 0264 CD01 movw r24,r26 443 0266 0E94 0000 call __ltsf2 444 026a 8823 tst r24 445 026c D4F4 brge .L8 121:MotorTask.c **** Theta += 2.0*M_PI; 447 .LM27: 448 026e 8091 0000 lds r24,Theta 449 0272 9091 0000 lds r25,(Theta)+1 450 0276 A091 0000 lds r26,(Theta)+2 451 027a B091 0000 lds r27,(Theta)+3 452 027e 2BED ldi r18,lo8(0x40c90fdb) 453 0280 3FE0 ldi r19,hi8(0x40c90fdb) 454 0282 49EC ldi r20,hlo8(0x40c90fdb) 455 0284 50E4 ldi r21,hhi8(0x40c90fdb) 456 0286 BC01 movw r22,r24 457 0288 CD01 movw r24,r26 458 028a 0E94 0000 call __addsf3 459 .L12: 460 028e DC01 movw r26,r24 461 0290 CB01 movw r24,r22 462 0292 8093 0000 sts Theta,r24 463 0296 9093 0000 sts (Theta)+1,r25 464 029a A093 0000 sts (Theta)+2,r26 465 029e B093 0000 sts (Theta)+3,r27 466 .L8: 122:MotorTask.c **** 123:MotorTask.c **** // Keep integer versions around for user interface 124:MotorTask.c **** BeginCritical(); // Just in case.. 468 .LM28: 469 /* #APP */ 470 02a2 F894 cli 471 125:MotorTask.c **** iTheta = Theta * (float)(360 / (2*M_PI)); // Convert to degrees 473 .LM29: 474 /* #NOAPP */ 475 02a4 8091 0000 lds r24,Theta 476 02a8 9091 0000 lds r25,(Theta)+1 477 02ac A091 0000 lds r26,(Theta)+2 478 02b0 B091 0000 lds r27,(Theta)+3 479 02b4 20EE ldi r18,lo8(0x42652ee0) 480 02b6 3EE2 ldi r19,hi8(0x42652ee0) 481 02b8 45E6 ldi r20,hlo8(0x42652ee0) 482 02ba 52E4 ldi r21,hhi8(0x42652ee0) 483 02bc BC01 movw r22,r24 484 02be CD01 movw r24,r26 485 02c0 0E94 0000 call __mulsf3 486 02c4 DC01 movw r26,r24 487 02c6 CB01 movw r24,r22 488 02c8 BC01 movw r22,r24 489 02ca CD01 movw r24,r26 490 02cc 0E94 0000 call __fixsfsi 491 02d0 DC01 movw r26,r24 492 02d2 CB01 movw r24,r22 493 02d4 9093 0000 sts (iTheta)+1,r25 494 02d8 8093 0000 sts iTheta,r24 126:MotorTask.c **** iX = X; 496 .LM30: 497 02dc 8091 0000 lds r24,X 498 02e0 9091 0000 lds r25,(X)+1 499 02e4 A091 0000 lds r26,(X)+2 500 02e8 B091 0000 lds r27,(X)+3 501 02ec BC01 movw r22,r24 502 02ee CD01 movw r24,r26 503 02f0 0E94 0000 call __fixsfsi 504 02f4 DC01 movw r26,r24 505 02f6 CB01 movw r24,r22 506 02f8 9093 0000 sts (iX)+1,r25 507 02fc 8093 0000 sts iX,r24 127:MotorTask.c **** iY = Y; 509 .LM31: 510 0300 8091 0000 lds r24,Y 511 0304 9091 0000 lds r25,(Y)+1 512 0308 A091 0000 lds r26,(Y)+2 513 030c B091 0000 lds r27,(Y)+3 514 0310 BC01 movw r22,r24 515 0312 CD01 movw r24,r26 516 0314 0E94 0000 call __fixsfsi 517 0318 DC01 movw r26,r24 518 031a CB01 movw r24,r22 519 031c 9093 0000 sts (iY)+1,r25 520 0320 8093 0000 sts iY,r24 128:MotorTask.c **** EndCritical(); 522 .LM32: 523 /* #APP */ 524 0324 7894 sei 525 129:MotorTask.c **** 130:MotorTask.c **** DoMotion(&Left); 527 .LM33: 528 /* #NOAPP */ 529 0326 80E0 ldi r24,lo8(Left) 530 0328 90E0 ldi r25,hi8(Left) 531 032a 0E94 0000 call DoMotion 131:MotorTask.c **** DoPid(&Left); 533 .LM34: 534 032e 80E0 ldi r24,lo8(Left) 535 0330 90E0 ldi r25,hi8(Left) 536 0332 0E94 0000 call DoPid 132:MotorTask.c **** DoMotion(&Right); 538 .LM35: 539 0336 80E0 ldi r24,lo8(Right) 540 0338 90E0 ldi r25,hi8(Right) 541 033a 0E94 0000 call DoMotion 133:MotorTask.c **** DoPid(&Right); 543 .LM36: 544 033e 80E0 ldi r24,lo8(Right) 545 0340 90E0 ldi r25,hi8(Right) 546 0342 0E94 0000 call DoPid 134:MotorTask.c **** 135:MotorTask.c **** #if pid_plot 136:MotorTask.c **** PutChar(','); 137:MotorTask.c **** PutDecByte(Right.pwm_out); 138:MotorTask.c **** PutChar(13); 139:MotorTask.c **** #endif 140:MotorTask.c **** 141:MotorTask.c **** #if pid_plot2 142:MotorTask.c **** if (plot_count < MAX_SAMPLES) 143:MotorTask.c **** { 144:MotorTask.c **** plot_array[plot_count].power = Right.pwm_out; 145:MotorTask.c **** plot_count ++; 146:MotorTask.c **** } 147:MotorTask.c **** #endif 148:MotorTask.c **** 149:MotorTask.c **** // Apply calculated pwm outputs simultaneously! 150:MotorTask.c **** PWM_R=Right.pwm_out; 548 .LM37: 549 0346 8091 0000 lds r24,Right+18 550 034a 8ABD out 74-0x20,r24 151:MotorTask.c **** PWM_L=Left.pwm_out; 552 .LM38: 553 034c 8091 0000 lds r24,Left+18 554 0350 88BD out 72-0x20,r24 152:MotorTask.c **** 153:MotorTask.c **** #if pid_debug5 154:MotorTask.c **** PutCRLF(); 155:MotorTask.c **** #endif 156:MotorTask.c **** 157:MotorTask.c **** AvrXWaitTimer(&MotorTimer); 556 .LM39: 557 0352 80E0 ldi r24,lo8(MotorTimer) 558 0354 90E0 ldi r25,hi8(MotorTimer) 559 0356 0E94 0000 call AvrXWaitTimer 560 035a 54CE rjmp .L11 158:MotorTask.c **** } 159:MotorTask.c **** } 562 .LM40: 563 .LBE2: 564 /* epilogue: frame size=0 */ 565 /* epilogue: naked */ 566 /* epilogue end (size=0) */ 567 /* function MotorTask size 444 (444) */ 573 .Lscope0: 576 .global MotorInit 578 MotorInit: 160:MotorTask.c **** 161:MotorTask.c **** void MotorInit(void) 162:MotorTask.c **** { 580 .LM41: 581 /* prologue: frame size=0 */ 582 /* prologue end (size=0) */ 163:MotorTask.c **** // Initialize Motion Control data 164:MotorTask.c **** 165:MotorTask.c **** //Kp = K_P; //eeprom_rb((unsigned)&Kp); 166:MotorTask.c **** //Kd = K_D; //eeprom_rb((unsigned)&Kd); 167:MotorTask.c **** //Ki = K_I; //eeprom_rb((unsigned)&Ki); 168:MotorTask.c **** //Ko = K_O; //eeprom_rb((unsigned)&Ko); 169:MotorTask.c **** Left.Acceleration = Right.Acceleration = K_ACCEL; //eeprom_rw((unsigned)&Acceleration); 584 .LM42: 585 035c 80E0 ldi r24,lo8(256) 586 035e 91E0 ldi r25,hi8(256) 587 0360 9093 0000 sts (Right+16)+1,r25 588 0364 8093 0000 sts Right+16,r24 589 0368 8091 0000 lds r24,Right+16 590 036c 9091 0000 lds r25,(Right+16)+1 591 0370 9093 0000 sts (Left+16)+1,r25 592 0374 8093 0000 sts Left+16,r24 170:MotorTask.c **** Left.Velocity = Right.Velocity = 0; 594 .LM43: 595 0378 1092 0000 sts (Right+12)+1,__zero_reg__ 596 037c 1092 0000 sts Right+12,__zero_reg__ 597 0380 8091 0000 lds r24,Right+12 598 0384 9091 0000 lds r25,(Right+12)+1 599 0388 9093 0000 sts (Left+12)+1,r25 600 038c 8093 0000 sts Left+12,r24 171:MotorTask.c **** Left.VelocitySetpoint = Right.VelocitySetpoint = 0; 602 .LM44: 603 0390 1092 0000 sts (Right+14)+1,__zero_reg__ 604 0394 1092 0000 sts Right+14,__zero_reg__ 605 0398 8091 0000 lds r24,Right+14 606 039c 9091 0000 lds r25,(Right+14)+1 607 03a0 9093 0000 sts (Left+14)+1,r25 608 03a4 8093 0000 sts Left+14,r24 172:MotorTask.c **** Left.PrevErr = Right.PrevErr = 0; 610 .LM45: 611 03a8 1092 0000 sts (Right+8)+1,__zero_reg__ 612 03ac 1092 0000 sts Right+8,__zero_reg__ 613 03b0 8091 0000 lds r24,Right+8 614 03b4 9091 0000 lds r25,(Right+8)+1 615 03b8 9093 0000 sts (Left+8)+1,r25 616 03bc 8093 0000 sts Left+8,r24 173:MotorTask.c **** Left.Ierror = Right.Ierror = 0; 618 .LM46: 619 03c0 1092 0000 sts (Right+10)+1,__zero_reg__ 620 03c4 1092 0000 sts Right+10,__zero_reg__ 621 03c8 8091 0000 lds r24,Right+10 622 03cc 9091 0000 lds r25,(Right+10)+1 623 03d0 9093 0000 sts (Left+10)+1,r25 624 03d4 8093 0000 sts Left+10,r24 174:MotorTask.c **** Left.Encoder = Right.Encoder = 0; 626 .LM47: 627 03d8 1092 0000 sts Right+4,__zero_reg__ 628 03dc 1092 0000 sts (Right+4)+1,__zero_reg__ 629 03e0 1092 0000 sts (Right+4)+2,__zero_reg__ 630 03e4 1092 0000 sts (Right+4)+3,__zero_reg__ 631 03e8 8091 0000 lds r24,Right+4 632 03ec 9091 0000 lds r25,(Right+4)+1 633 03f0 A091 0000 lds r26,(Right+4)+2 634 03f4 B091 0000 lds r27,(Right+4)+3 635 03f8 8093 0000 sts Left+4,r24 636 03fc 9093 0000 sts (Left+4)+1,r25 637 0400 A093 0000 sts (Left+4)+2,r26 638 0404 B093 0000 sts (Left+4)+3,r27 175:MotorTask.c **** 176:MotorTask.c **** OdometryResetPending = TRUE; // Reset on startup. 640 .LM48: 641 0408 81E0 ldi r24,lo8(1) 642 040a 8093 0000 sts OdometryResetPending,r24 177:MotorTask.c **** 178:MotorTask.c **** // Init encoder status 179:MotorTask.c **** encoders_old = ENCODER_PORT & ENCODER_MASK; 644 .LM49: 645 040e 81B1 in r24,33-0x20 646 0410 807F andi r24,lo8(-16) 647 0412 8093 0000 sts encoders_old,r24 180:MotorTask.c **** } 649 .LM50: 650 /* epilogue: frame size=0 */ 651 0416 0895 ret 652 /* epilogue end (size=1) */ 653 /* function MotorInit size 94 (93) */ 655 .Lscope1: 659 .global DoMotion 661 DoMotion: 181:MotorTask.c **** 182:MotorTask.c **** 183:MotorTask.c **** /* --------------------------------------------------------- 184:MotorTask.c **** void DoMotion(MotorInfo *p) 185:MotorTask.c **** 186:MotorTask.c **** Called at the loop rate to add "velocity" to the setpoint thus 187:MotorTask.c **** effecting a motion. 188:MotorTask.c **** 189:MotorTask.c **** Velocity is ramped up and down by "acceleration" 190:MotorTask.c **** 191:MotorTask.c **** Velocity and Acceleration are 8.8 numbers. Velocity 192:MotorTask.c **** is added to the setpoint (an 24.8 number) 193:MotorTask.c **** ----------------------------------------------------------- */ 194:MotorTask.c **** 195:MotorTask.c **** void DoMotion(MotorInfo *p) 196:MotorTask.c **** { 663 .LM51: 664 /* prologue: frame size=0 */ 665 /* prologue end (size=0) */ 666 0418 FC01 movw r30,r24 197:MotorTask.c **** if (p->Velocity < p->VelocitySetpoint) 668 .LM52: 669 041a 2485 ldd r18,Z+12 670 041c 3585 ldd r19,Z+13 671 041e 8685 ldd r24,Z+14 672 0420 9785 ldd r25,Z+15 673 0422 2817 cp r18,r24 674 0424 3907 cpc r19,r25 675 0426 84F4 brge .L15 198:MotorTask.c **** { 199:MotorTask.c **** p->Velocity += p->Acceleration; 677 .LM53: 678 0428 8485 ldd r24,Z+12 679 042a 9585 ldd r25,Z+13 680 042c 2089 ldd r18,Z+16 681 042e 3189 ldd r19,Z+17 682 0430 820F add r24,r18 683 0432 931F adc r25,r19 684 0434 8487 std Z+12,r24 685 0436 9587 std Z+13,r25 200:MotorTask.c **** if (p->Velocity > p->VelocitySetpoint) 687 .LM54: 688 0438 2485 ldd r18,Z+12 689 043a 3585 ldd r19,Z+13 690 043c 8685 ldd r24,Z+14 691 043e 9785 ldd r25,Z+15 692 0440 8217 cp r24,r18 693 0442 9307 cpc r25,r19 694 0444 A4F4 brge .L17 201:MotorTask.c **** p->Velocity = p->VelocitySetpoint; 696 .LM55: 697 0446 0FC0 rjmp .L19 698 .L15: 202:MotorTask.c **** } 203:MotorTask.c **** else 204:MotorTask.c **** { 205:MotorTask.c **** p->Velocity -= p->Acceleration; 700 .LM56: 701 0448 8485 ldd r24,Z+12 702 044a 9585 ldd r25,Z+13 703 044c 2089 ldd r18,Z+16 704 044e 3189 ldd r19,Z+17 705 0450 821B sub r24,r18 706 0452 930B sbc r25,r19 707 0454 8487 std Z+12,r24 708 0456 9587 std Z+13,r25 206:MotorTask.c **** if (p->Velocity < p->VelocitySetpoint) 710 .LM57: 711 0458 2485 ldd r18,Z+12 712 045a 3585 ldd r19,Z+13 713 045c 8685 ldd r24,Z+14 714 045e 9785 ldd r25,Z+15 715 0460 2817 cp r18,r24 716 0462 3907 cpc r19,r25 717 0464 24F4 brge .L17 207:MotorTask.c **** p->Velocity = p->VelocitySetpoint; 719 .LM58: 720 .L19: 721 0466 8685 ldd r24,Z+14 722 0468 9785 ldd r25,Z+15 723 046a 8487 std Z+12,r24 724 046c 9587 std Z+13,r25 725 .L17: 208:MotorTask.c **** } 209:MotorTask.c **** AddToPosition(p, p->Velocity); 727 .LM59: 728 046e 6485 ldd r22,Z+12 729 0470 7585 ldd r23,Z+13 730 0472 CF01 movw r24,r30 731 0474 0E94 0000 call AddToPosition 210:MotorTask.c **** } 733 .LM60: 734 /* epilogue: frame size=0 */ 735 0478 0895 ret 736 /* epilogue end (size=1) */ 737 /* function DoMotion size 49 (48) */ 739 .Lscope2: 744 .global AddToPosition 746 AddToPosition: 211:MotorTask.c **** 212:MotorTask.c **** 213:MotorTask.c **** /* ------------------------------------------------------------- 214:MotorTask.c **** void AddToPosition(pMotorInfo, int) 215:MotorTask.c **** 216:MotorTask.c **** Add an 8.8 value to the current setpoint. 217:MotorTask.c **** ------------------------------------------------------------- */ 218:MotorTask.c **** void AddToPosition(pMotorInfo p, int o) 219:MotorTask.c **** { 748 .LM61: 749 /* prologue: frame size=0 */ 750 /* prologue end (size=0) */ 751 047a FC01 movw r30,r24 752 047c CB01 movw r24,r22 220:MotorTask.c **** unsigned char s; 221:MotorTask.c **** 222:MotorTask.c **** s = SREG; // Save processor status 754 .LM62: 755 .LBB3: 756 047e 6FB7 in r22,95-0x20 223:MotorTask.c **** BeginCritical(); 758 .LM63: 759 /* #APP */ 760 0480 F894 cli 761 224:MotorTask.c **** 225:MotorTask.c **** p->Setpoint += o; // Set point is 24.8 number 763 .LM64: 764 /* #NOAPP */ 765 0482 9C01 movw r18,r24 766 0484 4427 clr r20 767 0486 37FD sbrc r19,7 768 0488 4095 com r20 769 048a 542F mov r21,r20 770 048c 8081 ld r24,Z 771 048e 9181 ldd r25,Z+1 772 0490 A281 ldd r26,Z+2 773 0492 B381 ldd r27,Z+3 774 0494 820F add r24,r18 775 0496 931F adc r25,r19 776 0498 A41F adc r26,r20 777 049a B51F adc r27,r21 778 049c 8083 st Z,r24 779 049e 9183 std Z+1,r25 780 04a0 A283 std Z+2,r26 781 04a2 B383 std Z+3,r27 226:MotorTask.c **** 227:MotorTask.c **** SREG = s; // Restore processor status 783 .LM65: 784 04a4 6FBF out 95-0x20,r22 228:MotorTask.c **** } 786 .LM66: 787 .LBE3: 788 /* epilogue: frame size=0 */ 789 04a6 0895 ret 790 /* epilogue end (size=1) */ 791 /* function AddToPosition size 26 (25) */ 796 .Lscope3: 800 .global DoPid 802 DoPid: 229:MotorTask.c **** 230:MotorTask.c **** /* ------------------------------------------------------------- 231:MotorTask.c **** uint8_t DoPid(pMotorInfo) 232:MotorTask.c **** 233:MotorTask.c **** The PID calculations. 234:MotorTask.c **** ------------------------------------------------------------- */ 235:MotorTask.c **** void DoPid(MotorInfo *p) 236:MotorTask.c **** { 804 .LM67: 805 /* prologue: frame size=0 */ 806 04a8 CF93 push r28 807 04aa DF93 push r29 808 /* prologue end (size=2) */ 809 04ac FC01 movw r30,r24 237:MotorTask.c **** int Perror, output; 238:MotorTask.c **** 239:MotorTask.c **** Perror = (p->Setpoint>>8) - p->Encoder; // Set point is an 24.8 number (+/- 1 mile..) 811 .LM68: 812 .LBB4: 813 04ae 8081 ld r24,Z 814 04b0 9181 ldd r25,Z+1 815 04b2 A281 ldd r26,Z+2 816 04b4 B381 ldd r27,Z+3 817 04b6 292F mov r18,r25 818 04b8 3A2F mov r19,r26 819 04ba 4B2F mov r20,r27 820 04bc 5527 clr r21 821 04be 47FD sbrc r20,7 822 04c0 5A95 dec r21 823 04c2 8481 ldd r24,Z+4 824 04c4 9581 ldd r25,Z+5 825 04c6 A681 ldd r26,Z+6 826 04c8 B781 ldd r27,Z+7 827 04ca E901 movw r28,r18 828 04cc C81B sub r28,r24 829 04ce D90B sbc r29,r25 240:MotorTask.c **** 241:MotorTask.c **** #if pid_debug2 242:MotorTask.c **** //Debug 243:MotorTask.c **** PutCRLF(); 244:MotorTask.c **** PutChar('E'); 245:MotorTask.c **** PutChar('='); 246:MotorTask.c **** PutHexWord(Perror); 247:MotorTask.c **** PutChar('('); 248:MotorTask.c **** PutDecSignedWord(Perror); 249:MotorTask.c **** PutChar(')'); 250:MotorTask.c **** #endif 251:MotorTask.c **** 252:MotorTask.c **** // Derivative error is the delta Perror over Td (scale output to permit use of fraction in K co 253:MotorTask.c **** output = (R_ParameterTable.Kp*Perror + R_ParameterTable.Kd*(Perror - p->PrevErr) + R_ParameterT 831 .LM69: 832 04d0 8091 0000 lds r24,R_ParameterTable+3 833 04d4 9927 clr r25 834 04d6 8C9F mul r24,r28 835 04d8 A001 movw r20,r0 836 04da 8D9F mul r24,r29 837 04dc 500D add r21,r0 838 04de 9C9F mul r25,r28 839 04e0 500D add r21,r0 840 04e2 1124 clr r1 841 04e4 8091 0000 lds r24,R_ParameterTable+5 842 04e8 282F mov r18,r24 843 04ea 3327 clr r19 844 04ec 8085 ldd r24,Z+8 845 04ee 9185 ldd r25,Z+9 846 04f0 BE01 movw r22,r28 847 04f2 681B sub r22,r24 848 04f4 790B sbc r23,r25 849 04f6 CB01 movw r24,r22 850 04f8 289F mul r18,r24 851 04fa B001 movw r22,r0 852 04fc 299F mul r18,r25 853 04fe 700D add r23,r0 854 0500 389F mul r19,r24 855 0502 700D add r23,r0 856 0504 1124 clr r1 857 0506 460F add r20,r22 858 0508 571F adc r21,r23 859 050a 8091 0000 lds r24,R_ParameterTable+4 860 050e 282F mov r18,r24 861 0510 3327 clr r19 862 0512 8285 ldd r24,Z+10 863 0514 9385 ldd r25,Z+11 864 0516 289F mul r18,r24 865 0518 B001 movw r22,r0 866 051a 299F mul r18,r25 867 051c 700D add r23,r0 868 051e 389F mul r19,r24 869 0520 700D add r23,r0 870 0522 1124 clr r1 871 0524 460F add r20,r22 872 0526 571F adc r21,r23 873 0528 CA01 movw r24,r20 874 052a 6AE0 ldi r22,lo8(10) 875 052c 70E0 ldi r23,hi8(10) 876 052e 0E94 0000 call __divmodhi4 254:MotorTask.c **** p->PrevErr = Perror; 878 .LM70: 879 0532 C087 std Z+8,r28 880 0534 D187 std Z+9,r29 255:MotorTask.c **** 256:MotorTask.c **** // Accumulate Integral error *or* Limit output. 257:MotorTask.c **** // Stop accumulating when output saturates 258:MotorTask.c **** 259:MotorTask.c **** if (output >= MAX_PWM_OUTPUT) 882 .LM71: 883 0536 6F37 cpi r22,127 884 0538 7105 cpc r23,__zero_reg__ 885 053a 1CF0 brlt .L22 260:MotorTask.c **** output = MAX_PWM_OUTPUT; 887 .LM72: 888 053c 6FE7 ldi r22,lo8(127) 889 053e 70E0 ldi r23,hi8(127) 890 0540 0DC0 rjmp .L23 891 .L22: 261:MotorTask.c **** else if (output <= -MAX_PWM_OUTPUT) 893 .LM73: 894 0542 8FEF ldi r24,hi8(-126) 895 0544 6238 cpi r22,lo8(-126) 896 0546 7807 cpc r23,r24 897 0548 1CF4 brge .L24 262:MotorTask.c **** output = -MAX_PWM_OUTPUT; 899 .LM74: 900 054a 61E8 ldi r22,lo8(-127) 901 054c 7FEF ldi r23,hi8(-127) 902 054e 06C0 rjmp .L23 903 .L24: 263:MotorTask.c **** else 264:MotorTask.c **** p->Ierror += Perror; 905 .LM75: 906 0550 8285 ldd r24,Z+10 907 0552 9385 ldd r25,Z+11 908 0554 8C0F add r24,r28 909 0556 9D1F adc r25,r29 910 0558 8287 std Z+10,r24 911 055a 9387 std Z+11,r25 912 .L23: 265:MotorTask.c **** 266:MotorTask.c **** // Convert output to 0xFF-0x7F-0x00 (Fwd, netural, Rev) 267:MotorTask.c **** // locked anti-phase drive. 268:MotorTask.c **** p->pwm_out = (unsigned char)(output - (MAX_PWM_OUTPUT+1)); 914 .LM76: 915 055c 862F mov r24,r22 916 055e 8058 subi r24,lo8(-(-128)) 917 0560 828B std Z+18,r24 269:MotorTask.c **** } 919 .LM77: 920 .LBE4: 921 /* epilogue: frame size=0 */ 922 0562 DF91 pop r29 923 0564 CF91 pop r28 924 0566 0895 ret 925 /* epilogue end (size=3) */ 926 /* function DoPid size 96 (91) */ 932 .Lscope4: 936 .global motors 938 motors: 270:MotorTask.c **** 271:MotorTask.c **** // Function Name: motors() 272:MotorTask.c **** // 273:MotorTask.c **** // Description: Apply motor commands (from arbitration task to motors). 274:MotorTask.c **** // 275:MotorTask.c **** // WARNING: the velocity setpoints below are directly proportional to the PID sampling rate! Keep t 276:MotorTask.c **** void motors(BehaviourInfo *pInfo) 277:MotorTask.c **** { 940 .LM78: 941 /* prologue: frame size=0 */ 942 0568 CF93 push r28 943 056a DF93 push r29 944 /* prologue end (size=2) */ 945 056c EC01 movw r28,r24 278:MotorTask.c **** if (pInfo->type == ABSOLUTE) 947 .LM79: 948 056e 8885 ldd r24,Y+8 949 0570 2B81 ldd r18,Y+3 950 0572 3C81 ldd r19,Y+4 951 0574 8130 cpi r24,lo8(1) 952 0576 B1F4 brne .L27 279:MotorTask.c **** { 280:MotorTask.c **** // Check for enhanced precision mode request and act accordingly. 281:MotorTask.c **** Left.VelocitySetpoint = (pInfo->precision == TRUE) ? pInfo->output_l : pInfo->output_l<<8; 954 .LM80: 955 0578 A901 movw r20,r18 956 057a 8F81 ldd r24,Y+7 957 057c 8130 cpi r24,lo8(1) 958 057e 11F0 breq .L28 959 0580 522F mov r21,r18 960 0582 4427 clr r20 961 .L28: 962 0584 5093 0000 sts (Left+14)+1,r21 963 0588 4093 0000 sts Left+14,r20 282:MotorTask.c **** Right.VelocitySetpoint = (pInfo->precision == TRUE) ? pInfo->output_r : pInfo->output_r<<8; 965 .LM81: 966 058c 2D81 ldd r18,Y+5 967 058e 3E81 ldd r19,Y+6 968 0590 8F81 ldd r24,Y+7 969 0592 8130 cpi r24,lo8(1) 970 0594 11F0 breq .L29 971 0596 322F mov r19,r18 972 0598 2227 clr r18 973 .L29: 974 059a 3093 0000 sts (Right+14)+1,r19 975 059e 2093 0000 sts Right+14,r18 976 05a2 10C0 rjmp .L26 977 .L27: 283:MotorTask.c **** } 284:MotorTask.c **** else 285:MotorTask.c **** { 286:MotorTask.c **** AddToPosition(&Left, pInfo->output_l<<8); 979 .LM82: 980 05a4 922F mov r25,r18 981 05a6 8827 clr r24 982 05a8 BC01 movw r22,r24 983 05aa 80E0 ldi r24,lo8(Left) 984 05ac 90E0 ldi r25,hi8(Left) 985 05ae 0E94 0000 call AddToPosition 287:MotorTask.c **** AddToPosition(&Right, pInfo->output_r<<8); 987 .LM83: 988 05b2 8D81 ldd r24,Y+5 989 05b4 9E81 ldd r25,Y+6 990 05b6 982F mov r25,r24 991 05b8 8827 clr r24 992 05ba BC01 movw r22,r24 993 05bc 80E0 ldi r24,lo8(Right) 994 05be 90E0 ldi r25,hi8(Right) 995 05c0 0E94 0000 call AddToPosition 288:MotorTask.c **** } 289:MotorTask.c **** } 997 .LM84: 998 .L26: 999 /* epilogue: frame size=0 */ 1000 05c4 DF91 pop r29 1001 05c6 CF91 pop r28 1002 05c8 0895 ret 1003 /* epilogue end (size=3) */ 1004 /* function motors size 49 (44) */ 1006 .Lscope5: 1011 .global AddToLong 1013 AddToLong: 290:MotorTask.c **** 291:MotorTask.c **** 292:MotorTask.c **** // Function Name: SynchWithMotion() 293:MotorTask.c **** // 294:MotorTask.c **** // Description: Synchronize with the motor control loop 295:MotorTask.c **** // 296:MotorTask.c **** // 297:MotorTask.c **** inline void SynchWithMotion(void) 298:MotorTask.c **** { 299:MotorTask.c **** AvrXWaitSemaphore(&MotorLoop); 300:MotorTask.c **** } 301:MotorTask.c **** 302:MotorTask.c **** void AddToLong(volatile long *p, signed char o) // Used by AddToPosition & AddToSetpoint macros 303:MotorTask.c **** { 1015 .LM85: 1016 /* prologue: frame size=0 */ 1017 /* prologue end (size=0) */ 1018 05ca FC01 movw r30,r24 1019 05cc 862F mov r24,r22 304:MotorTask.c **** unsigned char s; 305:MotorTask.c **** 306:MotorTask.c **** s = SREG; // Save processor status 1021 .LM86: 1022 .LBB5: 1023 05ce 6FB7 in r22,95-0x20 307:MotorTask.c **** BeginCritical(); 1025 .LM87: 1026 /* #APP */ 1027 05d0 F894 cli 1028 308:MotorTask.c **** *p += o; 1030 .LM88: 1031 /* #NOAPP */ 1032 05d2 282F mov r18,r24 1033 05d4 3327 clr r19 1034 05d6 27FD sbrc r18,7 1035 05d8 3095 com r19 1036 05da 432F mov r20,r19 1037 05dc 532F mov r21,r19 1038 05de 8081 ld r24,Z 1039 05e0 9181 ldd r25,Z+1 1040 05e2 A281 ldd r26,Z+2 1041 05e4 B381 ldd r27,Z+3 1042 05e6 820F add r24,r18 1043 05e8 931F adc r25,r19 1044 05ea A41F adc r26,r20 1045 05ec B51F adc r27,r21 1046 05ee 8083 st Z,r24 1047 05f0 9183 std Z+1,r25 1048 05f2 A283 std Z+2,r26 1049 05f4 B383 std Z+3,r27 309:MotorTask.c **** 310:MotorTask.c **** SREG = s; // Restore processor status 1051 .LM89: 1052 05f6 6FBF out 95-0x20,r22 311:MotorTask.c **** } 1054 .LM90: 1055 .LBE5: 1056 /* epilogue: frame size=0 */ 1057 05f8 0895 ret 1058 /* epilogue end (size=1) */ 1059 /* function AddToLong size 27 (26) */ 1064 .Lscope6: 1067 .global WaitForMotionToStop 1069 WaitForMotionToStop: 312:MotorTask.c **** 313:MotorTask.c **** // Function Name: EmergencyStop() 314:MotorTask.c **** // 315:MotorTask.c **** // Description: Break RIGHT NOW! (do not deccellerate!) 316:MotorTask.c **** // 317:MotorTask.c **** // 318:MotorTask.c **** inline void EmergencyStop(void) 319:MotorTask.c **** { 320:MotorTask.c **** Left.Velocity = Right.Velocity = 0; 321:MotorTask.c **** Left.VelocitySetpoint = Right.VelocitySetpoint = 0; 322:MotorTask.c **** //Left.PrevErr = Right.PrevErr = 0; 323:MotorTask.c **** //Left.Ierror = Right.Ierror = 0; 324:MotorTask.c **** //Left.Encoder = Right.Encoder = 0; 325:MotorTask.c **** //PWM_R = MAX_PWM_OUTPUT; 326:MotorTask.c **** //PWM_L = MAX_PWM_OUTPUT; 327:MotorTask.c **** } 328:MotorTask.c **** 329:MotorTask.c **** // 330:MotorTask.c **** // Decode quadrature signal from Left/Right encoders 331:MotorTask.c **** // (based on XOR algorithm from http://www.emesystems.com/BS2fsm.htm) 332:MotorTask.c **** // 333:MotorTask.c **** inline void QuadratureDecoder(void) 334:MotorTask.c **** { 335:MotorTask.c **** unsigned char new = PINE & ENCODER_MASK; 336:MotorTask.c **** unsigned char x = new ^ encoders_old; 337:MotorTask.c **** x |= (x>>1 & 0x55) | (x<<1 & 0xAA); 338:MotorTask.c **** x &= ((new>>1 ^ encoders_old) & 0x55) | ((new<<1 ^ encoders_old) & 0xAA); 339:MotorTask.c **** encoders_old = new; 340:MotorTask.c **** 341:MotorTask.c **** // Decode 342:MotorTask.c **** if (x & ENCODER_L_A) LeftEncoder++; 343:MotorTask.c **** if (x & ENCODER_L_B) LeftEncoder--; 344:MotorTask.c **** if (x & ENCODER_R_A) RightEncoder++; 345:MotorTask.c **** if (x & ENCODER_R_B) RightEncoder--; 346:MotorTask.c **** } 347:MotorTask.c **** 348:MotorTask.c **** // 349:MotorTask.c **** // Function Name: WaitForMotionToStop() 350:MotorTask.c **** // 351:MotorTask.c **** void WaitForMotionToStop(void) 352:MotorTask.c **** { 1071 .LM91: 1072 /* prologue: frame size=0 */ 1073 /* prologue end (size=0) */ 353:MotorTask.c **** while (Left.Velocity || Right.Velocity) 1075 .LM92: 1076 05fa 8091 0000 lds r24,Left+12 1077 05fe 9091 0000 lds r25,(Left+12)+1 1078 0602 892B or r24,r25 1079 0604 31F4 brne .L40 1080 0606 8091 0000 lds r24,Right+12 1081 060a 9091 0000 lds r25,(Right+12)+1 1082 060e 892B or r24,r25 1083 0610 81F0 breq .L39 1084 .L40: 1086 .LM93: 1087 .LBB6: 1088 0612 80E0 ldi r24,lo8(MotorLoop) 1089 0614 90E0 ldi r25,hi8(MotorLoop) 1090 0616 0E94 0000 call AvrXWaitSemaphore 1091 .LBE6: 1092 061a 8091 0000 lds r24,Left+12 1093 061e 9091 0000 lds r25,(Left+12)+1 1094 0622 892B or r24,r25 1095 0624 B1F7 brne .L40 1096 0626 8091 0000 lds r24,Right+12 1097 062a 9091 0000 lds r25,(Right+12)+1 1098 062e 892B or r24,r25 1099 0630 81F7 brne .L40 1100 .L39: 354:MotorTask.c **** { 355:MotorTask.c **** SynchWithMotion(); 356:MotorTask.c **** } 357:MotorTask.c **** } 1102 .LM94: 1103 0632 0895 ret 1104 /* epilogue: frame size=0 */ 1105 /* epilogue: noreturn */ 1106 /* epilogue end (size=0) */ 1107 /* function WaitForMotionToStop size 29 (29) */ 1109 .Lscope7: 1112 .global ResetOdometry 1114 ResetOdometry: 358:MotorTask.c **** 359:MotorTask.c **** // 360:MotorTask.c **** // Function Name: ResetOdometry() 361:MotorTask.c **** // 362:MotorTask.c **** void ResetOdometry(void) 363:MotorTask.c **** { 1116 .LM95: 1117 /* prologue: frame size=0 */ 1118 /* prologue end (size=0) */ 364:MotorTask.c **** OdometryResetPending = TRUE; 1120 .LM96: 1121 0634 81E0 ldi r24,lo8(1) 1122 0636 8093 0000 sts OdometryResetPending,r24 365:MotorTask.c **** } 1124 .LM97: 1125 /* epilogue: frame size=0 */ 1126 063a 0895 ret 1127 /* epilogue end (size=1) */ 1128 /* function ResetOdometry size 4 (3) */ 1130 .Lscope8: 1133 .global SynchWithMotion 1135 SynchWithMotion: 1137 .LM98: 1138 /* prologue: frame size=0 */ 1139 /* prologue end (size=0) */ 1141 .LM99: 1142 063c 80E0 ldi r24,lo8(MotorLoop) 1143 063e 90E0 ldi r25,hi8(MotorLoop) 1144 0640 0E94 0000 call AvrXWaitSemaphore 1146 .LM100: 1147 /* epilogue: frame size=0 */ 1148 0644 0895 ret 1149 /* epilogue end (size=1) */ 1150 /* function SynchWithMotion size 5 (4) */ 1152 .Lscope9: 1155 .global EmergencyStop 1157 EmergencyStop: 1159 .LM101: 1160 /* prologue: frame size=0 */ 1161 /* prologue end (size=0) */ 1163 .LM102: 1164 0646 1092 0000 sts (Right+12)+1,__zero_reg__ 1165 064a 1092 0000 sts Right+12,__zero_reg__ 1166 064e 8091 0000 lds r24,Right+12 1167 0652 9091 0000 lds r25,(Right+12)+1 1168 0656 9093 0000 sts (Left+12)+1,r25 1169 065a 8093 0000 sts Left+12,r24 1171 .LM103: 1172 065e 1092 0000 sts (Right+14)+1,__zero_reg__ 1173 0662 1092 0000 sts Right+14,__zero_reg__ 1174 0666 8091 0000 lds r24,Right+14 1175 066a 9091 0000 lds r25,(Right+14)+1 1176 066e 9093 0000 sts (Left+14)+1,r25 1177 0672 8093 0000 sts Left+14,r24 1179 .LM104: 1180 /* epilogue: frame size=0 */ 1181 0676 0895 ret 1182 /* epilogue end (size=1) */ 1183 /* function EmergencyStop size 25 (24) */ 1185 .Lscope10: 1188 .global QuadratureDecoder 1190 QuadratureDecoder: 1192 .LM105: 1193 /* prologue: frame size=0 */ 1194 /* prologue end (size=0) */ 1196 .LM106: 1197 .LBB7: 1198 0678 41B1 in r20,33-0x20 1199 067a 407F andi r20,lo8(-16) 1201 .LM107: 1202 067c 3091 0000 lds r19,encoders_old 1203 0680 542F mov r21,r20 1204 0682 5327 eor r21,r19 1206 .LM108: 1207 0684 952F mov r25,r21 1208 0686 9695 lsr r25 1209 0688 9575 andi r25,lo8(85) 1210 068a 852F mov r24,r21 1211 068c 880F lsl r24 1212 068e 8A7A andi r24,lo8(-86) 1213 0690 982B or r25,r24 1214 0692 592B or r21,r25 1216 .LM109: 1217 0694 242F mov r18,r20 1218 0696 2695 lsr r18 1219 0698 2327 eor r18,r19 1220 069a 2575 andi r18,lo8(85) 1221 069c 842F mov r24,r20 1222 069e 9927 clr r25 1223 06a0 880F lsl r24 1224 06a2 991F rol r25 1225 06a4 3827 eor r19,r24 1226 06a6 3A7A andi r19,lo8(-86) 1227 06a8 232B or r18,r19 1228 06aa 5223 and r21,r18 1230 .LM110: 1231 06ac 4093 0000 sts encoders_old,r20 1233 .LM111: 1234 06b0 252F mov r18,r21 1235 06b2 3327 clr r19 1236 06b4 24FF sbrs r18,4 1237 06b6 05C0 rjmp .L50 1238 06b8 8091 0000 lds r24,LeftEncoder 1239 06bc 8F5F subi r24,lo8(-(1)) 1240 06be 8093 0000 sts LeftEncoder,r24 1241 .L50: 1243 .LM112: 1244 06c2 25FF sbrs r18,5 1245 06c4 05C0 rjmp .L51 1246 06c6 8091 0000 lds r24,LeftEncoder 1247 06ca 8150 subi r24,lo8(-(-1)) 1248 06cc 8093 0000 sts LeftEncoder,r24 1249 .L51: 1251 .LM113: 1252 06d0 26FF sbrs r18,6 1253 06d2 05C0 rjmp .L52 1254 06d4 8091 0000 lds r24,RightEncoder 1255 06d8 8F5F subi r24,lo8(-(1)) 1256 06da 8093 0000 sts RightEncoder,r24 1257 .L52: 1259 .LM114: 1260 06de 57FF sbrs r21,7 1261 06e0 05C0 rjmp .L49 1262 06e2 8091 0000 lds r24,RightEncoder 1263 06e6 8150 subi r24,lo8(-(-1)) 1264 06e8 8093 0000 sts RightEncoder,r24 1266 .LM115: 1267 .L49: 1268 06ec 0895 ret 1269 .LBE7: 1270 /* epilogue: frame size=0 */ 1271 /* epilogue: noreturn */ 1272 /* epilogue end (size=0) */ 1273 /* function QuadratureDecoder size 59 (59) */ 1279 .Lscope11: 1281 .comm Left,19,1 1282 .comm Right,19,1 1283 .comm MotorLoop,2,1 1284 .comm MotorTaskStk,75,1 1285 .comm MotorTaskPid,6,1 1286 .comm OdometryResetPending,1,1 1287 .lcomm encoders_old,1 1288 .lcomm MotorTimer,6 1306 .text 1308 Letext: 1309 /* File "MotorTask.c": code 907 = 0x038b ( 890), prologues 4, epilogues 13 */ DEFINED SYMBOLS *ABS*:00000000 MotorTask.c *ABS*:0000003f __SREG__ *ABS*:0000003e __SP_H__ *ABS*:0000003d __SP_L__ *ABS*:00000000 __tmp_reg__ *ABS*:00000001 __zero_reg__ C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:101 .progmem.data:00000000 MotorTaskTcb *COM*:0000004b MotorTaskStk C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:159 .text:00000000 MotorTask *COM*:00000006 MotorTaskPid C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:111 .bss:00000000 RightEncoder C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:117 .bss:00000001 LeftEncoder C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:123 .bss:00000002 X C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:129 .bss:00000006 Y C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:135 .bss:0000000a Theta C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:141 .bss:0000000e iX C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:147 .bss:00000010 iY C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:153 .bss:00000012 iTheta C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:578 .text:0000035c MotorInit C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1287 .bss:00000015 MotorTimer *COM*:00000002 MotorLoop *COM*:00000013 Left C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1013 .text:000005ca AddToLong *COM*:00000013 Right *COM*:00000001 OdometryResetPending C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:661 .text:00000418 DoMotion C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:802 .text:000004a8 DoPid .bss:00000014 encoders_old C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:746 .text:0000047a AddToPosition C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:938 .text:00000568 motors C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1069 .text:000005fa WaitForMotionToStop C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1114 .text:00000634 ResetOdometry C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1135 .text:0000063c SynchWithMotion C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1157 .text:00000646 EmergencyStop C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1190 .text:00000678 QuadratureDecoder C:\DOCUME~1\palm3\LOCALS~1\Temp/cc61aaaa.s:1308 .text:000006ee Letext *ABS*:00000000 *ABS* UNDEFINED SYMBOLS __do_copy_data __do_clear_bss AvrXStartTimer AvrXSetSemaphore __floatsisf __mulsf3 __addsf3 cos sin __subsf3 __gesf2 __ltsf2 __fixsfsi AvrXWaitTimer R_ParameterTable __divmodhi4 AvrXWaitSemaphore