; Definitionen
.def temp1=r16
.def temp2=r17
.def temp3=r18
.def temp4=r19

.def PWMModus=r20	; 0 - High 256 Schritte
					; 1 - High n Schritte
.def PWMCounter=r21

.def Stellung1=r22 ; Stellung des Servos,1 - 255
.def Stellung2=r23

.def AStellung1=r24
.def AStellung2=r25


.def InputOld=r1 ; Enthält den alten Zustand des Eingangs 0 oder 1
.def Pulsdauer=r2
.def PulsdauerMode=r3 ; Mode0 : 256 Schritte Vorlauf - Mode1 : Messung

.def MachAus=r4 ; Bei Wert 0 wird nur ausgegeben, bei Wert 1 nur gemessen

.def MaxTimeCounter=r5
.def AnzahlCounter=r6
.def AnzahlMode=r7

.def RMaxInput=r8
.def RMidInput1=r9
.def RMinInput=r10
.def RMidInput2=r11

.def SetupMode=r12

; Konstanten

.equ Timer0An=1 
.equ Timer0Aus=0
.equ Timer0Max=255-18

.equ Vorlauf=250
.equ VorlaufI=180

.equ Hi=3
.equ Lo=2

;.equ MaxInput=$D0
;.equ MidInput1=$B0

;.equ MinInput=$30
;.equ MidInput2=$50

.equ MaxImpuls=30

.equ MaxStellung=$FF
.equ MinStellung=$01

.org $0000
	rjmp RESET
	rjmp EXT_INT0
	rjmp PCINT_0
	rjmp TIM0_OVF
	rjmp EE_RDY
	rjmp ANA_COMP
	rjmp TIM0_COMPA
	rjmp TIM0_COMPB
	rjmp WATCHDOG
	rjmp ADC_READY
	
RESET:
; Stackpointer laden
	ldi temp1,low(RAMEND)
	out SPL,temp1
	
; Timer initialisieren
	ldi temp1,0
	out TCCR0A,temp1
	ldi temp1,Timer0An
	out TCCR0B,temp1
	ldi temp1,Timer0Max
	out TCNT0,temp1
	
; Timer-Interrups aktivieren
	ldi temp1,2
	out TIMSK0,temp1
	; PWM initialisieren
	ldi PWMModus,0
	ldi PWMCounter,0

; Variablen initialisieren
	ldi Stellung1,MinStellung
	ldi Stellung2,MinStellung
	ldi AStellung1,MinStellung
	ldi AStellung2,MinStellung
	clr InputOld
	clr Machaus
	clr MaxTimeCounter
	ldi temp1,$FC
	mov SetupMode,temp1

	ldi temp1,$01
	ldi temp2,$02
	ldi temp3,$FE
	ldi temp4,$FF
	mov RMaxInput,temp1
	mov RMidInput1,temp2
	mov RMidInput2,temp3
	mov RMinInput,temp4

; Port B initialisieren
	ldi temp1,$00
	ldi temp2,255-2
	out PORTB,temp1
	out DDRB,temp2
	ldi temp3,$01
	out PORTB,temp3

; 10MHz einschalten 
	ldi temp1,$80
	ldi temp2,$00
	out CLKPR,temp1
	out CLKPR,temp2


; Interrupts aktivieren
	nop
	nop
	nop
	sei

LOOP1:
	nop
	nop
	nop
	nop
	nop
	rjmp LOOP1

TIM0_OVF:
; Machaus = 1 -> Ausgabe   0-> Eingabe
	mov temp1,Machaus
	cpi temp1,0
	breq Cont1a1

/* Ausgabe-Programmteil */
; 1ms = "Vorlauf" Schritte High ausgeben, dann n Schritte 0 ausgeben
	cpi PWMModus,1
	breq Modus1
	cpi PWMModus,2
	breq Modus2
; Modus 0
; 1 Auf Port ausgeben, "Vorlauf" Schritte
Modus0:
	ori temp3,5
	out PORTB,temp3
; PWMounter +1
	inc PWMCounter
	cpi PWMCounter,Vorlauf
	brne Cont1a3xx
; Beim nächsten Durchlauf weiter mit Modus1
	inc PWMModus
	ldi temp2,2
