; control servo connected to PortC1 via serial connection
; Timer1 in CTC Mode - Timer1 counts from 0 to OCR1A
; OCR1A is initialized to produce a periode of 20 ms
; servo pwm on OCR1B match
;
; Achim Walther, www.voidpointer.de, 23.11.2004

.include "m8def.inc"		; definitions for Mega8

.def temp = R16
.def flag = R17
.def mult = R18

.equ ram = 0x60
.equ cycle = 20000		; 20000us. Servo operates from 12 to 40 ms but the cycle 
				; time has influence on the servo position
.equ smid = 1500		; servo neutral 1500 us
.equ smin =  700		; servo min
.equ smax = 2500		; servo max

.equ CLOCK = 8000000
.equ BAUD = 9600
.equ UBRRVAL = CLOCK/(BAUD*16)-1

intvect:
	rjmp init		; RESET	
	reti			; INT0
	reti			; INT1
	reti			; T2 Compare
	reti			; T2 Overflow
	reti			; T1 Capture
	rjmp intoca		; T1 Compare A
	rjmp intocb		; T1 Compare B
	reti			; T1 Overflow
	reti			; T0 Overflow
	reti			; SPI Transfer complete
	rjmp int_rxc		; UART RX Complete
	reti			; UDR Empty
	reti			; UART TX Complete
	reti			; ADC complete
	reti			; EEPROM Ready
	reti			; Analog Comparator
	reti			; TWM
	reti			; SPM_RDY

init:	
	; init stack pointer
	ldi temp, LOW(RAMEND)
	out SPL, temp
	ldi temp, HIGH(RAMEND)
	out SPH, temp

	; configure Port C to ouput
	ldi temp, 0xFF
	out DDRC, temp

	; init X pointer to a free SRAM position
	ldi XL, ram

	; init Timer0 and compare values
	ldi R24, LOW(cycle)	; set OCR1A to cycle
	ldi R25, HIGH(cycle)
	out OCR1AH, R25
	out OCR1AL, R24

	ldi R24, LOW(smid)	; init OCR1B with start value
	ldi R25, HIGH(smid)
	out OCR1BH, R25
	out OCR1BL, R24

	in temp, TIMSK
	ori temp, (1<<OCIE1A) | (1<<OCIE1B)	; enable interrupt on OCR1B and OCR1A
	out TIMSK, temp

	;;ldi temp, 0b00001001	; no Prescaler
	ldi temp, 0b00001010	; Timer Prescaler 8
				; WGM13:2 = 01 -> CTC Mode
	out TCCR1B, temp

	; init UART
	ldi temp, LOW(UBRRVAL)	; set baud rate
	out UBRRL, temp
	ldi temp, HIGH(UBRRVAL)
	out UBRRH, temp


	ldi temp, (1<<URSEL)|(3<<UCSZ0)		; Frame-Format: 8 Bit
	out UCSRC, temp

	sbi UCSRB, RXCIE	; Interrupt on receive
	sbi UCSRB, RXEN		; activate RX (receive)

	sei

loop:	
	; main loop
	; check if a complete message was received via UART and compute it

	cpi flag, '*'		; check flag
	brne loop

	clr flag		; clear the flag register
	clr ZL			; set Z to 0
	clr ZH

	; a complete message was stored in memory
	; X is pointing to the end of the message

	; we expect a message in a format nnnn* 
	; where nnnn defines the servo pulse width in microsec decimal

	; start reading from the last character
	dec XL			; the '*' character is not needed
	ld temp, -X		; get next char
	subi temp, '0'		; subtract the ascii value of '0' to get the decimal value
	mov ZL, temp

	ld temp, -X
	subi temp, '0'		; subtract the ascii value of '0' to get the decimal value
	ldi mult, 10
	mul temp, mult		; R1:R0 = 10 * value
	add ZL, R0		; add the result to Z

	ld temp, -X
	subi temp, '0'		; subtract the ascii value of '0' to get the decimal value
	ldi mult, 100
	mul temp, mult		; R1:R0 = 100 * value
	add ZL, R0
	adc ZH, R1		; add the result to Z

	ldi R24, LOW(1000)	; prepare R24:R25 for multiplication
	ldi R25, HIGH(1000)
	ld temp, -X
	subi temp, '0'		; subtract the ascii value of '0' to get the decimal value
mltl:	breq mlte		; if temp == 0 -> end of multiplication
	add ZL, R24
	adc ZH, R25		; add 1000 to Z
	dec temp		; this sets the Z bit if temp == 0. The bit is tested at label mltl
	rjmp mltl		; and try once again

mlte:	
	; we decoded the nnnn* string. the Z register contains the result
	out OCR1BH, ZH		; set OCR1B with the content of Z
	out OCR1BL, ZL

	ldi XL, ram		; reset X

	rjmp loop		; endless loop

; Output Compare A Interrupt Rountine
intoca:	
	sbi PORTC, 1		; Servo High
	reti

; Output Compare B Interrupt Rountine
intocb:	
	cbi PORTC, 1		; Servo Low
	reti

; Interrupt handler for UART receive
int_rxc:
	push temp

	in temp, UDR
	st X+, temp		; store character to memory
	cpi temp, '*'		; compare with '*'
	brne int_e
	mov flag, temp		; set complete receive flag

int_e:	pop temp
	reti