; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		; SIMPLE SERVODRIVER 16x - FIRMWARE
		; Matěj Kubička, 2007, BSD Licence
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
			.include "m8def.inc"

			.equ fCPU = 16000000
			.equ PeriodTicksConst = (fCPU / 100000)

			.equ SramOffset = 0x044E

			.def NumCycles = r18
			.def ActServo = r19
			.def SregTemp = r20
			.def intAcc = r21
			
			.org 0x000	rjmp RESET          ; Reset handler
			.org 0x006	rjmp TIM1_CTC       ; CTC timer interrupt
			.org 0x00B	rjmp RSRXC          ; RS232
		
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		; Initialization of program
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-	
		RESET:
			; Nastavení zásobníku (SRAM)
			ldi r16, LOW(SramOffset - 1)								
			out SPL, r16
			ldi r16, HIGH(SramOffset - 1)
			out SPH, r16

			; Initialize timer, CTC mode
			ldi r16, (1 << WGM12) | (1 << CS10)
			out TCCR1B, r16
			ldi r16, (1 << OCIE1A)
			out TIMSK, r16

			; 10us timer period
			ldi r17,HIGH(PeriodTicksConst)
			ldi r16,LOW(PeriodTicksConst)
			out OCR1AH,r17
			out OCR1AL,r16

			; Initialization of RS232 at bitrate 4800 Bd
			ldi r17, 0
			ldi r16, 207
			out UBRRH, r17
			out UBRRL, r16

			; Enable RX and TX
			ldi r16, (1<<RXCIE)|(1<<RXEN)|(1<<TXEN)
			out UCSRB, r16

			; Parameters of RS232 - 2stopbit, 8databit
			ldi r16, (1<<URSEL)|(1<<USBS)|(3<<UCSZ0)
			out UCSRC, r16

			; Set portb as output
			ser r16
			out ddrb, r16

			; clear fields with motor speeds
			ldi YL, LOW(SramOffset)
			ldi YH, HIGH(SramOffset)
			ldi r16, 150
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16
			st Y+, r16

			; Set again Y pointer to servo 0
			ldi YL, LOW(SramOffset)
			ldi YH, HIGH(SramOffset)

			; Set interrupts enable
			sei

		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		; Main program
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		PROGRAM:
;			Generate modulation
			ld r16, Y
			cp NumCycles, r16
			brsh higher
			cbi portb, pb1
			rjmp PROGRAM
		higher:
			sbi portb, pb1			
			rjmp PROGRAM

		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		; Timer in CTC mode with 10us period
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		TIM1_CTC:
;			Save Global Interrupt Flag
			in SregTemp,SREG	
			cli		
;			Increment timer (time[us] = NumCycles*10)							
			inc NumCycles
;			If single servo period wasn't been finished, jump to the end		
			cpi NumCycles, 200					
			brlo TimEnd
;			Step to the next servo
			inc ActServo
;			Actualize port status (for DeMX)			
			mov intAcc, ActServo
			rol intAcc
			rol intAcc
			out PORTB, r21
;			Increment lower part of 16bit pointer Y
			inc YL
;			Clear NumCycles counter
			clr NumCycles
;			If period wasn't been finished, jump to the end
			cpi ActServo, 16
			brlo TimEnd
;			ActServo = 0 and set Y pointer to servo 0
			clr ActServo
			ldi YL, LOW(SramOffset)
			ldi YH, HIGH(SramOffset)			
		TimEnd:
			out SREG,SregTemp						
			reti

		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		; Receive service for data from USART
		; =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
		RSRXC:
;			Save Global Interrupt Flag
			in SregTemp,SREG
			cli

;			Receive data
			sbis UCSRA, RXC							
			rjmp RSRXC								
			in intAcc, UDR								

;			Is that inicialization of new packet? 
			cpi intAcc, 0xFF
			brne SetServoSpeed
;		Prepares to receive new data packet
		IsNewCommand:
			ldi XL, LOW(SramOffset)
			ldi XH, HIGH(SramOffset)
			rjmp RsEnd
;		Receiving part of data packet
		SetServoSpeed:
;			Test to overflow
			cpi YL, LOW(RAMEND)
			breq RsEnd
;			Store received byte
			st X+, intAcc
		RsEnd:
			out SREG,SregTemp	
			reti