; PWMCounter=0
	ldi PWMCounter,0
	rjmp Cont1a3xx
; Modus 1
; 1 auf Port ausgeben, "Stellung1" bzw. "Stellung2" Schritte
Modus1:
; 1 Auf Port ausgeben, "Stellung1" Schritte
	; PWMCounter +1
	inc PWMCounter
	cp PWMCounter,AStellung1
	brne Modus1a
	; 0 für Servo 1 ausgeben
	andi temp3,$FF-1
	out PORTB,temp3
	dec temp2
	breq Modus1b
Modus1a:
	cp PWMCounter,AStellung2
	brne Cont1a3
	; 0 füär Servo 2 ausgeben
	andi temp3,$FF-4
	out PORTB,temp3
	dec temp2
	breq Modus1b
Cont1a3xx:	
	rjmp Cont1a3
Modus1b:
; Beim nächsten Durchlauf weiter mit Modus2
	inc PWMModus
; PWMCounter=0
	ldi PWMCounter,0
	rjmp Cont1a3
; 0 auf Port ausgeben, Steuerung an Eingabe-Programmteil übergeben
Modus2:
; 0 Auf Port ausgeben
	andi temp3,$FF-5
	out PORTB,temp3
	clr MachAus
	cp AStellung1,Stellung1
	breq Modus2b
	brlo Modus2a
	dec AStellung1
	rjmp Modus2b
Modus2a:
	inc AStellung1
Modus2b:
	cp AStellung2,Stellung2
	breq Cont1a3
	brlo Modus2c
	dec AStellung2
	rjmp Cont1a3
Modus2c:
	inc AStellung2
	rjmp Cont1a3
	
/* Eingabe-Programmteil */
Cont1a1:
; Einlesen vom Port B1
	out PORTB,temp3
	ldi temp1,255-2
	out DDRB,temp1
	nop
	in temp4,PINB
; Flankenwechsel feststellen
	andi temp4,2
	breq Cont1a4
	
; Input High
	mov temp1,InputOld
	cpi temp1,1
	breq KeinWechsel01
; Flankenwechsel 0->1
	clr InputOld
	inc InputOld
; Pulsdauermessung initialisieren, zunächst Vorlauf messen
	clr PulsdauerMode
	clr Pulsdauer
	rjmp Cont1a3
KeinWechsel01:
; Puls addieren
	mov temp1,PulsdauerMode
	cpi temp1,0
	brne PMode1
; Vorlauf messen
	inc Pulsdauer
	mov temp1,Pulsdauer
	cpi temp1,VorlaufI
	brne Cont1a3
; Vorlauf abgearbeitet
	clr Pulsdauer
	clr PulsdauerMode
	inc PulsdauerMode
Cont1a3:
	ldi temp1,Timer0Max
	out TCNT0,temp1
	reti
PMode1:
; Pulsdauer nach Vorlauf messen
	inc Pulsdauer
	rjmp Cont1a3 
Cont1a4:
; Input Low
	mov temp1,InputOld
	cpi temp1,0
	breq Cont1a3
; Flankenwechsel 1->0
; Steuerung an Ausgabe-Programmteil übergeben

;SetupMode (ca. 5 Sekunden nach Einschalten aktiv)	
	mov temp1,SetupMode
	cpi temp1,0
	brne ContSMX
	rjmp ContSM1
; SetupMode noch aktiv	
ContSMX:	
	mov temp1,PulsdauerMode
	cpi temp1,0
	brne ContSM2
	ldi temp1,$1
	mov Pulsdauer,temp1
ContSM2:
	mov Stellung1,Pulsdauer
	mov Stellung2,SetupMode
	mov AStellung1,Pulsdauer
	mov AStellung2,SetupMode	
	mov temp1,Pulsdauer
	cpi temp1,$A0
	brlo ContSM3
	cp temp1,RMaxInput
	brlo ContSM3
	mov RMaxInput,temp1
ContSM3:
	mov temp1,Pulsdauer
	cpi temp1,$60
	brsh ContSM4
	cp temp1,RMinInput
	brsh ContSM4
	mov RMinInput,temp1
ContSM4:
	dec SetupMode
	dec SetupMode
	dec SetupMode
	dec SetupMode
	
	breq ContSM7
	rjmp ContSM6
