#define __SFR_OFFSET 0 #include // #define watchdog_enable #define interrupt_enable .data TIMER_0_INT: .space 1 data04C3: .space 1 data04D0: .space 1 data04D1: .space 1 data04D3: .space 1 data04D8: .space 1 data050A: .space 1 data0511: .space 1 TIMER_COUNT: .space 1 MOTION_PHASE: .space 1 DELAY_LOW: .space 1 DELAY_HIGH: .space 1 DELAY_BUSY: .space 1 C3024_STATUS: .space 1 DIR_G8A: .space 1 DIR_G8B: .space 1 DIR_G8C: .space 1 DIR_G8D: .space 1 PTP_STATE: .space 1 ENABLE_G8A: .space 1 ENABLE_G8B: .space 1 ENABLE_G8C: .space 1 ENABLE_G8D: .space 1 MOTION_G8A: .space 1 MOTION_G8B: .space 1 MOTION_G8C: .space 1 MOTION_G8D: .space 1 DEF_SPEED: .space 1 HIGH_SPEED: .space 1 // SERVO_EN_G8A: .space 1 // SERVO_EN_G8B: .space 1 // SERVO_EN_G8C: .space 1 // SERVO_EN_G8D: .space 1 SERVO_CURRENT: .space 32 ; 0x320 SERVO_DESIRED: .space 32 ; 0x300 SERVO_OFFSET: .space 32 ; 0x340 SERVO_SPEED: .space 32 ; 0x3A0 SERVO_CALC: .space 32 ; 0x360 SERVO_ACT: .space 32 ; 0x380 SERVO_DELTA: .space 32 ; 0x3C0 SERVO_TRIGGER: .space 32 ; 0x3E0 SERVO_MOVE: .space 32 ; 0x400 LARGEST_MOVE: .space 1 GYRO_ENABLE: .space 1 ; Gyro ; bit 0 = Gyro 1 ; Bit 1 = Gyro 2 ; bit 2 = Gyro 3 ; bit 3 = Gyro 4 ; bit 4 = 0 = GWS 1 = KRG-1 GYRO_VALUE_1: .space 1 ; Gyro 1 read value GYRO_VALUE_2: .space 1 ; Gyro 2 read value GYRO_VALUE_3: .space 1 ; Gyro 3 read value GYRO_VALUE_4: .space 1 ; Gyro 4 read value GYRO_SETTING: .space 32 GYRO_DIR: .space 32 GYRO_SENSE: .space 32 GYRO_VALUE: .space 32 GYRO_CALC: .space 1 TEMP: .space 32 .text .global motion_init ; .global motorin ; .global dir ; .global getmotorset ; .global motor ; .global motoroff ; .global zero ; .global speed .global move ; .global servo ; .global ptp ; .global servo_ctl ; .global servo_done ; .global delay_set .global delay_wait ; .global delay_busy ; .global gyro_set .global gryo_dir .global gyro_sense .global TIMER0_OVF_vect ;------------------------------------------------------------------------- ; ; Global function to initialise the C3024 motion controller ; this sets up the motion registers and timers ; also then enables interrupts ; ; void motion_init(void); motion_init: rcall INIT_OFFSET rcall INIT_POSITION rcall INIT_DIR rcall INIT_SPEED ; rcall INIT_WDOG ldi r18, 0x84 ; enable adc and interrupt out 0x06, r18 ldi r18, 0x05 ; set TCCR0 Tosc/128 = 17.3 usec out 0x33, r18 in r18, 0x37 ; enable t0 int sbr r18, 0x01 out 0x37,r18 ldi r18, 0x19 ; send 0x19 to timer count 0 out 0x32, r18 ; reset the count to 4 millisec (counts up) rcall MOVE_INIT rcall GYRO_INIT #ifdef interrupt_enable sei #endif /* interrupt_enable */ ret ;------------------------------------------------------------------------- ; set zero offset to 0 ; clear data memory . for 32 bytes INIT_OFFSET: ldi XH, hi8(SERVO_OFFSET) ldi XL, lo8(SERVO_OFFSET) ldi r18, 0x20 ldi r19, 0x00 IOFFSET1: st X+, r19 dec r18 brne IOFFSET1 ret ;------------------------------------------------------------------------ ; Set initial position data to 100 ; set data memory for 32 bytes INIT_POSITION: ldi XH, hi8(SERVO_CURRENT) ldi XL, lo8(SERVO_CURRENT) ldi r18, 0x20 ldi r19, 0x64 INITPOS1: st X+, r19 dec r18 brne INITPOS1 ldi XH, hi8(SERVO_DESIRED) ldi XL, lo8(SERVO_DESIRED) ldi r18, 0x20 ldi r19, 0x64 INITPOS2: st X+, r19 dec r18 brne INITPOS2 ret ;------------------------------------------------------------------------- ; set all initial directions ; clear data memory for 4 bytes INIT_DIR: ldi XH, hi8(DIR_G8A) ldi XL, lo8(DIR_G8A) ldi r18, 0x4 ldi r19, 0x00 INITDIR1: st X+, r19 dec r18 brne INITDIR1 ret ;------------------------------------------------------------------------- ; initialise Speeds ; initialise for 20 to 0x32 INIT_SPEED: ldi XH, hi8(SERVO_SPEED) ldi XL, lo8(SERVO_SPEED) ldi r18, 0x20 INITSPEED1: ldi r19, 0x32 st X+, r19 dec r18 brne INITSPEED1 ldi r19, 0x02 ; default speed = 2 sts DEF_SPEED, r19 ret ;------------------------------------------------------------------------- ; enable watchdog timer ; #ifdef watchdog_enable INIT_WDOG: ldi r18, 0x0F ; enable watchdog timer at 1.8 seconds out 0x21, r18 ret #endif /* watchdog_enable*/ ;------------------------------------------------------------------------- ; Initialise move data MOVE_INIT: clr r18 sts data04D3, r18 ; ? sts MOTION_PHASE, r18 ;Phase number = 0 sts ENABLE_G8A, r18 sts ENABLE_G8B, r18 sts ENABLE_G8C, r18 sts ENABLE_G8D, r18 sts DELAY_LOW, r18 ; Clear delay counter sts DELAY_HIGH, r18 sts DELAY_BUSY, r18 sts data04D0, r18 ; ? sts TIMER_0_INT, r18 ; Clear Timer 0 int in progress (bit 7) sts HIGH_SPEED, r18 ; 01CF 9300 04EF sts data04D1, r18 ; ? sts data04D8, r18 ; ? sts C3024_STATUS, r18 ; Clear controller Status sts data050A, r18 ; ? sts data04C3, r18 ; ? sts data0511, r18 ; ? sts GYRO_CALC, r18 ; ? ret ;------------------------------------------------------------- ; Initialise Gyros GYRO_INIT: ldi r18, 0x00 ; disable all gyro sts GYRO_ENABLE, r18 ldi r18, 0x2C ; Gyro read value for 4 locations = 44 ldi r19, 0x04 ldi XH, hi8(GYRO_VALUE_1) ldi XL, lo8(GYRO_VALUE_1) GYROINIT1: st X+, r18 dec r19 brne GYROINIT1 ldi r18, 0x2C ; for 32 locations = 44 ldi r19, 0x20 ldi XH, hi8(GYRO_VALUE) ldi XL, lo8(GYRO_VALUE) GYROINIT2: st X+, r18 dec r19 brne GYROINIT2 ldi r18, 0x00 ; for 32 locations = 0 ldi r19, 0x20 ldi XH, hi8(GYRO_SETTING) ldi XL, lo8(GYRO_SETTING) GYROINIT3: st X+, r18 dec r19 brne GYROINIT3 ldi r18, 0x01 ; for 32 locations = 1 ldi r19, 0x20 ldi XH, hi8(GYRO_DIR) ldi XL, lo8(GYRO_DIR) GYROINIT4: st X+, r18 dec r19 brne GYROINIT4 ldi r18, 0x01 ; for 32 locations = 1 ldi r19, 0x20 ldi XH, hi8(GYRO_SENSE) ldi XL, lo8(GYRO_SENSE) GYROINIT5: st X+, r18 dec r19 brne GYROINIT5 ret ;------------------------------------------------------------------------- ; global function to read servo position ; uint8_t motor_in(uint8_t) ; servo is in low 5 bits of r24, next 2 bits (5,6) are pulse length ( 0 = 50, 1 = 100, 2 = 150, 3 = 200) microsec ; result is in r24 motorin: ldi r21, 0x05 ; 45CD E015 default five tries mov r18, r24 ; 45CE 2F20 avr45CF: mov r20, r18 ; 45CF 2F02 rcall sub45D7 ; 45D0 D006 cpi r22, 0x00 ; 45D1 30C0 breq avr45D4 ; 45D2 F009 mov r24, r22 ret ; 45D3 9508 avr45D4: dec r21 ; 45D4 951A brne avr45CF ; 45D5 F7C9 ret ; 45D6 9508 sub45D7: #ifdef interrupt_enable cli #endif /* interrupt_enable */ push r21 ; 45D8 931F push r18 ; 45D9 932F mov r21, r20 ; 45DA 2F10 subi r21, 0x20 ; 45DB 5210 if < 50 uS pulse brlo avr45E7 ; 45DC F050 mov r21, r20 ; 45DD 2F10 subi r21, 0x40 ; 45DE 5410 if > 20 and < 40 100 uS pulse brlo avr45E9 ; 45DF F048 mov r21, r20 ; 45E0 2F10 subi r21, 0x60 ; 45E1 5610 if > 40 and < 60 150 uS pulse brlo avr45EC ; 45E2 F048 mov r21, r20 ; 45E3 2F10 subi r21, 0x80 ; 45E4 5810if > 60 and < 80 200uS pulse brlo avr45EF ; 45E5 F048 rjmp avr45E7 ; 45E6 C000 default 50 uS avr45E7: ldi r21, 0x00 ; 45E7 E010 rjmp avr45F1 ; 45E8 C008 avr45E9: subi r20, 0x20 ; 45E9 5200 ldi r21, 0x01 ; 45EA E011 rjmp avr45F1 ; 45EB C005 avr45EC: subi r20, 0x40 ; 45EC 5400 ldi r21, 0x02 ; 45ED E012 rjmp avr45F1 ; 45EE C002 avr45EF: subi r20, 0x60 ; 45EF 5600 ldi r21, 0x03 ; 45F0 E013 avr45F1: cpi r20, 0x00 ; 45F1 3000 go to individual position reads brne avr45F4 ; 45F2 F409 rjmp avr4656 ; 45F3 C062 avr45F4: cpi r20, 0x01 ; 45F4 3001 brne avr45F7 ; 45F5 F409 rjmp avr4696 ; 45F6 C09F avr45F7: cpi r20, 0x02 ; 45F7 3002 brne avr45FA ; 45F8 F409 rjmp avr46D0 ; 45F9 C0D6 avr45FA: cpi r20, 0x03 ; 45FA 3003 brne avr45FD ; 45FB F409 rjmp avr470A ; 45FC C10D avr45FD: cpi r20, 0x04 ; 45FD 3004 brne avr4600 ; 45FE F409 rjmp avr4744 ; 45FF C144 avr4600: cpi r20, 0x05 ; 4600 3005 brne avr4603 ; 4601 F409 rjmp avr477E ; 4602 C17B avr4603: cpi r20, 0x06 ; 4603 3006 brne avr4606 ; 4604 F409 rjmp avr47B8 ; 4605 C1B2 avr4606: cpi r20, 0x07 ; 4606 3007 brne avr4609 ; 4607 F409 rjmp avr47F2 ; 4608 C1E9 avr4609: cpi r20, 0x08 ; 4609 3008 brne avr460C ; 460A F409 rjmp avr482C ; 460B C220 avr460C: cpi r20, 0x09 ; 460C 3009 brne avr460F ; 460D F409 rjmp avr4866 ; 460E C257 avr460F: cpi r20, 0x0A ; 460F 300A brne avr4612 ; 4610 F409 rjmp avr48A0 ; 4611 C28E avr4612: cpi r20, 0x0B ; 4612 300B brne avr4615 ; 4613 F409 rjmp avr48DA ; 4614 C2C5 avr4615: cpi r20, 0x0C ; 4615 300C brne avr4618 ; 4616 F409 rjmp avr4914 ; 4617 C2FC avr4618: cpi r20, 0x0D ; 4618 300D brne avr461B ; 4619 F409 rjmp avr494E ; 461A C333 avr461B: cpi r20, 0x0E ; 461B 300E brne avr461E ; 461C F409 rjmp avr4988 ; 461D C36A avr461E: cpi r20, 0x0F ; 461E 300F brne avr4621 ; 461F F409 rjmp avr49C2 ; 4620 C3A1 avr4621: cpi r20, 0x10 ; 4621 3100 brne avr4624 ; 4622 F409 rjmp avr49FC ; 4623 C3D8 avr4624: cpi r20, 0x11 ; 4624 3101 brne avr4627 ; 4625 F409 rjmp avr4A36 ; 4626 C40F avr4627: cpi r20, 0x12 ; 4627 3102 brne avr462A ; 4628 F409 rjmp avr4A70 ; 4629 C446 avr462A: cpi r20, 0x13 ; 462A 3103 brne avr462D ; 462B F409 rjmp avr4AAA ; 462C C47D avr462D: cpi r20, 0x14 ; 462D 3104 brne avr4630 ; 462E F409 rjmp avr4AE4 ; 462F C4B4 avr4630: cpi r20, 0x15 ; 4630 3105 brne avr4633 ; 4631 F409 rjmp avr4B1E ; 4632 C4EB avr4633: cpi r20, 0x16 ; 4633 3106 brne avr4636 ; 4634 F409 rjmp avr4B58 ; 4635 C522 avr4636: cpi r20, 0x17 ; 4636 3107 brne avr4639 ; 4637 F409 rjmp avr4B92 ; 4638 C559 avr4639: cpi r20, 0x18 ; 4639 3108 brne avr463C ; 463A F409 rjmp avr4BCC ; 463B C590 avr463C: cpi r20, 0x19 ; 463C 3109 brne avr463F ; 463D F409 rjmp avr4C06 ; 463E C5C7 avr463F: cpi r20, 0x1A ; 463F 310A brne avr4642 ; 4640 F409 rjmp avr4C40 ; 4641 C5FE avr4642: cpi r20, 0x1B ; 4642 310B brne avr4645 ; 4643 F409 rjmp avr4C7A ; 4644 C635 avr4645: cpi r20, 0x1C ; 4645 310C brne avr4648 ; 4646 F409 rjmp avr4CB4 ; 4647 C66C avr4648: cpi r20, 0x1D ; 4648 310D brne avr464B ; 4649 F409 rjmp avr4CEE ; 464A C6A3 avr464B: cpi r20, 0x1E ; 464B 310E brne avr464E ; 464C F409 rjmp avr4D50 ; 464D C702 avr464E: cpi r20, 0x1F ; 464E 310F brne avr4651 ; 464F F409 rjmp avr4654 ; 4650 C003 avr4651: clr r22 ; 4651 27CC jmp avr4E14 ; 4652 940C 4E14 avr4654: jmp avr4DB2 ; 4654 940C 4DB2 ;---------------------------------------------------------------------- ; avr4656: cbi PORTA, 0 ; 4656 98D8 set output low sbi DDRA, 0 ; 4657 9AD0 ldi r20, 0x14 ; 4658 E104 hold low for 5 microsec avr4659: dec r20 ; 4659 950A brne avr4659 ; 465A F7F1 sbi PORTA, 0 ; 465B 9AD8 set hi call sub4E24 ; 465C 940E 4E24 hold for length of pulse in r21 nop ; 465E 0000 nop ; 465F 0000 nop ; 4660 0000 cbi PORTA, 0 ; 4661 98D8 set low again call sub4E24 ; 4662 940E 4E24 hold for length of time in r21 nop ; 4664 0000 nop ; 4665 0000 nop ; 4666 0000 nop ; 4667 0000 ldi r20, 0x64 ; 4668 E604 plus 40 microsec avr4669: dec r20 ; 4669 950A brne avr4669 ; 466A F7F1 cbi DDRA, 0 ; 466B 98D0 set to input sbi PORTA, 0 ; 466C 9AD8 nop ; 466D 0000 ldi r20, 0x0A ; 466E E00A avr466F: dec r20 ; 466F 950A brne avr466F ; 4670 F7F1 sbic PINA, 0 ; 4671 99C8 if it is high then exit rjmp avr4691 ; 4672 C01E ldi r22, 0x01 ; 4673 E0C1 result = 1 clr r20 ; 4674 2700 avr4675: nop ; 4675 0000 nop ; 4676 0000 nop ; 4677 0000 nop ; 4678 0000 nop ; 4679 0000 sbic PINA, 0 ; 467A 99C8 rjmp avr467F ; 467B C003 dec r20 ; 467C 950A brne avr4675 ; 467D F7B9 rjmp avr4691 ; 467E C012 avr467F: call sub4ECD ; 467F 940E 4ECD wait 4 microsec avr4681: sbis PINA, 0 ; 4681 9BC8 rjmp avr468C ; 4682 C009 cpi r22, 0xFA ; 4683 3FCA brne avr4688 ; 4684 F419 rjmp avr468C ; 4685 C006 nop ; 4686 0000 nop ; 4687 0000 avr4688: call sub4ED2 ; 4688 940E 4ED2 inc r22 ; 468A 95C3 rjmp avr4681 ; 468B CFF5 avr468C: cbi PORTA, 0 ; 468C 98D8 sbi DDRA, 0 ; 468D 9AD0 ldi r20, 0x00 ; 468E E000 jmp avr4E14 ; 468F 940C 4E14 avr4691: cbi PORTA, 0 ; 4691 98D8 sbi DDRA, 0 ; 4692 9AD0 clr r22 ; 4693 27CC jmp avr4E14 ; 4694 940C 4E14 avr4696: cbi PORTA, 1 ; 4696 98D9 sbi DDRA, 1 ; 4697 9AD1 ldi r20, 0x14 ; 4698 E104 avr4699: dec r20 ; 4699 950A brne avr4699 ; 469A F7F1 sbi PORTA, 1 ; 469B 9AD9 call sub4E24 ; 469C 940E 4E24 nop ; 469E 0000 nop ; 469F 0000 nop ; 46A0 0000 cbi PORTA, 1 ; 46A1 98D9 call sub4E24 ; 46A2 940E 4E24 nop ; 46A4 0000 nop ; 46A5 0000 nop ; 46A6 0000 nop ; 46A7 0000 sbi PORTA, 1 ; 46A8 9AD9 cbi DDRA, 1 ; 46A9 98D1 nop ; 46AA 0000 sbic PINA, 1 ; 46AB 99C9 rjmp avr46CB ; 46AC C01E ldi r22, 0x01 ; 46AD E0C1 clr r20 ; 46AE 2700 avr46AF: nop ; 46AF 0000 nop ; 46B0 0000 nop ; 46B1 0000 nop ; 46B2 0000 nop ; 46B3 0000 sbic PINA, 1 ; 46B4 99C9 rjmp avr46B9 ; 46B5 C003 dec r20 ; 46B6 950A brne avr46AF ; 46B7 F7B9 rjmp avr46CB ; 46B8 C012 avr46B9: call sub4ECD ; 46B9 940E 4ECD avr46BB: sbis PINA, 1 ; 46BB 9BC9 rjmp avr46C6 ; 46BC C009 cpi r22, 0xFA ; 46BD 3FCA brne avr46C2 ; 46BE F419 rjmp avr46C6 ; 46BF C006 nop ; 46C0 0000 nop ; 46C1 0000 avr46C2: call sub4ED2 ; 46C2 940E 4ED2 inc r22 ; 46C4 95C3 rjmp avr46BB ; 46C5 CFF5 avr46C6: cbi PORTA, 1 ; 46C6 98D9 sbi DDRA, 1 ; 46C7 9AD1 ldi r20, 0x01 ; 46C8 E001 jmp avr4E14 ; 46C9 940C 4E14 avr46CB: cbi PORTA, 1 ; 46CB 98D9 sbi DDRA, 1 ; 46CC 9AD1 clr r22 ; 46CD 27CC jmp avr4E14 ; 46CE 940C 4E14 avr46D0: cbi PORTA, 2 ; 46D0 98DA sbi DDRA, 2 ; 46D1 9AD2 ldi r20, 0x14 ; 46D2 E104 avr46D3: dec r20 ; 46D3 950A brne avr46D3 ; 46D4 F7F1 sbi PORTA, 2 ; 46D5 9ADA call sub4E24 ; 46D6 940E 4E24 nop ; 46D8 0000 nop ; 46D9 0000 nop ; 46DA 0000 cbi PORTA, 2 ; 46DB 98DA call sub4E24 ; 46DC 940E 4E24 nop ; 46DE 0000 nop ; 46DF 0000 nop ; 46E0 0000 nop ; 46E1 0000 sbi PORTA, 2 ; 46E2 9ADA cbi DDRA, 2 ; 46E3 98D2 nop ; 46E4 0000 sbic PINA, 2 ; 46E5 99CA rjmp avr4705 ; 46E6 C01E ldi r22, 0x01 ; 46E7 E0C1 clr r20 ; 46E8 2700 avr46E9: nop ; 46E9 0000 nop ; 46EA 0000 nop ; 46EB 0000 nop ; 46EC 0000 nop ; 46ED 0000 sbic PINA, 2 ; 46EE 99CA rjmp avr46F3 ; 46EF C003 dec r20 ; 46F0 950A brne avr46E9 ; 46F1 F7B9 rjmp avr4705 ; 46F2 C012 avr46F3: call sub4ECD ; 46F3 940E 4ECD avr46F5: sbis PINA, 2 ; 46F5 9BCA rjmp avr4700 ; 46F6 C009 cpi r22, 0xFA ; 46F7 3FCA brne avr46FC ; 46F8 F419 rjmp avr4700 ; 46F9 C006 nop ; 46FA 0000 nop ; 46FB 0000 avr46FC: call sub4ED2 ; 46FC 940E 4ED2 inc r22 ; 46FE 95C3 rjmp avr46F5 ; 46FF CFF5 avr4700: cbi PORTA, 2 ; 4700 98DA sbi DDRA, 2 ; 4701 9AD2 ldi r20, 0x02 ; 4702 E002 jmp avr4E14 ; 4703 940C 4E14 avr4705: cbi PORTA, 2 ; 4705 98DA sbi DDRA, 2 ; 4706 9AD2 clr r22 ; 4707 27CC jmp avr4E14 ; 4708 940C 4E14 avr470A: cbi PORTA, 3 ; 470A 98DB sbi DDRA, 3 ; 470B 9AD3 ldi r20, 0x14 ; 470C E104 avr470D: dec r20 ; 470D 950A brne avr470D ; 470E F7F1 sbi PORTA, 3 ; 470F 9ADB call sub4E24 ; 4710 940E 4E24 nop ; 4712 0000 nop ; 4713 0000 nop ; 4714 0000 cbi PORTA, 3 ; 4715 98DB call sub4E24 ; 4716 940E 4E24 nop ; 4718 0000 nop ; 4719 0000 nop ; 471A 0000 nop ; 471B 0000 sbi PORTA, 3 ; 471C 9ADB cbi DDRA, 3 ; 471D 98D3 nop ; 471E 0000 sbic PINA, 3 ; 471F 99CB rjmp avr473F ; 4720 C01E ldi r22, 0x01 ; 4721 E0C1 clr r20 ; 4722 2700 avr4723: nop ; 4723 0000 nop ; 4724 0000 nop ; 4725 0000 nop ; 4726 0000 nop ; 4727 0000 sbic PINA, 3 ; 4728 99CB rjmp avr472D ; 4729 C003 dec r20 ; 472A 950A brne avr4723 ; 472B F7B9 rjmp avr473F ; 472C C012 avr472D: call sub4ECD ; 472D 940E 4ECD avr472F: sbis PINA, 3 ; 472F 9BCB rjmp avr473A ; 4730 C009 cpi r22, 0xFA ; 4731 3FCA brne avr4736 ; 4732 F419 rjmp avr473A ; 4733 C006 nop ; 4734 0000 nop ; 4735 0000 avr4736: call sub4ED2 ; 4736 940E 4ED2 inc r22 ; 4738 95C3 rjmp avr472F ; 4739 CFF5 avr473A: cbi PORTA, 3 ; 473A 98DB sbi DDRA, 3 ; 473B 9AD3 ldi r20, 0x03 ; 473C E003 jmp avr4E14 ; 473D 940C 4E14 avr473F: cbi PORTA, 3 ; 473F 98DB sbi DDRA, 3 ; 4740 9AD3 clr r22 ; 4741 27CC jmp avr4E14 ; 4742 940C 4E14 avr4744: cbi PORTA, 4 ; 4744 98DC sbi DDRA, 4 ; 4745 9AD4 ldi r20, 0x14 ; 4746 E104 avr4747: dec r20 ; 4747 950A brne avr4747 ; 4748 F7F1 sbi PORTA, 4 ; 4749 9ADC call sub4E24 ; 474A 940E 4E24 nop ; 474C 0000 nop ; 474D 0000 nop ; 474E 0000 cbi PORTA, 4 ; 474F 98DC call sub4E24 ; 4750 940E 4E24 nop ; 4752 0000 nop ; 4753 0000 nop ; 4754 0000 nop ; 4755 0000 sbi PORTA, 4 ; 4756 9ADC cbi DDRA, 4 ; 4757 98D4 nop ; 4758 0000 sbic PINA, 4 ; 4759 99CC rjmp avr4779 ; 475A C01E ldi r22, 0x01 ; 475B E0C1 clr r20 ; 475C 2700 avr475D: nop ; 475D 0000 nop ; 475E 0000 nop ; 475F 0000 nop ; 4760 0000 nop ; 4761 0000 sbic PINA, 4 ; 4762 99CC rjmp avr4767 ; 4763 C003 dec r20 ; 4764 950A brne avr475D ; 4765 F7B9 rjmp avr4779 ; 4766 C012 avr4767: call sub4ECD ; 4767 940E 4ECD avr4769:sbis PINA, 4 ; 4769 9BCC rjmp avr4774 ; 476A C009 cpi r22, 0xFA ; 476B 3FCA brne avr4770 ; 476C F419 rjmp avr4774 ; 476D C006 nop ; 476E 0000 nop ; 476F 0000 avr4770: call sub4ED2 ; 4770 940E 4ED2 inc r22 ; 4772 95C3 rjmp avr4769 ; 4773 CFF5 avr4774: cbi PORTA, 4 ; 4774 98DC sbi DDRA, 4 ; 4775 9AD4 ldi r20, 0x04 ; 4776 E004 jmp avr4E14 ; 4777 940C 4E14 avr4779: cbi PORTA, 4 ; 4779 98DC sbi DDRA, 4 ; 477A 9AD4 clr r22 ; 477B 27CC jmp avr4E14 ; 477C 940C 4E14 avr477E: cbi PORTA, 5 ; 477E 98DD sbi DDRA, 5 ; 477F 9AD5 ldi r20, 0x14 ; 4780 E104 avr4781: dec r20 ; 4781 950A brne avr4781 ; 4782 F7F1 sbi PORTA, 5 ; 4783 9ADD call sub4E24 ; 4784 940E 4E24 nop ; 4786 0000 nop ; 4787 0000 nop ; 4788 0000 cbi PORTA, 5 ; 4789 98DD call sub4E24 ; 478A 940E 4E24 nop ; 478C 0000 nop ; 478D 0000 nop ; 478E 0000 nop ; 478F 0000 sbi PORTA, 5 ; 4790 9ADD cbi DDRA, 5 ; 4791 98D5 nop ; 4792 0000 sbic PINA, 5 ; 4793 99CD rjmp avr47B3 ; 4794 C01E ldi r22, 0x01 ; 4795 E0C1 clr r20 ; 4796 2700 avr4797: nop ; 4797 0000 nop ; 4798 0000 nop ; 4799 0000 nop ; 479A 0000 nop ; 479B 0000 sbic PINA, 5 ; 479C 99CD rjmp avr47A1 ; 479D C003 dec r20 ; 479E 950A brne avr4797 ; 479F F7B9 rjmp avr47B3 ; 47A0 C012 avr47A1: call sub4ECD ; 47A1 940E 4ECD avr47A3: sbis PINA, 5 ; 47A3 9BCD rjmp avr47AE ; 47A4 C009 cpi r22, 0xFA ; 47A5 3FCA brne avr47AA ; 47A6 F419 rjmp avr47AE ; 47A7 C006 nop ; 47A8 0000 nop ; 47A9 0000 avr47AA: call sub4ED2 ; 47AA 940E 4ED2 inc r22 ; 47AC 95C3 rjmp avr47A3 ; 47AD CFF5 avr47AE: cbi PORTA, 5 ; 47AE 98DD sbi DDRA, 5 ; 47AF 9AD5 ldi r20, 0x05 ; 47B0 E005 jmp avr4E14 ; 47B1 940C 4E14 avr47B3: cbi PORTA, 5 ; 47B3 98DD sbi DDRA, 5 ; 47B4 9AD5 clr r22 ; 47B5 27CC jmp avr4E14 ; 47B6 940C 4E14 avr47B8: cbi PORTA, 6 ; 47B8 98DE sbi DDRA, 6 ; 47B9 9AD6 ldi r20, 0x14 ; 47BA E104 avr47BB: dec r20 ; 47BB 950A brne avr47BB ; 47BC F7F1 sbi PORTA, 6 ; 47BD 9ADE call sub4E24 ; 47BE 940E 4E24 nop ; 47C0 0000 nop ; 47C1 0000 nop ; 47C2 0000 cbi PORTA, 6 ; 47C3 98DE call sub4E24 ; 47C4 940E 4E24 nop ; 47C6 0000 nop ; 47C7 0000 nop ; 47C8 0000 nop ; 47C9 0000 sbi PORTA, 6 ; 47CA 9ADE cbi DDRA, 6 ; 47CB 98D6 nop ; 47CC 0000 sbic PINA, 6 ; 47CD 99CE rjmp avr47ED ; 47CE C01E ldi r22, 0x01 ; 47CF E0C1 clr r20 ; 47D0 2700 avr47D1: nop ; 47D1 0000 nop ; 47D2 0000 nop ; 47D3 0000 nop ; 47D4 0000 nop ; 47D5 0000 sbic PINA, 6 ; 47D6 99CE rjmp avr47DB ; 47D7 C003 dec r20 ; 47D8 950A brne avr47D1 ; 47D9 F7B9 rjmp avr47ED ; 47DA C012 avr47DB: call sub4ECD ; 47DB 940E 4ECD avr47DD: sbis PINA, 6 ; 47DD 9BCE rjmp avr47E8 ; 47DE C009 cpi r22, 0xFA ; 47DF 3FCA brne avr47E4 ; 47E0 F419 rjmp avr47E8 ; 47E1 C006 nop ; 47E2 0000 nop ; 47E3 0000 avr47E4: call sub4ED2 ; 47E4 940E 4ED2 inc r22 ; 47E6 95C3 rjmp avr47DD ; 47E7 CFF5 avr47E8: cbi PORTA, 6 ; 47E8 98DE sbi DDRA, 6 ; 47E9 9AD6 ldi r20, 0x06 ; 47EA E006 jmp avr4E14 ; 47EB 940C 4E14 avr47ED: cbi PORTA, 6 ; 47ED 98DE sbi DDRA, 6 ; 47EE 9AD6 clr r22 ; 47EF 27CC jmp avr4E14 ; 47F0 940C 4E14 avr47F2: cbi PORTA, 7 ; 47F2 98DF sbi DDRA, 7 ; 47F3 9AD7 ldi r20, 0x14 ; 47F4 E104 avr47F5: dec r20 ; 47F5 950A brne avr47F5 ; 47F6 F7F1 sbi PORTA, 7 ; 47F7 9ADF call sub4E24 ; 47F8 940E 4E24 nop ; 47FA 0000 nop ; 47FB 0000 nop ; 47FC 0000 cbi PORTA, 7 ; 47FD 98DF call sub4E24 ; 47FE 940E 4E24 nop ; 4800 0000 nop ; 4801 0000 nop ; 4802 0000 nop ; 4803 0000 sbi PORTA, 7 ; 4804 9ADF cbi DDRA, 7 ; 4805 98D7 nop ; 4806 0000 sbic PINA, 7 ; 4807 99CF rjmp avr4827 ; 4808 C01E ldi r22, 0x01 ; 4809 E0C1 clr r20 ; 480A 2700 avr480B: nop ; 480B 0000 nop ; 480C 0000 nop ; 480D 0000 nop ; 480E 0000 nop ; 480F 0000 sbic PINA, 7 ; 4810 99CF rjmp avr4815 ; 4811 C003 dec r20 ; 4812 950A brne avr480B ; 4813 F7B9 rjmp avr4827 ; 4814 C012 avr4815: call sub4ECD ; 4815 940E 4ECD avr4817: sbis PINA, 7 ; 4817 9BCF rjmp avr4822 ; 4818 C009 cpi r22, 0xFA ; 4819 3FCA brne avr481E ; 481A F419 rjmp avr4822 ; 481B C006 nop ; 481C 0000 nop ; 481D 0000 avr481E: call sub4ED2 ; 481E 940E 4ED2 inc r22 ; 4820 95C3 rjmp avr4817 ; 4821 CFF5 avr4822: cbi PORTA, 7 ; 4822 98DF sbi DDRA, 7 ; 4823 9AD7 ldi r20, 0x07 ; 4824 E007 jmp avr4E14 ; 4825 940C 4E14 avr4827: cbi PORTA, 7 ; 4827 98DF sbi DDRA, 7 ; 4828 9AD7 clr r22 ; 4829 27CC jmp avr4E14 ; 482A 940C 4E14 avr482C: cbi PORTB, 0 ; 482C 98C0 sbi DDRB, 0 ; 482D 9AB8 ldi r20, 0x14 ; 482E E104 avr482F: dec r20 ; 482F 950A brne avr482F ; 4830 F7F1 sbi PORTB, 0 ; 4831 9AC0 call sub4E24 ; 4832 940E 4E24 nop ; 4834 0000 nop ; 4835 0000 nop ; 4836 0000 cbi PORTB, 0 ; 4837 98C0 call sub4E24 ; 4838 940E 4E24 nop ; 483A 0000 nop ; 483B 0000 nop ; 483C 0000 nop ; 483D 0000 sbi PORTB, 0 ; 483E 9AC0 cbi DDRB, 0 ; 483F 98B8 nop ; 4840 0000 sbic PINB, 0 ; 4841 99B0 rjmp avr4861 ; 4842 C01E ldi r22, 0x01 ; 4843 E0C1 clr r20 ; 4844 2700 avr4845: nop ; 4845 0000 nop ; 4846 0000 nop ; 4847 0000 nop ; 4848 0000 nop ; 4849 0000 sbic PINB, 0 ; 484A 99B0 rjmp avr484F ; 484B C003 dec r20 ; 484C 950A brne avr4845 ; 484D F7B9 rjmp avr4861 ; 484E C012 avr484F: call sub4ECD ; 484F 940E 4ECD avr4851: sbis PINB, 0 ; 4851 9BB0 rjmp avr485C ; 4852 C009 cpi r22, 0xFA ; 4853 3FCA brne avr4858 ; 4854 F419 rjmp avr485C ; 4855 C006 nop ; 4856 0000 nop ; 4857 0000 avr4858: call sub4ED2 ; 4858 940E 4ED2 inc r22 ; 485A 95C3 rjmp avr4851 ; 485B CFF5 avr485C: cbi PORTB, 0 ; 485C 98C0 sbi DDRB, 0 ; 485D 9AB8 ldi r20, 0x08 ; 485E E008 jmp avr4E14 ; 485F 940C 4E14 avr4861: cbi PORTB, 0 ; 4861 98C0 sbi DDRB, 0 ; 4862 9AB8 clr r22 ; 4863 27CC jmp avr4E14 ; 4864 940C 4E14 avr4866: cbi PORTB, 1 ; 4866 98C1 sbi DDRB, 1 ; 4867 9AB9 ldi r20, 0x14 ; 4868 E104 avr4869: dec r20 ; 4869 950A brne avr4869 ; 486A F7F1 sbi PORTB, 1 ; 486B 9AC1 call sub4E24 ; 486C 940E 4E24 nop ; 486E 0000 nop ; 486F 0000 nop ; 4870 0000 cbi PORTB, 1 ; 4871 98C1 call sub4E24 ; 4872 940E 4E24 nop ; 4874 0000 nop ; 4875 0000 nop ; 4876 0000 nop ; 4877 0000 sbi PORTB, 1 ; 4878 9AC1 cbi DDRB, 1 ; 4879 98B9 nop ; 487A 0000 sbic PINB, 1 ; 487B 99B1 rjmp avr489B ; 487C C01E ldi r22, 0x01 ; 487D E0C1 clr r20 ; 487E 2700 avr487F: nop ; 487F 0000 nop ; 4880 0000 nop ; 4881 0000 nop ; 4882 0000 nop ; 4883 0000 sbic PINB, 1 ; 4884 99B1 rjmp avr4889 ; 4885 C003 dec r20 ; 4886 950A brne avr487F ; 4887 F7B9 rjmp avr489B ; 4888 C012 avr4889: call sub4ECD ; 4889 940E 4ECD avr488B: sbis PINB, 1 ; 488B 9BB1 rjmp avr4896 ; 488C C009 cpi r22, 0xFA ; 488D 3FCA brne avr4892 ; 488E F419 rjmp avr4896 ; 488F C006 nop ; 4890 0000 nop ; 4891 0000 avr4892: call sub4ED2 ; 4892 940E 4ED2 inc r22 ; 4894 95C3 rjmp avr488B ; 4895 CFF5 avr4896: cbi PORTB, 1 ; 4896 98C1 sbi DDRB, 1 ; 4897 9AB9 ldi r20, 0x09 ; 4898 E009 jmp avr4E14 ; 4899 940C 4E14 avr489B: cbi PORTB, 1 ; 489B 98C1 sbi DDRB, 1 ; 489C 9AB9 clr r22 ; 489D 27CC jmp avr4E14 ; 489E 940C 4E14 avr48A0: cbi PORTB, 2 ; 48A0 98C2 sbi DDRB, 2 ; 48A1 9ABA ldi r20, 0x14 ; 48A2 E104 avr48A3: dec r20 ; 48A3 950A brne avr48A3 ; 48A4 F7F1 sbi PORTB, 2 ; 48A5 9AC2 call sub4E24 ; 48A6 940E 4E24 nop ; 48A8 0000 nop ; 48A9 0000 nop ; 48AA 0000 cbi PORTB, 2 ; 48AB 98C2 call sub4E24 ; 48AC 940E 4E24 nop ; 48AE 0000 nop ; 48AF 0000 nop ; 48B0 0000 nop ; 48B1 0000 sbi PORTB, 2 ; 48B2 9AC2 cbi DDRB, 2 ; 48B3 98BA nop ; 48B4 0000 sbic PINB, 2 ; 48B5 99B2 rjmp avr48D5 ; 48B6 C01E ldi r22, 0x01 ; 48B7 E0C1 clr r20 ; 48B8 2700 avr48B9: nop ; 48B9 0000 nop ; 48BA 0000 nop ; 48BB 0000 nop ; 48BC 0000 nop ; 48BD 0000 sbic PINB, 2 ; 48BE 99B2 rjmp avr48C3 ; 48BF C003 dec r20 ; 48C0 950A brne avr48B9 ; 48C1 F7B9 rjmp avr48D5 ; 48C2 C012 avr48C3: call sub4ECD ; 48C3 940E 4ECD avr48C5: sbis PINB, 2 ; 48C5 9BB2 rjmp avr48D0 ; 48C6 C009 cpi r22, 0xFA ; 48C7 3FCA brne avr48CC ; 48C8 F419 rjmp avr48D0 ; 48C9 C006 nop ; 48CA 0000 nop ; 48CB 0000 avr48CC: call sub4ED2 ; 48CC 940E 4ED2 inc r22 ; 48CE 95C3 rjmp avr48C5 ; 48CF CFF5 avr48D0: cbi PORTB, 2 ; 48D0 98C2 sbi DDRB, 2 ; 48D1 9ABA ldi r20, 0x0A ; 48D2 E00A jmp avr4E14 ; 48D3 940C 4E14 avr48D5: cbi PORTB, 2 ; 48D5 98C2 sbi DDRB, 2 ; 48D6 9ABA clr r22 ; 48D7 27CC jmp avr4E14 ; 48D8 940C 4E14 avr48DA: cbi PORTB, 3 ; 48DA 98C3 sbi DDRB, 3 ; 48DB 9ABB ldi r20, 0x14 ; 48DC E104 avr48DD: dec r20 ; 48DD 950A brne avr48DD ; 48DE F7F1 sbi PORTB, 3 ; 48DF 9AC3 call sub4E24 ; 48E0 940E 4E24 nop ; 48E2 0000 nop ; 48E3 0000 nop ; 48E4 0000 cbi PORTB, 3 ; 48E5 98C3 call sub4E24 ; 48E6 940E 4E24 nop ; 48E8 0000 nop ; 48E9 0000 nop ; 48EA 0000 nop ; 48EB 0000 sbi PORTB, 3 ; 48EC 9AC3 cbi DDRB, 3 ; 48ED 98BB nop ; 48EE 0000 sbic PINB, 3 ; 48EF 99B3 rjmp avr490F ; 48F0 C01E ldi r22, 0x01 ; 48F1 E0C1 clr r20 ; 48F2 2700 avr48F3: nop ; 48F3 0000 nop ; 48F4 0000 nop ; 48F5 0000 nop ; 48F6 0000 nop ; 48F7 0000 sbic PINB, 3 ; 48F8 99B3 rjmp avr48FD ; 48F9 C003 dec r20 ; 48FA 950A brne avr48F3 ; 48FB F7B9 rjmp avr490F ; 48FC C012 avr48FD: call sub4ECD ; 48FD 940E 4ECD avr48FF: sbis PINB, 3 ; 48FF 9BB3 rjmp avr490A ; 4900 C009 cpi r22, 0xFA ; 4901 3FCA brne avr4906 ; 4902 F419 rjmp avr490A ; 4903 C006 nop ; 4904 0000 nop ; 4905 0000 avr4906: call sub4ED2 ; 4906 940E 4ED2 inc r22 ; 4908 95C3 rjmp avr48FF ; 4909 CFF5 avr490A: cbi PORTB, 3 ; 490A 98C3 sbi DDRB, 3 ; 490B 9ABB ldi r20, 0x0B ; 490C E00B jmp avr4E14 ; 490D 940C 4E14 avr490F: cbi PORTB, 3 ; 490F 98C3 sbi DDRB, 3 ; 4910 9ABB clr r22 ; 4911 27CC jmp avr4E14 ; 4912 940C 4E14 avr4914: cbi PORTB, 4 ; 4914 98C4 sbi DDRB, 4 ; 4915 9ABC ldi r20, 0x14 ; 4916 E104 avr4917: dec r20 ; 4917 950A brne avr4917 ; 4918 F7F1 sbi PORTB, 4 ; 4919 9AC4 call sub4E24 ; 491A 940E 4E24 nop ; 491C 0000 nop ; 491D 0000 nop ; 491E 0000 cbi PORTB, 4 ; 491F 98C4 call sub4E24 ; 4920 940E 4E24 nop ; 4922 0000 nop ; 4923 0000 nop ; 4924 0000 nop ; 4925 0000 sbi PORTB, 4 ; 4926 9AC4 cbi DDRB, 4 ; 4927 98BC nop ; 4928 0000 sbic PINB, 4 ; 4929 99B4 rjmp avr4949 ; 492A C01E ldi r22, 0x01 ; 492B E0C1 clr r20 ; 492C 2700 avr492D: nop ; 492D 0000 nop ; 492E 0000 nop ; 492F 0000 nop ; 4930 0000 nop ; 4931 0000 sbic PINB, 4 ; 4932 99B4 rjmp avr4937 ; 4933 C003 dec r20 ; 4934 950A brne avr492D ; 4935 F7B9 rjmp avr4949 ; 4936 C012 avr4937: call sub4ECD ; 4937 940E 4ECD avr4939: sbis PINB, 4 ; 4939 9BB4 rjmp avr4944 ; 493A C009 cpi r22, 0xFA ; 493B 3FCA brne avr4940 ; 493C F419 rjmp avr4944 ; 493D C006 nop ; 493E 0000 nop ; 493F 0000 avr4940: call sub4ED2 ; 4940 940E 4ED2 inc r22 ; 4942 95C3 rjmp avr4939 ; 4943 CFF5 avr4944: cbi PORTB, 4 ; 4944 98C4 sbi DDRB, 4 ; 4945 9ABC ldi r20, 0x0C ; 4946 E00C jmp avr4E14 ; 4947 940C 4E14 avr4949: cbi PORTB, 4 ; 4949 98C4 sbi DDRB, 4 ; 494A 9ABC clr r22 ; 494B 27CC jmp avr4E14 ; 494C 940C 4E14 avr494E: cbi PORTB, 5 ; 494E 98C5 sbi DDRB, 5 ; 494F 9ABD ldi r20, 0x14 ; 4950 E104 avr4951: dec r20 ; 4951 950A brne avr4951 ; 4952 F7F1 sbi PORTB, 5 ; 4953 9AC5 call sub4E24 ; 4954 940E 4E24 nop ; 4956 0000 nop ; 4957 0000 nop ; 4958 0000 cbi PORTB, 5 ; 4959 98C5 call sub4E24 ; 495A 940E 4E24 nop ; 495C 0000 nop ; 495D 0000 nop ; 495E 0000 nop ; 495F 0000 sbi PORTB, 5 ; 4960 9AC5 cbi DDRB, 5 ; 4961 98BD nop ; 4962 0000 sbic PINB, 5 ; 4963 99B5 rjmp avr4983 ; 4964 C01E ldi r22, 0x01 ; 4965 E0C1 clr r20 ; 4966 2700 avr4967: nop ; 4967 0000 nop ; 4968 0000 nop ; 4969 0000 nop ; 496A 0000 nop ; 496B 0000 sbic PINB, 5 ; 496C 99B5 rjmp avr4971 ; 496D C003 dec r20 ; 496E 950A brne avr4967 ; 496F F7B9 rjmp avr4983 ; 4970 C012 avr4971: call sub4ECD ; 4971 940E 4ECD avr4973: sbis PINB, 5 ; 4973 9BB5 rjmp avr497E ; 4974 C009 cpi r22, 0xFA ; 4975 3FCA brne avr497A ; 4976 F419 rjmp avr497E ; 4977 C006 nop ; 4978 0000 nop ; 4979 0000 avr497A: call sub4ED2 ; 497A 940E 4ED2 inc r22 ; 497C 95C3 rjmp avr4973 ; 497D CFF5 avr497E: cbi PORTB, 5 ; 497E 98C5 sbi DDRB, 5 ; 497F 9ABD ldi r20, 0x0D ; 4980 E00D jmp avr4E14 ; 4981 940C 4E14 avr4983: cbi PORTB, 5 ; 4983 98C5 sbi DDRB, 5 ; 4984 9ABD clr r22 ; 4985 27CC jmp avr4E14 ; 4986 940C 4E14 avr4988: cbi PORTB, 6 ; 4988 98C6 sbi DDRB, 6 ; 4989 9ABE ldi r20, 0x14 ; 498A E104 avr498B: dec r20 ; 498B 950A brne avr498B ; 498C F7F1 sbi PORTB, 6 ; 498D 9AC6 call sub4E24 ; 498E 940E 4E24 nop ; 4990 0000 nop ; 4991 0000 nop ; 4992 0000 cbi PORTB, 6 ; 4993 98C6 call sub4E24 ; 4994 940E 4E24 nop ; 4996 0000 nop ; 4997 0000 nop ; 4998 0000 nop ; 4999 0000 sbi PORTB, 6 ; 499A 9AC6 cbi DDRB, 6 ; 499B 98BE nop ; 499C 0000 sbic PINB, 6 ; 499D 99B6 rjmp avr49BD ; 499E C01E ldi r22, 0x01 ; 499F E0C1 clr r20 ; 49A0 2700 avr49A1: nop ; 49A1 0000 nop ; 49A2 0000 nop ; 49A3 0000 nop ; 49A4 0000 nop ; 49A5 0000 sbic PINB, 6 ; 49A6 99B6 rjmp avr49AB ; 49A7 C003 dec r20 ; 49A8 950A brne avr49A1 ; 49A9 F7B9 rjmp avr49BD ; 49AA C012 avr49AB: call sub4ECD ; 49AB 940E 4ECD avr49AD: sbis PINB, 6 ; 49AD 9BB6 rjmp avr49B8 ; 49AE C009 cpi r22, 0xFA ; 49AF 3FCA brne avr49B4 ; 49B0 F419 rjmp avr49B8 ; 49B1 C006 nop ; 49B2 0000 nop ; 49B3 0000 avr49B4: call sub4ED2 ; 49B4 940E 4ED2 inc r22 ; 49B6 95C3 rjmp avr49AD ; 49B7 CFF5 avr49B8:cbi PORTB, 6 ; 49B8 98C6 sbi DDRB, 6 ; 49B9 9ABE ldi r20, 0x0E ; 49BA E00E jmp avr4E14 ; 49BB 940C 4E14 avr49BD: cbi PORTB, 6 ; 49BD 98C6 sbi DDRB, 6 ; 49BE 9ABE clr r22 ; 49BF 27CC jmp avr4E14 ; 49C0 940C 4E14 avr49C2: cbi PORTB, 7 ; 49C2 98C7 sbi DDRB, 7 ; 49C3 9ABF ldi r20, 0x14 ; 49C4 E104 avr49C5: dec r20 ; 49C5 950A brne avr49C5 ; 49C6 F7F1 sbi PORTB, 7 ; 49C7 9AC7 call sub4E24 ; 49C8 940E 4E24 nop ; 49CA 0000 nop ; 49CB 0000 nop ; 49CC 0000 cbi PORTB, 7 ; 49CD 98C7 call sub4E24 ; 49CE 940E 4E24 nop ; 49D0 0000 nop ; 49D1 0000 nop ; 49D2 0000 nop ; 49D3 0000 sbi PORTB, 7 ; 49D4 9AC7 cbi DDRB, 7 ; 49D5 98BF nop ; 49D6 0000 sbic PINB, 7 ; 49D7 99B7 rjmp avr49F7 ; 49D8 C01E ldi r22, 0x01 ; 49D9 E0C1 clr r20 ; 49DA 2700 avr49DB: nop ; 49DB 0000 nop ; 49DC 0000 nop ; 49DD 0000 nop ; 49DE 0000 nop ; 49DF 0000 sbic PINB, 7 ; 49E0 99B7 rjmp avr49E5 ; 49E1 C003 dec r20 ; 49E2 950A brne avr49DB ; 49E3 F7B9 rjmp avr49F7 ; 49E4 C012 avr49E5: call sub4ECD ; 49E5 940E 4ECD avr49E7: sbis PINB, 7 ; 49E7 9BB7 rjmp avr49F2 ; 49E8 C009 cpi r22, 0xFA ; 49E9 3FCA brne avr49EE ; 49EA F419 rjmp avr49F2 ; 49EB C006 nop ; 49EC 0000 nop ; 49ED 0000 avr49EE: call sub4ED2 ; 49EE 940E 4ED2 inc r22 ; 49F0 95C3 rjmp avr49E7 ; 49F1 CFF5 avr49F2: cbi PORTB, 7 ; 49F2 98C7 sbi DDRB, 7 ; 49F3 9ABF ldi r20, 0x0F ; 49F4 E00F jmp avr4E14 ; 49F5 940C 4E14 avr49F7: cbi PORTB, 7 ; 49F7 98C7 sbi DDRB, 7 ; 49F8 9ABF clr r22 ; 49F9 27CC jmp avr4E14 ; 49FA 940C 4E14 avr49FC: cbi PORTC, 7 ; 49FC 98AF sbi DDRC, 7 ; 49FD 9AA7 ldi r20, 0x14 ; 49FE E104 avr49FF: dec r20 ; 49FF 950A brne avr49FF ; 4A00 F7F1 sbi PORTC, 7 ; 4A01 9AAF call sub4E24 ; 4A02 940E 4E24 nop ; 4A04 0000 nop ; 4A05 0000 nop ; 4A06 0000 cbi PORTC, 7 ; 4A07 98AF call sub4E24 ; 4A08 940E 4E24 nop ; 4A0A 0000 nop ; 4A0B 0000 nop ; 4A0C 0000 nop ; 4A0D 0000 sbi PORTC, 7 ; 4A0E 9AAF cbi DDRC, 7 ; 4A0F 98A7 nop ; 4A10 0000 sbic PINC, 7 ; 4A11 999F rjmp avr4A31 ; 4A12 C01E ldi r22, 0x01 ; 4A13 E0C1 clr r20 ; 4A14 2700 avr4A15: nop ; 4A15 0000 nop ; 4A16 0000 nop ; 4A17 0000 nop ; 4A18 0000 nop ; 4A19 0000 sbic PINC, 7 ; 4A1A 999F rjmp avr4A1F ; 4A1B C003 dec r20 ; 4A1C 950A brne avr4A15 ; 4A1D F7B9 rjmp avr4A31 ; 4A1E C012 avr4A1F: call sub4ECD ; 4A1F 940E 4ECD avr4A21: sbis PINC, 7 ; 4A21 9B9F rjmp avr4A2C ; 4A22 C009 cpi r22, 0xFA ; 4A23 3FCA brne avr4A28 ; 4A24 F419 rjmp avr4A2C ; 4A25 C006 nop ; 4A26 0000 nop ; 4A27 0000 avr4A28: call sub4ED2 ; 4A28 940E 4ED2 inc r22 ; 4A2A 95C3 rjmp avr4A21 ; 4A2B CFF5 avr4A2C: cbi PORTC, 7 ; 4A2C 98AF sbi DDRC, 7 ; 4A2D 9AA7 ldi r20, 0x10 ; 4A2E E100 jmp avr4E14 ; 4A2F 940C 4E14 avr4A31: cbi PORTC, 7 ; 4A31 98AF sbi DDRC, 7 ; 4A32 9AA7 clr r22 ; 4A33 27CC jmp avr4E14 ; 4A34 940C 4E14 avr4A36: cbi PORTC, 6 ; 4A36 98AE sbi DDRC, 6 ; 4A37 9AA6 ldi r20, 0x14 ; 4A38 E104 avr4A39: dec r20 ; 4A39 950A brne avr4A39 ; 4A3A F7F1 sbi PORTC, 6 ; 4A3B 9AAE call sub4E24 ; 4A3C 940E 4E24 nop ; 4A3E 0000 nop ; 4A3F 0000 nop ; 4A40 0000 cbi PORTC, 6 ; 4A41 98AE call sub4E24 ; 4A42 940E 4E24 nop ; 4A44 0000 nop ; 4A45 0000 nop ; 4A46 0000 nop ; 4A47 0000 sbi PORTC, 6 ; 4A48 9AAE cbi DDRC, 6 ; 4A49 98A6 nop ; 4A4A 0000 sbic PINC, 6 ; 4A4B 999E rjmp avr4A6B ; 4A4C C01E ldi r22, 0x01 ; 4A4D E0C1 clr r20 ; 4A4E 2700 avr4A4F: nop ; 4A4F 0000 nop ; 4A50 0000 nop ; 4A51 0000 nop ; 4A52 0000 nop ; 4A53 0000 sbic PINC, 6 ; 4A54 999E rjmp avr4A59 ; 4A55 C003 dec r20 ; 4A56 950A brne avr4A4F ; 4A57 F7B9 rjmp avr4A6B ; 4A58 C012 avr4A59: call sub4ECD ; 4A59 940E 4ECD avr4A5B: sbis PINC, 6 ; 4A5B 9B9E rjmp avr4A66 ; 4A5C C009 cpi r22, 0xFA ; 4A5D 3FCA brne avr4A62 ; 4A5E F419 rjmp avr4A66 ; 4A5F C006 nop ; 4A60 0000 nop ; 4A61 0000 avr4A62: call sub4ED2 ; 4A62 940E 4ED2 inc r22 ; 4A64 95C3 rjmp avr4A5B ; 4A65 CFF5 avr4A66: cbi PORTC, 6 ; 4A66 98AE sbi DDRC, 6 ; 4A67 9AA6 ldi r20, 0x11 ; 4A68 E101 jmp avr4E14 ; 4A69 940C 4E14 avr4A6B: cbi PORTC, 6 ; 4A6B 98AE sbi DDRC, 6 ; 4A6C 9AA6 clr r22 ; 4A6D 27CC jmp avr4E14 ; 4A6E 940C 4E14 avr4A70: cbi PORTC, 5 ; 4A70 98AD sbi DDRC, 5 ; 4A71 9AA5 ldi r20, 0x14 ; 4A72 E104 avr4A73: dec r20 ; 4A73 950A brne avr4A73 ; 4A74 F7F1 sbi PORTC, 5 ; 4A75 9AAD call sub4E24 ; 4A76 940E 4E24 nop ; 4A78 0000 nop ; 4A79 0000 nop ; 4A7A 0000 cbi PORTC, 5 ; 4A7B 98AD call sub4E24 ; 4A7C 940E 4E24 nop ; 4A7E 0000 nop ; 4A7F 0000 nop ; 4A80 0000 nop ; 4A81 0000 sbi PORTC, 5 ; 4A82 9AAD cbi DDRC, 5 ; 4A83 98A5 nop ; 4A84 0000 sbic PINC, 5 ; 4A85 999D rjmp avr4AA5 ; 4A86 C01E ldi r22, 0x01 ; 4A87 E0C1 clr r20 ; 4A88 2700 avr4A89: nop ; 4A89 0000 nop ; 4A8A 0000 nop ; 4A8B 0000 nop ; 4A8C 0000 nop ; 4A8D 0000 sbic PINC, 5 ; 4A8E 999D rjmp avr4A93 ; 4A8F C003 dec r20 ; 4A90 950A brne avr4A89 ; 4A91 F7B9 rjmp avr4AA5 ; 4A92 C012 avr4A93: call sub4ECD ; 4A93 940E 4ECD avr4A95: sbis PINC, 5 ; 4A95 9B9D rjmp avr4AA0 ; 4A96 C009 cpi r22, 0xFA ; 4A97 3FCA brne avr4A9C ; 4A98 F419 rjmp avr4AA0 ; 4A99 C006 nop ; 4A9A 0000 nop ; 4A9B 0000 avr4A9C: call sub4ED2 ; 4A9C 940E 4ED2 inc r22 ; 4A9E 95C3 rjmp avr4A95 ; 4A9F CFF5 avr4AA0: cbi PORTC, 5 ; 4AA0 98AD sbi DDRC, 5 ; 4AA1 9AA5 ldi r20, 0x12 ; 4AA2 E102 jmp avr4E14 ; 4AA3 940C 4E14 avr4AA5: cbi PORTC, 5 ; 4AA5 98AD sbi DDRC, 5 ; 4AA6 9AA5 clr r22 ; 4AA7 27CC jmp avr4E14 ; 4AA8 940C 4E14 avr4AAA: cbi PORTC, 4 ; 4AAA 98AC sbi DDRC, 4 ; 4AAB 9AA4 ldi r20, 0x14 ; 4AAC E104 avr4AAD: dec r20 ; 4AAD 950A brne avr4AAD ; 4AAE F7F1 sbi PORTC, 4 ; 4AAF 9AAC call sub4E24 ; 4AB0 940E 4E24 nop ; 4AB2 0000 nop ; 4AB3 0000 nop ; 4AB4 0000 cbi PORTC, 4 ; 4AB5 98AC call sub4E24 ; 4AB6 940E 4E24 nop ; 4AB8 0000 nop ; 4AB9 0000 nop ; 4ABA 0000 nop ; 4ABB 0000 sbi PORTC, 4 ; 4ABC 9AAC cbi DDRC, 4 ; 4ABD 98A4 nop ; 4ABE 0000 sbic PINC, 4 ; 4ABF 999C rjmp avr4ADF ; 4AC0 C01E ldi r22, 0x01 ; 4AC1 E0C1 clr r20 ; 4AC2 2700 avr4AC3: nop ; 4AC3 0000 nop ; 4AC4 0000 nop ; 4AC5 0000 nop ; 4AC6 0000 nop ; 4AC7 0000 sbic PINC, 4 ; 4AC8 999C rjmp avr4ACD ; 4AC9 C003 dec r20 ; 4ACA 950A brne avr4AC3 ; 4ACB F7B9 rjmp avr4ADF ; 4ACC C012 avr4ACD: call sub4ECD ; 4ACD 940E 4ECD avr4ACF: sbis PINC, 4 ; 4ACF 9B9C rjmp avr4ADA ; 4AD0 C009 cpi r22, 0xFA ; 4AD1 3FCA brne avr4AD6 ; 4AD2 F419 rjmp avr4ADA ; 4AD3 C006 nop ; 4AD4 0000 nop ; 4AD5 0000 avr4AD6: call sub4ED2 ; 4AD6 940E 4ED2 inc r22 ; 4AD8 95C3 rjmp avr4ACF ; 4AD9 CFF5 avr4ADA: cbi PORTC, 4 ; 4ADA 98AC sbi DDRC, 4 ; 4ADB 9AA4 ldi r20, 0x13 ; 4ADC E103 jmp avr4E14 ; 4ADD 940C 4E14 avr4ADF: cbi PORTC, 4 ; 4ADF 98AC sbi DDRC, 4 ; 4AE0 9AA4 clr r22 ; 4AE1 27CC jmp avr4E14 ; 4AE2 940C 4E14 avr4AE4: cbi PORTC, 3 ; 4AE4 98AB sbi DDRC, 3 ; 4AE5 9AA3 ldi r20, 0x14 ; 4AE6 E104 avr4AE7: dec r20 ; 4AE7 950A brne avr4AE7 ; 4AE8 F7F1 sbi PORTC, 3 ; 4AE9 9AAB call sub4E24 ; 4AEA 940E 4E24 nop ; 4AEC 0000 nop ; 4AED 0000 nop ; 4AEE 0000 cbi PORTC, 3 ; 4AEF 98AB call sub4E24 ; 4AF0 940E 4E24 nop ; 4AF2 0000 nop ; 4AF3 0000 nop ; 4AF4 0000 nop ; 4AF5 0000 sbi PORTC, 3 ; 4AF6 9AAB cbi DDRC, 3 ; 4AF7 98A3 nop ; 4AF8 0000 sbic PINC, 3 ; 4AF9 999B rjmp avr4B19 ; 4AFA C01E ldi r22, 0x01 ; 4AFB E0C1 clr r20 ; 4AFC 2700 avr4AFD: nop ; 4AFD 0000 nop ; 4AFE 0000 nop ; 4AFF 0000 nop ; 4B00 0000 nop ; 4B01 0000 sbic PINC, 3 ; 4B02 999B rjmp avr4B07 ; 4B03 C003 dec r20 ; 4B04 950A brne avr4AFD ; 4B05 F7B9 rjmp avr4B19 ; 4B06 C012 avr4B07: call sub4ECD ; 4B07 940E 4ECD avr4B09: sbis PINC, 3 ; 4B09 9B9B rjmp avr4B14 ; 4B0A C009 cpi r22, 0xFA ; 4B0B 3FCA brne avr4B10 ; 4B0C F419 rjmp avr4B14 ; 4B0D C006 nop ; 4B0E 0000 nop ; 4B0F 0000 avr4B10: call sub4ED2 ; 4B10 940E 4ED2 inc r22 ; 4B12 95C3 rjmp avr4B09 ; 4B13 CFF5 avr4B14: cbi PORTC, 3 ; 4B14 98AB sbi DDRC, 3 ; 4B15 9AA3 ldi r20, 0x14 ; 4B16 E104 jmp avr4E14 ; 4B17 940C 4E14 avr4B19: cbi PORTC, 3 ; 4B19 98AB sbi DDRC, 3 ; 4B1A 9AA3 clr r22 ; 4B1B 27CC jmp avr4E14 ; 4B1C 940C 4E14 avr4B1E: cbi PORTC, 2 ; 4B1E 98AA sbi DDRC, 2 ; 4B1F 9AA2 ldi r20, 0x14 ; 4B20 E104 avr4B21: dec r20 ; 4B21 950A brne avr4B21 ; 4B22 F7F1 sbi PORTC, 2 ; 4B23 9AAA call sub4E24 ; 4B24 940E 4E24 nop ; 4B26 0000 nop ; 4B27 0000 nop ; 4B28 0000 cbi PORTC, 2 ; 4B29 98AA call sub4E24 ; 4B2A 940E 4E24 nop ; 4B2C 0000 nop ; 4B2D 0000 nop ; 4B2E 0000 nop ; 4B2F 0000 sbi PORTC, 2 ; 4B30 9AAA cbi DDRC, 2 ; 4B31 98A2 nop ; 4B32 0000 sbic PINC, 2 ; 4B33 999A rjmp avr4B53 ; 4B34 C01E ldi r22, 0x01 ; 4B35 E0C1 clr r20 ; 4B36 2700 avr4B37: nop ; 4B37 0000 nop ; 4B38 0000 nop ; 4B39 0000 nop ; 4B3A 0000 nop ; 4B3B 0000 sbic PINC, 2 ; 4B3C 999A rjmp avr4B41 ; 4B3D C003 dec r20 ; 4B3E 950A brne avr4B37 ; 4B3F F7B9 rjmp avr4B53 ; 4B40 C012 avr4B41: call sub4ECD ; 4B41 940E 4ECD avr4B43: sbis PINC, 2 ; 4B43 9B9A rjmp avr4B4E ; 4B44 C009 cpi r22, 0xFA ; 4B45 3FCA brne avr4B4A ; 4B46 F419 rjmp avr4B4E ; 4B47 C006 nop ; 4B48 0000 nop ; 4B49 0000 avr4B4A: call sub4ED2 ; 4B4A 940E 4ED2 inc r22 ; 4B4C 95C3 rjmp avr4B43 ; 4B4D CFF5 avr4B4E: cbi PORTC, 2 ; 4B4E 98AA sbi DDRC, 2 ; 4B4F 9AA2 ldi r20, 0x15 ; 4B50 E105 jmp avr4E14 ; 4B51 940C 4E14 avr4B53: cbi PORTC, 2 ; 4B53 98AA sbi DDRC, 2 ; 4B54 9AA2 clr r22 ; 4B55 27CC jmp avr4E14 ; 4B56 940C 4E14 avr4B58: cbi PORTC, 1 ; 4B58 98A9 sbi DDRC, 1 ; 4B59 9AA1 ldi r20, 0x14 ; 4B5A E104 avr4B5B: dec r20 ; 4B5B 950A brne avr4B5B ; 4B5C F7F1 sbi PORTC, 1 ; 4B5D 9AA9 call sub4E24 ; 4B5E 940E 4E24 nop ; 4B60 0000 nop ; 4B61 0000 nop ; 4B62 0000 cbi PORTC, 1 ; 4B63 98A9 call sub4E24 ; 4B64 940E 4E24 nop ; 4B66 0000 nop ; 4B67 0000 nop ; 4B68 0000 nop ; 4B69 0000 sbi PORTC, 1 ; 4B6A 9AA9 cbi DDRC, 1 ; 4B6B 98A1 nop ; 4B6C 0000 sbic PINC, 1 ; 4B6D 9999 rjmp avr4B8D ; 4B6E C01E ldi r22, 0x01 ; 4B6F E0C1 clr r20 ; 4B70 2700 avr4B71: nop ; 4B71 0000 nop ; 4B72 0000 nop ; 4B73 0000 nop ; 4B74 0000 nop ; 4B75 0000 sbic PINC, 1 ; 4B76 9999 rjmp avr4B7B ; 4B77 C003 dec r20 ; 4B78 950A brne avr4B71 ; 4B79 F7B9 rjmp avr4B8D ; 4B7A C012 avr4B7B: call sub4ECD ; 4B7B 940E 4ECD avr4B7D: sbis PINC, 1 ; 4B7D 9B99 rjmp avr4B88 ; 4B7E C009 cpi r22, 0xFA ; 4B7F 3FCA brne avr4B84 ; 4B80 F419 rjmp avr4B88 ; 4B81 C006 nop ; 4B82 0000 nop ; 4B83 0000 avr4B84: call sub4ED2 ; 4B84 940E 4ED2 inc r22 ; 4B86 95C3 rjmp avr4B7D ; 4B87 CFF5 avr4B88: cbi PORTC, 1 ; 4B88 98A9 sbi DDRC, 1 ; 4B89 9AA1 ldi r20, 0x16 ; 4B8A E106 jmp avr4E14 ; 4B8B 940C 4E14 avr4B8D: cbi PORTC, 1 ; 4B8D 98A9 sbi DDRC, 1 ; 4B8E 9AA1 clr r22 ; 4B8F 27CC jmp avr4E14 ; 4B90 940C 4E14 avr4B92: cbi PORTC, 0 ; 4B92 98A8 sbi DDRC, 0 ; 4B93 9AA0 ldi r20, 0x14 ; 4B94 E104 avr4B95: dec r20 ; 4B95 950A brne avr4B95 ; 4B96 F7F1 sbi PORTC, 0 ; 4B97 9AA8 call sub4E24 ; 4B98 940E 4E24 nop ; 4B9A 0000 nop ; 4B9B 0000 nop ; 4B9C 0000 cbi PORTC, 0 ; 4B9D 98A8 call sub4E24 ; 4B9E 940E 4E24 nop ; 4BA0 0000 nop ; 4BA1 0000 nop ; 4BA2 0000 nop ; 4BA3 0000 sbi PORTC, 0 ; 4BA4 9AA8 cbi DDRC, 0 ; 4BA5 98A0 nop ; 4BA6 0000 sbic PINC, 0 ; 4BA7 9998 rjmp avr4BC7 ; 4BA8 C01E ldi r22, 0x01 ; 4BA9 E0C1 clr r20 ; 4BAA 2700 avr4BAB: nop ; 4BAB 0000 nop ; 4BAC 0000 nop ; 4BAD 0000 nop ; 4BAE 0000 nop ; 4BAF 0000 sbic PINC, 0 ; 4BB0 9998 rjmp avr4BB5 ; 4BB1 C003 dec r20 ; 4BB2 950A brne avr4BAB ; 4BB3 F7B9 rjmp avr4BC7 ; 4BB4 C012 avr4BB5: call sub4ECD ; 4BB5 940E 4ECD avr4BB7: sbis PINC, 0 ; 4BB7 9B98 rjmp avr4BC2 ; 4BB8 C009 cpi r22, 0xFA ; 4BB9 3FCA brne avr4BBE ; 4BBA F419 rjmp avr4BC2 ; 4BBB C006 nop ; 4BBC 0000 nop ; 4BBD 0000 avr4BBE: call sub4ED2 ; 4BBE 940E 4ED2 inc r22 ; 4BC0 95C3 rjmp avr4BB7 ; 4BC1 CFF5 avr4BC2: cbi PORTC, 0 ; 4BC2 98A8 sbi DDRC, 0 ; 4BC3 9AA0 ldi r20, 0x17 ; 4BC4 E107 jmp avr4E14 ; 4BC5 940C 4E14 avr4BC7: cbi PORTC, 0 ; 4BC7 98A8 sbi DDRC, 0 ; 4BC8 9AA0 clr r22 ; 4BC9 27CC jmp avr4E14 ; 4BCA 940C 4E14 avr4BCC: cbi 03, 7 ; 4BCC 981F sbi 02, 7 ; 4BCD 9A17 ldi r20, 0x14 ; 4BCE E104 avr4BCF: dec r20 ; 4BCF 950A brne avr4BCF ; 4BD0 F7F1 sbi 03, 7 ; 4BD1 9A1F call sub4E24 ; 4BD2 940E 4E24 nop ; 4BD4 0000 nop ; 4BD5 0000 nop ; 4BD6 0000 cbi 03, 7 ; 4BD7 981F call sub4E24 ; 4BD8 940E 4E24 nop ; 4BDA 0000 nop ; 4BDB 0000 nop ; 4BDC 0000 nop ; 4BDD 0000 sbi 03, 7 ; 4BDE 9A1F cbi 02, 7 ; 4BDF 9817 nop ; 4BE0 0000 sbic 01, 7 ; 4BE1 990F rjmp avr4C01 ; 4BE2 C01E ldi r22, 0x01 ; 4BE3 E0C1 clr r20 ; 4BE4 2700 avr4BE5: nop ; 4BE5 0000 nop ; 4BE6 0000 nop ; 4BE7 0000 nop ; 4BE8 0000 nop ; 4BE9 0000 sbic 01, 7 ; 4BEA 990F rjmp avr4BEF ; 4BEB C003 dec r20 ; 4BEC 950A brne avr4BE5 ; 4BED F7B9 rjmp avr4C01 ; 4BEE C012 avr4BEF: call sub4ECD ; 4BEF 940E 4ECD avr4BF1: sbis 01, 7 ; 4BF1 9B0F rjmp avr4BFC ; 4BF2 C009 cpi r22, 0xFA ; 4BF3 3FCA brne avr4BF8 ; 4BF4 F419 rjmp avr4BFC ; 4BF5 C006 nop ; 4BF6 0000 nop ; 4BF7 0000 avr4BF8: call sub4ED2 ; 4BF8 940E 4ED2 inc r22 ; 4BFA 95C3 rjmp avr4BF1 ; 4BFB CFF5 avr4BFC: cbi 03, 7 ; 4BFC 981F sbi 02, 7 ; 4BFD 9A17 ldi r20, 0x18 ; 4BFE E108 jmp avr4E14 ; 4BFF 940C 4E14 avr4C01: cbi 03, 7 ; 4C01 981F sbi 02, 7 ; 4C02 9A17 clr r22 ; 4C03 27CC jmp avr4E14 ; 4C04 940C 4E14 avr4C06: cbi 03, 6 ; 4C06 981E sbi 02, 6 ; 4C07 9A16 ldi r20, 0x14 ; 4C08 E104 avr4C09: dec r20 ; 4C09 950A brne avr4C09 ; 4C0A F7F1 sbi 03, 6 ; 4C0B 9A1E call sub4E24 ; 4C0C 940E 4E24 nop ; 4C0E 0000 nop ; 4C0F 0000 nop ; 4C10 0000 cbi 03, 6 ; 4C11 981E call sub4E24 ; 4C12 940E 4E24 nop ; 4C14 0000 nop ; 4C15 0000 nop ; 4C16 0000 nop ; 4C17 0000 sbi 03, 6 ; 4C18 9A1E cbi 02, 6 ; 4C19 9816 nop ; 4C1A 0000 sbic 01, 6 ; 4C1B 990E rjmp avr4C3B ; 4C1C C01E ldi r22, 0x01 ; 4C1D E0C1 clr r20 ; 4C1E 2700 avr4C1F: nop ; 4C1F 0000 nop ; 4C20 0000 nop ; 4C21 0000 nop ; 4C22 0000 nop ; 4C23 0000 sbic 01, 6 ; 4C24 990E rjmp avr4C29 ; 4C25 C003 dec r20 ; 4C26 950A brne avr4C1F ; 4C27 F7B9 rjmp avr4C3B ; 4C28 C012 avr4C29: call sub4ECD ; 4C29 940E 4ECD avr4C2B: sbis 01, 6 ; 4C2B 9B0E rjmp avr4C36 ; 4C2C C009 cpi r22, 0xFA ; 4C2D 3FCA brne avr4C32 ; 4C2E F419 rjmp avr4C36 ; 4C2F C006 nop ; 4C30 0000 nop ; 4C31 0000 avr4C32: call sub4ED2 ; 4C32 940E 4ED2 inc r22 ; 4C34 95C3 rjmp avr4C2B ; 4C35 CFF5 avr4C36: cbi 03, 6 ; 4C36 981E sbi 02, 6 ; 4C37 9A16 ldi r20, 0x19 ; 4C38 E109 jmp avr4E14 ; 4C39 940C 4E14 avr4C3B: cbi 03, 6 ; 4C3B 981E sbi 02, 6 ; 4C3C 9A16 clr r22 ; 4C3D 27CC jmp avr4E14 ; 4C3E 940C 4E14 avr4C40: cbi PORTD, 7 ; 4C40 9897 sbi DDRD, 7 ; 4C41 9A8F ldi r20, 0x14 ; 4C42 E104 avr4C43: dec r20 ; 4C43 950A brne avr4C43 ; 4C44 F7F1 sbi PORTD, 7 ; 4C45 9A97 call sub4E24 ; 4C46 940E 4E24 nop ; 4C48 0000 nop ; 4C49 0000 nop ; 4C4A 0000 cbi PORTD, 7 ; 4C4B 9897 call sub4E24 ; 4C4C 940E 4E24 nop ; 4C4E 0000 nop ; 4C4F 0000 nop ; 4C50 0000 nop ; 4C51 0000 sbi PORTD, 7 ; 4C52 9A97 cbi DDRD, 7 ; 4C53 988F nop ; 4C54 0000 sbic PIND, 7 ; 4C55 9987 rjmp avr4C75 ; 4C56 C01E ldi r22, 0x01 ; 4C57 E0C1 clr r20 ; 4C58 2700 avr4C59: nop ; 4C59 0000 nop ; 4C5A 0000 nop ; 4C5B 0000 nop ; 4C5C 0000 nop ; 4C5D 0000 sbic PIND, 7 ; 4C5E 9987 rjmp avr4C63 ; 4C5F C003 dec r20 ; 4C60 950A brne avr4C59 ; 4C61 F7B9 rjmp avr4C75 ; 4C62 C012 avr4C63: call sub4ECD ; 4C63 940E 4ECD avr4C65: sbis PIND, 7 ; 4C65 9B87 rjmp avr4C70 ; 4C66 C009 cpi r22, 0xFA ; 4C67 3FCA brne avr4C6C ; 4C68 F419 rjmp avr4C70 ; 4C69 C006 nop ; 4C6A 0000 nop ; 4C6B 0000 avr4C6C: call sub4ED2 ; 4C6C 940E 4ED2 inc r22 ; 4C6E 95C3 rjmp avr4C65 ; 4C6F CFF5 avr4C70: cbi PORTD, 7 ; 4C70 9897 sbi DDRD, 7 ; 4C71 9A8F ldi r20, 0x1A ; 4C72 E10A jmp avr4E14 ; 4C73 940C 4E14 avr4C75: cbi PORTD, 7 ; 4C75 9897 sbi DDRD, 7 ; 4C76 9A8F clr r22 ; 4C77 27CC jmp avr4E14 ; 4C78 940C 4E14 avr4C7A: cbi PORTD, 6 ; 4C7A 9896 sbi DDRD, 6 ; 4C7B 9A8E ldi r20, 0x14 ; 4C7C E104 avr4C7D: dec r20 ; 4C7D 950A brne avr4C7D ; 4C7E F7F1 sbi PORTD, 6 ; 4C7F 9A96 call sub4E24 ; 4C80 940E 4E24 nop ; 4C82 0000 nop ; 4C83 0000 nop ; 4C84 0000 cbi PORTD, 6 ; 4C85 9896 call sub4E24 ; 4C86 940E 4E24 nop ; 4C88 0000 nop ; 4C89 0000 nop ; 4C8A 0000 nop ; 4C8B 0000 sbi PORTD, 6 ; 4C8C 9A96 cbi DDRD, 6 ; 4C8D 988E nop ; 4C8E 0000 sbic PIND, 6 ; 4C8F 9986 rjmp avr4CAF ; 4C90 C01E ldi r22, 0x01 ; 4C91 E0C1 clr r20 ; 4C92 2700 avr4C93: nop ; 4C93 0000 nop ; 4C94 0000 nop ; 4C95 0000 nop ; 4C96 0000 nop ; 4C97 0000 sbic PIND, 6 ; 4C98 9986 rjmp avr4C9D ; 4C99 C003 dec r20 ; 4C9A 950A brne avr4C93 ; 4C9B F7B9 rjmp avr4CAF ; 4C9C C012 avr4C9D: call sub4ECD ; 4C9D 940E 4ECD avr4C9F: sbis PIND, 6 ; 4C9F 9B86 rjmp avr4CAA ; 4CA0 C009 cpi r22, 0xFA ; 4CA1 3FCA brne avr4CA6 ; 4CA2 F419 rjmp avr4CAA ; 4CA3 C006 nop ; 4CA4 0000 nop ; 4CA5 0000 avr4CA6: call sub4ED2 ; 4CA6 940E 4ED2 inc r22 ; 4CA8 95C3 rjmp avr4C9F ; 4CA9 CFF5 avr4CAA: cbi PORTD, 6 ; 4CAA 9896 sbi DDRD, 6 ; 4CAB 9A8E ldi r20, 0x1B ; 4CAC E10B jmp avr4E14 ; 4CAD 940C 4E14 avr4CAF: cbi PORTD, 6 ; 4CAF 9896 sbi DDRD, 6 ; 4CB0 9A8E clr r22 ; 4CB1 27CC jmp avr4E14 ; 4CB2 940C 4E14 avr4CB4: cbi PORTD, 5 ; 4CB4 9895 sbi DDRD, 5 ; 4CB5 9A8D ldi r20, 0x14 ; 4CB6 E104 avr4CB7: dec r20 ; 4CB7 950A brne avr4CB7 ; 4CB8 F7F1 sbi PORTD, 5 ; 4CB9 9A95 call sub4E24 ; 4CBA 940E 4E24 nop ; 4CBC 0000 nop ; 4CBD 0000 nop ; 4CBE 0000 cbi PORTD, 5 ; 4CBF 9895 call sub4E24 ; 4CC0 940E 4E24 nop ; 4CC2 0000 nop ; 4CC3 0000 nop ; 4CC4 0000 nop ; 4CC5 0000 sbi PORTD, 5 ; 4CC6 9A95 cbi DDRD, 5 ; 4CC7 988D nop ; 4CC8 0000 sbic PIND, 5 ; 4CC9 9985 rjmp avr4CE9 ; 4CCA C01E ldi r22, 0x01 ; 4CCB E0C1 clr r20 ; 4CCC 2700 avr4CCD: nop ; 4CCD 0000 nop ; 4CCE 0000 nop ; 4CCF 0000 nop ; 4CD0 0000 nop ; 4CD1 0000 sbic PIND, 5 ; 4CD2 9985 rjmp avr4CD7 ; 4CD3 C003 dec r20 ; 4CD4 950A brne avr4CCD ; 4CD5 F7B9 rjmp avr4CE9 ; 4CD6 C012 avr4CD7: call sub4ECD ; 4CD7 940E 4ECD avr4CD9: sbis PIND, 5 ; 4CD9 9B85 rjmp avr4CE4 ; 4CDA C009 cpi r22, 0xFA ; 4CDB 3FCA brne avr4CE0 ; 4CDC F419 rjmp avr4CE4 ; 4CDD C006 nop ; 4CDE 0000 nop ; 4CDF 0000 avr4CE0: call sub4ED2 ; 4CE0 940E 4ED2 inc r22 ; 4CE2 95C3 rjmp avr4CD9 ; 4CE3 CFF5 avr4CE4: cbi PORTD, 5 ; 4CE4 9895 sbi DDRD, 5 ; 4CE5 9A8D ldi r20, 0x1C ; 4CE6 E10C jmp avr4E14 ; 4CE7 940C 4E14 avr4CE9: cbi PORTD, 5 ; 4CE9 9895 sbi DDRD, 5 ; 4CEA 9A8D clr r22 ; 4CEB 27CC jmp avr4E14 ; 4CEC 940C 4E14 avr4CEE: lds r20, 0x0065 ; 4CEE 9100 0065 andi r20, 0xFB ; 4CF0 7F0B sts 0x0065, r20 ; 4CF1 9300 0065 lds r20, 0x0064 ; 4CF3 9100 0064 ori r20, 0x04 ; 4CF5 6004 sts 0x0064, r20 ; 4CF6 9300 0064 ldi r20, 0x14 ; 4CF8 E104 avr4CF9: dec r20 ; 4CF9 950A brne avr4CF9 ; 4CFA F7F1 lds r20, 0x0065 ; 4CFB 9100 0065 ori r20, 0x04 ; 4CFD 6004 sts 0x0065, r20 ; 4CFE 9300 0065 call sub4E24 ; 4D00 940E 4E24 lds r20, 0x0065 ; 4D02 9100 0065 andi r20, 0xFB ; 4D04 7F0B sts 0x0065, r20 ; 4D05 9300 0065 call sub4E24 ; 4D07 940E 4E24 nop ; 4D09 0000 nop ; 4D0A 0000 nop ; 4D0B 0000 nop ; 4D0C 0000 lds r20, 0x0065 ; 4D0D 9100 0065 ori r20, 0x04 ; 4D0F 6004 sts 0x0065, r20 ; 4D10 9300 0065 lds r20, 0x0064 ; 4D12 9100 0064 andi r20, 0xFB ; 4D14 7F0B sts 0x0064, r20 ; 4D15 9300 0064 nop ; 4D17 0000 lds r21, 0x0063 ; 4D18 9110 0063 sbrc r21, 2 ; 4D1A FD12 rjmp avr4D43 ; 4D1B C027 ldi r22, 0x01 ; 4D1C E0C1 clr r20 ; 4D1D 2700 avr4D1E: nop ; 4D1E 0000 nop ; 4D1F 0000 nop ; 4D20 0000 lds r21, 0x0063 ; 4D21 9110 0063 sbrc r21, 2 ; 4D23 FD12 rjmp avr4D28 ; 4D24 C003 dec r20 ; 4D25 950A brne avr4D1E ; 4D26 F7B9 rjmp avr4D43 ; 4D27 C01B avr4D28: call sub4ECD ; 4D28 940E 4ECD avr4D2A: lds r21, 0x0063 ; 4D2A 9110 0063 sbrs r21, 2 ; 4D2C FF12 rjmp avr4D37 ; 4D2D C009 cpi r22, 0xFA ; 4D2E 3FCA brne avr4D33 ; 4D2F F419 rjmp avr4D37 ; 4D30 C006 nop ; 4D31 0000 nop ; 4D32 0000 avr4D33: call sub4ED2 ; 4D33 940E 4ED2 inc r22 ; 4D35 95C3 rjmp avr4D2A ; 4D36 CFF3 avr4D37: lds r20, 0x0065 ; 4D37 9100 0065 andi r20, 0xFB ; 4D39 7F0B sts 0x0065, r20 ; 4D3A 9300 0065 lds r20, 0x0064 ; 4D3C 9100 0064 ori r20, 0x04 ; 4D3E 6004 sts 0x0064, r20 ; 4D3F 9300 0064 jmp avr4E14 ; 4D41 940C 4E14 avr4D43: lds r20, 0x0065 ; 4D43 9100 0065 andi r20, 0xFB ; 4D45 7F0B sts 0x0065, r20 ; 4D46 9300 0065 lds r20, 0x0064 ; 4D48 9100 0064 ori r20, 0x04 ; 4D4A 6004 sts 0x0064, r20 ; 4D4B 9300 0064 clr r22 ; 4D4D 27CC jmp avr4E14 ; 4D4E 940C 4E14 avr4D50: lds r20, 0x0065 ; 4D50 9100 0065 andi r20, 0xFD ; 4D52 7F0D sts 0x0065, r20 ; 4D53 9300 0065 lds r20, 0x0064 ; 4D55 9100 0064 ori r20, 0x02 ; 4D57 6002 sts 0x0064, r20 ; 4D58 9300 0064 ldi r20, 0x14 ; 4D5A E104 avr4D5B: dec r20 ; 4D5B 950A brne avr4D5B ; 4D5C F7F1 lds r20, 0x0065 ; 4D5D 9100 0065 ori r20, 0x02 ; 4D5F 6002 sts 0x0065, r20 ; 4D60 9300 0065 call sub4E24 ; 4D62 940E 4E24 lds r20, 0x0065 ; 4D64 9100 0065 andi r20, 0xFD ; 4D66 7F0D sts 0x0065, r20 ; 4D67 9300 0065 call sub4E24 ; 4D69 940E 4E24 nop ; 4D6B 0000 nop ; 4D6C 0000 nop ; 4D6D 0000 nop ; 4D6E 0000 lds r20, 0x0065 ; 4D6F 9100 0065 ori r20, 0x02 ; 4D71 6002 sts 0x0065, r20 ; 4D72 9300 0065 lds r20, 0x0064 ; 4D74 9100 0064 andi r20, 0xFD ; 4D76 7F0D sts 0x0064, r20 ; 4D77 9300 0064 nop ; 4D79 0000 lds r21, 0x0063 ; 4D7A 9110 0063 sbrc r21, 1 ; 4D7C FD11 rjmp avr4DA5 ; 4D7D C027 ldi r22, 0x01 ; 4D7E E0C1 clr r20 ; 4D7F 2700 avr4D80: nop ; 4D80 0000 nop ; 4D81 0000 nop ; 4D82 0000 lds r21, 0x0063 ; 4D83 9110 0063 sbrc r21, 1 ; 4D85 FD11 rjmp avr4D8A ; 4D86 C003 dec r20 ; 4D87 950A brne avr4D80 ; 4D88 F7B9 rjmp avr4DA5 ; 4D89 C01B avr4D8A: call sub4ECD ; 4D8A 940E 4ECD avr4D8C: lds r21, 0x0063 ; 4D8C 9110 0063 sbrs r21, 1 ; 4D8E FF11 rjmp avr4D99 ; 4D8F C009 cpi r22, 0xFA ; 4D90 3FCA brne avr4D95 ; 4D91 F419 rjmp avr4D99 ; 4D92 C006 nop ; 4D93 0000 nop ; 4D94 0000 avr4D95: call sub4ED2 ; 4D95 940E 4ED2 inc r22 ; 4D97 95C3 rjmp avr4D8C ; 4D98 CFF3 avr4D99: lds r20, 0x0065 ; 4D99 9100 0065 andi r20, 0xFD ; 4D9B 7F0D sts 0x0065, r20 ; 4D9C 9300 0065 lds r20, 0x0064 ; 4D9E 9100 0064 ori r20, 0x02 ; 4DA0 6002 sts 0x0064, r20 ; 4DA1 9300 0064 jmp avr4E14 ; 4DA3 940C 4E14 avr4DA5: lds r20, 0x0065 ; 4DA5 9100 0065 andi r20, 0xFD ; 4DA7 7F0D sts 0x0065, r20 ; 4DA8 9300 0065 lds r20, 0x0064 ; 4DAA 9100 0064 ori r20, 0x02 ; 4DAC 6002 sts 0x0064, r20 ; 4DAD 9300 0064 clr r22 ; 4DAF 27CC jmp avr4E14 ; 4DB0 940C 4E14 avr4DB2:lds r20, 0x0065 ; 4DB2 9100 0065 andi r20, 0xFE ; 4DB4 7F0E sts 0x0065, r20 ; 4DB5 9300 0065 lds r20, 0x0064 ; 4DB7 9100 0064 ori r20, 0x01 ; 4DB9 6001 sts 0x0064, r20 ; 4DBA 9300 0064 ldi r20, 0x14 ; 4DBC E104 avr4DBD: dec r20 ; 4DBD 950A brne avr4DBD ; 4DBE F7F1 lds r20, 0x0065 ; 4DBF 9100 0065 ori r20, 0x01 ; 4DC1 6001 sts 0x0065, r20 ; 4DC2 9300 0065 call sub4E24 ; 4DC4 940E 4E24 lds r20, 0x0065 ; 4DC6 9100 0065 andi r20, 0xFE ; 4DC8 7F0E sts 0x0065, r20 ; 4DC9 9300 0065 call sub4E24 ; 4DCB 940E 4E24 nop ; 4DCD 0000 nop ; 4DCE 0000 nop ; 4DCF 0000 nop ; 4DD0 0000 lds r20, 0x0065 ; 4DD1 9100 0065 ori r20, 0x01 ; 4DD3 6001 sts 0x0065, r20 ; 4DD4 9300 0065 lds r20, 0x0064 ; 4DD6 9100 0064 andi r20, 0xFE ; 4DD8 7F0E sts 0x0064, r20 ; 4DD9 9300 0064 nop ; 4DDB 0000 lds r21, 0x0063 ; 4DDC 9110 0063 sbrc r21, 0 ; 4DDE FD10 rjmp avr4E07 ; 4DDF C027 ldi r22, 0x01 ; 4DE0 E0C1 clr r20 ; 4DE1 2700 avr4DE2: nop ; 4DE2 0000 nop ; 4DE3 0000 nop ; 4DE4 0000 lds r21, 0x0063 ; 4DE5 9110 0063 sbrc r21, 0 ; 4DE7 FD10 rjmp avr4DEC ; 4DE8 C003 dec r20 ; 4DE9 950A brne avr4DE2 ; 4DEA F7B9 rjmp avr4E07 ; 4DEB C01B avr4DEC: call sub4ECD ; 4DEC 940E 4ECD avr4DEE: lds r21, 0x0063 ; 4DEE 9110 0063 sbrs r21, 0 ; 4DF0 FF10 rjmp avr4DFB ; 4DF1 C009 cpi r22, 0xFA ; 4DF2 3FCA brne avr4DF7 ; 4DF3 F419 rjmp avr4DFB ; 4DF4 C006 nop ; 4DF5 0000 nop ; 4DF6 0000 avr4DF7: call sub4ED2 ; 4DF7 940E 4ED2 inc r22 ; 4DF9 95C3 rjmp avr4DEE ; 4DFA CFF3 avr4DFB: lds r20, 0x0065 ; 4DFB 9100 0065 andi r20, 0xFE ; 4DFD 7F0E sts 0x0065, r20 ; 4DFE 9300 0065 lds r20, 0x0064 ; 4E00 9100 0064 ori r20, 0x01 ; 4E02 6001 sts 0x0064, r20 ; 4E03 9300 0064 jmp avr4E14 ; 4E05 940C 4E14 avr4E07: lds r20, 0x0065 ; 4E07 9100 0065 andi r20, 0xFE ; 4E09 7F0E sts 0x0065, r20 ; 4E0A 9300 0065 lds r20, 0x0064 ; 4E0C 9100 0064 ori r20, 0x01 ; 4E0E 6001 sts 0x0064, r20 ; 4E0F 9300 0064 clr r22 ; 4E11 27CC jmp avr4E14 ; 4E12 940C 4E14 ;---------------------------------------------------------------------------- ; Read done avr4E14: cpi r22, 0x00 ; 4E14 30C0 is result 0 then exit breq avr4E1F ; 4E15 F049 cpi r22, 0x3F ; 4E16 33CF brsh avr4E19 ; 4E17 F408 ldi r22, 0x3F ; 4E18 E3CF avr4E19: subi r22, 0x32 ; 4E19 53C2 brsh avr4E1C ; 4E1A F408 ldi r22, 0x01 ; 4E1B E0C1 avr4E1C: mov r21, r22 ; 4E1C 2F1C rcall sub4E63 ; 4E1D D045 mov r22, r21 ; 4E1E 2FC1 avr4E1F: clr r24 ; 4E1F 27DD pop r18 ; 4E20 912F pop r21 ; 4E21 911F #ifdef interrupt_enable sei #endif /* interrupt_enable */ ret ; 4E23 9508 ;------------------------------------------------------------------------- ; delay routine length based on r21 ; 0 = 50 microsec ; 1 = 100 microsec ; 2 = 150 microsec ; 3 = 200 microsec sub4E24: cpi r21, 0x00 ; 4E24 3010 breq avr4E2D ; 4E25 F039 cpi r21, 0x01 ; 4E26 3011 breq avr4E39 ; 4E27 F089 cpi r21, 0x02 ; 4E28 3012 breq avr4E46 ; 4E29 F0E1 cpi r21, 0x03 ; 4E2A 3013 breq avr4E54 ; 4E2B F141 rjmp avr4E39 ; 4E2C C00C avr4E2D: ldi r23, 0x02 ; 50 microsec avr4E2E: ldi r20, 0x39 ; 4E2E E309 avr4E2F: dec r20 ; 4E2F 950A brne avr4E2F ; 4E30 F7F1 dec r23 ; 4E31 951A brne avr4E2E ; 4E32 F7D9 nop ; 4E33 0000 nop ; 4E34 0000 nop ; 4E35 0000 nop ; 4E36 0000 nop ; 4E37 0000 ret ; 4E38 9508 avr4E39: ldi r23, 0x02 ; 100 microsec avr4E3A: ldi r20, 0x76 ; 4E3A E706 avr4E3B: dec r20 ; 4E3B 950A brne avr4E3B ; 4E3C F7F1 dec r23 ; 4E3D 951A brne avr4E3A ; 4E3E F7D9 nop ; 4E3F 0000 nop ; 4E40 0000 nop ; 4E41 0000 nop ; 4E42 0000 nop ; 4E43 0000 nop ; 4E44 0000 ret ; 4E45 9508 avr4E46: ldi r23, 0x02 ; 150 microsec avr4E47: ldi r20, 0xB3 ; 4E47 EB03 avr4E48: dec r20 ; 4E48 950A brne avr4E48 ; 4E49 F7F1 dec r23 ; 4E4A 951A brne avr4E47 ; 4E4B F7D9 nop ; 4E4C 0000 nop ; 4E4D 0000 nop ; 4E4E 0000 nop ; 4E4F 0000 nop ; 4E50 0000 nop ; 4E51 0000 nop ; 4E52 0000 ret ; 4E53 9508 avr4E54: ldi r23, 0x02 ; 200 microsec avr4E55: ldi r20, 0xF0 ; 4E55 EF00 avr4E56: dec r20 ; 4E56 950A brne avr4E56 ; 4E57 F7F1 dec r23 ; 4E58 951A brne avr4E55 ; 4E59 F7D9 nop ; 4E5A 0000 nop ; 4E5B 0000 nop ; 4E5C 0000 nop ; 4E5D 0000 nop ; 4E5E 0000 nop ; 4E5F 0000 nop ; 4E60 0000 nop ; 4E61 0000 ret ; 4E62 9508 ;------------------------------------------------------------------------- ; Deconvert servo position ; r21 = pulse length ; r24 = servo number sub4E63: ldi XH, hi8(SERVO_OFFSET) ; 4E69 E0B3 X = Zero offset ldi XL, lo8(SERVO_OFFSET) ; 4E6A E4A0 ldi r20,0x1F ; isolate servo number and r24,r20 add XL, r24 ; 4E71 0FA0 brsh avr4E74 ; 4E72 F408 inc XH ; 4E73 95B3 avr4E74: mov r18, r24 ; 4E74 2F20 G8D ? subi r18, 0x18 ; 4E75 5128 brlo avr4E78 ; 4E76 F008 rjmp avr4EB1 ; 4E77 C039 avr4E78: mov r18, r24 ; 4E78 2F20 G8C ? subi r18, 0x10 ; 4E79 5120 brlo avr4E7C ; 4E7A F008 rjmp avr4EA1 ; 4E7B C025 avr4E7C: mov r18, r24 ; 4E7C 2F20 G8B ? subi r18, 0x08 ; 4E7D 5028 brlo avr4E80 ; 4E7E F008 rjmp avr4E91 ; 4E7F C011 avr4E80: mov r18, r24 ; 4E80 2F20 G8A ldi r19, 0x01 ; 4E81 E031 avr4E82: cpi r18, 0x00 ; 4E82 3020 breq avr4E87 ; 4E83 F019 dec r18 ; 4E84 952A lsl r19 ; 4E85 0F33 rjmp avr4E82 ; 4E86 CFFB avr4E87: lds r18, DIR_G8A ; 4E87 9120 04E7 get servo direction and r19, r18 ; 4E89 2332 tst r19 ; 4E8A 2333 breq avr4E8D ; 4E8B F009 rjmp avr4EC1 ; 4E8C C034 avr4E8D: ldi r18, 0xC8 ; 4E8D EC28 backwards subtract from 200 sub r18, r21 ; 4E8E 1B21 mov r21, r18 ; 4E8F 2F12 rjmp avr4EC1 ; 4E90 C030 avr4E91: ldi r19, 0x01 ; 4E91 E031 G8B avr4E92: cpi r18, 0x00 ; 4E92 3020 breq avr4E97 ; 4E93 F019 dec r18 ; 4E94 952A lsl r19 ; 4E95 0F33 rjmp avr4E92 ; 4E96 CFFB avr4E97: lds r18, DIR_G8B ; 4E97 9120 04E8 and r19, r18 ; 4E99 2332 tst r19 ; 4E9A 2333 breq avr4E9D ; 4E9B F009 rjmp avr4EC1 ; 4E9C C024 avr4E9D: ldi r18, 0xC8 ; 4E9D EC28 sub r18, r21 ; 4E9E 1B21 mov r21, r18 ; 4E9F 2F12 rjmp avr4EC1 ; 4EA0 C020 avr4EA1: ldi r19, 0x01 ; 4EA1 E031 G8C avr4EA2: cpi r18, 0x00 ; 4EA2 3020 breq avr4EA7 ; 4EA3 F019 dec r18 ; 4EA4 952A lsl r19 ; 4EA5 0F33 rjmp avr4EA2 ; 4EA6 CFFB avr4EA7: lds r18, DIR_G8C ; 4EA7 9120 04E9 and r19, r18 ; 4EA9 2332 tst r19 ; 4EAA 2333 breq avr4EAD ; 4EAB F009 rjmp avr4EC1 ; 4EAC C014 avr4EAD: ldi r18, 0xC8 ; 4EAD EC28 sub r18, r21 ; 4EAE 1B21 mov r21, r18 ; 4EAF 2F12 rjmp avr4EC1 ; 4EB0 C010 avr4EB1: ldi r19, 0x01 ; 4EB1 E031 G8D avr4EB2: cpi r18, 0x00 ; 4EB2 3020 set bit breq avr4EB7 ; 4EB3 F019 dec r18 ; 4EB4 952A lsl r19 ; 4EB5 0F33 rjmp avr4EB2 ; 4EB6 CFFB avr4EB7: lds r18, DIR_G8D ; 4EB7 9120 04EA and r19, r18 ; 4EB9 2332 tst r19 ; 4EBA 2333 breq avr4EBD ; 4EBB F009 rjmp avr4EC1 ; 4EBC C004 avr4EBD: ldi r18, 0xC8 ; 4EBD EC28 sub r18, r21 ; 4EBE 1B21 mov r21, r18 ; 4EBF 2F12 rjmp avr4EC1 ; 4EC0 C000 avr4EC1: ldi r19, 0x14 ; 4EC1 E134 add r21, r19 ; 4EC2 0F13 add 20 to result brsh avr4EC5 ; 4EC3 F408 ser r21 ; 4EC4 EF1F avr4EC5: ld r18, X ; 4EC5 912C get offset sub r21, r18 ; 4EC6 1B12 subi r21, 0x14 ; 4EC7 5114 brsh avr4ECA ; 4EC8 F408 ldi r21, 0x00 ; 4EC9 E010 avr4ECA: ret ; 4ECC 9508 ;------------------------------------------------------------------------- ; delay 4 microsec sub4ECD: ldi r20, 0x09 ; 4ECD E009 avr4ECE: dec r20 ; 4ECE 950A brne avr4ECE ; 4ECF F7F1 nop ; 4ED0 0000 ret ; 4ED1 9508 ;------------------------------------------------------------------------- ; delay 8 microsec sub4ED2: ldi r20, 0x13 ; 4ED2 E103 avr4ED3: dec r20 ; 4ED3 950A brne avr4ED3 ; 4ED4 F7F1 nop ; 4ED5 0000 ret ; 4ED6 9508 ;------------------------------------------------------------------------- ;------------------------------------------------------------------------- ; Global function to set servo directions ; r24 = starting servo, r22 = count , r20 = direction bits dir: push ZH push ZL ldi ZH, hi8(TEMP) ; 2360 E0FE Buffer ldi ZL, lo8(TEMP) ; 2361 E0E0 ldi XH, hi8(DIR_G8A) ; 2362 E0D4 Direction bits ldi XL, lo8(DIR_G8A) ; 2363 EEC7 ldi r23, 0x04 ; 2364 E014 4 bytes DIRA: ldi r18, 0x08 ; 2365 E028 8 bits ld r25, X+ ; 2366 9109 get direction from RAM DIRB: ror r25 ; 2367 9507 brlo DIRC ; 2368 F010 ldi r19, 0x00 ; 2369 E030 rjmp DIRD ; 236A C001 DIRC: ser r19 ; 236B EF3F DIRD: st Z+, r19 ; 236C 9331 buffer (x) = 0 or FF depending on bit dec r18 ; 236D 952A brne DIRB ; 236E F7C1 dec r23 ; 236F 951A brne DIRA ; 2370 F7A1 ldi ZH, hi8(TEMP) ; 2373 E0FE ldi ZL, lo8(TEMP) ; 2374 E0E0 call ZPLUSR24 ; 2375 940E 23D4 Z = Z + r20 DIRE: ror r20 ; 2377 9567 set buffer based on direction bits brsh DIRF ; 2378 F410 ser r19 ; 2379 EF0F rjmp DIRG ; 237A C001 DIRF: ldi r19, 0x00 ; 237B E000 DIRG: st Z+, r19 ; 237C 9301 dec r22 ; 237D 952A brne DIRE ; 237E F7C1 ldi ZH, hi8(TEMP) ; 237F E0FE save direction to RAM from buffer ldi ZL, lo8(TEMP) ; 2380 E0E0 ldi XH, hi8(DIR_G8A) ; 2381 E0D4 ldi XL, lo8(DIR_G8A) ; 2382 EEC7 ldi r23, 0x04 ; 2383 E024 DIRH: ldi r20, 0x00 ; 2384 E040 ldi r18, 0x08 ; 2385 E038 DIRI: ld r19, Z+ ; 2386 9101 tst r19 ; 2387 2300 brne DIRJ ; 2388 F411 clc ; 2389 9488 rjmp DIRK ; 238A C001 DIRJ: sec ; 238B 9408 DIRK: ror r20 ; 238C 9547 dec r18 ; 238D 953A brne DIRI ; 238E F7B9 st X+, r20 ; 238F 9349 dec r23 ; 2390 952A brne DIRH ; 2391 F791 pop ZL pop ZH ret ; 23A6 9508 ;------------------------------------------------------------------------- ; Z = Z + r24 ZPLUSR24: add ZL,r24 brsh ZPLUSR24B inc ZH ZPLUSR24B: ret ;------------------------------------------------------------- ; global function GETMOTORSET ; get measured position and saves in desired and actual position ; void getmotorset (uint8_t, uint8_t, *uint8_t) ; r24 is first servo, r22 is count r20:r21 is value pointer ; getmotorset:push YH push YL push ZH push ZL ldi YH, hi8(SERVO_CURRENT) ; 2216 E0D3 ldi YL, lo8(SERVO_CURRENT) ; 2217 E0C0 ldi ZH, hi8(SERVO_DESIRED) ; 2218 E0F3 ldi ZL, lo8(SERVO_DESIRED) ; 2219 E2E0 mov XH, r21 mov XL, r20 rcall YZPLUSR24 ; 221A D1A4 Y = y +r24 and Z= Z+r24 MOTORSETA: push r24 ; 221B 934F push first servo push r22 ; 221C 935F push number of servos ld r22, X+ ; 221D 940E 07D6 get value cpi r22, 0x00 ; 221F 3000 is it zero ? breq MOTORSETB ; 2220 F091 push YH ; 2221 93DF no then push YL ; 2222 93CF push ZH ; 2223 93FF push ZL ; 2224 93EF push XH push XL call motorin ; 2226 940E 45CD get current position pop XL pop XH pop ZL ; 2229 91EF pop ZH ; 222A 91FF pop YL ; 222B 91CF pop YH ; 222C 91DF cpi r24, 0x0A ; 222D 300A in range ? brlo MOTORSETB ; 222E F020 cpi r24, 0xBF ; 222F 3B0F brsh MOTORSETB ; 2230 F410 st Y, r24 ; 2231 8308 store in 0x300 + offset and 0x320 + offset st Z, r24 ; 2232 8300 MOTORSETB: adiw YL, 0x01 ; 2233 9621 adiw ZL, 0x01 ; 2234 9631 pop r22 ; 2235 915F next servo pop r24 ; 2236 914F inc r24 ; 2237 9543 dec r22 ; 2238 955A brne MOTORSETA ; 2239 F709 pop ZL pop ZH pop YL pop YH ret ;------------------------------------------------------------------------- ; Y= Y + r24 and Z = Z+r24 YZPLUSR24: add YL,r24 brsh YZPLUSR24B inc YH YZPLUSR24B: add ZL,r24 brsh YZPLUSR24C inc ZH YZPLUSR24C: ret ;------------------------------------------------------------------------- ; Global function to turn on a servo ; servo number is in r24 ;void motor(uint8_t); motor: cpi r24, 0x00 ; 1D1D 3000 brne avr1D22 ; 1D1E F419 avr1D1F: ldi r24, 0x01 ; 1D1F E001 sbi DDRA, 0 ; 1D20 9AD0 rjmp avr1DCA ; 1D21 C0A8 avr1D22: cpi r24, 0x01 ; 1D22 3001 brne avr1D27 ; 1D23 F419 ldi r24, 0x02 ; 1D24 E002 sbi DDRA, 1 ; 1D25 9AD1 rjmp avr1DCA ; 1D26 C0A3 avr1D27: cpi r24, 0x02 ; 1D27 3002 brne avr1D2C ; 1D28 F419 ldi r24, 0x04 ; 1D29 E004 avr1D2A: sbi DDRA, 2 ; 1D2A 9AD2 rjmp avr1DCA ; 1D2B C09E avr1D2C: cpi r24, 0x03 ; 1D2C 3003 brne avr1D31 ; 1D2D F419 ldi r24, 0x08 ; 1D2E E008 sbi DDRA, 3 ; 1D2F 9AD3 rjmp avr1DCA ; 1D30 C099 avr1D31: cpi r24, 0x04 ; 1D31 3004 avr1D32: brne avr1D36 ; 1D32 F419 ldi r24, 0x10 ; 1D33 E100 sbi DDRA, 4 ; 1D34 9AD4 rjmp avr1DCA ; 1D35 C094 avr1D36: cpi r24, 0x05 ; 1D36 3005 brne avr1D3B ; 1D37 F419 avr1D38: ldi r24, 0x20 ; 1D38 E200 sbi DDRA, 5 ; 1D39 9AD5 rjmp avr1DCA ; 1D3A C08F avr1D3B: cpi r24, 0x06 ; 1D3B 3006 brne avr1D40 ; 1D3C F419 avr1D3D: ldi r24, 0x40 ; 1D3D E400 sbi DDRA, 6 ; 1D3E 9AD6 rjmp avr1DCA ; 1D3F C08A avr1D40: cpi r24, 0x07 ; 1D40 3007 brne avr1D45 ; 1D41 F419 ldi r24, 0x80 ; 1D42 E800 sbi DDRA, 7 ; 1D43 9AD7 rjmp avr1DCA ; 1D44 C085 avr1D45: cpi r24, 0x08 ; 1D45 3008 brne avr1D4A ; 1D46 F419 ldi r24, 0x01 ; 1D47 E001 sbi DDRB, 0 ; 1D48 9AB8 rjmp avr1DD0 ; 1D49 C086 avr1D4A: cpi r24, 0x09 ; 1D4A 3009 brne avr1D4F ; 1D4B F419 ldi r24, 0x02 ; 1D4C E002 sbi DDRB, 1 ; 1D4D 9AB9 rjmp avr1DD0 ; 1D4E C081 avr1D4F: cpi r24, 0x0A ; 1D4F 300A brne avr1D54 ; 1D50 F419 ldi r24, 0x04 ; 1D51 E004 avr1D52: sbi DDRB, 2 ; 1D52 9ABA rjmp avr1DD0 ; 1D53 C07C avr1D54: cpi r24, 0x0B ; 1D54 300B brne avr1D59 ; 1D55 F419 ldi r24, 0x08 ; 1D56 E008 sbi DDRB, 3 ; 1D57 9ABB rjmp avr1DD0 ; 1D58 C077 avr1D59: cpi r24, 0x0C ; 1D59 300C brne avr1D5E ; 1D5A F419 ldi r24, 0x10 ; 1D5B E100 sbi DDRB, 4 ; 1D5C 9ABC avr1D5D: rjmp avr1DD0 ; 1D5D C072 avr1D5E: cpi r24, 0x0D ; 1D5E 300D brne avr1D63 ; 1D5F F419 ldi r24, 0x20 ; 1D60 E200 sbi DDRB, 5 ; 1D61 9ABD rjmp avr1DD0 ; 1D62 C06D avr1D63: cpi r24, 0x0E ; 1D63 300E brne avr1D68 ; 1D64 F419 ldi r24, 0x40 ; 1D65 E400 sbi DDRB, 6 ; 1D66 9ABE rjmp avr1DD0 ; 1D67 C068 avr1D68: cpi r24, 0x0F ; 1D68 300F brne avr1D6D ; 1D69 F419 ldi r24, 0x80 ; 1D6A E800 sbi DDRB, 7 ; 1D6B 9ABF rjmp avr1DD0 ; 1D6C C063 avr1D6D: cpi r24, 0x10 ; 1D6D 3100 brne avr1D72 ; 1D6E F419 ldi r24, 0x01 ; 1D6F E001 avr1D70: sbi DDRC, 7 ; 1D70 9AA7 rjmp avr1DD6 ; 1D71 C064 avr1D72: cpi r24, 0x11 ; 1D72 3101 brne avr1D77 ; 1D73 F419 ldi r24, 0x02 ; 1D74 E002 sbi DDRC, 6 ; 1D75 9AA6 avr1D76: rjmp avr1DD6 ; 1D76 C05F avr1D77: cpi r24, 0x12 ; 1D77 3102 brne avr1D7C ; 1D78 F419 ldi r24, 0x04 ; 1D79 E004 sbi DDRC, 5 ; 1D7A 9AA5 avr1D7B: rjmp avr1DD6 ; 1D7B C05A avr1D7C: cpi r24, 0x13 ; 1D7C 3103 brne avr1D81 ; 1D7D F419 ldi r24, 0x08 ; 1D7E E008 sbi DDRC, 4 ; 1D7F 9AA4 rjmp avr1DD6 ; 1D80 C055 avr1D81: cpi r24, 0x14 ; 1D81 3104 brne avr1D86 ; 1D82 F419 ldi r24, 0x10 ; 1D83 E100 sbi DDRC, 3 ; 1D84 9AA3 rjmp avr1DD6 ; 1D85 C050 avr1D86: cpi r24, 0x15 ; 1D86 3105 brne avr1D8B ; 1D87 F419 avr1D88: ldi r24, 0x20 ; 1D88 E200 sbi DDRC, 2 ; 1D89 9AA2 rjmp avr1DD6 ; 1D8A C04B avr1D8B: cpi r24, 0x16 ; 1D8B 3106 brne avr1D90 ; 1D8C F419 ldi r24, 0x40 ; 1D8D E400 sbi DDRC, 1 ; 1D8E 9AA1 rjmp avr1DD6 ; 1D8F C046 avr1D90: cpi r24, 0x17 ; 1D90 3107 brne avr1D95 ; 1D91 F419 ldi r24, 0x80 ; 1D92 E800 sbi DDRC, 0 ; 1D93 9AA0 rjmp avr1DD6 ; 1D94 C041 avr1D95: cpi r24, 0x18 ; 1D95 3108 brne avr1D9A ; 1D96 F419 avr1D97: ldi r24, 0x01 ; 1D97 E001 sbi 0x02, 7 ; 1D98 9A17 rjmp avr1DDC ; 1D99 C042 avr1D9A: cpi r24, 0x19 ; 1D9A 3109 avr1D9B: brne avr1D9F ; 1D9B F419 ldi r24, 0x02 ; 1D9C E002 sbi 0x02, 6 ; 1D9D 9A16 rjmp avr1DDC ; 1D9E C03D avr1D9F: cpi r24, 0x1A ; 1D9F 310A brne avr1DA4 ; 1DA0 F419 ldi r24, 0x04 ; 1DA1 E004 sbi DDRD, 7 ; 1DA2 9A8F rjmp avr1DDC ; 1DA3 C038 avr1DA4: cpi r24, 0x1B ; 1DA4 310B brne avr1DA9 ; 1DA5 F419 avr1DA6: ldi r24, 0x08 ; 1DA6 E008 sbi DDRD, 6 ; 1DA7 9A8E rjmp avr1DDC ; 1DA8 C033 avr1DA9: cpi r24, 0x1C ; 1DA9 310C brne avr1DAE ; 1DAA F419 ldi r24, 0x10 ; 1DAB E100 sbi DDRD, 5 ; 1DAC 9A8D rjmp avr1DDC ; 1DAD C02E avr1DAE: cpi r24, 0x1D ; 1DAE 310D brne avr1DB7 ; 1DAF F439 ldi r24, 0x20 ; 1DB0 E200 lds r18, 0x0064 ; 1DB1 9120 0064 ori r18, 0x04 ; 1DB3 6024 avr1DB4: sts 0x0064, r18 ; 1DB4 9320 0064 rjmp avr1DDC ; 1DB6 C025 avr1DB7: cpi r24, 0x1E ; 1DB7 310E brne avr1DC0 ; 1DB8 F439 avr1DB9: ldi r24, 0x40 ; 1DB9 E400 lds r18, 0x0064 ; 1DBA 9120 0064 ori r18, 0x02 ; 1DBC 6022 sts 0x0064, r18 ; 1DBD 9320 0064 rjmp avr1DDC ; 1DBF C01C avr1DC0: cpi r24, 0x1F ; 1DC0 310F brne avr1DC9 ; 1DC1 F439 ldi r24, 0x80 ; 1DC2 E800 lds r18, 0x0064 ; 1DC3 9120 0064 ori r18, 0x01 ; 1DC5 6021 avr1DC6: sts 0x0064, r18 ; 1DC6 9320 0064 rjmp avr1DDC ; 1DC8 C013 avr1DC9: rjmp avr1DE2 ; 1DC9 C018 avr1DCA: lds r25, ENABLE_G8A ; 1DCA 9110 04EB or r24, r25 ; 1DCC 2B01 sts ENABLE_G8A, r24 ; 1DCD 9300 04EB rjmp avr1DE2 ; 1DCF C012 avr1DD0: lds r25, ENABLE_G8B ; 1DD0 9110 04EC or r24, r25 ; 1DD2 2B01 sts ENABLE_G8B, r24 ; 1DD3 9300 04EC avr1DD5: rjmp avr1DE2 ; 1DD5 C00C avr1DD6: lds r25, ENABLE_G8C ; 1DD6 9110 04ED or r24, r25 ; 1DD8 2B01 avr1DD9: sts ENABLE_G8C, r24 ; 1DD9 9300 04ED rjmp avr1DE2 ; 1DDB C006 avr1DDC: lds r25, ENABLE_G8D ; 1DDC 9110 04EE or r24, r25 ; 1DDE 2B01 sts ENABLE_G8D, r24 ; 1DDF 9300 04EE avr1DE2: ret ; ;------------------------------------------------------------------------- ; Global function to turn off a servo ; servo number is in r24 ; void motor_off (uint8_t); motor_off: cpi r24, 0x00 ; 1DEB 3000 avr1DEC: brne avr1DEF ; 1DEC F411 ldi r24, 0x01 ; 1DED E001 rjmp avr1E6C ; 1DEE C07D avr1DEF: cpi r24, 0x01 ; 1DEF 3001 brne avr1DF3 ; 1DF0 F411 ldi r24, 0x02 ; 1DF1 E002 avr1DF2: rjmp avr1E6C ; 1DF2 C079 avr1DF3: cpi r24, 0x02 ; 1DF3 3002 brne avr1DF7 ; 1DF4 F411 ldi r24, 0x04 ; 1DF5 E004 rjmp avr1E6C ; 1DF6 C075 avr1DF7: cpi r24, 0x03 ; 1DF7 3003 brne avr1DFB ; 1DF8 F411 ldi r24, 0x08 ; 1DF9 E008 rjmp avr1E6C ; 1DFA C071 avr1DFB: cpi r24, 0x04 ; 1DFB 3004 brne avr1DFF ; 1DFC F411 ldi r24, 0x10 ; 1DFD E100 rjmp avr1E6C ; 1DFE C06D avr1DFF: cpi r24, 0x05 ; 1DFF 3005 brne avr1E03 ; 1E00 F411 ldi r24, 0x20 ; 1E01 E200 rjmp avr1E6C ; 1E02 C069 avr1E03: cpi r24, 0x06 ; 1E03 3006 avr1E04: brne avr1E07 ; 1E04 F411 ldi r24, 0x40 ; 1E05 E400 rjmp avr1E6C ; 1E06 C065 avr1E07: cpi r24, 0x07 ; 1E07 3007 brne avr1E0B ; 1E08 F411 ldi r24, 0x80 ; 1E09 E800 rjmp avr1E6C ; 1E0A C061 avr1E0B: cpi r24, 0x08 ; 1E0B 3008 avr1E0C: brne avr1E0F ; 1E0C F411 ldi r24, 0x01 ; 1E0D E001 rjmp avr1E73 ; 1E0E C064 avr1E0F: cpi r24, 0x09 ; 1E0F 3009 brne avr1E13 ; 1E10 F411 ldi r24, 0x02 ; 1E11 E002 rjmp avr1E73 ; 1E12 C060 avr1E13: cpi r24, 0x0A ; 1E13 300A brne avr1E17 ; 1E14 F411 ldi r24, 0x04 ; 1E15 E004 rjmp avr1E73 ; 1E16 C05C avr1E17: cpi r24, 0x0B ; 1E17 300B brne avr1E1B ; 1E18 F411 ldi r24, 0x08 ; 1E19 E008 rjmp avr1E73 ; 1E1A C058 avr1E1B: cpi r24, 0x0C ; 1E1B 300C brne avr1E1F ; 1E1C F411 ldi r24, 0x10 ; 1E1D E100 rjmp avr1E73 ; 1E1E C054 avr1E1F: cpi r24, 0x0D ; 1E1F 300D brne avr1E23 ; 1E20 F411 ldi r24, 0x20 ; 1E21 E200 avr1E22: rjmp avr1E73 ; 1E22 C050 avr1E23: cpi r24, 0x0E ; 1E23 300E brne avr1E27 ; 1E24 F411 ldi r24, 0x40 ; 1E25 E400 rjmp avr1E73 ; 1E26 C04C avr1E27: cpi r24, 0x0F ; 1E27 300F brne avr1E2B ; 1E28 F411 ldi r24, 0x80 ; 1E29 E800 avr1E2A: rjmp avr1E73 ; 1E2A C048 avr1E2B: cpi r24, 0x10 ; 1E2B 3100 brne avr1E2F ; 1E2C F411 ldi r24, 0x01 ; 1E2D E001 rjmp avr1E7A ; 1E2E C04B avr1E2F: cpi r24, 0x11 ; 1E2F 3101 avr1E30: brne avr1E33 ; 1E30 F411 ldi r24, 0x02 ; 1E31 E002 rjmp avr1E7A ; 1E32 C047 avr1E33: cpi r24, 0x12 ; 1E33 3102 brne avr1E37 ; 1E34 F411 avr1E35: ldi r24, 0x04 ; 1E35 E004 rjmp avr1E7A ; 1E36 C043 avr1E37: cpi r24, 0x13 ; 1E37 3103 brne avr1E3B ; 1E38 F411 ldi r24, 0x08 ; 1E39 E008 rjmp avr1E7A ; 1E3A C03F avr1E3B: cpi r24, 0x14 ; 1E3B 3104 brne avr1E3F ; 1E3C F411 ldi r24, 0x10 ; 1E3D E100 rjmp avr1E7A ; 1E3E C03B avr1E3F: cpi r24, 0x15 ; 1E3F 3105 brne avr1E43 ; 1E40 F411 ldi r24, 0x20 ; 1E41 E200 avr1E42: rjmp avr1E7A ; 1E42 C037 avr1E43: cpi r24, 0x16 ; 1E43 3106 brne avr1E47 ; 1E44 F411 ldi r24, 0x40 ; 1E45 E400 rjmp avr1E7A ; 1E46 C033 avr1E47: cpi r24, 0x17 ; 1E47 3107 brne avr1E4B ; 1E48 F411 ldi r24, 0x80 ; 1E49 E800 avr1E4A: rjmp avr1E7A ; 1E4A C02F avr1E4B: cpi r24, 0x18 ; 1E4B 3108 brne avr1E4F ; 1E4C F411 ldi r24, 0x01 ; 1E4D E001 rjmp avr1E81 ; 1E4E C032 avr1E4F: cpi r24, 0x19 ; 1E4F 3109 brne avr1E53 ; 1E50 F411 avr1E51: ldi r24, 0x02 ; 1E51 E002 rjmp avr1E81 ; 1E52 C02E avr1E53: cpi r24, 0x1A ; 1E53 310A brne avr1E57 ; 1E54 F411 avr1E55: ldi r24, 0x04 ; 1E55 E004 rjmp avr1E81 ; 1E56 C02A avr1E57: cpi r24, 0x1B ; 1E57 310B brne avr1E5B ; 1E58 F411 ldi r24, 0x08 ; 1E59 E008 rjmp avr1E81 ; 1E5A C026 avr1E5B: cpi r24, 0x1C ; 1E5B 310C brne avr1E5F ; 1E5C F411 ldi r24, 0x10 ; 1E5D E100 rjmp avr1E81 ; 1E5E C022 avr1E5F: cpi r24, 0x1D ; 1E5F 310D avr1E60: brne avr1E63 ; 1E60 F411 ldi r24, 0x20 ; 1E61 E200 rjmp avr1E81 ; 1E62 C01E avr1E63: cpi r24, 0x1E ; 1E63 310E brne avr1E67 ; 1E64 F411 ldi r24, 0x40 ; 1E65 E400 rjmp avr1E81 ; 1E66 C01A avr1E67: cpi r24, 0x1F ; 1E67 310F avr1E68: brne avr1E6B ; 1E68 F411 ldi r24, 0x80 ; 1E69 E800 rjmp avr1E81 ; 1E6A C016 avr1E6B: rjmp avr1E88 ; 1E6B C01C avr1E6C: lds r25, ENABLE_G8A ; 1E6C 9110 04EB avr1E6E: com r24 ; 1E6E 9500 and r24, r25 ; 1E6F 2301 sts ENABLE_G8A, r24 ; 1E70 9300 04EB rjmp avr1E88 ; 1E72 C015 avr1E73: lds r25, ENABLE_G8B ; 1E73 9110 04EC com r24 ; 1E75 9500 and r24, r25 ; 1E76 2301 sts ENABLE_G8B, r24 ; 1E77 9300 04EC rjmp avr1E88 ; 1E79 C00E avr1E7A: lds r25, ENABLE_G8C ; 1E7A 9110 04ED com r24 ; 1E7C 9500 and r24, r25 ; 1E7D 2301 sts ENABLE_G8C, r24 ; 1E7E 9300 04ED avr1E80: rjmp avr1E88 ; 1E80 C007 avr1E81: lds r25, ENABLE_G8D ; 1E81 9110 04EE com r24 ; 1E83 9500 and r24, r25 ; 1E84 2301 sts ENABLE_G8D, r16 ; 1E85 9300 04EE avr1E88: ret ; ;--------------------------------------------------------------- ; Global function to set zero of servos ; store zero offsets in RAM ; r24 = first servo number, r22 = number of servos, r21,r20 = pointer to values zero: ldi XH, hi8(SERVO_OFFSET) ldi XL, lo8(SERVO_OFFSET) push YH push YL mov YH, r21 mov YL, r20 rcall XPLUSR24 ; X = X + r24 rcall sub22F9 ; 1FD1 940E 22F9 store zero offsets in RAM pop YL pop YH ret ;------------------------------------------------------------------------- ; store zero values as difference from 100 position sub22F9: ld r18, Y+ ; get value tst r18 ; skip if zero breq avr2300 subi r18, 0x64 ; subtract 64 and store st X+, r18 rjmp avr2301 avr2300: adiw XL, 0x01 avr2301: dec r22 brne sub22F9 ret ;------------------------------------------------------------------------- ; X = X + r24 XPLUSR24: add XL,r24 brsh XPLUSR24A inc XH XPLUSR24A: ret ;------------------------------------------------------------------------- ; X = X + r24, Y= Y + r24 and Z = Z+r24 XYZPLUSR24: add XL,r24 brsh XYZPLUSR24A inc XH XYZPLUSR24A: add YL,r24 brsh XYZPLUSR24B inc YH XYZPLUSR24B: add ZL,r24 brsh XYZPLUSR24C inc ZH XYZPLUSR24C: ret ;--------------------------------------------------------------- ; Global function to set PTP on/off SETPTP ; ; 0/1 in r24 to set/clear ptp: sts PTP_STATE, r24 ret ;-------------------------------------------------------------------------- ;global function for SERVO CONTROL servo_ctl: cpi r24, 0x00 ; HIGH SPEED SET OFF breq avr2028 cpi r24, 0x01 ; HIGH SPEED SET ON breq avr202C cpi r24, 0x05 ; PTPALL OFF breq avr2042 cpi r24, 0x06 ; PTP ALL ON breq avr2048 ret avr2028: ldi r24, 0x00 ; High Speed Off sts HIGH_SPEED, r24 ret avr202C: ser r24 ; High Speed On sts HIGH_SPEED, r24 ret avr2042: lds r24, C3024_STATUS ; PTP OFF clears bit 7 andi r24, 0x7F sts C3024_STATUS, r24 ret avr2048: lds r24, C3024_STATUS ; PTP On sets bit 7 ori r24, 0x80 sts C3024_STATUS, r24 ret ;------------------------------------------------------ ; Global function to move a single servo ; r24 = servo number, r22 = position ; void servo(uint8_t, uint8_t) ; servo: ldi XH, 0x03 ; 1F16 E0F3 ldi XL, 0x00 ; 1F17 E0E0 add XL, r24 ; 1F1B 0FE7 point into RAM 0x300 + servo number brsh avr1F1E ; 1F1C F408 inc XH ; 1F1D 95F3 avr1F1E: tst r22 ; 1F20 2377 is it null ? breq avr1F25 ; 1F21 F019 st X, r22 ; 1F22 8370 store the value avr1F23: call sub1F30 ; 1F23 940E 1F30 avr1F25: ret ;------------------------------------------------------------------------- ; Sets the RAM at 0x3C0 to Speed * 17 ; r24 = servo number, sub1F30: ldi XH, 0x03 ; 1F30 E0D3 point to ram 0x3C0 + servo number ldi XL, 0xC0 ; 1F31 ECC0 add XL, r24 ; 1F32 0FC1 brsh avr1F35 ; 1F33 F408 inc XH ; 1F34 95D3 avr1F35: lds r22, DEF_SPEED ; 1F35 9100 04D4 get speed ldi r23, 0x00 ; 1F37 E010 ldi r21, 0x11 ; 1F38 E131 avr1F39: add r23, r21 ; 1F39 0F13 dec r22 ; 1F3A 950A brne avr1F39 ; 1F3B F7E9 st X, r17 ; 1F3C 8318 r17 = speed * 17 avr1F3D: ret ; 1F3D 9508 ;------------------------------------------------------------------------- ;------------------------------------------------------------------------- ; global function for a group move ; ; r24 = starting servo, r22 = number of servos, r21:r20 = servo values move: push r16 mov r16, r24 rcall servo_done ;are any of these servos busy ? tst r24 breq MOVEC mov r24, r16 push ZH push ZL push YH push YL ldi XH, hi8(SERVO_DESIRED) ; X = pointer to desired ldi XL, lo8(SERVO_DESIRED) mov ZH, r21 ; point to array of values mov ZL, r20 rcall XPLUSR24 ; X = X + r24 mov r16, r21 rcall MOVED ; fill (X + r24)++ with next bytes lds r16, C3024_STATUS ; If PTP all on then set for all servos sbrs r16, 7 rjmp MOVEB ldi r24, 0x00 ; if PTP ALL then set to change all servo settings ldi r22, 0x20 MOVEB: ldi XH, hi8(SERVO_DESIRED) ldi XL, lo8(SERVO_DESIRED) ldi YH, hi8(SERVO_CURRENT) ldi YL, lo8(SERVO_CURRENT) ldi ZH, hi8(SERVO_MOVE) ; Z = mod(difference in position) ldi ZL, lo8(SERVO_MOVE) rcall XYZPLUSR24 ; add r24 to X, Y, Z mov r16, r22 ; move count to r16 rcall MOVEI ; Z = how far to move ldi ZH, hi8(SERVO_MOVE) ldi ZL, lo8(SERVO_MOVE) rcall ZPLUSR24 ; Z = Z + r24 mov r16, r22 ; count to r16 dec r16 ; count = count -1 (31 compares) rcall MOVEM ; get largest move into LARGEST_MOVE ldi YH, hi8(SERVO_DELTA) ; Y = SERVO_DELTA ldi YL, lo8(SERVO_DELTA) ; ldi ZH, hi8(SERVO_MOVE) ; Z = dist to move ldi ZL, lo8(SERVO_MOVE) ; rcall YZPLUSR24 ; Add r24 to both Y and Z mov r16, r22 ; count in r16 rcall MOVEP rcall MOTION ; 1F6B D3C5 sets sync move in progress bits pop YL pop YH pop ZL pop ZH MOVEC: pop r16 ret ; fills SERVO_DESIRED with values MOVED: mov r18, r22 ; 2298 2F10 MOVEE: ld r19, Z+ ; 2299 940E 07D6 cpi r19, 0x00 ; 229B 3000 breq MOVEG ; 229C F021 st X+, r19 ; 229D 9309 MOVEF: dec r18 ; 229E 951A brne MOVEE ; 229F F7C9 rjmp MOVEH ; 22A0 C002 MOVEG: adiw XL, 0x01 ; 22A1 9621 rjmp MOVEF ; 22A2 CFFB MOVEH: ret ; 22A3 9508 ;------------------------------------------------------------------------- ; Z is how far to move ; r16 = count, X = , Y = Z = MOVEI: mov r19, r16 ; 22A4 2F30 MOVEJ: ld r17, X+ ; 22A5 911D ld r18, Y+ ; 22A6 9129 cp r17, r18 ; 22A7 1712 brsh MOVEK ; 22A8 F418 sub r18, r17 ; 22A9 1B21 mov r16, r18 ; 22AA 2F02 rjmp MOVEL ; 22AB C002 MOVEK: sub r17, r18 ; 22AC 1B12 mov r16, r17 ; 22AD 2F01 MOVEL: inc r16 ; 22AE 9503 st Z+, r16 ; 22AF 9301 dec r19 ; 22B0 953A brne MOVEJ ; 22B1 F799 ret ; 22B2 9508 ;------------------------------------------------------------------------ ; get largest move into 0x4DB MOVEM: mov r18, r16 ; 22B3 2F20 ld r16, Z+ ; 22B4 9101 MOVEN: ld r17, Z+ ; 22B5 9111 cp r16, r17 ; 22B6 1701 brsh MOVEO ; 22B7 F408 mov r16, r17 ; 22B8 2F01 MOVEO: dec r18 ; 22B9 952A brne MOVEN ; 22BA F7D1 inc r16 ; 22BB 9503 sts LARGEST_MOVE, r16 ; 22BC 9300 04DB ret ; 22BE 9508 ;------------------------------------------------------------------------ ;------------------------------------------------------------------------ ; MOVEP: push r23 mov r23, r16 ; 22C0 2F70 r23 is count push r16 push r17 push r18 push r19 push r20 push r21 push r22 lds r16, PTP_STATE ; Test PTP ALL tst r16 brne MOVES MOVEQ: ldi r16, 0x11 ; PTP ALL OFF ldi r17, 0x00 lds r18, DEF_SPEED ; get Default speed to r18 ldi r19, 0x00 call U_MULTIPLY ; speed * 17 cpi r18, 0x00 brne MOVER ldi r18, 0x01 MOVER : st Y+, r18 ; value is between 1 and 255 dec r23 brne MOVER rjmp MOVEU MOVES: ldi r16, 0x11 ; PTP ALL ON ldi r17, 0x00 lds r18, DEF_SPEED ldi r19, 0x00 call U_MULTIPLY ; r18:r19 = DEF_SPEED * 17 ld r16, Z+ ; get move size ldi r17, 0x00 call U_MULTIPLY ; r18:r19 = DEF_SPEED * 17 * SERVO_MOVE[X] lds r20, LARGEST_MOVE ldi r21, 0x00 call U_DIVIDE ; r18 = (DEF_SPEED * 17 * SERVO_MOVE[X]) / LARGEST_MOVE cpi r18, 0x00 ; 22EA 3020 brne MOVET ; 22EB F409 ldi r18, 0x01 ; 22EC E021 MOVET: st Y+, r18 ; 22ED 9329 dec r23 ; 22EE 957A brne MOVES ; 22EF F751 MOVEU: pop r22 ; 22F0 916F pop r21 ; 22F1 915F pop r20 ; 22F2 914F pop r19 ; 22F3 913F pop r18 ; 22F4 912F pop r17 ; 22F5 911F pop r16 ; 22F6 910F pop r23 ; 22F7 917F ret ; 22F8 9508 ;------------------------------------------------------------------------- ; sets the sync move in progress for desired servos MOTION: ldi ZH, hi8(TEMP) ldi ZL, lo8(TEMP) ldi YH, hi8(MOTION_G8A) ldi YL, lo8(MOTION_G8A) ldi r17, 0x04 ; 4 bytes MOTIONA: ldi r18, 0x08 ; make array in TEMP buffer of byte corresponding to bits ld r16, Y+ MOTIONB: ror r16 brlo MOTIONC ldi r19, 0x00 rjmp MOTIOND MOTIONC: ser r19 MOTIOND: st Z+, r19 dec r18 brne MOTIONB dec r17 brne MOTIONA mov r18, r22 ldi ZH, hi8(TEMP) ldi ZL, lo8(TEMP) call ZPLUSR24 MOTIONE: ldi r16, 0x00 ; clear the bits st Z+, r16 dec r18 brne MOTIONE ldi ZH, hi8(TEMP) ;save back into 0x4E3 to 0x4E7 again ldi ZL, lo8(TEMP) ldi YH, hi8(MOTION_G8A) ldi YL, lo8(MOTION_G8A) ldi r18, 0x04 MOTIONF: ldi r20, 0x00 ldi r19, 0x08 MOTIONG: ld r16, Z+ tst r16 brne MOTIONH clc rjmp MOTIONI MOTIONH: sec MOTIONI: ror r20 dec r19 brne MOTIONG st Y+, r20 dec r18 brne MOTIONF ret ;------------------------------------------------------------------------- ;------------------------------------------------------------------------- ; global function servo_done, also used by move command ; r24 = start servo number, r22 = servo number ; returns 0 in r24 if busy else 0xFF servo_done: push ZH push ZL push r16 push r17 ldi ZH, hi8(TEMP) ; Z = 0xE00 ldi ZL, lo8(TEMP) ldi XH, hi8(MOTION_G8A) ; Y = servo in motion bits ldi XL, lo8(MOTION_G8A) ldi r17, 0x04 ; numer of bytes SERVOBUSYA: ldi r18, 0x08 ; 8 bits in byte ld r16, X+ ; get value SERVOBUSYB: ror r16 ; test bits brlo SERVOBUSYC ldi r19, 0x00 ; 2319 E030 bit 0 = 0 save 0 rjmp SERVOBUSYD SERVOBUSYC: ser r19 ; bit 0 = 1 save FF SERVOBUSYD: st Z+, r19 dec r18 brne SERVOBUSYB dec r17 brne SERVOBUSYA ; checks locations E00 up for count in r21, and offset in 20 ; returns FF if none are busy mov r17, r22 ldi ZH, hi8(TEMP) ldi ZL, lo8(TEMP) add ZL, r24 brsh SERVOBUSYE inc ZH SERVOBUSYE: ld r16, Z+ cpi r16, 0xFF brne SERVOBUSYF dec r17 brne SERVOBUSYE pop r17 pop r16 pop ZL pop ZH ser r24 ret SERVOBUSYF: pop r17 pop r16 pop ZL pop ZH ldi r24, 0x00 ret ;------------------------------------------------------------------- ; global function to set default speed ; ; speed is in r24 ; void speed(uint8_t) ; speed: cpi r24, 0x10 ; 1EBC 3110 brlo avr1EBF ; 1EBD F008 ldi r24, 0x0F ; 1EBE E01F if > 15 then 15 avr1EBF: tst r24 ; 1EBF 2311 brne avr1EC2 ; 1EC0 F409 ldi r24, 0x01 ; 1EC1 E011 Speed must be in range 1 to 15 avr1EC2: sts DEF_SPEED, r24 ; 1EC2 9310 04D4 put speed in location 0x4D4 ret ; 1EC7 9508 if bit 5 in 0x4D2 is set ;------------------------------------------------------------------------- ;16 by 16 unsigned muliply ; r16 = multiplicand low r17 = multiplicand high ; r18 = multiplier low r19 = multiplier high ; r18 = product low, r19 = product mid low, r20 = product mid high, r21 = product high U_MULTIPLY: clr r21 ; 02EE 2755 clr r20 ; 02EF 2744 ldi r22, 0x10 ; 02F0 E160 lsr r19 ; 02F1 9536 ror r18 ; 02F2 9527 MULTIPLY1: brsh MULTIPLY2 ; 02F3 F410 add r20, r16 ; 02F4 0F40 adc r21, r17 ; 02F5 1F51 MULTIPLY2: ror r21 ; 02F6 9557 ror r20 ; 02F7 9547 ror r19 ; 02F8 9537 ror r18 ; 02F9 9527 dec r22 ; 02FA 956A brne MULTIPLY1 ; 02FB F7B9 ret ; 02FC 9508 ;------------------------------------------------------------------------ ; 16/16 unsigned division ; Remainder = r16(l), r17(h) ; Dividend = r18(l), r19(h) ; Divisor = r20 (l), r21(h) ; Result = r18 (l), r19(h) U_DIVIDE: clr r16 ; 02FD 2700 sub r17, r17 ; 02FE 1B11 ldi r22, 0x11 ; 02FF E161 loop count DIVIDE1: rol r18 ; 0300 1F22 rol r19 ; 0301 1F33 dec r22 ; 0302 956A brne DIVIDE2 ; 0303 F409 ret ; 0304 9508 DIVIDE2: rol r16 ; 0305 1F00 rol r17 ; 0306 1F11 sub r16, r20 ; 0307 1B04 sbc r17, r21 ; 0308 0B15 brsh DIVIDE3 ; 0309 F420 add r16, r20 ; 030A 0F04 adc r17, r21 ; 030B 1F15 clc ; 030C 9488 rjmp DIVIDE1 ; 030D CFF2 DIVIDE3: sec ; 030E 9408 rjmp DIVIDE1 ; 030F CFF0 ;------------------------------------------------------------------------- ; global function delay_set ; set the delay based on the timer 0 (motion interrupt) ; r25:r24 = delay in mSec delay_set: clc ; divide by 4 since delay is based on 4 millisec timer int ror r25 ; shift right ror r24 clc ror r25 ; shift right ror r24 sts DELAY_LOW, r24 ; store low count sts DELAY_HIGH, r25 ; store high count ser r16 sts DELAY_BUSY, r16 ; set counter busy flag ret ;------------------------------------------------------------------------- ; global function delay_busy ; check if delay complete ; r24 0xFF if busy, else 0 if done delay_busy: lds r24, DELAY_BUSY ret ;------------------------------------------------------------------------- ; global function delay_wait ; wait for delay complete ; delay_wait: lds r18, DELAY_BUSY tst r18 brne delay_wait ret ;---------------------------------------------------------- ; Gyro Routines part 1 ;first check if KRG or GWS sub5011: lds r16, GYRO_ENABLE andi r16, 0xF0 cpi r16, 0x00 ; GWS ? brne avr5017 rjmp avr5025 avr5017: cpi r16, 0x10 ; KRG ? brne avr501A rjmp avr50B6 avr501A: ret ;--------------------------------------------------------- ; Gyroroutines part 2 ; first check if KRG or GWS sub501B: lds r16, GYRO_ENABLE andi r16, 0xF0 cpi r16, 0x00 ; GWS ? brne avr5021 rjmp avr5048 avr5021: cpi r16, 0x10 ; KRG ? brne avr5024 rjmp avr50D9 avr5024: ret ;------------------------------------------------------------------------- avr5025: clr r17 ; GWS Servo Part 1 lds r16, GYRO_ENABLE andi r16, 0x0F sbrc r16, 0 ori r17, 0x01 sbrc r16, 1 ori r17, 0x02 sbrc r16, 2 ori r17, 0x04 sbrc r16, 3 ori r17, 0x08 sbrs r16, 0 rjmp avr5033 avr5033: sbrs r16, 1 rjmp avr5035 avr5035: sbrs r16, 2 rjmp avr5037 avr5037: sbrs r16, 3 rjmp avr5039 avr5039: mov r18, r17 swap r18 andi r18, 0xF0 or r18, r17 lds r16, 0x0062 or r18, r16 sts 0x0062, r18 lds r16, 0x0061 or r17, r16 sts 0x0061, r17 ret ;------------------------------------------------------------------------- avr5048: lds r16, GYRO_ENABLE ; GWS Servo part 2 andi r16, 0x0F ; 504A 700F ldi r19, 0x00 ; 504B E030 ldi r18, 0x7F ; 504C E72F mov r3, r18 ; 504D 2E32 mov r4, r18 ; 504E 2E42 mov r5, r18 ; 504F 2E52 mov r6, r18 ; 5050 2E62 clr r7 ; 5051 2477 clr r8 ; 5052 2488 clr r9 ; 5053 2499 clr r10 ; 5054 24AA avr5055: ldi r17, 0x00 ; 5055 E010 cp r3, r19 ; 5056 1633 brne avr505A ; 5057 F411 sbrc r16, 0 ; 5058 FD00 ori r17, 0x01 ; 5059 6011 avr505A: cp r4, r19 ; 505A 1643 brne avr505E ; 505B F411 sbrc r16, 1 ; 505C FD01 ori r17, 0x02 ; 505D 6012 avr505E: cp r5, r19 ; 505E 1653 brne avr5062 ; 505F F411 sbrc r16, 2 ; 5060 FD02 ori r17, 0x04 ; 5061 6014 avr5062: cp r6, r19 ; 5062 1663 brne avr5066 ; 5063 F411 sbrc r16, 3 ; 5064 FD03 ori r17, 0x08 ; 5065 6018 avr5066: lds r18, 0x0062 ; 5066 9120 0062 com r17 ; 5068 9510 and r17, r18 ; 5069 2312 sts 0x0062, r17 ; 506A 9310 0062 nop ; 506C 0000 nop ; 506D 0000 nop ; 506E 0000 nop ; 506F 0000 nop ; 5070 0000 nop ; 5071 0000 nop ; 5072 0000 nop ; 5073 0000 nop ; 5074 0000 nop ; 5075 0000 sbrs r16, 0 ; 5076 FF00 rjmp avr507B ; 5077 C003 sbic 0x00, 4 ; 5078 9904 mov r7, r19 ; 5079 2E73 rjmp avr507E ; 507A C003 avr507B: nop ; 507B 0000 nop ; 507C 0000 nop ; 507D 0000 avr507E: sbrs r16, 1 ; 507E FF01 rjmp avr5083 ; 507F C003 sbic 0x00, 5 ; 5080 9905 mov r8, r19 ; 5081 2E83 rjmp avr5086 ; 5082 C003 avr5083: nop ; 5083 0000 nop ; 5084 0000 nop ; 5085 0000 avr5086: sbrs r16, 2 ; 5086 FF02 rjmp avr508B ; 5087 C003 sbic 0x00, 6 ; 5088 9906 mov r9, r19 ; 5089 2E93 rjmp avr508E ; 508A C003 avr508B: nop ; 508B 0000 nop ; 508C 0000 nop ; 508D 0000 avr508E: sbrs r16, 3 ; 508E FF03 rjmp avr5093 ; 508F C003 sbic 0x00, 7 ; 5090 9907 mov r10, r19 ; 5091 2EA3 rjmp avr5096 ; 5092 C003 avr5093: nop ; 5093 0000 nop ; 5094 0000 nop ; 5095 0000 avr5096: cpi r19, 0xC8 ; 5096 3C38 breq avr509A ; 5097 F011 inc r19 ; 5098 9533 rjmp avr5055 ; 5099 CFBB avr509A: ldi r16, 0x90 ; 509A E900 sub r7, r16 ; 509B 1A70 sub r8, r16 ; 509C 1A80 sub r9, r16 ; 509D 1A90 sub r10, r16 ; 509E 1AA0 ldi r16, 0x38 ; 509F E308 ldi r19, 0x2C ; 50A0 E23C cp r16, r7 ; 50A1 1507 brne avr50A4 ; 50A2 F409 mov r7, r19 ; 50A3 2E73 avr50A4: cp r16, r8 ; 50A4 1508 brne avr50A7 ; 50A5 F409 mov r7, r19 ; 50A6 2E73 avr50A7: cp r16, r9 ; 50A7 1509 brne avr50AA ; 50A8 F409 mov r7, r19 ; 50A9 2E73 avr50AA: cp r16, r10 ; 50AA 150A brne avr50AD ; 50AB F409 mov r7, r19 ; 50AC 2E73 avr50AD: sts GYRO_VALUE_1, r7 ; 50AD 9270 0530 sts GYRO_VALUE_2, r8 ; 50AF 9280 0531 sts GYRO_VALUE_3, r9 ; 50B1 9290 0532 sts GYRO_VALUE_4, r10 ; 50B3 92A0 0533 ret ; 50B5 9508 ;------------------------------------------------------------------------- ; avr50B6: clr r17 ; KRG Gyro part 1 lds r16, GYRO_ENABLE andi r16, 0x0F sbrc r16, 0 ori r17, 0x01 sbrc r16, 1 ori r17, 0x02 sbrc r16, 2 ori r17, 0x04 sbrc r16, 3 ori r17, 0x08 ; 50C1 6018 sbrs r16, 0 ; 50C2 FF00 rjmp avr50C4 ; 50C3 C000 avr50C4: sbrs r16, 1 ; 50C4 FF01 rjmp avr50C6 ; 50C5 C000 avr50C6: sbrs r16, 2 ; 50C6 FF02 rjmp avr50C8 ; 50C7 C000 avr50C8: sbrs r16, 3 ; 50C8 FF03 rjmp avr50CA ; 50C9 C000 avr50CA: mov r18, r17 ; 50CA 2F21 swap r18 ; 50CB 9522 andi r18, 0xF0 ; 50CC 7F20 or r18, r17 ; 50CD 2B21 lds r16, 0x0062 ; 50CE 9100 0062 or r18, r16 ; 50D0 2B20 sts 0x0062, r18 ; 50D1 9320 0062 lds r16, 0x0061 ; 50D3 9100 0061 or r17, r16 ; 50D5 2B10 sts 0x0061, r17 ; 50D6 9310 0061 ret ; 50D8 9508 ;------------------------------------------------------------------------- avr50D9: lds r16, GYRO_ENABLE ; 50D9 9100 052F andi r16, 0x0F ; 50DB 700F ldi r19, 0x00 ; 50DC E030 ldi r18, 0x7F ; 50DD E72F mov r3, r18 ; 50DE 2E32 mov r4, r18 ; 50DF 2E42 mov r5, r18 ; 50E0 2E52 mov r6, r18 ; 50E1 2E62 clr r7 ; 50E2 2477 clr r8 ; 50E3 2488 clr r9 ; 50E4 2499 clr r10 ; 50E5 24AA avr50E6: ldi r17, 0x00 ; 50E6 E010 cp r3, r19 ; 50E7 1633 brne avr50EB ; 50E8 F411 sbrc r16, 0 ; 50E9 FD00 ori r17, 0x01 ; 50EA 6011 avr50EB: cp r4, r19 ; 50EB 1643 brne avr50EF ; 50EC F411 sbrc r16, 1 ; 50ED FD01 ori r17, 0x02 ; 50EE 6012 avr50EF: cp r5, r19 ; 50EF 1653 brne avr50F3 ; 50F0 F411 sbrc r16, 2 ; 50F1 FD02 ori r17, 0x04 ; 50F2 6014 avr50F3: cp r6, r19 ; 50F3 1663 brne avr50F7 ; 50F4 F411 sbrc r16, 3 ; 50F5 FD03 ori r17, 0x08 ; 50F6 6018 avr50F7: lds r18, 0x0062 ; 50F7 9120 0062 com r17 ; 50F9 9510 and r17, r18 ; 50FA 2312 sts 0x0062, r17 ; 50FB 9310 0062 nop ; 50FD 0000 nop ; 50FE 0000 nop ; 50FF 0000 nop ; 5100 0000 nop ; 5101 0000 nop ; 5102 0000 nop ; 5103 0000 nop ; 5104 0000 nop ; 5105 0000 nop ; 5106 0000 sbrs r16, 0 ; 5107 FF00 rjmp avr510C ; 5108 C003 sbic 0x00, 4 ; 5109 9904 mov r7, r19 ; 510A 2E73 rjmp avr510F ; 510B C003 avr510C: nop ; 510C 0000 nop ; 510D 0000 nop ; 510E 0000 avr510F: sbrs r16, 1 ; 510F FF01 rjmp avr5114 ; 5110 C003 sbic 0x00, 5 ; 5111 9905 mov r8, r19 ; 5112 2E83 rjmp avr5117 ; 5113 C003 avr5114: nop ; 5114 0000 nop ; 5115 0000 nop ; 5116 0000 avr5117: sbrs r16, 2 ; 5117 FF02 rjmp avr511C ; 5118 C003 sbic 0x00, 6 ; 5119 9906 mov r9, r19 ; 511A 2E93 rjmp avr511F ; 511B C003 avr511C: nop ; 511C 0000 nop ; 511D 0000 nop ; 511E 0000 avr511F: sbrs r16, 3 ; 511F FF03 rjmp avr5124 ; 5120 C003 sbic 0x00, 7 ; 5121 9907 mov r10, r19 ; 5122 2EA3 rjmp avr5127 ; 5123 C003 avr5124: nop ; 5124 0000 nop ; 5125 0000 nop ; 5126 0000 avr5127: cpi r19, 0xBE ; 5127 3B3E breq avr512B ; 5128 F011 inc r19 ; 5129 9533 rjmp avr50E6 ; 512A CFBB avr512B: ldi r19, 0x00 ; 512B E030 avr512C: ldi r17, 0x00 ; 512C E010 cp r3, r19 ; 512D 1633 brne avr5131 ; 512E F411 sbrc r16, 0 ; 512F FD00 ori r17, 0x01 ; 5130 6011 avr5131: cp r4, r19 ; 5131 1643 brne avr5135 ; 5132 F411 sbrc r16, 1 ; 5133 FD01 ori r17, 0x02 ; 5134 6012 avr5135: cp r5, r19 ; 5135 1653 brne avr5139 ; 5136 F411 sbrc r16, 2 ; 5137 FD02 ori r17, 0x04 ; 5138 6014 avr5139: cp r6, r19 ; 5139 1663 brne avr513D ; 513A F411 sbrc r16, 3 ; 513B FD03 ori r17, 0x08 ; 513C 6018 avr513D: lds r18, 0x0062 ; 513D 9120 0062 com r17 ; 513F 9510 and r17, r18 ; 5140 2312 nop ; 5141 0000 nop ; 5142 0000 nop ; 5143 0000 nop ; 5144 0000 nop ; 5145 0000 nop ; 5146 0000 nop ; 5147 0000 nop ; 5148 0000 nop ; 5149 0000 nop ; 514A 0000 push r16 ; 514B 930F ldi r16, 0x14 ; 514C E104 avr514D: dec r16 ; 514D 950A brne avr514D ; 514E F7F1 pop r16 ; 514F 910F sbrs r16, 0 ; 5150 FF00 rjmp avr5155 ; 5151 C003 sbic 0x00, 4 ; 5152 9904 mov r7, r19 ; 5153 2E73 rjmp avr5158 ; 5154 C003 avr5155: nop ; 5155 0000 nop ; 5156 0000 nop ; 5157 0000 avr5158: sbrs r16, 1 ; 5158 FF01 rjmp avr515D ; 5159 C003 sbic 0x00, 5 ; 515A 9905 mov r8, r19 ; 515B 2E83 rjmp avr5160 ; 515C C003 avr515D: nop ; 515D 0000 nop ; 515E 0000 nop ; 515F 0000 avr5160: sbrs r16, 2 ; 5160 FF02 rjmp avr5165 ; 5161 C003 sbic 0x00, 6 ; 5162 9906 mov r9, r19 ; 5163 2E93 rjmp avr5168 ; 5164 C003 avr5165: nop ; 5165 0000 nop ; 5166 0000 nop ; 5167 0000 avr5168: sbrs r16, 3 ; 5168 FF03 rjmp avr516D ; 5169 C003 sbic 0x00, 7 ; 516A 9907 mov r10, r19 ; 516B 2EA3 rjmp avr5170 ; 516C C003 avr516D: nop ; 516D 0000 nop ; 516E 0000 nop ; 516F 0000 avr5170: cpi r19, 0xA0 ; 5170 3A30 breq avr5174 ; 5171 F011 inc r19 ; 5172 9533 rjmp avr512C ; 5173 CFB8 avr5174: ldi r16, 0x01 ; 5174 E001 sub r7, r16 ; 5175 1A70 sub r8, r16 ; 5176 1A80 sub r9, r16 ; 5177 1A90 sub r10, r16 ; 5178 1AA0 ldi r16, 0x38 ; 5179 E308 ldi r19, 0x2C ; 517A E23C cp r16, r7 ; 517B 1507 brne avr517E ; 517C F409 mov r7, r19 ; 517D 2E73 avr517E: cp r16, r8 ; 517E 1508 brne avr5181 ; 517F F409 mov r7, r19 ; 5180 2E73 avr5181: cp r16, r9 ; 5181 1509 brne avr5184 ; 5182 F409 mov r7, r19 ; 5183 2E73 avr5184: cp r16, r10 ; 5184 150A brne avr5187 ; 5185 F409 mov r7, r19 ; 5186 2E73 avr5187: sts GYRO_VALUE_1, r7 ; 5187 9270 0530 sts GYRO_VALUE_2, r8 ; 5189 9280 0531 sts GYRO_VALUE_3, r9 ; 518B 9290 0532 sts GYRO_VALUE_4, r10 ; 518D 92A0 0533 ret ; 518F 9508 ; ;------------------------------------------------------------------------- ; Timer 0 Interrupt ; This is the motion timer interrupt ; TIMER0_OVF_vect: wdr ; Reset watchdog push r16 in r16, SREG ; save status register push r16 ldi r16, 0x19 ; send 0x19 to timer count 0 out 0x32, r16 ; reset the count to 4 millisec (counts up) lds r16, TIMER_0_INT ; Test Interupt already in progress flag sbrc r16, 7 ; if bit 7 set in 0x4f4 then skip this routine rjmp avr5214 ori r16, 0x80 ; set bit 7 in 0x4f4 Interrupt in progress sts TIMER_0_INT, r16 push r17 ; save registers push r18 push r19 push r20 push r21 push XL push XH push YL push YH push ZL push ZH push r0 rcall DELAY ; increment delay counter and clear delay busy if done rcall COUNT ; increment (4 ms timer) lds r16, ENABLE_G8A ; any G8A servos in use ? tst r16 brne avr51C4 lds r16, ENABLE_G8B ; any G8B servos in use ? tst r16 brne avr51C4 lds r16, ENABLE_G8C ; any G8C servos in use ? tst r16 brne avr51C4 lds r16, ENABLE_G8D ; any G8D servos in use ? tst r16 brne avr51C4 lds r16, GYRO_ENABLE ; any Gyros in use ? tst r16 brne avr51C4 rjmp avr51DD ; no motion actions to do avr51C4: ldi r17, 0x03 lds r16, GYRO_ENABLE ; if any Gyro enabled then enable phase 4 andi r16, 0x0F tst r16 breq avr51CD ldi r17, 0x04 avr51CD: lds r16, MOTION_PHASE ; get phase ( 1 to 4) and increment inc r16 ; next phase cp r16, r17 brlo avr51D3 clr r16 avr51D3: sts MOTION_PHASE, r16 cpi r16, 0x00 breq avr51DE cpi r16, 0x01 breq avr51E2 cpi r16, 0x02 breq avr51E6 cpi r16, 0x03 breq avr51EA avr51DD: rjmp avr51EF avr51DE: rcall sub5218 ; Phase 1 sets first 12 on rcall sub5474 ; minimum pulse delay rcall sub52E1 ; off after length rjmp avr51EF ; done avr51E2: rcall sub5258 ; Phase 2 sets second 12 on rcall sub5474 ; minimum pulse delay rcall sub5366 ; off after length rjmp avr51EF ; done avr51E6: rcall sub5298 ; Phase 3 sets last 8 on rcall sub5474 ; minimum pulse delay rcall sub53EA ; off after length rjmp avr51EF ; done avr51EA: call sub5011 ; Phase 4 Gyro start pulse out call sub501B ; Gyro get result values rjmp avr51EF ; done avr51EF: rcall sub54A6 ; set change angle rcall sub54B8 ; move angle if change lds r16, HIGH_SPEED ; Test high speed tst r16 breq avr51F9 rcall sub54A6 ; set change angle rcall sub54B8 ; move angle if change rcall sub54A6 ; set change angle rcall sub54B8 ; move angle if change avr51F9: rcall sub54D4 ; modify enables rcall sub5503 ; Gyro correction rcall sub54EA ; Direction rcall sub5487 ; do zero correction avr5203: pop r0 pop ZH pop ZL pop YH pop YL pop XH pop XL pop r21 pop r20 pop r19 pop r18 pop r17 lds r16, TIMER_0_INT ; Clear the interrupt in progress flag andi r16, 0x7F sts TIMER_0_INT, r16 avr5214: pop r16 out SREG, r16 pop r16 reti ;------------------------------------------------------------------------- ; first 12 servos Based on the bits in ENABLE_G sets the bits, and equalises the delay sub5218: lds r16, ENABLE_G8A ; sets port a 0 to 7 and port b 0 to 3 if enabled lds r17, ENABLE_G8B sbrc r16, 0 sbi PORTA, 0 nop sbrc r16, 1 sbi PORTA, 1 nop sbrc r16, 2 sbi PORTA, 2 nop sbrc r16, 3 sbi PORTA, 3 nop sbrc r16, 4 sbi PORTA, 4 nop sbrc r16, 5 sbi PORTA, 5 nop sbrc r16, 6 sbi PORTA, 6 nop sbrc r16, 7 sbi PORTA, 7 nop sbrc r17, 0 sbi PORTB, 0 nop sbrc r17, 1 sbi PORTB, 1 nop sbrc r17, 2 sbi PORTB, 2 nop sbrc r17, 3 sbi PORTB, 3 sbrs r16, 0 ; Next part probably equalises the time delay rjmp avr5241 ; avr5241: sbrs r16, 1 rjmp avr5243 avr5243: sbrs r16, 2 rjmp avr5245 avr5245: sbrs r16, 3 rjmp avr5247 avr5247: sbrs r16, 4 rjmp avr5249 avr5249: sbrs r16, 5 rjmp avr524B avr524B: sbrs r16, 6 rjmp avr524D avr524D: sbrs r16, 7 rjmp avr524F avr524F: sbrs r17, 0 rjmp avr5251 avr5251: sbrs r17, 1 rjmp avr5253 avr5253: sbrs r17, 2 rjmp avr5255 avr5255: sbrs r17, 3 rjmp avr5257 avr5257: ret ;------------------------------------------------------------------------- ;second 12 servos Based on the bits in ENABLE_G sets the bits, and equalises the delay sub5258: lds r16, ENABLE_G8B lds r17, ENABLE_G8C sbrc r16, 4 sbi PORTB, 4 nop sbrc r16, 5 sbi PORTB, 5 nop sbrc r16, 6 sbi PORTB, 6 nop sbrc r16, 7 sbi PORTB, 7 nop sbrc r17, 0 sbi PORTC, 7 nop sbrc r17, 1 sbi PORTC, 6 nop sbrc r17, 2 sbi PORTC, 5 nop sbrc r17, 3 sbi PORTC, 4 nop sbrc r17, 4 sbi PORTC, 3 nop sbrc r17, 5 sbi PORTC, 2 nop sbrc r17, 6 sbi PORTC, 1 nop sbrc r17, 7 sbi PORTC, 0 sbrs r16, 4 rjmp avr5281 avr5281: sbrs r16, 5 rjmp avr5283 avr5283: sbrs r16, 6 rjmp avr5285 avr5285: sbrs r16, 7 rjmp avr5287 avr5287: sbrs r17, 0 rjmp avr5289 avr5289: sbrs r17, 1 rjmp avr528B avr528B: sbrs r17, 2 rjmp avr528D avr528D: sbrs r17, 3 rjmp avr528F avr528F: sbrs r17, 4 rjmp avr5291 avr5291: sbrs r17, 5 rjmp avr5293 avr5293: sbrs r17, 6 rjmp avr5295 avr5295: sbrs r17, 7 rjmp avr5297 avr5297: ret ;------------------------------------------------------------------------- ; last 8 servos Based on the bits in ENABLE_G sets the bits, and equalises the delay sub5298: lds r16, ENABLE_G8D sbrc r16, 0 sbi 0x03, 7 nop sbrc r16, 1 sbi 0x03, 6 nop sbrc r16, 2 sbi PORTD, 7 nop sbrc r16, 3 sbi PORTD, 6 nop sbrc r16, 4 sbi PORTD, 5 nop sbrs r16, 5 rjmp avr52B0 lds r18, 0x0065 ori r18, 0x04 sts 0x0065, r18 avr52B0: sbrs r16, 6 rjmp avr52B7 lds r18, 0x0065 ori r18, 0x02 sts 0x0065, r18 avr52B7: sbrs r16, 7 rjmp avr52BE lds r18, 0x0065 ori r18, 0x01 sts 0x0065, r18 avr52BE: sbrc r17, 0 nop nop sbrc r17, 1 nop nop sbrc r17, 2 nop nop sbrc r17, 3 sbrs r16, 0 rjmp avr52CA avr52CA: sbrs r16, 1 rjmp avr52CC avr52CC: sbrs r16, 2 rjmp avr52CE avr52CE: sbrs r16, 3 rjmp avr52D0 avr52D0: sbrs r16, 4 rjmp avr52D2 avr52D2: sbrs r16, 5 rjmp avr52D4 avr52D4: sbrs r16, 6 rjmp avr52D6 avr52D6: sbrs r16, 7 rjmp avr52D8 avr52D8: sbrs r17, 0 rjmp avr52DA avr52DA: sbrs r17, 1 rjmp avr52DC avr52DC: sbrs r17, 2 rjmp avr52DE avr52DE: sbrs r17, 3 rjmp avr52E0 avr52E0: ret ;------------------------------------------------------------------------- ; first 12 servos output pulse length sub52E1: lds r3, SERVO_ACT lds r4, SERVO_ACT+1 lds r5, SERVO_ACT+2 lds r6, SERVO_ACT+3 lds r7, SERVO_ACT+4 lds r8, SERVO_ACT+5 lds r9, SERVO_ACT+6 lds r10, SERVO_ACT+7 lds r11, SERVO_ACT+8 lds r12, SERVO_ACT+9 lds r13, SERVO_ACT+10 lds r14, SERVO_ACT+11 ldi r16, 0x00 lds r17, ENABLE_G8A lds r18, ENABLE_G8B nop nop nop nop clr XL avr5303: cp r3, r16 brne avr5307 sbrc r17, 0 cbi PORTA, 0 avr5307: cp r4, r16 brne avr530B sbrc r17, 1 cbi PORTA, 1 avr530B: cp r5, r16 brne avr530F sbrc r17, 2 cbi PORTA, 2 avr530F: cp r6, r16 brne avr5313 sbrc r17, 3 cbi PORTA, 3 avr5313: cp r7, r16 brne avr5317 sbrc r17, 4 cbi PORTA, 4 avr5317: cp r8, r16 brne avr531B sbrc r17, 5 cbi PORTA, 5 avr531B: cp r9, r16 brne avr531F sbrc r17, 6 cbi PORTA, 6 avr531F: cp r10, r16 brne avr5323 sbrc r17, 7 cbi PORTA, 7 avr5323: cp r11, r16 brne avr5327 sbrc r18, 0 cbi PORTB, 0 avr5327: cp r12, r16 brne avr532B sbrc r18, 1 cbi PORTB, 1 avr532B: cp r13, r16 brne avr532F sbrc r18, 2 cbi PORTB, 2 avr532F: cp r14, r16 brne avr5333 sbrc r18, 3 cbi PORTB, 3 avr5333: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop sbrs r19, 6 rjmp avr5353 sbis 0x0B, 5 rjmp avr535A nop cpi XL, 0x04 breq avr535F inc XL nop rjmp avr5361 avr5353: nop nop nop nop nop nop rjmp avr5361 avr535A: nop nop nop nop rjmp avr5361 avr535F: nop rjmp avr5361 avr5361: inc r16 cpi r16, 0xCD breq avr5365 rjmp avr5303 avr5365: ret ;------------------------------------------------------------------------- ; second 12 servos output pulse length sub5366: lds r3, SERVO_ACT+12 lds r4, SERVO_ACT+13 lds r5, SERVO_ACT+14 lds r6, SERVO_ACT+15 lds r7, SERVO_ACT+16 lds r8, SERVO_ACT+17 lds r9, SERVO_ACT+18 lds r10, SERVO_ACT+19 lds r11, SERVO_ACT+20 lds r12, SERVO_ACT+21 lds r13, SERVO_ACT+22 lds r14, SERVO_ACT+23 ldi r16, 0x00 lds r17, ENABLE_G8B lds r18, ENABLE_G8C nop nop nop nop clr XL avr5388: cp r3, r16 brne avr538C sbrc r17, 4 cbi PORTB, 4 avr538C: cp r4, r16 brne avr5390 sbrc r17, 5 cbi PORTB, 5 avr5390: cp r5, r16 brne avr5394 sbrc r17, 6 cbi PORTB, 6 avr5394: cp r6, r16 brne avr5398 sbrc r17, 7 cbi PORTB, 7 avr5398: cp r7, r16 brne avr539C sbrc r18, 0 cbi PORTC, 7 avr539C: cp r8, r16 brne avr53A0 sbrc r18, 1 cbi PORTC, 6 avr53A0: cp r9, r16 brne avr53A4 sbrc r18, 2 cbi PORTC, 5 avr53A4: cp r10, r16 brne avr53A8 sbrc r18, 3 cbi PORTC, 4 avr53A8: cp r11, r16 brne avr53AC sbrc r18, 4 cbi PORTC, 3 avr53AC: cp r12, r16 brne avr53B0 sbrc r18, 5 cbi PORTC, 2 avr53B0: cp r13, r16 brne avr53B4 sbrc r18, 6 cbi PORTC, 1 avr53B4: cp r14, r16 brne avr53B8 sbrc r18, 7 cbi PORTC, 0 avr53B8: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop sbrs r19, 6 rjmp avr53D7 sbis 0x0B, 5 rjmp avr53DE nop cpi XL, 0x04 breq avr53E3 inc XL nop rjmp avr53E5 avr53D7: nop nop nop nop nop nop rjmp avr53E5 avr53DE: nop nop nop nop rjmp avr53E5 avr53E3: nop rjmp avr53E5 avr53E5: inc r16 cpi r16, 0xCD breq avr53E9 rjmp avr5388 avr53E9: ret ;------------------------------------------------------------------------- ; last 8 servos output pulse length sub53EA: lds r3, SERVO_ACT+24 lds r4, SERVO_ACT+25 lds r5, SERVO_ACT+26 lds r6, SERVO_ACT+27 lds r7, SERVO_ACT+28 lds r8, SERVO_ACT+29 lds r9, SERVO_ACT+30 lds r10, SERVO_ACT+31 ldi r16, 0x00 lds r17, ENABLE_G8D nop nop nop clr XL avr5402: cp r3, r16 brne avr5406 sbrc r17, 0 cbi 0x03, 7 avr5406: cp r4, r16 brne avr540A sbrc r17, 1 cbi 0x03, 6 avr540A: cp r5, r16 brne avr540E sbrc r17, 2 cbi PORTD, 7 avr540E: cp r6, r16 brne avr5412 sbrc r17, 3 cbi PORTD, 6 avr5412: cp r7, r16 brne avr5416 sbrc r17, 4 cbi PORTD, 5 avr5416: cp r8, r16 brne avr541F sbrs r17, 5 rjmp avr541F lds r18, 0x0065 andi r18, 0xFB sts 0x0065, r18 avr541F: cp r9, r16 brne avr5428 sbrs r17, 6 rjmp avr5428 lds r18, 0x0065 andi r18, 0xFD sts 0x0065, r18 avr5428: cp r10, r16 brne avr5431 sbrs r17, 7 rjmp avr5431 lds r18, 0x0065 andi r18, 0xFE sts 0x0065, r18 avr5431: cp r11, r16 brne avr5435 sbrc r17, 0 nop avr5435: cp r12, r16 brne avr5439 sbrc r17, 1 nop avr5439: cp r13, r16 brne avr543D sbrc r17, 2 nop avr543D: cp r14, r16 brne avr5441 sbrc r17, 3 nop avr5441: nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop nop sbrs r19, 6 rjmp avr5461 sbis 0x0B, 5 rjmp avr5468 nop cpi XL, 0x04 breq avr546D inc XL nop rjmp avr546F avr5461: nop nop nop nop nop nop rjmp avr546F avr5468: nop nop nop nop rjmp avr546F avr546D: nop rjmp avr546F avr546F: inc r16 cpi r16, 0xCD breq avr5473 rjmp avr5402 avr5473: ret ;------------------------------------------------------------------------- ; 500 microsec delay sub5474: ldi r16, 0xB0 ; Delay 500 microsec this is the minimum pulse length nop nop nop nop nop nop nop nop nop nop avr547F: nop nop ldi r17, 0x05 avr5482: dec r17 brne avr5482 dec r16 brne avr547F ret ;------------------------------------------------------------------------- ; Do zero correction sub5487: ldi XH, hi8(SERVO_CURRENT) ; X = current ldi XL, lo8(SERVO_CURRENT) ldi YH, hi8(SERVO_OFFSET) ; Y = zero offset data ldi YL, lo8(SERVO_OFFSET) ldi ZH, hi8(SERVO_CALC) ; z = zero corrected ldi ZL, lo8(SERVO_CALC) ldi r18, 0x20 ; for 32 servo avr548E: ld r16, Y+ ; get zero data sbrs r16, 7 ;top bit set ? rjmp avr549A ld r17, X+ ; get present add offset add r16, r17 brlo avr5496 ldi r16, 0x01 rjmp avr54A2 avr5496: cpi r16, 0x01 brsh avr54A2 ldi r16, 0x01 rjmp avr54A2 avr549A: ld r17, X+ ; get present add offset add r16, r17 brsh avr549F ldi r16, 0xC7 rjmp avr54A2 avr549F: cpi r16, 0xC7 brlo avr54A2 ldi r16, 0xC7 avr54A2: st Z+, r16 dec r18 brne avr548E ret ;------------------------------------------------------------------------- ; Set change angle sub54A6: ldi XH, hi8(SERVO_SPEED) ; speed counters ldi XL, lo8(SERVO_SPEED) ldi YH, hi8(SERVO_DELTA) ; drift speed ldi YL, lo8(SERVO_DELTA) ldi ZH, hi8(SERVO_TRIGGER) ; angle modify trigger ldi ZL, lo8(SERVO_TRIGGER) ldi r19, 0x20 ; for 32 servos avr54AD: ldi r18, 0x00 ld r16, Y+ ; get the drift speed ld r17, X ; get the speed counter add r17, r16 ; add st X+, r17 ; save in speed counter brsh avr54B4 ser r18 avr54B4: st Z+, r18 ; angle modify set if non zero dec r19 brne avr54AD ret ;------------------------------------------------------------------------- ; change angle if modify ; if modify set then add or subtract one sub54B8: ldi XH, hi8(SERVO_CURRENT) ; X = present angle ldi XL, lo8(SERVO_CURRENT) ldi YH, hi8(SERVO_DESIRED) ; Y = desired angle ldi YL, lo8(SERVO_DESIRED) ldi ZH, hi8(SERVO_TRIGGER) ; Z = modify trigger ldi ZL, lo8(SERVO_TRIGGER) ldi r18, 0x20 ; for all servos avr54BF: ld r16, Z ; modify set ? tst r16 breq avr54CE ld r16, X ; get present ld r17, Y ; get dest cp r17, r16 brlo avr54CC cp r16, r17 brlo avr54C9 rjmp avr54CE avr54C9: inc r16 ; if lower add one st X, r16 rjmp avr54CE avr54CC: dec r16 ; if higher lose one st X, r16 avr54CE: adiw XL, 0x01 ; else next adiw YL, 0x01 adiw ZL, 0x01 dec r18 brne avr54BF ret ;------------------------------------------------------------------------- ; set motion complete sub54D4:ldi XH, hi8(SERVO_DESIRED) ; X = desired ldi XL, lo8(SERVO_DESIRED) ldi YH, hi8(SERVO_CURRENT) ; Y = current ldi YL, lo8(SERVO_CURRENT) ldi ZH, hi8(MOTION_G8A) ; Z = motion ldi ZL, lo8(MOTION_G8A) ldi r21, 0x04 ; for 4 bytes avr54DB:ld r16, Z ; get motion ldi r17, 0x01 ldi r20, 0x08 avr54DE:ld r18, X+ ld r19, Y+ cp r18, r19 ; if equal set motion complete brne avr54E3 or r16, r17 avr54E3:lsl r17 dec r20 brne avr54DE st Z+, r16 dec r21 brne avr54DB ret ;------------------------------------------------------------------------- ; make the actual move by adding direction sub54EA: push XH push XL ldi XH, hi8(SERVO_CALC) ; X = after zero and gyro ldi XL, lo8(SERVO_CALC) ldi YH, hi8(SERVO_ACT) ; y = after direction ldi YL, lo8(SERVO_ACT) ldi ZH, hi8(DIR_G8A) ; Z = direction ldi ZL, lo8(DIR_G8A) ldi r20, 0x04 avr54F3: ldi r19, 0x08 ld r16, Z+ ; get direction avr54F5: ld r17, X+ ror r16 ; if direction set then position = 200- value brlo avr54FB ldi r18, 0xC8 ; r18 = 200 sub r18, r17 mov r17, r18 avr54FB: st Y+, r17 ; save in after direction dec r19 brne avr54F5 dec r20 brne avr54F3 pop XH pop XL ret ;------------------------------------------------------------------------- ; Any Gyro enabled ? sub5503: lds r16, GYRO_ENABLE andi r16, 0x0F tst r16 brne avr5509 ret ;------------------------------------------------------------------------- avr5509: push XH ; Add Gyro push XL ldi r16, 0x00 ldi ZH, hi8(SERVO_CALC) ; after gyro pos ldi ZL, lo8(SERVO_CALC) ldi YH, hi8(GYRO_SETTING) ; Gyro set ldi YL, lo8(GYRO_SETTING) ldi XH, hi8(GYRO_DIR) ; Gyro direction ldi XL, lo8(GYRO_DIR) avr5512: push r16 ld r16, Y ; get set cpi r16, 0x01 breq avr551D cpi r16, 0x02 breq avr5520 cpi r16, 0x03 breq avr5523 cpi r16, 0x04 breq avr5526 rjmp avr5573 avr551D: lds r17, GYRO_VALUE_1 rjmp avr5529 avr5520: lds r17, GYRO_VALUE_2 rjmp avr5529 avr5523: lds r17, GYRO_VALUE_3 rjmp avr5529 avr5526: lds r17, GYRO_VALUE_4 rjmp avr5529 avr5529: ldi r18, 0x38 ; r17 = value of Gyro chosen for this servo cp r18, r17 brsh avr552D ldi r17, 0x38 avr552D: ldi r18, 0x04 ; limit to max of 56 and min of 4 cp r17, r18 brsh avr5531 ldi r17, 0x04 avr5531: ld r16, X ; get direction and if reverse then r17 = 58 - value tst r16 brne avr5537 ldi r16, 0x58 sub r16, r17 mov r17, r16 avr5537: mov r12, XH ; save X and Y mov r13, XL mov r14, YH mov r15, YL ldi XH, hi8(GYRO_VALUE) ldi XL, lo8(GYRO_VALUE) ldi YH, hi8(GYRO_SENSE) ; Gyro sensitivity ldi YL, lo8(GYRO_SENSE) pop r16 ; r16 = servo number ldi r18, 0x00 ; r18 = 0 add XL, r16 adc YH, r18 ; add servo number to X and Y add YL, r16 adc YH, r18 push r16 ld r16, X ; get current Gyro value for servo ldi r18, 0x01 add r16, r18 cp r16, r17 ; add one and compare to read value brsh avr5555 ld r16, Y ; if bigger then add GYRO_CALC ld r17, X lds r18, GYRO_CALC add r18, r16 sts GYRO_CALC, r18 brsh avr5554 inc r17 avr5554: rjmp avr5565 avr5555: ld r16, X ldi r18, 0x01 sub r16, r18 cp r17, r16 brsh avr5564 ld r16, Y ld r17, X lds r18, GYRO_CALC add r18, r16 sts GYRO_CALC, r18 brsh avr5563 dec r17 avr5563: rjmp avr5565 avr5564: rjmp avr5566 avr5565: st X, r17 ; store as new value avr5566: mov XH, r12 ;recover X and Y mov XL, r13 mov YH, r14 mov YL, r15 ldi r16, 0x00 ldi r18, 0x00 ld r19, Z add r17, r19 adc r16, r18 ldi r18, 0x00 ldi r19, 0x2C sbc r17, r19 st Z, r17 avr5573: adiw XL, 0x01 adiw YL, 0x01 adiw ZL, 0x01 pop r16 inc r16 cpi r16, 0x20 breq avr557B rjmp avr5512 avr557B: pop XL pop XH ret ;------------------------------------------------------------------------- ; global function gyro_set gyro_set: ldi XH, hi8(GYRO_SETTING) ldi XL, lo8(GYRO_SETTING) push YH push YL mov YH, r21 mov YL, r20 rcall XPLUSR24 avr23F1: ld r18, Y+ st X+, r18 dec r22 brne avr23F1 pop YL pop YH ldi r22, 0x20 ; for all locations ldi XH, hi8(GYRO_SETTING) ldi XL, lo8(GYRO_SETTING) clr r20 sts GYRO_ENABLE, r20 ; clear GYRO_ENABLE avr2401: ld r18, X ; set GYRO enable and type mov r19, r18 subi r19, 0x0A brsh avr2409 ldi r19, 0x00 subi r18, 0x00 rjmp avr240F avr2409: subi r19, 0x0A brsh avr240E ldi r20, 0x10 subi r18, 0x0A rjmp avr240F avr240E: ldi r20, 0x00 avr240F: st X+, r18 cpi r18, 0x01 brne avr2413 ori r20, 0x01 avr2413: cpi r18, 0x02 brne avr2416 ori r20, 0x02 avr2416: cpi r18, 0x03 brne avr2419 ori r20, 0x04