8a: 0c 94 f2 06 jmp 0xde4 ; 0xde4 <_exit> 0000008e <__bad_interrupt>: 8e: 0c 94 00 00 jmp 0 ; 0x0 <__heap_end> 00000092 : 8 send device model (servo number) {servo number} */ unsigned int servoCmd(unsigned int command, unsigned int argument) { 92: cf 93 push r28 94: df 93 push r29 96: ec 01 movw r28, r24 unsigned int reply; static unsigned int chainAddress = 1023; reply = 0; switch (command) { 98: 84 30 cpi r24, 0x04 ; 4 9a: 91 05 cpc r25, r1 9c: 09 f4 brne .+2 ; 0xa0 9e: a0 c1 rjmp .+832 ; 0x3e0 a0: 85 30 cpi r24, 0x05 ; 5 a2: 91 05 cpc r25, r1 a4: 80 f4 brcc .+32 ; 0xc6 a6: 81 30 cpi r24, 0x01 ; 1 a8: 91 05 cpc r25, r1 aa: 09 f4 brne .+2 ; 0xae ac: 66 c0 rjmp .+204 ; 0x17a ae: 81 30 cpi r24, 0x01 ; 1 b0: 91 05 cpc r25, r1 b2: d0 f0 brcs .+52 ; 0xe8 b4: 82 30 cpi r24, 0x02 ; 2 b6: 91 05 cpc r25, r1 b8: 09 f4 brne .+2 ; 0xbc ba: 94 c0 rjmp .+296 ; 0x1e4 bc: 83 30 cpi r24, 0x03 ; 3 be: 91 05 cpc r25, r1 c0: 09 f0 breq .+2 ; 0xc4 c2: a1 c2 rjmp .+1346 ; 0x606 c4: ef c0 rjmp .+478 ; 0x2a4 c6: 86 30 cpi r24, 0x06 ; 6 c8: 91 05 cpc r25, r1 ca: 09 f4 brne .+2 ; 0xce cc: cd c1 rjmp .+922 ; 0x468 ce: 86 30 cpi r24, 0x06 ; 6 d0: 91 05 cpc r25, r1 d2: 08 f4 brcc .+2 ; 0xd6 d4: a7 c1 rjmp .+846 ; 0x424 d6: 87 30 cpi r24, 0x07 ; 7 d8: 91 05 cpc r25, r1 da: 09 f4 brne .+2 ; 0xde dc: 26 c2 rjmp .+1100 ; 0x52a de: 88 30 cpi r24, 0x08 ; 8 e0: 91 05 cpc r25, r1 e2: 09 f0 breq .+2 ; 0xe6 e4: 90 c2 rjmp .+1312 ; 0x606 e6: 89 c2 rjmp .+1298 ; 0x5fa case 0: // listen(id) chainAddress = 1023 ; e8: 8f ef ldi r24, 0xFF ; 255 ea: 93 e0 ldi r25, 0x03 ; 3 ec: 90 93 61 00 sts 0x0061, r25 f0: 80 93 60 00 sts 0x0060, r24 if (argument == ID0) listen[0] |= 2; f4: 64 30 cpi r22, 0x04 ; 4 f6: 71 05 cpc r23, r1 f8: 41 f4 brne .+16 ; 0x10a fa: e1 e9 ldi r30, 0x91 ; 145 fc: f0 e0 ldi r31, 0x00 ; 0 fe: 80 81 ld r24, Z 100: 82 60 ori r24, 0x02 ; 2 102: 80 83 st Z, r24 104: 20 e0 ldi r18, 0x00 ; 0 106: 30 e0 ldi r19, 0x00 ; 0 108: c7 c2 rjmp .+1422 ; 0x698 else if (argument == ID1) listen[1] |= 2; 10a: 65 30 cpi r22, 0x05 ; 5 10c: 71 05 cpc r23, r1 10e: 41 f4 brne .+16 ; 0x120 110: e2 e9 ldi r30, 0x92 ; 146 112: f0 e0 ldi r31, 0x00 ; 0 114: 80 81 ld r24, Z 116: 82 60 ori r24, 0x02 ; 2 118: 80 83 st Z, r24 11a: 20 e0 ldi r18, 0x00 ; 0 11c: 30 e0 ldi r19, 0x00 ; 0 11e: bc c2 rjmp .+1400 ; 0x698 else if (argument == ID2) listen[2] |= 2; 120: 66 30 cpi r22, 0x06 ; 6 122: 71 05 cpc r23, r1 124: 41 f4 brne .+16 ; 0x136 126: e3 e9 ldi r30, 0x93 ; 147 128: f0 e0 ldi r31, 0x00 ; 0 12a: 80 81 ld r24, Z 12c: 82 60 ori r24, 0x02 ; 2 12e: 80 83 st Z, r24 130: 20 e0 ldi r18, 0x00 ; 0 132: 30 e0 ldi r19, 0x00 ; 0 134: b1 c2 rjmp .+1378 ; 0x698 else if (argument == ID3) listen[3] |= 2; 136: 67 30 cpi r22, 0x07 ; 7 138: 71 05 cpc r23, r1 13a: 41 f4 brne .+16 ; 0x14c 13c: e4 e9 ldi r30, 0x94 ; 148 13e: f0 e0 ldi r31, 0x00 ; 0 140: 80 81 ld r24, Z 142: 82 60 ori r24, 0x02 ; 2 144: 80 83 st Z, r24 146: 20 e0 ldi r18, 0x00 ; 0 148: 30 e0 ldi r19, 0x00 ; 0 14a: a6 c2 rjmp .+1356 ; 0x698 else if (argument == 256) { listen[0] |= 2; listen[1] |= 2; listen[2] |= 2; listen[3] |= 2; } 14c: 60 50 subi r22, 0x00 ; 0 14e: 71 40 sbci r23, 0x01 ; 1 150: 09 f0 breq .+2 ; 0x154 152: 59 c2 rjmp .+1202 ; 0x606 154: a1 e9 ldi r26, 0x91 ; 145 156: b0 e0 ldi r27, 0x00 ; 0 158: 8c 91 ld r24, X 15a: 82 60 ori r24, 0x02 ; 2 15c: fd 01 movw r30, r26 15e: 81 93 st Z+, r24 160: 80 81 ld r24, Z 162: 82 60 ori r24, 0x02 ; 2 164: 80 83 st Z, r24 166: fd 01 movw r30, r26 168: 32 96 adiw r30, 0x02 ; 2 16a: 80 81 ld r24, Z 16c: 82 60 ori r24, 0x02 ; 2 16e: 80 83 st Z, r24 170: 13 96 adiw r26, 0x03 ; 3 172: 8c 91 ld r24, X 174: 82 60 ori r24, 0x02 ; 2 176: 8c 93 st X, r24 178: 46 c2 rjmp .+1164 ; 0x606 break; case 1: // ignore(id) chainAddress = 1023 ; 17a: 8f ef ldi r24, 0xFF ; 255 17c: 93 e0 ldi r25, 0x03 ; 3 17e: 90 93 61 00 sts 0x0061, r25 182: 80 93 60 00 sts 0x0060, r24 if (argument == ID0) listen[0] = 0; 186: 64 30 cpi r22, 0x04 ; 4 188: 71 05 cpc r23, r1 18a: 29 f4 brne .+10 ; 0x196 18c: 10 92 91 00 sts 0x0091, r1 190: 20 e0 ldi r18, 0x00 ; 0 192: 30 e0 ldi r19, 0x00 ; 0 194: 81 c2 rjmp .+1282 ; 0x698 else if (argument == ID1) listen[1] = 0; 196: 65 30 cpi r22, 0x05 ; 5 198: 71 05 cpc r23, r1 19a: 29 f4 brne .+10 ; 0x1a6 19c: 10 92 92 00 sts 0x0092, r1 1a0: 20 e0 ldi r18, 0x00 ; 0 1a2: 30 e0 ldi r19, 0x00 ; 0 1a4: 79 c2 rjmp .+1266 ; 0x698 else if (argument == ID2) listen[2] = 0; 1a6: 66 30 cpi r22, 0x06 ; 6 1a8: 71 05 cpc r23, r1 1aa: 29 f4 brne .+10 ; 0x1b6 1ac: 10 92 93 00 sts 0x0093, r1 1b0: 20 e0 ldi r18, 0x00 ; 0 1b2: 30 e0 ldi r19, 0x00 ; 0 1b4: 71 c2 rjmp .+1250 ; 0x698 else if (argument == ID3) listen[3] = 0; 1b6: 67 30 cpi r22, 0x07 ; 7 1b8: 71 05 cpc r23, r1 1ba: 29 f4 brne .+10 ; 0x1c6 1bc: 10 92 94 00 sts 0x0094, r1 1c0: 20 e0 ldi r18, 0x00 ; 0 1c2: 30 e0 ldi r19, 0x00 ; 0 1c4: 69 c2 rjmp .+1234 ; 0x698 else if (argument == 256) { listen[0] = 0; listen[1] = 0; listen[2] = 0; listen[3] = 0; } 1c6: 60 50 subi r22, 0x00 ; 0 1c8: 71 40 sbci r23, 0x01 ; 1 1ca: 09 f0 breq .+2 ; 0x1ce 1cc: 1c c2 rjmp .+1080 ; 0x606 1ce: a1 e9 ldi r26, 0x91 ; 145 1d0: b0 e0 ldi r27, 0x00 ; 0 1d2: fd 01 movw r30, r26 1d4: 11 92 st Z+, r1 1d6: 10 82 st Z, r1 1d8: 12 96 adiw r26, 0x02 ; 2 1da: 1c 92 st X, r1 1dc: 12 97 sbiw r26, 0x02 ; 2 1de: 13 96 adiw r26, 0x03 ; 3 1e0: 1c 92 st X, r1 1e2: 11 c2 rjmp .+1058 ; 0x606 break; case 2: // listen to only the next command chainAddress = 1023; 1e4: 8f ef ldi r24, 0xFF ; 255 1e6: 93 e0 ldi r25, 0x03 ; 3 1e8: 90 93 61 00 sts 0x0061, r25 1ec: 80 93 60 00 sts 0x0060, r24 if (argument == ID0) listen[0] |= 1; 1f0: 64 30 cpi r22, 0x04 ; 4 1f2: 71 05 cpc r23, r1 1f4: 41 f4 brne .+16 ; 0x206 1f6: e1 e9 ldi r30, 0x91 ; 145 1f8: f0 e0 ldi r31, 0x00 ; 0 1fa: 80 81 ld r24, Z 1fc: 81 60 ori r24, 0x01 ; 1 1fe: 80 83 st Z, r24 200: 20 e0 ldi r18, 0x00 ; 0 202: 30 e0 ldi r19, 0x00 ; 0 204: 49 c2 rjmp .+1170 ; 0x698 else if (argument == ID1) listen[1] |= 1; 206: 65 30 cpi r22, 0x05 ; 5 208: 71 05 cpc r23, r1 20a: 41 f4 brne .+16 ; 0x21c 20c: e2 e9 ldi r30, 0x92 ; 146 20e: f0 e0 ldi r31, 0x00 ; 0 210: 80 81 ld r24, Z 212: 81 60 ori r24, 0x01 ; 1 214: 80 83 st Z, r24 216: 20 e0 ldi r18, 0x00 ; 0 218: 30 e0 ldi r19, 0x00 ; 0 21a: 3e c2 rjmp .+1148 ; 0x698 else if (argument == ID2) listen[2] |= 1; 21c: 66 30 cpi r22, 0x06 ; 6 21e: 71 05 cpc r23, r1 220: 41 f4 brne .+16 ; 0x232 222: e3 e9 ldi r30, 0x93 ; 147 224: f0 e0 ldi r31, 0x00 ; 0 226: 80 81 ld r24, Z 228: 81 60 ori r24, 0x01 ; 1 22a: 80 83 st Z, r24 22c: 20 e0 ldi r18, 0x00 ; 0 22e: 30 e0 ldi r19, 0x00 ; 0 230: 33 c2 rjmp .+1126 ; 0x698 else if (argument == ID3) listen[3] |= 1; 232: 67 30 cpi r22, 0x07 ; 7 234: 71 05 cpc r23, r1 236: 41 f4 brne .+16 ; 0x248 238: e4 e9 ldi r30, 0x94 ; 148 23a: f0 e0 ldi r31, 0x00 ; 0 23c: 80 81 ld r24, Z 23e: 81 60 ori r24, 0x01 ; 1 240: 80 83 st Z, r24 242: 20 e0 ldi r18, 0x00 ; 0 244: 30 e0 ldi r19, 0x00 ; 0 246: 28 c2 rjmp .+1104 ; 0x698 else if (argument == 256) { listen[0] |= 1; listen[1] |= 1; listen[2] |= 1; listen[3] |= 1; } 248: e1 e0 ldi r30, 0x01 ; 1 24a: 60 30 cpi r22, 0x00 ; 0 24c: 7e 07 cpc r23, r30 24e: 99 f4 brne .+38 ; 0x276 250: a1 e9 ldi r26, 0x91 ; 145 252: b0 e0 ldi r27, 0x00 ; 0 254: 8c 91 ld r24, X 256: 81 60 ori r24, 0x01 ; 1 258: fd 01 movw r30, r26 25a: 81 93 st Z+, r24 25c: 80 81 ld r24, Z 25e: 81 60 ori r24, 0x01 ; 1 260: 80 83 st Z, r24 262: fd 01 movw r30, r26 264: 32 96 adiw r30, 0x02 ; 2 266: 80 81 ld r24, Z 268: 81 60 ori r24, 0x01 ; 1 26a: 80 83 st Z, r24 26c: 13 96 adiw r26, 0x03 ; 3 26e: 8c 91 ld r24, X 270: 81 60 ori r24, 0x01 ; 1 272: 8c 93 st X, r24 274: c8 c1 rjmp .+912 ; 0x606 else if (argument >= 512) { chainAddress = argument - 512; listen[0] = 0; listen[1] = 0; listen[2] = 0; listen[3] = 0;} 276: f2 e0 ldi r31, 0x02 ; 2 278: 60 30 cpi r22, 0x00 ; 0 27a: 7f 07 cpc r23, r31 27c: 08 f4 brcc .+2 ; 0x280 27e: c3 c1 rjmp .+902 ; 0x606 280: cb 01 movw r24, r22 282: 80 50 subi r24, 0x00 ; 0 284: 92 40 sbci r25, 0x02 ; 2 286: 90 93 61 00 sts 0x0061, r25 28a: 80 93 60 00 sts 0x0060, r24 28e: a1 e9 ldi r26, 0x91 ; 145 290: b0 e0 ldi r27, 0x00 ; 0 292: fd 01 movw r30, r26 294: 11 92 st Z+, r1 296: 10 82 st Z, r1 298: 12 96 adiw r26, 0x02 ; 2 29a: 1c 92 st X, r1 29c: 12 97 sbiw r26, 0x02 ; 2 29e: 13 96 adiw r26, 0x03 ; 3 2a0: 1c 92 st X, r1 2a2: b1 c1 rjmp .+866 ; 0x606 2 turn servo off {obey if listening} 3 set cmdpos to curpos {obey if listening} */ case 3: // set flags if (IsHigh(ENGCACHE, argument)) { 2a4: 60 ff sbrs r22, 0 2a6: 1a c0 rjmp .+52 ; 0x2dc cmdpos[0] = cachepos[0]; 2a8: 41 e8 ldi r20, 0x81 ; 129 2aa: 50 e0 ldi r21, 0x00 ; 0 2ac: 29 e8 ldi r18, 0x89 ; 137 2ae: 30 e0 ldi r19, 0x00 ; 0 2b0: d9 01 movw r26, r18 2b2: 8d 91 ld r24, X+ 2b4: 9d 91 ld r25, X+ 2b6: fa 01 movw r30, r20 2b8: 81 93 st Z+, r24 2ba: 91 93 st Z+, r25 cmdpos[1] = cachepos[1]; 2bc: 8d 91 ld r24, X+ 2be: 9c 91 ld r25, X 2c0: 91 83 std Z+1, r25 ; 0x01 2c2: 80 83 st Z, r24 cmdpos[2] = cachepos[2]; 2c4: f9 01 movw r30, r18 2c6: 84 81 ldd r24, Z+4 ; 0x04 2c8: 95 81 ldd r25, Z+5 ; 0x05 2ca: fa 01 movw r30, r20 2cc: 95 83 std Z+5, r25 ; 0x05 2ce: 84 83 std Z+4, r24 ; 0x04 cmdpos[3] = cachepos[3]; 2d0: f9 01 movw r30, r18 2d2: 86 81 ldd r24, Z+6 ; 0x06 2d4: 97 81 ldd r25, Z+7 ; 0x07 2d6: fa 01 movw r30, r20 2d8: 97 83 std Z+7, r25 ; 0x07 2da: 86 83 std Z+6, r24 ; 0x06 } if (IsHigh(CMD2CUR, argument)) { 2dc: 63 ff sbrs r22, 3 2de: 30 c0 rjmp .+96 ; 0x340 if (listen[0]){ cmdpos[0] = AdcValues[1];} 2e0: 80 91 91 00 lds r24, 0x0091 2e4: 88 23 and r24, r24 2e6: 41 f0 breq .+16 ; 0x2f8 2e8: 80 91 70 00 lds r24, 0x0070 2ec: 90 91 71 00 lds r25, 0x0071 2f0: 90 93 82 00 sts 0x0082, r25 2f4: 80 93 81 00 sts 0x0081, r24 if (listen[1]){ cmdpos[1] = AdcValues[3];} 2f8: 80 91 92 00 lds r24, 0x0092 2fc: 88 23 and r24, r24 2fe: 41 f0 breq .+16 ; 0x310 300: 80 91 74 00 lds r24, 0x0074 304: 90 91 75 00 lds r25, 0x0075 308: 90 93 84 00 sts 0x0084, r25 30c: 80 93 83 00 sts 0x0083, r24 if (listen[2]){ cmdpos[2] = AdcValues[5];} 310: 80 91 93 00 lds r24, 0x0093 314: 88 23 and r24, r24 316: 41 f0 breq .+16 ; 0x328 318: 80 91 78 00 lds r24, 0x0078 31c: 90 91 79 00 lds r25, 0x0079 320: 90 93 86 00 sts 0x0086, r25 324: 80 93 85 00 sts 0x0085, r24 if (listen[3]){ cmdpos[3] = AdcValues[7];} 328: 80 91 94 00 lds r24, 0x0094 32c: 88 23 and r24, r24 32e: 41 f0 breq .+16 ; 0x340 330: 80 91 7c 00 lds r24, 0x007C 334: 90 91 7d 00 lds r25, 0x007D 338: 90 93 88 00 sts 0x0088, r25 33c: 80 93 87 00 sts 0x0087, r24 } if (IsHigh(SERVOON, argument)) { 340: 61 ff sbrs r22, 1 342: 26 c0 rjmp .+76 ; 0x390 if (listen[0]){ pwm0on(); } 344: 80 91 91 00 lds r24, 0x0091 348: 88 23 and r24, r24 34a: 29 f0 breq .+10 ; 0x356 34c: e3 e5 ldi r30, 0x53 ; 83 34e: f0 e0 ldi r31, 0x00 ; 0 350: 80 81 ld r24, Z 352: 80 62 ori r24, 0x20 ; 32 354: 80 83 st Z, r24 if (listen[1]){ pwm1on(); } 356: 80 91 92 00 lds r24, 0x0092 35a: 88 23 and r24, r24 35c: 29 f0 breq .+10 ; 0x368 35e: ef e4 ldi r30, 0x4F ; 79 360: f0 e0 ldi r31, 0x00 ; 0 362: 80 81 ld r24, Z 364: 80 68 ori r24, 0x80 ; 128 366: 80 83 st Z, r24 if (listen[2]){ pwm2on(); } 368: 80 91 93 00 lds r24, 0x0093 36c: 88 23 and r24, r24 36e: 29 f0 breq .+10 ; 0x37a 370: ef e4 ldi r30, 0x4F ; 79 372: f0 e0 ldi r31, 0x00 ; 0 374: 80 81 ld r24, Z 376: 80 62 ori r24, 0x20 ; 32 378: 80 83 st Z, r24 if (listen[3]){ pwm3on(); } 37a: 80 91 94 00 lds r24, 0x0094 37e: 88 23 and r24, r24 380: 09 f4 brne .+2 ; 0x384 382: 41 c1 rjmp .+642 ; 0x606 384: e5 e4 ldi r30, 0x45 ; 69 386: f0 e0 ldi r31, 0x00 ; 0 388: 80 81 ld r24, Z 38a: 80 62 ori r24, 0x20 ; 32 38c: 80 83 st Z, r24 38e: 3b c1 rjmp .+630 ; 0x606 } else if (IsHigh(SERVOOFF, argument)) { 390: 62 ff sbrs r22, 2 392: 39 c1 rjmp .+626 ; 0x606 if (listen[0]){ pwm0off(); } 394: 80 91 91 00 lds r24, 0x0091 398: 88 23 and r24, r24 39a: 29 f0 breq .+10 ; 0x3a6 39c: e3 e5 ldi r30, 0x53 ; 83 39e: f0 e0 ldi r31, 0x00 ; 0 3a0: 80 81 ld r24, Z 3a2: 8f 7d andi r24, 0xDF ; 223 3a4: 80 83 st Z, r24 if (listen[1]){ pwm1off(); } 3a6: 80 91 92 00 lds r24, 0x0092 3aa: 88 23 and r24, r24 3ac: 29 f0 breq .+10 ; 0x3b8 3ae: ef e4 ldi r30, 0x4F ; 79 3b0: f0 e0 ldi r31, 0x00 ; 0 3b2: 80 81 ld r24, Z 3b4: 8f 77 andi r24, 0x7F ; 127 3b6: 80 83 st Z, r24 if (listen[2]){ pwm2off(); } 3b8: 80 91 93 00 lds r24, 0x0093 3bc: 88 23 and r24, r24 3be: 29 f0 breq .+10 ; 0x3ca 3c0: ef e4 ldi r30, 0x4F ; 79 3c2: f0 e0 ldi r31, 0x00 ; 0 3c4: 80 81 ld r24, Z 3c6: 8f 7d andi r24, 0xDF ; 223 3c8: 80 83 st Z, r24 if (listen[3]){ pwm3off(); } 3ca: 80 91 94 00 lds r24, 0x0094 3ce: 88 23 and r24, r24 3d0: 09 f4 brne .+2 ; 0x3d4 3d2: 19 c1 rjmp .+562 ; 0x606 3d4: e5 e4 ldi r30, 0x45 ; 69 3d6: f0 e0 ldi r31, 0x00 ; 0 3d8: 80 81 ld r24, Z 3da: 8f 7d andi r24, 0xDF ; 223 3dc: 80 83 st Z, r24 3de: 13 c1 rjmp .+550 ; 0x606 } break; case 4: // set servo position if (listen[0]){ cmdpos[0] = argument; } 3e0: 80 91 91 00 lds r24, 0x0091 3e4: 88 23 and r24, r24 3e6: 21 f0 breq .+8 ; 0x3f0 3e8: 70 93 82 00 sts 0x0082, r23 3ec: 60 93 81 00 sts 0x0081, r22 if (listen[1]){ cmdpos[1] = argument; } 3f0: 80 91 92 00 lds r24, 0x0092 3f4: 88 23 and r24, r24 3f6: 21 f0 breq .+8 ; 0x400 3f8: 70 93 84 00 sts 0x0084, r23 3fc: 60 93 83 00 sts 0x0083, r22 if (listen[2]){ cmdpos[2] = argument; } 400: 80 91 93 00 lds r24, 0x0093 404: 88 23 and r24, r24 406: 21 f0 breq .+8 ; 0x410 408: 70 93 86 00 sts 0x0086, r23 40c: 60 93 85 00 sts 0x0085, r22 if (listen[3]){ cmdpos[3] = argument; } 410: 80 91 94 00 lds r24, 0x0094 414: 88 23 and r24, r24 416: 09 f4 brne .+2 ; 0x41a 418: f6 c0 rjmp .+492 ; 0x606 41a: 70 93 88 00 sts 0x0088, r23 41e: 60 93 87 00 sts 0x0087, r22 422: f1 c0 rjmp .+482 ; 0x606 break; case 5: // set cached position if (listen[0]){ cachepos[0] = argument; } 424: 80 91 91 00 lds r24, 0x0091 428: 88 23 and r24, r24 42a: 21 f0 breq .+8 ; 0x434 42c: 70 93 8a 00 sts 0x008A, r23 430: 60 93 89 00 sts 0x0089, r22 if (listen[1]){ cachepos[1] = argument; } 434: 80 91 92 00 lds r24, 0x0092 438: 88 23 and r24, r24 43a: 21 f0 breq .+8 ; 0x444 43c: 70 93 8c 00 sts 0x008C, r23 440: 60 93 8b 00 sts 0x008B, r22 if (listen[2]){ cachepos[2] = argument; } 444: 80 91 93 00 lds r24, 0x0093 448: 88 23 and r24, r24 44a: 21 f0 breq .+8 ; 0x454 44c: 70 93 8e 00 sts 0x008E, r23 450: 60 93 8d 00 sts 0x008D, r22 if (listen[3]){ cachepos[3] = argument; } 454: 80 91 94 00 lds r24, 0x0094 458: 88 23 and r24, r24 45a: 09 f4 brne .+2 ; 0x45e 45c: d4 c0 rjmp .+424 ; 0x606 45e: 70 93 90 00 sts 0x0090, r23 462: 60 93 8f 00 sts 0x008F, r22 466: cf c0 rjmp .+414 ; 0x606 break; case 6: // get servo current if (argument == ID0){ reply = ((AdcValues[0] << 5) & 0xFF00)|(AdcValues[0] & 0x07)|0x8000|(0<<3); } 468: 64 30 cpi r22, 0x04 ; 4 46a: 71 05 cpc r23, r1 46c: a9 f4 brne .+42 ; 0x498 46e: ee e6 ldi r30, 0x6E ; 110 470: f0 e0 ldi r31, 0x00 ; 0 472: 20 81 ld r18, Z 474: 31 81 ldd r19, Z+1 ; 0x01 476: 80 81 ld r24, Z 478: 91 81 ldd r25, Z+1 ; 0x01 47a: 87 70 andi r24, 0x07 ; 7 47c: 90 70 andi r25, 0x00 ; 0 47e: 90 68 ori r25, 0x80 ; 128 480: 22 0f add r18, r18 482: 33 1f adc r19, r19 484: 22 95 swap r18 486: 32 95 swap r19 488: 30 7f andi r19, 0xF0 ; 240 48a: 32 27 eor r19, r18 48c: 20 7f andi r18, 0xF0 ; 240 48e: 32 27 eor r19, r18 490: 20 70 andi r18, 0x00 ; 0 492: 28 2b or r18, r24 494: 39 2b or r19, r25 496: 00 c1 rjmp .+512 ; 0x698 else if (argument == ID1){ reply = ((AdcValues[2] << 5) & 0xFF00)|(AdcValues[2] & 0x07)|0x8000|(0<<3); } 498: 65 30 cpi r22, 0x05 ; 5 49a: 71 05 cpc r23, r1 49c: a9 f4 brne .+42 ; 0x4c8 49e: e2 e7 ldi r30, 0x72 ; 114 4a0: f0 e0 ldi r31, 0x00 ; 0 4a2: 20 81 ld r18, Z 4a4: 31 81 ldd r19, Z+1 ; 0x01 4a6: 80 81 ld r24, Z 4a8: 91 81 ldd r25, Z+1 ; 0x01 4aa: 87 70 andi r24, 0x07 ; 7 4ac: 90 70 andi r25, 0x00 ; 0 4ae: 90 68 ori r25, 0x80 ; 128 4b0: 22 0f add r18, r18 4b2: 33 1f adc r19, r19 4b4: 22 95 swap r18 4b6: 32 95 swap r19 4b8: 30 7f andi r19, 0xF0 ; 240 4ba: 32 27 eor r19, r18 4bc: 20 7f andi r18, 0xF0 ; 240 4be: 32 27 eor r19, r18 4c0: 20 70 andi r18, 0x00 ; 0 4c2: 28 2b or r18, r24 4c4: 39 2b or r19, r25 4c6: e8 c0 rjmp .+464 ; 0x698 else if (argument == ID2){ reply = ((AdcValues[4] << 5) & 0xFF00)|(AdcValues[4] & 0x07)|0x8000|(0<<3); } 4c8: 66 30 cpi r22, 0x06 ; 6 4ca: 71 05 cpc r23, r1 4cc: a9 f4 brne .+42 ; 0x4f8 4ce: e6 e7 ldi r30, 0x76 ; 118 4d0: f0 e0 ldi r31, 0x00 ; 0 4d2: 20 81 ld r18, Z 4d4: 31 81 ldd r19, Z+1 ; 0x01 4d6: 80 81 ld r24, Z 4d8: 91 81 ldd r25, Z+1 ; 0x01 4da: 87 70 andi r24, 0x07 ; 7 4dc: 90 70 andi r25, 0x00 ; 0 4de: 90 68 ori r25, 0x80 ; 128 4e0: 22 0f add r18, r18 4e2: 33 1f adc r19, r19 4e4: 22 95 swap r18 4e6: 32 95 swap r19 4e8: 30 7f andi r19, 0xF0 ; 240 4ea: 32 27 eor r19, r18 4ec: 20 7f andi r18, 0xF0 ; 240 4ee: 32 27 eor r19, r18 4f0: 20 70 andi r18, 0x00 ; 0 4f2: 28 2b or r18, r24 4f4: 39 2b or r19, r25 4f6: d0 c0 rjmp .+416 ; 0x698 else if (argument == ID3){ reply = ((AdcValues[6] << 5) & 0xFF00)|(AdcValues[6] & 0x07)|0x8000|(0<<3); } 4f8: 67 30 cpi r22, 0x07 ; 7 4fa: 71 05 cpc r23, r1 4fc: 09 f0 breq .+2 ; 0x500 4fe: 83 c0 rjmp .+262 ; 0x606 500: ea e7 ldi r30, 0x7A ; 122 502: f0 e0 ldi r31, 0x00 ; 0 504: 20 81 ld r18, Z 506: 31 81 ldd r19, Z+1 ; 0x01 508: 80 81 ld r24, Z 50a: 91 81 ldd r25, Z+1 ; 0x01 50c: 87 70 andi r24, 0x07 ; 7 50e: 90 70 andi r25, 0x00 ; 0 510: 90 68 ori r25, 0x80 ; 128 512: 22 0f add r18, r18 514: 33 1f adc r19, r19 516: 22 95 swap r18 518: 32 95 swap r19 51a: 30 7f andi r19, 0xF0 ; 240 51c: 32 27 eor r19, r18 51e: 20 7f andi r18, 0xF0 ; 240 520: 32 27 eor r19, r18 522: 20 70 andi r18, 0x00 ; 0 524: 28 2b or r18, r24 526: 39 2b or r19, r25 528: b7 c0 rjmp .+366 ; 0x698 break; case 7: // get servo position if (argument == ID0){ reply = ((AdcValues[1] << 5) & 0xFF00)|(AdcValues[0] & 0x07)|0x8000|(1<<3); } 52a: 64 30 cpi r22, 0x04 ; 4 52c: 71 05 cpc r23, r1 52e: b9 f4 brne .+46 ; 0x55e 530: e0 e7 ldi r30, 0x70 ; 112 532: f0 e0 ldi r31, 0x00 ; 0 534: 20 81 ld r18, Z 536: 31 81 ldd r19, Z+1 ; 0x01 538: 32 97 sbiw r30, 0x02 ; 2 53a: 80 81 ld r24, Z 53c: 91 81 ldd r25, Z+1 ; 0x01 53e: 87 70 andi r24, 0x07 ; 7 540: 90 70 andi r25, 0x00 ; 0 542: 88 60 ori r24, 0x08 ; 8 544: 90 68 ori r25, 0x80 ; 128 546: 22 0f add r18, r18 548: 33 1f adc r19, r19 54a: 22 95 swap r18 54c: 32 95 swap r19 54e: 30 7f andi r19, 0xF0 ; 240 550: 32 27 eor r19, r18 552: 20 7f andi r18, 0xF0 ; 240 554: 32 27 eor r19, r18 556: 20 70 andi r18, 0x00 ; 0 558: 28 2b or r18, r24 55a: 39 2b or r19, r25 55c: 9d c0 rjmp .+314 ; 0x698 else if (argument == ID1){ reply = ((AdcValues[3] << 5) & 0xFF00)|(AdcValues[2] & 0x07)|0x8000|(1<<3); } 55e: 65 30 cpi r22, 0x05 ; 5 560: 71 05 cpc r23, r1 562: b9 f4 brne .+46 ; 0x592 564: e4 e7 ldi r30, 0x74 ; 116 566: f0 e0 ldi r31, 0x00 ; 0 568: 20 81 ld r18, Z 56a: 31 81 ldd r19, Z+1 ; 0x01 56c: 32 97 sbiw r30, 0x02 ; 2 56e: 80 81 ld r24, Z 570: 91 81 ldd r25, Z+1 ; 0x01 572: 87 70 andi r24, 0x07 ; 7 574: 90 70 andi r25, 0x00 ; 0 576: 88 60 ori r24, 0x08 ; 8 578: 90 68 ori r25, 0x80 ; 128 57a: 22 0f add r18, r18 57c: 33 1f adc r19, r19 57e: 22 95 swap r18 580: 32 95 swap r19 582: 30 7f andi r19, 0xF0 ; 240 584: 32 27 eor r19, r18 586: 20 7f andi r18, 0xF0 ; 240 588: 32 27 eor r19, r18 58a: 20 70 andi r18, 0x00 ; 0 58c: 28 2b or r18, r24 58e: 39 2b or r19, r25 590: 83 c0 rjmp .+262 ; 0x698 else if (argument == ID2){ reply = ((AdcValues[5] << 5) & 0xFF00)|(AdcValues[4] & 0x07)|0x8000|(1<<3); } 592: 66 30 cpi r22, 0x06 ; 6 594: 71 05 cpc r23, r1 596: b9 f4 brne .+46 ; 0x5c6 598: e8 e7 ldi r30, 0x78 ; 120 59a: f0 e0 ldi r31, 0x00 ; 0 59c: 20 81 ld r18, Z 59e: 31 81 ldd r19, Z+1 ; 0x01 5a0: 32 97 sbiw r30, 0x02 ; 2 5a2: 80 81 ld r24, Z 5a4: 91 81 ldd r25, Z+1 ; 0x01 5a6: 87 70 andi r24, 0x07 ; 7 5a8: 90 70 andi r25, 0x00 ; 0 5aa: 88 60 ori r24, 0x08 ; 8 5ac: 90 68 ori r25, 0x80 ; 128 5ae: 22 0f add r18, r18 5b0: 33 1f adc r19, r19 5b2: 22 95 swap r18 5b4: 32 95 swap r19 5b6: 30 7f andi r19, 0xF0 ; 240 5b8: 32 27 eor r19, r18 5ba: 20 7f andi r18, 0xF0 ; 240 5bc: 32 27 eor r19, r18 5be: 20 70 andi r18, 0x00 ; 0 5c0: 28 2b or r18, r24 5c2: 39 2b or r19, r25 5c4: 69 c0 rjmp .+210 ; 0x698 else if (argument == ID3){ reply = ((AdcValues[7] << 5) & 0xFF00)|(AdcValues[6] & 0x07)|0x8000|(1<<3); } 5c6: 67 30 cpi r22, 0x07 ; 7 5c8: 71 05 cpc r23, r1 5ca: e9 f4 brne .+58 ; 0x606 5cc: ec e7 ldi r30, 0x7C ; 124 5ce: f0 e0 ldi r31, 0x00 ; 0 5d0: 20 81 ld r18, Z 5d2: 31 81 ldd r19, Z+1 ; 0x01 5d4: 32 97 sbiw r30, 0x02 ; 2 5d6: 80 81 ld r24, Z 5d8: 91 81 ldd r25, Z+1 ; 0x01 5da: 87 70 andi r24, 0x07 ; 7 5dc: 90 70 andi r25, 0x00 ; 0 5de: 88 60 ori r24, 0x08 ; 8 5e0: 90 68 ori r25, 0x80 ; 128 5e2: 22 0f add r18, r18 5e4: 33 1f adc r19, r19 5e6: 22 95 swap r18 5e8: 32 95 swap r19 5ea: 30 7f andi r19, 0xF0 ; 240 5ec: 32 27 eor r19, r18 5ee: 20 7f andi r18, 0xF0 ; 240 5f0: 32 27 eor r19, r18 5f2: 20 70 andi r18, 0x00 ; 0 5f4: 28 2b or r18, r24 5f6: 39 2b or r19, r25 5f8: 4f c0 rjmp .+158 ; 0x698 break; case 8: // get model if ((argument == ID0) || (argument == ID1) || (argument == ID2)|| (argument == ID3)){ 5fa: cb 01 movw r24, r22 5fc: 04 97 sbiw r24, 0x04 ; 4 5fe: 04 97 sbiw r24, 0x04 ; 4 600: 08 f4 brcc .+2 ; 0x604 602: 4f c0 rjmp .+158 ; 0x6a2 604: 51 c0 rjmp .+162 ; 0x6a8 } break; } switch(command) { // clear one time flags 606: ce 01 movw r24, r28 608: 03 97 sbiw r24, 0x03 ; 3 60a: 03 97 sbiw r24, 0x03 ; 3 60c: 08 f0 brcs .+2 ; 0x610 60e: 42 c0 rjmp .+132 ; 0x694 case 3: case 4: case 5: listen[0] &= 2; 610: a1 e9 ldi r26, 0x91 ; 145 612: b0 e0 ldi r27, 0x00 ; 0 614: 8c 91 ld r24, X 616: 82 70 andi r24, 0x02 ; 2 618: fd 01 movw r30, r26 61a: 81 93 st Z+, r24 listen[1] &= 2; 61c: 80 81 ld r24, Z 61e: 82 70 andi r24, 0x02 ; 2 620: 80 83 st Z, r24 listen[2] &= 2; 622: fd 01 movw r30, r26 624: 32 96 adiw r30, 0x02 ; 2 626: 80 81 ld r24, Z 628: 82 70 andi r24, 0x02 ; 2 62a: 80 83 st Z, r24 listen[3] &= 2; 62c: 13 96 adiw r26, 0x03 ; 3 62e: 8c 91 ld r24, X 630: 82 70 andi r24, 0x02 ; 2 632: 8c 93 st X, r24 if (chainAddress != 1023) { 634: 80 91 60 00 lds r24, 0x0060 638: 90 91 61 00 lds r25, 0x0061 63c: f3 e0 ldi r31, 0x03 ; 3 63e: 8f 3f cpi r24, 0xFF ; 255 640: 9f 07 cpc r25, r31 642: 41 f1 breq .+80 ; 0x694 chainAddress++; 644: 01 96 adiw r24, 0x01 ; 1 646: 90 93 61 00 sts 0x0061, r25 64a: 80 93 60 00 sts 0x0060, r24 if (chainAddress == ID0) listen[0] = 1; 64e: 84 30 cpi r24, 0x04 ; 4 650: 91 05 cpc r25, r1 652: 31 f4 brne .+12 ; 0x660 654: 81 e0 ldi r24, 0x01 ; 1 656: 80 93 91 00 sts 0x0091, r24 65a: 20 e0 ldi r18, 0x00 ; 0 65c: 30 e0 ldi r19, 0x00 ; 0 65e: 1c c0 rjmp .+56 ; 0x698 else if (chainAddress == ID1) listen[1] = 1; 660: 85 30 cpi r24, 0x05 ; 5 662: 91 05 cpc r25, r1 664: 31 f4 brne .+12 ; 0x672 666: 81 e0 ldi r24, 0x01 ; 1 668: 80 93 92 00 sts 0x0092, r24 66c: 20 e0 ldi r18, 0x00 ; 0 66e: 30 e0 ldi r19, 0x00 ; 0 670: 13 c0 rjmp .+38 ; 0x698 else if (chainAddress == ID2) listen[2] = 1; 672: 86 30 cpi r24, 0x06 ; 6 674: 91 05 cpc r25, r1 676: 31 f4 brne .+12 ; 0x684 678: 81 e0 ldi r24, 0x01 ; 1 67a: 80 93 93 00 sts 0x0093, r24 67e: 20 e0 ldi r18, 0x00 ; 0 680: 30 e0 ldi r19, 0x00 ; 0 682: 0a c0 rjmp .+20 ; 0x698 else if (chainAddress == ID3) listen[3] = 1; 684: 07 97 sbiw r24, 0x07 ; 7 686: 31 f4 brne .+12 ; 0x694 688: 81 e0 ldi r24, 0x01 ; 1 68a: 80 93 94 00 sts 0x0094, r24 68e: 20 e0 ldi r18, 0x00 ; 0 690: 30 e0 ldi r19, 0x00 ; 0 692: 02 c0 rjmp .+4 ; 0x698 694: 20 e0 ldi r18, 0x00 ; 0 696: 30 e0 ldi r19, 0x00 ; 0 return reply; } 698: 82 2f mov r24, r18 69a: 93 2f mov r25, r19 69c: df 91 pop r29 69e: cf 91 pop r28 6a0: 08 95 ret if (chainAddress != 1023) { chainAddress++; if (chainAddress == ID0) listen[0] = 1; else if (chainAddress == ID1) listen[1] = 1; else if (chainAddress == ID2) listen[2] = 1; else if (chainAddress == ID3) listen[3] = 1; 6a2: 23 e1 ldi r18, 0x13 ; 19 6a4: 30 e8 ldi r19, 0x80 ; 128 6a6: f8 cf rjmp .-16 ; 0x698 6a8: 20 e0 ldi r18, 0x00 ; 0 6aa: 30 e0 ldi r19, 0x00 ; 0 6ac: f5 cf rjmp .-22 ; 0x698 000006ae : //------------------------| FUNCTIONS |------------------------ unsigned int dobyte(char data) { 6ae: 28 2f mov r18, r24 static unsigned char state = 0; static unsigned int command; static unsigned int argument; if (state == 0) { 6b0: 80 91 66 00 lds r24, 0x0066 6b4: 88 23 and r24, r24 6b6: 21 f5 brne .+72 ; 0x700 if ((data & 0x80) == 0) { 6b8: 22 23 and r18, r18 6ba: 0c f4 brge .+2 ; 0x6be 6bc: 45 c0 rjmp .+138 ; 0x748 state = 1; 6be: 81 e0 ldi r24, 0x01 ; 1 6c0: 80 93 66 00 sts 0x0066, r24 command = (data >> 3); 6c4: 82 2f mov r24, r18 6c6: 85 95 asr r24 6c8: 85 95 asr r24 6ca: 85 95 asr r24 6cc: 99 27 eor r25, r25 6ce: 87 fd sbrc r24, 7 6d0: 90 95 com r25 6d2: 90 93 65 00 sts 0x0065, r25 6d6: 80 93 64 00 sts 0x0064, r24 argument = (argument & 0xFFF8) | (data & 0x07); // glue in its 0 through 2 6da: 33 27 eor r19, r19 6dc: 27 fd sbrc r18, 7 6de: 30 95 com r19 6e0: 27 70 andi r18, 0x07 ; 7 6e2: 30 70 andi r19, 0x00 ; 0 6e4: 80 91 62 00 lds r24, 0x0062 6e8: 90 91 63 00 lds r25, 0x0063 6ec: 88 7f andi r24, 0xF8 ; 248 6ee: 28 2b or r18, r24 6f0: 39 2b or r19, r25 6f2: 30 93 63 00 sts 0x0063, r19 6f6: 20 93 62 00 sts 0x0062, r18 6fa: 20 e0 ldi r18, 0x00 ; 0 6fc: 30 e0 ldi r19, 0x00 ; 0 6fe: 26 c0 rjmp .+76 ; 0x74c } } else { state = 0; 700: 10 92 66 00 sts 0x0066, r1 if ((data & 0x80) != 0) { 704: 22 23 and r18, r18 706: 04 f5 brge .+64 ; 0x748 argument = (argument & 0x0007) | ((data & 0x7F) << 3); //glue in bits 3 through 9 708: 62 2f mov r22, r18 70a: 77 27 eor r23, r23 70c: 67 fd sbrc r22, 7 70e: 70 95 com r23 710: 6f 77 andi r22, 0x7F ; 127 712: 70 70 andi r23, 0x00 ; 0 714: 66 0f add r22, r22 716: 77 1f adc r23, r23 718: 66 0f add r22, r22 71a: 77 1f adc r23, r23 71c: 66 0f add r22, r22 71e: 77 1f adc r23, r23 720: 80 91 62 00 lds r24, 0x0062 724: 90 91 63 00 lds r25, 0x0063 728: 87 70 andi r24, 0x07 ; 7 72a: 90 70 andi r25, 0x00 ; 0 72c: 68 2b or r22, r24 72e: 79 2b or r23, r25 730: 70 93 63 00 sts 0x0063, r23 734: 60 93 62 00 sts 0x0062, r22 return servoCmd(command, argument); 738: 80 91 64 00 lds r24, 0x0064 73c: 90 91 65 00 lds r25, 0x0065 740: 0e 94 49 00 call 0x92 ; 0x92 744: 9c 01 movw r18, r24 746: 02 c0 rjmp .+4 ; 0x74c 748: 20 e0 ldi r18, 0x00 ; 0 74a: 30 e0 ldi r19, 0x00 ; 0 } } return 0; } 74c: 82 2f mov r24, r18 74e: 93 2f mov r25, r19 750: 08 95 ret 00000752 : void AnalogInit (void) { int i; // Activate ADC with Prescaler ADCSRA = 1 << ADEN | 752: 8c ec ldi r24, 0xCC ; 204 754: 86 b9 out 0x06, r24 ; 6 756: 20 e0 ldi r18, 0x00 ; 0 758: 30 e0 ldi r19, 0x00 ; 0 1 << ADPS2 | 0 << ADPS1 | 0 << ADPS0 ; for (i = 0; i < 8; AdcValues[i++] = 0); 75a: 4e e6 ldi r20, 0x6E ; 110 75c: 50 e0 ldi r21, 0x00 ; 0 75e: c9 01 movw r24, r18 760: 88 0f add r24, r24 762: 99 1f adc r25, r25 764: fa 01 movw r30, r20 766: e8 0f add r30, r24 768: f9 1f adc r31, r25 76a: 11 82 std Z+1, r1 ; 0x01 76c: 10 82 st Z, r1 76e: 2f 5f subi r18, 0xFF ; 255 770: 3f 4f sbci r19, 0xFF ; 255 772: 28 30 cpi r18, 0x08 ; 8 774: 31 05 cpc r19, r1 776: 99 f7 brne .-26 ; 0x75e } 778: 08 95 ret 0000077a : /1024 = ~61Hz */ void pwmInit() { // clear pwm levels OCR0 = 0; 77a: 1c be out 0x3c, r1 ; 60 OCR2 = 0; 77c: 13 bc out 0x23, r1 ; 35 OCR1A = 0; 77e: 1b bc out 0x2b, r1 ; 43 780: 1a bc out 0x2a, r1 ; 42 OCR1B = 0; 782: 19 bc out 0x29, r1 ; 41 784: 18 bc out 0x28, r1 ; 40 // set up WGM, clock, and mode for timer 0 TCCR0 = 0 << FOC0 | /* force output compare */ 786: 8b e4 ldi r24, 0x4B ; 75 788: 83 bf out 0x33, r24 ; 51 0 << CS02 | /* CLKio/64 */ 1 << CS01 | 1 << CS00 ; // set up WGM, clock, and mode for timer 1A / 1B TCCR1A = 0 << COM1A1 | /* ** normal polarity */ 78a: 81 e0 ldi r24, 0x01 ; 1 78c: 8f bd out 0x2f, r24 ; 47 0 << FOC1A | /* force output compare */ 0 << FOC1B | /* force output compare */ 0 << WGM11 | /* 8 bit fast pwm */ 1 << WGM10 ; TCCR1B = 0 << ICNC1 | /* input capture noise cancel */ 78e: 8b e0 ldi r24, 0x0B ; 11 790: 8e bd out 0x2e, r24 ; 46 1 << CS11 | 1 << CS10 ; // set up WGM, clock, and mode for timer 2 TCCR2 = 0 << FOC2 | /* force output compare */ 792: 8c e4 ldi r24, 0x4C ; 76 794: 85 bd out 0x25, r24 ; 37 1 << WGM21 | /* fast pwm */ 1 << CS22 | /* CLKio/64 */ 0 << CS21 | 0 << CS20 ; } 796: 08 95 ret 00000798 : int Analog (int n) { return AdcValues[n & 7]; 798: fc 01 movw r30, r24 79a: e7 70 andi r30, 0x07 ; 7 79c: f0 70 andi r31, 0x00 ; 0 79e: ee 0f add r30, r30 7a0: ff 1f adc r31, r31 7a2: e2 59 subi r30, 0x92 ; 146 7a4: ff 4f sbci r31, 0xFF ; 255 7a6: 20 81 ld r18, Z 7a8: 31 81 ldd r19, Z+1 ; 0x01 } 7aa: 82 2f mov r24, r18 7ac: 93 2f mov r25, r19 7ae: 08 95 ret 000007b0 <__vector_16>: SIGNAL(SIG_ADC) { 7b0: 1f 92 push r1 7b2: 0f 92 push r0 7b4: 0f b6 in r0, 0x3f ; 63 7b6: 0f 92 push r0 7b8: 11 24 eor r1, r1 7ba: 2f 93 push r18 7bc: 3f 93 push r19 7be: 8f 93 push r24 7c0: 9f 93 push r25 7c2: af 93 push r26 7c4: bf 93 push r27 7c6: ef 93 push r30 7c8: ff 93 push r31 int i; i = ADMUX & 7; // what channel we on? 7ca: a7 e2 ldi r26, 0x27 ; 39 7cc: b0 e0 ldi r27, 0x00 ; 0 7ce: 8c 91 ld r24, X 7d0: 90 e0 ldi r25, 0x00 ; 0 7d2: 87 70 andi r24, 0x07 ; 7 7d4: 90 70 andi r25, 0x00 ; 0 AdcValues[i] = ADC; // save value 7d6: 24 b1 in r18, 0x04 ; 4 7d8: 35 b1 in r19, 0x05 ; 5 7da: fc 01 movw r30, r24 7dc: ee 0f add r30, r30 7de: ff 1f adc r31, r31 7e0: e2 59 subi r30, 0x92 ; 146 7e2: ff 4f sbci r31, 0xFF ; 255 7e4: 31 83 std Z+1, r19 ; 0x01 7e6: 20 83 st Z, r18 i++; // next value ADMUX = i & 7; // select channel 7e8: 01 96 adiw r24, 0x01 ; 1 7ea: 87 70 andi r24, 0x07 ; 7 7ec: 8c 93 st X, r24 ADCSRA |= _BV(ADSC); // start next conversion 7ee: e6 e2 ldi r30, 0x26 ; 38 7f0: f0 e0 ldi r31, 0x00 ; 0 7f2: 80 81 ld r24, Z 7f4: 80 64 ori r24, 0x40 ; 64 7f6: 80 83 st Z, r24 if (!ADMUX) { 7f8: 8c 91 ld r24, X 7fa: 88 23 and r24, r24 7fc: 11 f4 brne .+4 ; 0x802 <__vector_16+0x52> // debugToggle(); calcWait = 0; 7fe: 10 92 7e 00 sts 0x007E, r1 } return; } 802: ff 91 pop r31 804: ef 91 pop r30 806: bf 91 pop r27 808: af 91 pop r26 80a: 9f 91 pop r25 80c: 8f 91 pop r24 80e: 3f 91 pop r19 810: 2f 91 pop r18 812: 0f 90 pop r0 814: 0f be out 0x3f, r0 ; 63 816: 0f 90 pop r0 818: 1f 90 pop r1 81a: 18 95 reti 0000081c

: unsigned int servoCmd(unsigned int command, unsigned int argument) ; /****************************| CODE... |***********************************/ int main (void) { 81c: 2f 92 push r2 81e: 3f 92 push r3 820: 4f 92 push r4 822: 5f 92 push r5 824: 6f 92 push r6 826: 7f 92 push r7 828: 8f 92 push r8 82a: 9f 92 push r9 82c: af 92 push r10 82e: bf 92 push r11 830: cf 92 push r12 832: df 92 push r13 834: ef 92 push r14 836: ff 92 push r15 838: 0f 93 push r16 83a: 1f 93 push r17 83c: cf 93 push r28 83e: df 93 push r29 int error; int In[4]; unsigned int txtemp; // set up directions DDRA = (INPUT << PA0 | INPUT << PA1 |INPUT << PA2 |INPUT << PA3 |INPUT << PA4 |INPUT << PA5 |INPUT << PA6 |INPUT << PA7); 840: 1a ba out 0x1a, r1 ; 26 DDRB = (INPUT << PB0 | INPUT << PB1 |INPUT << PB2 |OUTPUT << PB3 |OUTPUT << PB4 |OUTPUT << PB5 |OUTPUT << PB6 |OUTPUT << PB7); 842: 88 ef ldi r24, 0xF8 ; 248 844: 87 bb out 0x17, r24 ; 23 DDRC = (OUTPUT << PC0 | OUTPUT << PC1 |OUTPUT << PC2 |OUTPUT << PC3 |OUTPUT << PC4 |OUTPUT << PC5 |OUTPUT << PC6 |OUTPUT << PC7); 846: 8f ef ldi r24, 0xFF ; 255 848: 84 bb out 0x14, r24 ; 20 DDRD = (INPUT << PD0 | INPUT << PD1 |INPUT << PD2 |INPUT << PD3 |OUTPUT << PD4 |OUTPUT << PD5 |OUTPUT << PD6 |OUTPUT << PD7); 84a: 80 ef ldi r24, 0xF0 ; 240 84c: 81 bb out 0x11, r24 ; 17 PORTB = 0; 84e: 18 ba out 0x18, r1 ; 24 pwm0 = 0; 850: 1c be out 0x3c, r1 ; 60 pwm1 = 0; 852: 1b bc out 0x2b, r1 ; 43 854: 1a bc out 0x2a, r1 ; 42 pwm2 = 0; 856: 19 bc out 0x29, r1 ; 41 858: 18 bc out 0x28, r1 ; 40 pwm3 = 0; 85a: 13 bc out 0x23, r1 ; 35 pwm0off(); 85c: e3 e5 ldi r30, 0x53 ; 83 85e: f0 e0 ldi r31, 0x00 ; 0 860: 80 81 ld r24, Z 862: 8f 7d andi r24, 0xDF ; 223 864: 80 83 st Z, r24 pwm1off(); 866: ef e4 ldi r30, 0x4F ; 79 868: f0 e0 ldi r31, 0x00 ; 0 86a: 80 81 ld r24, Z 86c: 8f 77 andi r24, 0x7F ; 127 86e: 80 83 st Z, r24 pwm2off(); 870: 80 81 ld r24, Z 872: 8f 7d andi r24, 0xDF ; 223 874: 80 83 st Z, r24 pwm3off(); 876: e5 e4 ldi r30, 0x45 ; 69 878: f0 e0 ldi r31, 0x00 ; 0 87a: 80 81 ld r24, Z 87c: 8f 7d andi r24, 0xDF ; 223 87e: 80 83 st Z, r24 Status0Red(); 880: e5 e3 ldi r30, 0x35 ; 53 882: f0 e0 ldi r31, 0x00 ; 0 884: 80 81 ld r24, Z 886: 82 60 ori r24, 0x02 ; 2 888: 80 83 st Z, r24 88a: 80 81 ld r24, Z 88c: 8e 7f andi r24, 0xFE ; 254 88e: 80 83 st Z, r24 Status1Red(); 890: 80 81 ld r24, Z 892: 88 60 ori r24, 0x08 ; 8 894: 80 83 st Z, r24 896: 80 81 ld r24, Z 898: 8b 7f andi r24, 0xFB ; 251 89a: 80 83 st Z, r24 Status2Red(); 89c: 80 81 ld r24, Z 89e: 80 62 ori r24, 0x20 ; 32 8a0: 80 83 st Z, r24 8a2: 80 81 ld r24, Z 8a4: 8f 7e andi r24, 0xEF ; 239 8a6: 80 83 st Z, r24 Status3Red(); 8a8: 80 81 ld r24, Z 8aa: 80 68 ori r24, 0x80 ; 128 8ac: 80 83 st Z, r24 8ae: 80 81 ld r24, Z 8b0: 8f 7b andi r24, 0xBF ; 191 8b2: 80 83 st Z, r24 USART_Init( 103 ); // 9600 @ 16Mhz 8b4: 87 e6 ldi r24, 0x67 ; 103 8b6: 90 e0 ldi r25, 0x00 ; 0 8b8: 0e 94 b3 06 call 0xd66 ; 0xd66 AnalogInit(); 8bc: 0e 94 a9 03 call 0x752 ; 0x752 pwmInit(); 8c0: 0e 94 bd 03 call 0x77a ; 0x77a In[0] = 0; In[1] = 0; In[2] = 0; In[3] = 0; // reset Integrators // turn interrupts on sei(); 8c4: 78 94 sei calcWait = 0; 8c6: 10 92 7e 00 sts 0x007E, r1 8ca: 66 24 eor r6, r6 8cc: 77 24 eor r7, r7 8ce: 88 24 eor r8, r8 8d0: 99 24 eor r9, r9 8d2: aa 24 eor r10, r10 8d4: bb 24 eor r11, r11 8d6: cc 24 eor r12, r12 8d8: dd 24 eor r13, r13 } else if ((AdcValues[7]>768)&(AdcValues[7]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 Status3Red(); } else if (pwm3 > 64) { // piles of error yet Status3Off(); } else { // ok Status3Green(); 8da: 05 e3 ldi r16, 0x35 ; 53 8dc: 10 e0 ldi r17, 0x00 ; 0 */ if (0) { } else if (!pwm0stat()) { // channel off Status0Tog(); } else if ((AdcValues[1]>768)&(AdcValues[1]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 8de: 0f 2e mov r0, r31 8e0: f0 e7 ldi r31, 0x70 ; 112 8e2: 2f 2e mov r2, r31 8e4: f0 e0 ldi r31, 0x00 ; 0 8e6: 3f 2e mov r3, r31 8e8: f0 2d mov r31, r0 if (ABS(error) < INTTHRESH) { In[3] = limit(In[3] + error, -INTTHRESH, INTTHRESH); } else { In[3] = 0; } error = (Kp*error)+(In[3]/Ki); pwm3 = limit(ABS(error), 0, 255); if (error > 0) SetBit(DIR3BIT, DIR3PORT); else ClearBit(DIR3BIT, DIR3PORT); 8ea: 0f 2e mov r0, r31 8ec: f8 e3 ldi r31, 0x38 ; 56 8ee: ef 2e mov r14, r31 8f0: ff 24 eor r15, r15 8f2: f0 2d mov r31, r0 error = cmdpos[3]-AdcValues[7]; if (ABS(error) < INTTHRESH) { In[3] = limit(In[3] + error, -INTTHRESH, INTTHRESH); } else { In[3] = 0; } error = (Kp*error)+(In[3]/Ki); pwm3 = limit(ABS(error), 0, 255); 8f4: 44 24 eor r4, r4 8f6: 55 24 eor r5, r5 8f8: f4 c0 rjmp .+488 ; 0xae2 <__stack+0x283> while(1){ // while we wait to be cleared for recalculating, check the uart while(calcWait){ // check for uart byte if ( (UCSRA & (1< if ((txtemp = dobyte(USART_Receive())) != 0) { 904: 0e 94 bc 06 call 0xd78 ; 0xd78 908: 0e 94 57 03 call 0x6ae ; 0x6ae 90c: ec 01 movw r28, r24 90e: 00 97 sbiw r24, 0x00 ; 0 910: 21 f0 breq .+8 ; 0x91a <__stack+0xbb> USART_Transmit( txtemp & 0x00FF ); 912: 0e 94 c3 06 call 0xd86 ; 0xd86 txbuff = (txtemp >> 8); 916: d0 93 6d 00 sts 0x006D, r29 } } if ( (UCSRA & (1< if (txbuff != 0) { 924: 80 91 6d 00 lds r24, 0x006D 928: 88 23 and r24, r24 92a: 21 f0 breq .+8 ; 0x934 <__stack+0xd5> USART_Transmit( txbuff ); 92c: 0e 94 c3 06 call 0xd86 ; 0xd86 txbuff = 0; 930: 10 92 6d 00 sts 0x006D, r1 off = error greater than thresh green = ok */ if (0) { } else if (!pwm0stat()) { // channel off 934: e3 e5 ldi r30, 0x53 ; 83 936: f0 e0 ldi r31, 0x00 ; 0 938: 80 81 ld r24, Z 93a: 85 fd sbrc r24, 5 93c: 07 c0 rjmp .+14 ; 0x94c <__stack+0xed> Status0Tog(); 93e: f8 01 movw r30, r16 940: 80 81 ld r24, Z 942: f3 e0 ldi r31, 0x03 ; 3 944: 8f 27 eor r24, r31 946: f8 01 movw r30, r16 948: 80 83 st Z, r24 94a: 28 c0 rjmp .+80 ; 0x99c <__stack+0x13d> } else if ((AdcValues[1]>768)&(AdcValues[1]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 94c: f1 01 movw r30, r2 94e: 80 81 ld r24, Z 950: 91 81 ldd r25, Z+1 ; 0x01 952: 20 81 ld r18, Z 954: 31 81 ldd r19, Z+1 ; 0x01 956: 81 50 subi r24, 0x01 ; 1 958: 93 40 sbci r25, 0x03 ; 3 95a: 64 f0 brlt .+24 ; 0x974 <__stack+0x115> 95c: 2f 3f cpi r18, 0xFF ; 255 95e: 31 05 cpc r19, r1 960: 09 f0 breq .+2 ; 0x964 <__stack+0x105> 962: 44 f4 brge .+16 ; 0x974 <__stack+0x115> Status0Red(); 964: f8 01 movw r30, r16 966: 80 81 ld r24, Z 968: 82 60 ori r24, 0x02 ; 2 96a: 80 83 st Z, r24 96c: 80 81 ld r24, Z 96e: 8e 7f andi r24, 0xFE ; 254 970: 80 83 st Z, r24 972: 14 c0 rjmp .+40 ; 0x99c <__stack+0x13d> } else if (pwm0 > 64) { // piles of error yet 974: ec e5 ldi r30, 0x5C ; 92 976: f0 e0 ldi r31, 0x00 ; 0 978: 80 81 ld r24, Z 97a: 81 34 cpi r24, 0x41 ; 65 97c: 40 f0 brcs .+16 ; 0x98e <__stack+0x12f> Status0Off(); 97e: f8 01 movw r30, r16 980: 80 81 ld r24, Z 982: 8e 7f andi r24, 0xFE ; 254 984: 80 83 st Z, r24 986: 80 81 ld r24, Z 988: 8d 7f andi r24, 0xFD ; 253 98a: 80 83 st Z, r24 98c: 07 c0 rjmp .+14 ; 0x99c <__stack+0x13d> } else { // ok Status0Green(); 98e: f8 01 movw r30, r16 990: 80 81 ld r24, Z 992: 81 60 ori r24, 0x01 ; 1 994: 80 83 st Z, r24 996: 80 81 ld r24, Z 998: 8d 7f andi r24, 0xFD ; 253 99a: 80 83 st Z, r24 } if (0) { } else if (!pwm1stat()) { // channel off 99c: ef e4 ldi r30, 0x4F ; 79 99e: f0 e0 ldi r31, 0x00 ; 0 9a0: 80 81 ld r24, Z 9a2: 88 23 and r24, r24 9a4: 3c f0 brlt .+14 ; 0x9b4 <__stack+0x155> Status1Tog(); 9a6: f8 01 movw r30, r16 9a8: 80 81 ld r24, Z 9aa: fc e0 ldi r31, 0x0C ; 12 9ac: 8f 27 eor r24, r31 9ae: f8 01 movw r30, r16 9b0: 80 83 st Z, r24 9b2: 2b c0 rjmp .+86 ; 0xa0a <__stack+0x1ab> } else if ((AdcValues[3]>768)&(AdcValues[3]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 9b4: e4 e7 ldi r30, 0x74 ; 116 9b6: f0 e0 ldi r31, 0x00 ; 0 9b8: 80 81 ld r24, Z 9ba: 91 81 ldd r25, Z+1 ; 0x01 9bc: 20 81 ld r18, Z 9be: 31 81 ldd r19, Z+1 ; 0x01 9c0: 81 50 subi r24, 0x01 ; 1 9c2: 93 40 sbci r25, 0x03 ; 3 9c4: 64 f0 brlt .+24 ; 0x9de <__stack+0x17f> 9c6: 2f 3f cpi r18, 0xFF ; 255 9c8: 31 05 cpc r19, r1 9ca: 09 f0 breq .+2 ; 0x9ce <__stack+0x16f> 9cc: 44 f4 brge .+16 ; 0x9de <__stack+0x17f> Status1Red(); 9ce: f8 01 movw r30, r16 9d0: 80 81 ld r24, Z 9d2: 88 60 ori r24, 0x08 ; 8 9d4: 80 83 st Z, r24 9d6: 80 81 ld r24, Z 9d8: 8b 7f andi r24, 0xFB ; 251 9da: 80 83 st Z, r24 9dc: 16 c0 rjmp .+44 ; 0xa0a <__stack+0x1ab> } else if (pwm1 > 64) { // piles of error yet 9de: ea e4 ldi r30, 0x4A ; 74 9e0: f0 e0 ldi r31, 0x00 ; 0 9e2: 80 81 ld r24, Z 9e4: 91 81 ldd r25, Z+1 ; 0x01 9e6: 81 34 cpi r24, 0x41 ; 65 9e8: 91 05 cpc r25, r1 9ea: 40 f0 brcs .+16 ; 0x9fc <__stack+0x19d> Status1Off(); 9ec: f8 01 movw r30, r16 9ee: 80 81 ld r24, Z 9f0: 8b 7f andi r24, 0xFB ; 251 9f2: 80 83 st Z, r24 9f4: 80 81 ld r24, Z 9f6: 87 7f andi r24, 0xF7 ; 247 9f8: 80 83 st Z, r24 9fa: 07 c0 rjmp .+14 ; 0xa0a <__stack+0x1ab> } else { // ok Status1Green(); 9fc: f8 01 movw r30, r16 9fe: 80 81 ld r24, Z a00: 84 60 ori r24, 0x04 ; 4 a02: 80 83 st Z, r24 a04: 80 81 ld r24, Z a06: 87 7f andi r24, 0xF7 ; 247 a08: 80 83 st Z, r24 } if (0) { } else if (!pwm2stat()) { // channel off a0a: ef e4 ldi r30, 0x4F ; 79 a0c: f0 e0 ldi r31, 0x00 ; 0 a0e: 80 81 ld r24, Z a10: 85 fd sbrc r24, 5 a12: 07 c0 rjmp .+14 ; 0xa22 <__stack+0x1c3> Status2Tog(); a14: f8 01 movw r30, r16 a16: 80 81 ld r24, Z a18: f0 e3 ldi r31, 0x30 ; 48 a1a: 8f 27 eor r24, r31 a1c: f8 01 movw r30, r16 a1e: 80 83 st Z, r24 a20: 2b c0 rjmp .+86 ; 0xa78 <__stack+0x219> } else if ((AdcValues[5]>768)&(AdcValues[5]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 a22: e8 e7 ldi r30, 0x78 ; 120 a24: f0 e0 ldi r31, 0x00 ; 0 a26: 80 81 ld r24, Z a28: 91 81 ldd r25, Z+1 ; 0x01 a2a: 20 81 ld r18, Z a2c: 31 81 ldd r19, Z+1 ; 0x01 a2e: 81 50 subi r24, 0x01 ; 1 a30: 93 40 sbci r25, 0x03 ; 3 a32: 64 f0 brlt .+24 ; 0xa4c <__stack+0x1ed> a34: 2f 3f cpi r18, 0xFF ; 255 a36: 31 05 cpc r19, r1 a38: 09 f0 breq .+2 ; 0xa3c <__stack+0x1dd> a3a: 44 f4 brge .+16 ; 0xa4c <__stack+0x1ed> Status2Red(); a3c: f8 01 movw r30, r16 a3e: 80 81 ld r24, Z a40: 80 62 ori r24, 0x20 ; 32 a42: 80 83 st Z, r24 a44: 80 81 ld r24, Z a46: 8f 7e andi r24, 0xEF ; 239 a48: 80 83 st Z, r24 a4a: 16 c0 rjmp .+44 ; 0xa78 <__stack+0x219> } else if (pwm2 > 64) { // piles of error yet a4c: e8 e4 ldi r30, 0x48 ; 72 a4e: f0 e0 ldi r31, 0x00 ; 0 a50: 80 81 ld r24, Z a52: 91 81 ldd r25, Z+1 ; 0x01 a54: 81 34 cpi r24, 0x41 ; 65 a56: 91 05 cpc r25, r1 a58: 40 f0 brcs .+16 ; 0xa6a <__stack+0x20b> Status2Off(); a5a: f8 01 movw r30, r16 a5c: 80 81 ld r24, Z a5e: 8f 7e andi r24, 0xEF ; 239 a60: 80 83 st Z, r24 a62: 80 81 ld r24, Z a64: 8f 7d andi r24, 0xDF ; 223 a66: 80 83 st Z, r24 a68: 07 c0 rjmp .+14 ; 0xa78 <__stack+0x219> } else { // ok Status2Green(); a6a: f8 01 movw r30, r16 a6c: 80 81 ld r24, Z a6e: 80 61 ori r24, 0x10 ; 16 a70: 80 83 st Z, r24 a72: 80 81 ld r24, Z a74: 8f 7d andi r24, 0xDF ; 223 a76: 80 83 st Z, r24 } if (0) { } else if (!pwm3stat()) { // channel off a78: e5 e4 ldi r30, 0x45 ; 69 a7a: f0 e0 ldi r31, 0x00 ; 0 a7c: 80 81 ld r24, Z a7e: 85 fd sbrc r24, 5 a80: 07 c0 rjmp .+14 ; 0xa90 <__stack+0x231> Status3Tog(); a82: f8 01 movw r30, r16 a84: 80 81 ld r24, Z a86: f0 ec ldi r31, 0xC0 ; 192 a88: 8f 27 eor r24, r31 a8a: f8 01 movw r30, r16 a8c: 80 83 st Z, r24 a8e: 29 c0 rjmp .+82 ; 0xae2 <__stack+0x283> } else if ((AdcValues[7]>768)&(AdcValues[7]<256)) { // (-3A) 0 (-1.5A)256 (0A)512 (1.5A)768 (3A) 1023 a90: ec e7 ldi r30, 0x7C ; 124 a92: f0 e0 ldi r31, 0x00 ; 0 a94: 80 81 ld r24, Z a96: 91 81 ldd r25, Z+1 ; 0x01 a98: 20 81 ld r18, Z a9a: 31 81 ldd r19, Z+1 ; 0x01 a9c: 81 50 subi r24, 0x01 ; 1 a9e: 93 40 sbci r25, 0x03 ; 3 aa0: 64 f0 brlt .+24 ; 0xaba <__stack+0x25b> aa2: 2f 3f cpi r18, 0xFF ; 255 aa4: 31 05 cpc r19, r1 aa6: 09 f0 breq .+2 ; 0xaaa <__stack+0x24b> aa8: 44 f4 brge .+16 ; 0xaba <__stack+0x25b> Status3Red(); aaa: f8 01 movw r30, r16 aac: 80 81 ld r24, Z aae: 80 68 ori r24, 0x80 ; 128 ab0: 80 83 st Z, r24 ab2: 80 81 ld r24, Z ab4: 8f 7b andi r24, 0xBF ; 191 ab6: 80 83 st Z, r24 ab8: 14 c0 rjmp .+40 ; 0xae2 <__stack+0x283> } else if (pwm3 > 64) { // piles of error yet aba: e3 e4 ldi r30, 0x43 ; 67 abc: f0 e0 ldi r31, 0x00 ; 0 abe: 80 81 ld r24, Z ac0: 81 34 cpi r24, 0x41 ; 65 ac2: 40 f0 brcs .+16 ; 0xad4 <__stack+0x275> Status3Off(); ac4: f8 01 movw r30, r16 ac6: 80 81 ld r24, Z ac8: 8f 7b andi r24, 0xBF ; 191 aca: 80 83 st Z, r24 acc: 80 81 ld r24, Z ace: 8f 77 andi r24, 0x7F ; 127 ad0: 80 83 st Z, r24 ad2: 07 c0 rjmp .+14 ; 0xae2 <__stack+0x283> } else { // ok Status3Green(); ad4: f8 01 movw r30, r16 ad6: 80 81 ld r24, Z ad8: 80 64 ori r24, 0x40 ; 64 ada: 80 83 st Z, r24 adc: 80 81 ld r24, Z ade: 8f 77 andi r24, 0x7F ; 127 ae0: 80 83 st Z, r24 calcWait = 0; // calculate servo loops while(1){ // while we wait to be cleared for recalculating, check the uart while(calcWait){ ae2: 80 91 7e 00 lds r24, 0x007E ae6: 88 23 and r24, r24 ae8: 09 f0 breq .+2 ; 0xaec <__stack+0x28d> aea: 07 cf rjmp .-498 ; 0x8fa <__stack+0x9b> output = (p->Kp * Perror + p->Kd * (Perror - p->PrevErr) + p->Ki * p->Ierror)/p->Ko; Kp = 30; Kd = 10; Ki = 2; Ko = 15; */ error = cmdpos[0]-AdcValues[1]; aec: f1 01 movw r30, r2 aee: 80 81 ld r24, Z af0: 91 81 ldd r25, Z+1 ; 0x01 af2: e1 e8 ldi r30, 0x81 ; 129 af4: f0 e0 ldi r31, 0x00 ; 0 af6: 20 81 ld r18, Z af8: 31 81 ldd r19, Z+1 ; 0x01 afa: 28 1b sub r18, r24 afc: 39 0b sbc r19, r25 if (ABS(error) < INTTHRESH) { In[0] = limit(In[0] + error, -INTTHRESH, INTTHRESH); } afe: c9 01 movw r24, r18 b00: 85 52 subi r24, 0x25 ; 37 b02: 9f 4f sbci r25, 0xFF ; 255 b04: 87 5b subi r24, 0xB7 ; 183 b06: 91 40 sbci r25, 0x01 ; 1 b08: 10 f0 brcs .+4 ; 0xb0e <__stack+0x2af> b0a: 62 01 movw r12, r4 b0c: 13 c0 rjmp .+38 ; 0xb34 <__stack+0x2d5> b0e: c6 01 movw r24, r12 b10: 82 0f add r24, r18 b12: 93 1f adc r25, r19 b14: 4f ef ldi r20, 0xFF ; 255 b16: 84 32 cpi r24, 0x24 ; 36 b18: 94 07 cpc r25, r20 b1a: 14 f4 brge .+4 ; 0xb20 <__stack+0x2c1> b1c: 84 e2 ldi r24, 0x24 ; 36 b1e: 9f ef ldi r25, 0xFF ; 255 b20: 6c 01 movw r12, r24 b22: 8d ed ldi r24, 0xDD ; 221 b24: c8 16 cp r12, r24 b26: d1 04 cpc r13, r1 b28: 2c f0 brlt .+10 ; 0xb34 <__stack+0x2d5> b2a: 0f 2e mov r0, r31 b2c: fc ed ldi r31, 0xDC ; 220 b2e: cf 2e mov r12, r31 b30: dd 24 eor r13, r13 b32: f0 2d mov r31, r0 else { In[0] = 0; } error = (Kp*error)+(In[0]/Ki); b34: c6 01 movw r24, r12 b36: dd 20 and r13, r13 b38: 0c f4 brge .+2 ; 0xb3c <__stack+0x2dd> b3a: 03 96 adiw r24, 0x03 ; 3 b3c: 95 95 asr r25 b3e: 87 95 ror r24 b40: 95 95 asr r25 b42: 87 95 ror r24 b44: 28 0f add r18, r24 b46: 39 1f adc r19, r25 pwm0 = limit(ABS(error), 0, 255); b48: a9 01 movw r20, r18 b4a: 33 23 and r19, r19 b4c: 1c f4 brge .+6 ; 0xb54 <__stack+0x2f5> b4e: 50 95 com r21 b50: 41 95 neg r20 b52: 5f 4f sbci r21, 0xFF ; 255 b54: 4f 3f cpi r20, 0xFF ; 255 b56: 51 05 cpc r21, r1 b58: 19 f0 breq .+6 ; 0xb60 <__stack+0x301> b5a: 14 f0 brlt .+4 ; 0xb60 <__stack+0x301> b5c: 8f ef ldi r24, 0xFF ; 255 b5e: 04 c0 rjmp .+8 ; 0xb68 <__stack+0x309> b60: 55 23 and r21, r21 b62: 0c f4 brge .+2 ; 0xb66 <__stack+0x307> b64: a2 01 movw r20, r4 b66: 84 2f mov r24, r20 b68: ec e5 ldi r30, 0x5C ; 92 b6a: f0 e0 ldi r31, 0x00 ; 0 b6c: 80 83 st Z, r24 if (error > 0) SetBit(DIR0BIT, DIR0PORT); b6e: 12 16 cp r1, r18 b70: 13 06 cpc r1, r19 b72: 2c f4 brge .+10 ; 0xb7e <__stack+0x31f> b74: f7 01 movw r30, r14 b76: 80 81 ld r24, Z b78: 80 61 ori r24, 0x10 ; 16 b7a: 80 83 st Z, r24 b7c: 04 c0 rjmp .+8 ; 0xb86 <__stack+0x327> else ClearBit(DIR0BIT, DIR0PORT); b7e: f7 01 movw r30, r14 b80: 80 81 ld r24, Z b82: 8f 7e andi r24, 0xEF ; 239 b84: 80 83 st Z, r24 error = cmdpos[1]-AdcValues[3]; b86: e4 e7 ldi r30, 0x74 ; 116 b88: f0 e0 ldi r31, 0x00 ; 0 b8a: 80 81 ld r24, Z b8c: 91 81 ldd r25, Z+1 ; 0x01 b8e: e3 e8 ldi r30, 0x83 ; 131 b90: f0 e0 ldi r31, 0x00 ; 0 b92: 20 81 ld r18, Z b94: 31 81 ldd r19, Z+1 ; 0x01 b96: 28 1b sub r18, r24 b98: 39 0b sbc r19, r25 if (ABS(error) < INTTHRESH) { In[1] = limit(In[1] + error, -INTTHRESH, INTTHRESH); } b9a: c9 01 movw r24, r18 b9c: 85 52 subi r24, 0x25 ; 37 b9e: 9f 4f sbci r25, 0xFF ; 255 ba0: 87 5b subi r24, 0xB7 ; 183 ba2: 91 40 sbci r25, 0x01 ; 1 ba4: 10 f0 brcs .+4 ; 0xbaa <__stack+0x34b> ba6: 52 01 movw r10, r4 ba8: 13 c0 rjmp .+38 ; 0xbd0 <__stack+0x371> baa: c5 01 movw r24, r10 bac: 82 0f add r24, r18 bae: 93 1f adc r25, r19 bb0: 4f ef ldi r20, 0xFF ; 255 bb2: 84 32 cpi r24, 0x24 ; 36 bb4: 94 07 cpc r25, r20 bb6: 14 f4 brge .+4 ; 0xbbc <__stack+0x35d> bb8: 84 e2 ldi r24, 0x24 ; 36 bba: 9f ef ldi r25, 0xFF ; 255 bbc: 5c 01 movw r10, r24 bbe: 8d ed ldi r24, 0xDD ; 221 bc0: a8 16 cp r10, r24 bc2: b1 04 cpc r11, r1 bc4: 2c f0 brlt .+10 ; 0xbd0 <__stack+0x371> bc6: 0f 2e mov r0, r31 bc8: fc ed ldi r31, 0xDC ; 220 bca: af 2e mov r10, r31 bcc: bb 24 eor r11, r11 bce: f0 2d mov r31, r0 else { In[1] = 0; } error = (Kp*error)+(In[1]/Ki); bd0: c5 01 movw r24, r10 bd2: bb 20 and r11, r11 bd4: 0c f4 brge .+2 ; 0xbd8 <__stack+0x379> bd6: 03 96 adiw r24, 0x03 ; 3 bd8: 95 95 asr r25 bda: 87 95 ror r24 bdc: 95 95 asr r25 bde: 87 95 ror r24 be0: 28 0f add r18, r24 be2: 39 1f adc r19, r25 pwm1 = limit(ABS(error), 0, 255); be4: a9 01 movw r20, r18 be6: 33 23 and r19, r19 be8: 1c f4 brge .+6 ; 0xbf0 <__stack+0x391> bea: 50 95 com r21 bec: 41 95 neg r20 bee: 5f 4f sbci r21, 0xFF ; 255 bf0: 4f 3f cpi r20, 0xFF ; 255 bf2: 51 05 cpc r21, r1 bf4: 21 f0 breq .+8 ; 0xbfe <__stack+0x39f> bf6: 1c f0 brlt .+6 ; 0xbfe <__stack+0x39f> bf8: 4f ef ldi r20, 0xFF ; 255 bfa: 50 e0 ldi r21, 0x00 ; 0 bfc: 03 c0 rjmp .+6 ; 0xc04 <__stack+0x3a5> bfe: 55 23 and r21, r21 c00: 0c f4 brge .+2 ; 0xc04 <__stack+0x3a5> c02: a2 01 movw r20, r4 c04: ea e4 ldi r30, 0x4A ; 74 c06: f0 e0 ldi r31, 0x00 ; 0 c08: 51 83 std Z+1, r21 ; 0x01 c0a: 40 83 st Z, r20 if (error > 0) SetBit(DIR1BIT, DIR1PORT); c0c: 12 16 cp r1, r18 c0e: 13 06 cpc r1, r19 c10: 2c f4 brge .+10 ; 0xc1c <__stack+0x3bd> c12: f7 01 movw r30, r14 c14: 80 81 ld r24, Z c16: 80 62 ori r24, 0x20 ; 32 c18: 80 83 st Z, r24 c1a: 04 c0 rjmp .+8 ; 0xc24 <__stack+0x3c5> else ClearBit(DIR1BIT, DIR1PORT); c1c: f7 01 movw r30, r14 c1e: 80 81 ld r24, Z c20: 8f 7d andi r24, 0xDF ; 223 c22: 80 83 st Z, r24 error = cmdpos[2]-AdcValues[5]; c24: e8 e7 ldi r30, 0x78 ; 120 c26: f0 e0 ldi r31, 0x00 ; 0 c28: 80 81 ld r24, Z c2a: 91 81 ldd r25, Z+1 ; 0x01 c2c: e5 e8 ldi r30, 0x85 ; 133 c2e: f0 e0 ldi r31, 0x00 ; 0 c30: 20 81 ld r18, Z c32: 31 81 ldd r19, Z+1 ; 0x01 c34: 28 1b sub r18, r24 c36: 39 0b sbc r19, r25 if (ABS(error) < INTTHRESH) { In[2] = limit(In[2] + error, -INTTHRESH, INTTHRESH); } c38: c9 01 movw r24, r18 c3a: 85 52 subi r24, 0x25 ; 37 c3c: 9f 4f sbci r25, 0xFF ; 255 c3e: 87 5b subi r24, 0xB7 ; 183 c40: 91 40 sbci r25, 0x01 ; 1 c42: 10 f0 brcs .+4 ; 0xc48 <__stack+0x3e9> c44: 42 01 movw r8, r4 c46: 13 c0 rjmp .+38 ; 0xc6e <__stack+0x40f> c48: c4 01 movw r24, r8 c4a: 82 0f add r24, r18 c4c: 93 1f adc r25, r19 c4e: 4f ef ldi r20, 0xFF ; 255 c50: 84 32 cpi r24, 0x24 ; 36 c52: 94 07 cpc r25, r20 c54: 14 f4 brge .+4 ; 0xc5a <__stack+0x3fb> c56: 84 e2 ldi r24, 0x24 ; 36 c58: 9f ef ldi r25, 0xFF ; 255 c5a: 4c 01 movw r8, r24 c5c: 8d ed ldi r24, 0xDD ; 221 c5e: 88 16 cp r8, r24 c60: 91 04 cpc r9, r1 c62: 2c f0 brlt .+10 ; 0xc6e <__stack+0x40f> c64: 0f 2e mov r0, r31 c66: fc ed ldi r31, 0xDC ; 220 c68: 8f 2e mov r8, r31 c6a: 99 24 eor r9, r9 c6c: f0 2d mov r31, r0 else { In[2] = 0; } error = (Kp*error)+(In[2]/Ki); c6e: c4 01 movw r24, r8 c70: 99 20 and r9, r9 c72: 0c f4 brge .+2 ; 0xc76 <__stack+0x417> c74: 03 96 adiw r24, 0x03 ; 3 c76: 95 95 asr r25 c78: 87 95 ror r24 c7a: 95 95 asr r25 c7c: 87 95 ror r24 c7e: 28 0f add r18, r24 c80: 39 1f adc r19, r25 pwm2 = limit(ABS(error), 0, 255); c82: a9 01 movw r20, r18 c84: 33 23 and r19, r19 c86: 1c f4 brge .+6 ; 0xc8e <__stack+0x42f> c88: 50 95 com r21 c8a: 41 95 neg r20 c8c: 5f 4f sbci r21, 0xFF ; 255 c8e: 4f 3f cpi r20, 0xFF ; 255 c90: 51 05 cpc r21, r1 c92: 21 f0 breq .+8 ; 0xc9c <__stack+0x43d> c94: 1c f0 brlt .+6 ; 0xc9c <__stack+0x43d> c96: 4f ef ldi r20, 0xFF ; 255 c98: 50 e0 ldi r21, 0x00 ; 0 c9a: 03 c0 rjmp .+6 ; 0xca2 <__stack+0x443> c9c: 55 23 and r21, r21 c9e: 0c f4 brge .+2 ; 0xca2 <__stack+0x443> ca0: a2 01 movw r20, r4 ca2: e8 e4 ldi r30, 0x48 ; 72 ca4: f0 e0 ldi r31, 0x00 ; 0 ca6: 51 83 std Z+1, r21 ; 0x01 ca8: 40 83 st Z, r20 if (error > 0) SetBit(DIR2BIT, DIR2PORT); caa: 12 16 cp r1, r18 cac: 13 06 cpc r1, r19 cae: 2c f4 brge .+10 ; 0xcba <__stack+0x45b> cb0: f7 01 movw r30, r14 cb2: 80 81 ld r24, Z cb4: 80 64 ori r24, 0x40 ; 64 cb6: 80 83 st Z, r24 cb8: 04 c0 rjmp .+8 ; 0xcc2 <__stack+0x463> else ClearBit(DIR2BIT, DIR2PORT); cba: f7 01 movw r30, r14 cbc: 80 81 ld r24, Z cbe: 8f 7b andi r24, 0xBF ; 191 cc0: 80 83 st Z, r24 error = cmdpos[3]-AdcValues[7]; cc2: ec e7 ldi r30, 0x7C ; 124 cc4: f0 e0 ldi r31, 0x00 ; 0 cc6: 80 81 ld r24, Z cc8: 91 81 ldd r25, Z+1 ; 0x01 cca: e7 e8 ldi r30, 0x87 ; 135 ccc: f0 e0 ldi r31, 0x00 ; 0 cce: 20 81 ld r18, Z cd0: 31 81 ldd r19, Z+1 ; 0x01 cd2: 28 1b sub r18, r24 cd4: 39 0b sbc r19, r25 if (ABS(error) < INTTHRESH) { In[3] = limit(In[3] + error, -INTTHRESH, INTTHRESH); } cd6: c9 01 movw r24, r18 cd8: 85 52 subi r24, 0x25 ; 37 cda: 9f 4f sbci r25, 0xFF ; 255 cdc: 87 5b subi r24, 0xB7 ; 183 cde: 91 40 sbci r25, 0x01 ; 1 ce0: 10 f0 brcs .+4 ; 0xce6 <__stack+0x487> ce2: 32 01 movw r6, r4 ce4: 13 c0 rjmp .+38 ; 0xd0c <__stack+0x4ad> ce6: c3 01 movw r24, r6 ce8: 82 0f add r24, r18 cea: 93 1f adc r25, r19 cec: 4f ef ldi r20, 0xFF ; 255 cee: 84 32 cpi r24, 0x24 ; 36 cf0: 94 07 cpc r25, r20 cf2: 14 f4 brge .+4 ; 0xcf8 <__stack+0x499> cf4: 84 e2 ldi r24, 0x24 ; 36 cf6: 9f ef ldi r25, 0xFF ; 255 cf8: 3c 01 movw r6, r24 cfa: 8d ed ldi r24, 0xDD ; 221 cfc: 68 16 cp r6, r24 cfe: 71 04 cpc r7, r1 d00: 2c f0 brlt .+10 ; 0xd0c <__stack+0x4ad> d02: 0f 2e mov r0, r31 d04: fc ed ldi r31, 0xDC ; 220 d06: 6f 2e mov r6, r31 d08: 77 24 eor r7, r7 d0a: f0 2d mov r31, r0 else { In[3] = 0; } error = (Kp*error)+(In[3]/Ki); d0c: c3 01 movw r24, r6 d0e: 77 20 and r7, r7 d10: 0c f4 brge .+2 ; 0xd14 <__stack+0x4b5> d12: 03 96 adiw r24, 0x03 ; 3 d14: 95 95 asr r25 d16: 87 95 ror r24 d18: 95 95 asr r25 d1a: 87 95 ror r24 d1c: 28 0f add r18, r24 d1e: 39 1f adc r19, r25 pwm3 = limit(ABS(error), 0, 255); d20: a9 01 movw r20, r18 d22: 33 23 and r19, r19 d24: 1c f4 brge .+6 ; 0xd2c <__stack+0x4cd> d26: 50 95 com r21 d28: 41 95 neg r20 d2a: 5f 4f sbci r21, 0xFF ; 255 d2c: 4f 3f cpi r20, 0xFF ; 255 d2e: 51 05 cpc r21, r1 d30: 19 f0 breq .+6 ; 0xd38 <__stack+0x4d9> d32: 14 f0 brlt .+4 ; 0xd38 <__stack+0x4d9> d34: 8f ef ldi r24, 0xFF ; 255 d36: 04 c0 rjmp .+8 ; 0xd40 <__stack+0x4e1> d38: 55 23 and r21, r21 d3a: 0c f4 brge .+2 ; 0xd3e <__stack+0x4df> d3c: a2 01 movw r20, r4 d3e: 84 2f mov r24, r20 d40: e3 e4 ldi r30, 0x43 ; 67 d42: f0 e0 ldi r31, 0x00 ; 0 d44: 80 83 st Z, r24 if (error > 0) SetBit(DIR3BIT, DIR3PORT); d46: 12 16 cp r1, r18 d48: 13 06 cpc r1, r19 d4a: 2c f4 brge .+10 ; 0xd56 <__stack+0x4f7> d4c: f7 01 movw r30, r14 d4e: 80 81 ld r24, Z d50: 80 68 ori r24, 0x80 ; 128 d52: 80 83 st Z, r24 d54: 04 c0 rjmp .+8 ; 0xd5e <__stack+0x4ff> else ClearBit(DIR3BIT, DIR3PORT); d56: f7 01 movw r30, r14 d58: 80 81 ld r24, Z d5a: 8f 77 andi r24, 0x7F ; 127 d5c: 80 83 st Z, r24 calcWait = 1; d5e: f1 e0 ldi r31, 0x01 ; 1 d60: f0 93 7e 00 sts 0x007E, r31 d64: be ce rjmp .-644 ; 0xae2 <__stack+0x283> 00000d66 : | +---------+ V+ . PB2 [o o o] | | PB3 [o o o] | chan0 pwm | PB4 [o o o] | chan0 dir | PB5 [o o o] | chan1 dir | PB6 [o o o] | chan2 dir | PB7 [o o o] | chan3 dir d66: e0 e4 ldi r30, 0x40 ; 64 d68: f0 e0 ldi r31, 0x00 ; 0 d6a: 90 83 st Z, r25 | PA0 [o o o] | d6c: 89 b9 out 0x09, r24 ; 9 | PA1 [o o o] | chan0 feedback voltage | +----------+ PA2 [o o o] | chan0 current sense | |O | PA3 [o o o] | chan1 feedback voltage d6e: 88 e1 ldi r24, 0x18 ; 24 d70: 8a b9 out 0x0a, r24 ; 10 | | | PA4 [o o o] | chan1 current sense | | | PA5 [o o o] | chan2 feedback voltage | | | PA6 [o o o] | chan2 current sense | | | PA7 [o o o] | chan3 feedback voltage d72: 8e e8 ldi r24, 0x8E ; 142 d74: 80 83 st Z, r24 | | | PC7 [o o o] | chan3 current sense d76: 08 95 ret 00000d78 : | | | PC5 [o o o] | | | ATMEGA32 | PC4 [o o o] | | | | PC3 [o o o] | | | | PC2 [o o o] | | | | PC1 [o o o] | | | | PC0 [o o o] | d78: eb e2 ldi r30, 0x2B ; 43 d7a: f0 e0 ldi r31, 0x00 ; 0 d7c: 80 81 ld r24, Z d7e: 88 23 and r24, r24 d80: ec f7 brge .-6 ; 0xd7c | | | PD3 [o o o] | | | | PD4 [o o o] | chan2 pwm | | | PD5 [o o o] | chan1 pwm | +----------+ PD6 [o o o] | Debug | E.D.S BABYBOARD III | +-------------------------------------+ d82: 8c b1 in r24, 0x0c ; 12 d84: 08 95 ret 00000d86 : */ /* Servo Recieves: d86: 98 2f mov r25, r24 [0][c3][c2][c1][c0][v2][v1][v0] d88: eb e2 ldi r30, 0x2B ; 43 d8a: f0 e0 ldi r31, 0x00 ; 0 d8c: 80 81 ld r24, Z d8e: 85 ff sbrs r24, 5 d90: fd cf rjmp .-6 ; 0xd8c [1][v9][v8][v7][v6][v5][v4][v3] c0-c3 are command d92: 9c b9 out 0x0c, r25 ; 12 v0-v9 are value d94: 08 95 ret 00000d96 : Commands: 0 Clear ping counter {always obey command} d96: 1f 93 push r17 d98: 28 2f mov r18, r24 1 pong if ping counter matches id and inc ping counter {always obey command} 2 listen (servo number) 256 = all {always obey command} // sticks through listen once 3 ignore (servo number) 256 = all {always obey command} // overrides listen once 4 turn servo on {obey if listening} d9a: 82 95 swap r24 d9c: 8f 70 andi r24, 0x0F ; 15 5 turn servo off {obey if listening} d9e: 98 2f mov r25, r24 da0: 90 5d subi r25, 0xD0 ; 208 6 set servo position to argument {obey if listening} da2: 9a 33 cpi r25, 0x3A ; 58 da4: 08 f0 brcs .+2 ; 0xda8 7 get servo position {obey if listening} da6: 99 5f subi r25, 0xF9 ; 249 - 8 One Time listen (servo number) {always obey command} da8: 82 2f mov r24, r18 daa: 8f 70 andi r24, 0x0F ; 15 dac: 18 2f mov r17, r24 dae: 10 5d subi r17, 0xD0 ; 208 - 9 set cached position to argument {obey if listening} db0: 1a 33 cpi r17, 0x3A ; 58 db2: 08 f0 brcs .+2 ; 0xdb6 - 10 enguage cached position {always obey command} db4: 19 5f subi r17, 0xF9 ; 249 11 db6: 89 2f mov r24, r25 db8: 0e 94 c3 06 call 0xd86 ; 0xd86 12 dbc: 81 2f mov r24, r17 dbe: 0e 94 c3 06 call 0xd86 ; 0xd86 13 dc2: 1f 91 pop r17 dc4: 08 95 ret 00000dc6 : 14 15 send device model {obey if listening} dc6: cf 93 push r28 dc8: df 93 push r29 dca: ec 01 movw r28, r24 Servo Sends: dcc: 88 81 ld r24, Y dce: 88 23 and r24, r24 dd0: 31 f0 breq .+12 ; 0xdde [0][p3][p2][p1][p0][v2][v1][v0] dd2: 0e 94 c3 06 call 0xd86 ; 0xd86 [1][v9][v8][v7][v6][v5][v4][v3] dd6: 21 96 adiw r28, 0x01 ; 1 13 14 15 send device model {obey if listening} Servo Sends: dd8: 88 81 ld r24, Y dda: 88 23 and r24, r24 ddc: d1 f7 brne .-12 ; 0xdd2 [0][p3][p2][p1][p0][v2][v1][v0] [1][v9][v8][v7][v6][v5][v4][v3] p0-p3 are paramiter number v0-v9 are value dde: df 91 pop r29 de0: cf 91 pop r28 de2: 08 95 ret 00000de4 <_exit>: de4: f8 94 cli 00000de6 <__stop_program>: de6: ff cf rjmp .-2 ; 0xde6 <__stop_program>