ContSM7:
	mov temp1,RMaxInput
	cpi temp1,$A0
	brsh ContSM8
	rjmp ContSM5
ContSM8:	
	mov temp2,RMinInput
	cpi temp2,$60
	brlo ContSM9
	rjmp ContSM5
ContSM9:
; RMinInput +=$5
; RMaxInput -=$5
; Mitte bestimmen
; Mitte1 +=Mid//2
; Mitte2 -=Mid/2
	ldi temp3,$5
	add RMinInput,temp3
	sub RMaxInput,temp3
	mov temp3,temp1
	sub  temp3,temp2
	lsr temp3
	mov temp4,temp3
	lsr temp4
	add temp3,temp2
	mov RMidInput1,temp3
	mov RMidInput2,temp3
	add RMidInput1,temp4
	sub RMidInput2,temp4
; In EEProm speichern
	ldi Stellung1,MinStellung
	ldi Stellung2,MinStellung
	ldi AStellung1,MinStellung
	ldi AStellung2,MinStellung
EEPROM_write:
	;push temp1
	;push temp2
EEPROM_write1:
	SBIC EECR,EEPE
	rjmp EEPROM_write1
	nop
	ldi temp1, 0
	out EECR,temp1
	ldi temp2,0
	nop
	out EEARL,temp2
	mov temp1,RMaxInput
	out EEDR,temp1
	sbi EECR,EEMPE
	sbi EECR,EEPE
	nop
EEPROM_write2:
	SBIC EECR,EEPE
	rjmp EEPROM_write2
	nop
	ldi temp1, 0
	out EECR,temp1
	ldi temp2,1
	nop
	out EEARL,temp2
	nop
	mov temp1,RMinInput
	out EEDR,temp1
	sbi EECR,EEMPE
	sbi EECR,EEPE
	nop
EEPROM_write3:
	SBIC EECR,EEPE
	rjmp EEPROM_write3
	nop
	ldi temp1, 0
	out EECR,temp1
	ldi temp2,2
	nop
	out EEARL,temp2
	nop
	mov temp1,RMidInput1
	out EEDR,temp1
	sbi EECR,EEMPE
	sbi EECR,EEPE
	nop
EEPROM_write4:
	SBIC EECR,EEPE
	rjmp EEPROM_write4
	nop
	ldi temp1, 0
	out EECR,temp1
	ldi temp2,3
	nop
	out EEARL,temp2
	mov temp1,RMidInput2
	out EEDR,temp1
	sbi EECR,EEMPE
	sbi EECR,EEPE
EEPROM_write5:
	SBIC EECR,EEPE
	rjmp EEPROM_write5
	rcall EEPROM_read
	;pop temp2
	;pop temp1
;	rcall EEPROM_write
	ldi PWMCounter,0
	ldi PWMModus,0
	clr InputOld
	clr Machaus
	;inc Machaus
	clr MaxTimeCounter
	ldi temp1,0
	mov SetupMode,temp1
	rjmp ContSM5
ContSM6:
	clr Machaus
	inc Machaus
	ldi PWMCounter,0
	ldi PWMModus,0
	clr InputOld
	rjmp Cont1a3

ContSM5:
;EEProm lesen
	ldi Stellung1,MinStellung
	ldi Stellung2,MinStellung
	ldi AStellung1,MinStellung
	ldi AStellung2,MinStellung
	rcall EEPROM_read
		
ContSM1:	
	clr Machaus
	inc Machaus
	ldi PWMCounter,0
	ldi PWMModus,0
	clr InputOld
; Pulsdauer auswerten
	mov temp1,PulsdauerMode
	cpi temp1,0
	brne Cont1a5
	ldi temp1,$1
	mov Pulsdauer,temp1
Cont1a5:
; MaxTimeCounter wird auf die Maximale Impulszahl gesetzt, in deren Dauer der Knüppel bewegt werden muss.
; Max-Mid-Max-Mid
; AnzahlCounter zählt die Knüppelbewegungen, Gesetztes Bit8 für Kanalbestimmung
	mov temp1,MaxTimeCounter
	cpi temp1,0
	brne Cont1a6
