; control up to 5 servos connected to PortC(1-5) 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
; ZL is used as a servo counter
;
; Achim Walther, www.voidpointer.de, 23.11.2004

.include "m8def.inc"		; definitions for Mega8

.def regs = R15
.def temp = R16
.def flag = R17
.def mult = R18
.def patn = R19			; contains the bit pattern to trigger the servo lines

.equ svno = 5			; max number of servos
.equ zmax = svno * 2 + 2	; max value ZL can reach when accessing the servo table

.equ srvtab = 0x100		; the table where servo timings are saved. 2 Bytes for each servo
				; this value has to be a multiple of 0x100 because the servo table
				; is accessed using the Z register
				; servo times are stored HIGH(Time):LOW(Time)
.equ ram = srvtab + zmax	; temporary space for keeping UART commands
.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, LOW(ram)
	ldi XH, HIGH(ram)

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

	; init servo table with servo neutral position
	ldi ZL, LOW(srvtab)	; init Z with the start address of the servo table
	ldi ZH, HIGH (srvtab)
istab:	ldi temp, HIGH(smid)
	st Z+, temp
	ldi temp, LOW(smid)
	st Z+, temp
	cpi ZL, zmax		; max number of servos?
	brne istab

	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 servo table pointer and servo signals
	ldi ZL, LOW(srvtab)	; init Z with the start address of the servo table
	ldi ZH, HIGH (srvtab)

	; 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
	; to prevent concurrent access to the servo table the following code
	; is executed only after all servos have been served

	cpi ZL, zmax		; check if the servo counter contains the last servo number
	brne loop		; if not continue waiting

	; all servos have been triggered 
	cpi flag, '*'		; check flag if a new message was received
	brne loop

	cli			; disable interrupts
	clr flag		; clear the flag register
	clr R22			; set R22:R23 to 0
	clr R23

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

	; expected message format is "x:nnnn*"
	; where x is servo number and 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 R22, 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 R22, R0		; add the result to R22:R23

	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 R22, R0
	adc R23, R1		; add the result to R22:R23

	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 R22, R24
	adc R23, R25		; add 1000 to R22:R23
	dec temp		; this sets the Z bit if temp == 0. The bit is tested at label mltl
	rjmp mltl		; and try once again

mlte:	
	; the nnnn* string is now decoded. the R22:R23 register contains the result
	; now read the servo number and store the value to the memory

	dec XL			; throw away the ':' character
	ld temp, -X		; get next char
	subi temp, '0'		; subtract the ascii value of '0' to get the decimal value
	lsl temp		; multiply by 2 to be able to address the servo table
	ldi YL, LOW(srvtab)	; load Y with start address of servo table
	ldi YH, HIGH(srvtab)
	add YL, temp		; now Y contains the target address
	st Y+, R23		; store HIGH(servotime) to the address in Y
	st Y, R22		; store LOW(servotime) to the address in Y+1

	ldi XL, LOW(ram)	; reset X
	ldi XH, HIGH(ram)

	sei			; enable interrupts again

	rjmp loop		; endless loop

; Output Compare A Interrupt Rountine
intoca:
	in regs, SREG
	push temp

	; init servo table pointer and servo signals
	ldi patn, 1		; init servo pattern to trigger servo connected to PORTC0
	out PORTC, patn		; set current servo line High

	clr ZL			; init ZL with the start address of the servo table (0x00)

	; set OCR1B to the first value by accessing the first address of the servo table
	ld temp, Z+		; load HIGH(servotime)
	out OCR1BH, temp	; store to HIGH(OCR1B)
	ld temp, Z+		; same with LOW(servotime)
	out OCR1BL, temp

	pop temp
	out SREG, regs
	reti

; Output Compare B Interrupt Rountine
intocb:
	in regs, SREG

	lsl patn		; left shift servo pattern
	out PORTC, patn		; set current servo line Low and next line High

	ld R23, Z+		; load HIGH(servotime)
	ld R22, Z+		; load LOW(servotime)

	cpi ZL, zmax		; max number of servos?
	breq eintb		; -> all servos have been triggered

	; set OCR1B to the next value by adding the servo time to the current value
	in R20, OCR1BL		; load current value to R20:R21
	in R21, OCR1BH

	add R20, R22		; add servo time stored in R22:R23
	adc R21, R23
	out OCR1BH, R21		; set OCR1B with the new content of R20:R21
	out OCR1BL, R20

eintb:	out SREG, regs
	reti

; Interrupt handler for UART receive
int_rxc:
	in regs, SREG
	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
	out SREG, regs
	reti