; Servo-Ansteuerung
; Timer1 im CTC Mode - Timer1 zählt von 0 bis OCR1A
; OCR1A wird so festgelegt, dass die Periode 20 ms entsteht
; durch Teilung von 8MHz durch 8 ergibt sich eine Timer Clock von 1MHz oder 1us
; Interrupt durch Match von OCR1B
;
; Achim Walther, www.voidpointer.de, 22.11.2004

.include "m8def.inc"		; Definitionsdatei einbinden

.def temp = R16
.equ cycle = 20000		; 20000us
.equ smid = 1500		; Mittelstellung des Servos 1500 us
.equ smin =  700		; Minimalstellung
.equ smax = 2300		; Maximalstellung

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
	reti			; 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:	
	ldi temp, LOW(RAMEND)
	out SPL, temp
	ldi temp, HIGH(RAMEND)
	out SPH, temp		; init stack pointer

	ldi temp, 0xFF
	out DDRC, temp		; Port C als Ausgang konfigurieren

	ldi temp, 0x00
	out DDRD, temp		; Port D als Eingang konfigurieren

	ldi temp, 0xFF
	out PORTC, temp		; PORTC auf 0xFF setzen -> LED aus

	ldi R24, LOW(cycle)	; OCR1A auf Periode setzen
	ldi R25, HIGH(cycle)
	out OCR1AH, R25
	out OCR1AL, R24	

	ldi R24, LOW(smid)	; OCR1B mit Startwert initialisieren
	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, 0b00000000	; WGM11:0 = 0 -> CTC Mode, Top = OCR1A
	;;out TCCR1A, temp

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

	sei

up:
	sbic PIND, 7		; Taste 1 gedrückt? -> Phase verlängern
	rjmp down
	in R24, OCR1BL		; Aktuelle Einstellung von Output Compare B laden
	in R25, OCR1BH

	ldi temp, HIGH(smax)	; Mit Max vergleichen
	cp temp, R25		; Vergleich der High Bytes 
	brne upinc		; High-Byte ungleich -> Increment
	ldi temp, LOW(smax)	; High-Byte gleich -> Vergleich von Low-Byte
	cp temp, R24
	breq down		; Obergrenze erreicht - Abbruch

upinc:	adiw R24, 1		; um 1 erhöhen
	out OCR1BH, R25		; Ergebnis wieder nach Output Compare B schreiben
	out OCR1BL, R24
	rcall delay

down:
	sbic PIND, 6		; Taste 2 gedrückt? -> Phase verkürzen
	rjmp loop
	in R24, OCR1BL		; Aktuelle Einstellung von Output Compare B laden
	in R25, OCR1BH

	ldi temp, HIGH(smin)	; Mit Min vergleichen
	cp temp, R25		; Vergleich der High Bytes
	brne dwdec		; High-Byte ungleich -> Decrement
	ldi temp, LOW(smin)	; High-Byte gleich -> Vergleich von Low-Byte
	cp temp, R24
	breq loop		; wenn Untergrenze erreicht -> keine weiteres Dekrement

dwdec:	sbiw R24, 1		; 1 abziehen
	out OCR1BH, R25		; Ergebnis wieder nach Output Compare B schreiben
	out OCR1BL, R24
	rcall delay

loop:	rjmp up			; Endlosschleife

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

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


delay:
	ldi temp, 0xFF
dls:	nop
	nop
	nop
	dec temp
	brne dls
	ret