; Erste Knüppelbewegung muss gezählt werden, MaxCounter gesetzt werden
	mov temp1,Pulsdauer
	cp temp1,RMaxInput
	brlo Cont1aa
	ldi temp1,MaxImpuls
	mov MaxTimeCounter,temp1
	clr AnzahlCounter
	clr AnzahlMode
	rjmp Cont1a3
Cont1aa:
	mov temp1,Pulsdauer
	cp temp1,RMinInput
	brsh Cont1a3yy
	ldi temp1,MaxImpuls
	mov MaxTimeCounter,temp1
	clr AnzahlCounter
	clr AnzahlMode
	inc AnzahlMode
	rjmp Cont1a3
Cont1a6:
	dec MaxTimeCounter
	brne Cont1x1
	clr AnzahlCounter
	clr AnzahlMode
Cont1a3yy:
	rjmp Cont1a3
Cont1x1:	
	mov temp1,AnzahlMode
	cpi temp1,0
	brne Cont2a1
; Knüppel Max - Mid - Max - Mid	
	mov temp1,AnzahlCounter
	cpi temp1,0
	brne Cont1a7
	mov temp1,Pulsdauer
	cp temp1,RMidInput1
	brsh Cont1a3yy
	inc AnzahlCounter	
	rjmp Cont1a3	
Cont1a7:
	mov temp1,AnzahlCounter
	cpi temp1,1
	brne Cont1a7b
	mov temp1,Pulsdauer
	cp temp1,RMaxInput
	brlo Cont1a3yy
; Aktion gewählt, ausführen
	inc AnzahlCounter
	cpi Stellung1,$01
	brne Cont1a8
	ldi Stellung1,$FF
	rjmp Cont1a3
Cont1a8:
	ldi Stellung1,$01
	rjmp Cont1a3
Cont1a7b:
	mov temp1,Pulsdauer
	cp temp1,RMidInput1
	brsh Cont1a3yy	
	clr MaxTimeCounter
	clr AnzahlCounter
	rjmp Cont1a3

;Knüppel Min - Mid - Mkin - Mid
Cont2a1:
	mov temp1,AnzahlCounter
	cpi temp1,0
	brne Cont2a7
	mov temp1,Pulsdauer
	cp temp1,RMidInput2
	brlo Cont1a3yy
	inc AnzahlCounter	
	rjmp Cont1a3	
Cont2a7:
	mov temp1,AnzahlCounter
	cpi temp1,1
	brne Cont2a7b
	mov temp1,Pulsdauer
	cp temp1,RMinInput
	brsh Cont1a3yy
; Aktion gewählt, ausführen
	inc AnzahlCounter
	cpi Stellung2,$01
	brne Cont2a8
	ldi Stellung2,$FF
	rjmp Cont1a3
Cont2a8:
	ldi Stellung2,$01
	rjmp Cont1a3
Cont2a7b:
	mov temp1,Pulsdauer
	cp temp1,RMidInput2
	brlo Cont1a3yy	
	clr MaxTimeCounter
	clr AnzahlCounter
	rjmp Cont1a3


/* Weitere Interrupts */
TIM0_COMPA:
TIM0_COMPB:
EXT_INT0:
PCINT_0:
EE_RDY:
ANA_COMP:
WATCHDOG:
ADC_READY:
	reti

	
EEPROM_read:
	push temp1
	push temp2
EEPROM_read1:
		SBIC EECR,EEPE
		rjmp EEPROM_read1
		ldi temp2,0
		out EEARL,temp2
		sbi EECR,EERE
		in RMaxInput,EEDR
EEPROM_read2:
		SBIC EECR,EEPE
		rjmp EEPROM_read2
		ldi temp2,1
		out EEARL,temp2
		sbi EECR,EERE
		in RMinInput,EEDR
EEPROM_read3:
		SBIC EECR,EEPE
		rjmp EEPROM_read3
		ldi temp2,2
		out EEARL,temp2
		sbi EECR,EERE
		in RMidInput1,EEDR
EEPROM_read4:
		SBIC EECR,EEPE
		rjmp EEPROM_read4
		ldi temp2,3
		out EEARL,temp2
		sbi EECR,EERE
		in RMidInput2,EEDR
		pop temp2
		pop temp1
		ret