;****** Augenzwinker 0.1 (c) Oliver Mezger 31.05.2007.

.include "tn26def.inc"

;********* Hardware-Verbindungen

;*** PortB
.equ BlinkLED = 6
.equ sLo = 5    ;Pin von linkem Servo-Ausgang
.equ sRo = 4    ;Pin von rechtem Servo-Ausgang
.equ selfTest = 3

;********* Software 

.def stat   = R3        ;Speichern des Status waehrend Interrupt
.def itempL = R4        ;Temoraere Register fuer Interrupts
.def temp = R16         ;Temp-Register zum Arbeiten
.def temp2 = R17
.def servoL = R18       ;Position von ServoL
.def servoR = R19       ;Position von ServoR
.def itemp  = R20       ;Temoraere Register fuer Interrupts
.def sysTick = R21      ;Zaehlt die Timer1 Ueberlaeufe
.def sysTime = R22      ;Interne Zeit
.def eyeWait = R23
.def eyeData = R24

.set cpuClock = 1       ;Frequenz 1 MHz
.set timePrescale = 64
.set timeBase = timePrescale / cpuClock
.set servoNeutralTime = 1420        ;Mittelstellung in mikroSec
.set servoMi = servoNeutralTime / timeBase  ;Mittelstellung der Servos
.set sysTimeDiv = 500000 / 256 / timeBase   ;damit SysTime 


    rjmp init           ;Reset
    reti                ;IRQ0
    reti                ;IRQ1
    rjmp T1_Comp        ;T1 Compare A
    reti                ;T1 Compare B
    rjmp T1_Over        ;T1 Overflow
    reti                ;T0 Overflow
init:
    ldi temp,0b01110000
    out DDRB,temp       ;PortB als Ausgang
    ldi temp,0b11001111
    out PORTB,temp
    ldi temp,0xff
    out PORTA,temp      ;Alle Pull-Up an

    ldi temp,RAMEND     ;Oberste RAM-Adresse holen
    out SP, temp        ;Stack-Pointer initialisieren

    ldi servoL,servoMi
    ldi servoR,servoMi

timer1_first_set:
    ldi temp,7
    out TCCR1B,temp     ;PreScaler /64
    ldi temp,0b01000100 ;Timer1 Overflow Interupt Flag
    out TIMSK,temp      ;Timer-Interrupt freigeben 
    ldi ZH,high(serPositionen*2)
    ldi ZL,low(serPositionen*2)
    ldi XH,high(serPos)
    ldi XL,low(serPos)
ladeSerPositionen:
    lpm temp,Z
    cpi temp,0
    breq endeLade
    adiw Z,1
    st X+,temp
    rjmp ladeSerPositionen
endeLade:
    rcall ladeMoves
    ldi itemp,8
startwarte:
    sbi PORTB,BlinkLED
    rcall wait
    cbi PORTB,BlinkLED
    rcall wait
    dec itemp
    brne startwarte
    sbi PORTB,BlinkLED
    ldi eyeWait,2
    sei                 ;Interrupts global freigeben (I-Bit)
    rjmp main

T1_Over:
    in stat,SREG        ;Statusregister retten
    inc sysTick ;
    cpi sysTick,sysTimeDiv
    brmi t1over1
    clr sysTick
    inc sysTime
t1over1:
    mov itempL,servoL   ;Linker Servo
    rcall T1_timeMark   ;Ende Marke setzen
    sbi PORTB,sLo       ;Linger Servo start
t1overout:  
    out SREG,stat       ;Statusregister herstellen
    reti

T1_Comp:
    in stat,SREG        ;Statusregister retten
    sbic PORTB,sLo      ;Test, ob ServoL set
    rjmp serLfin        ;Ja, dann zu ServoL-Finish
    sbic PORTB,sRo      ;Test, ob ServoR set
    rjmp serRfin        ;Ja, dann zu ServoR-Finish
    rjmp T1_fin         ;Hier weitere Servos einfuegen
serLfin:
    cbi PORTB,sLo       ;Bit ServoL clear
    mov itempL,servoR   ;Weiter mit ServoR
    rcall T1_timeMark   ;Ende Marke setzen
    sbi PORTB,sRo       ;ServoR set
    rjmp T1_fin
serRfin:
    cbi PORTB,sRo       ;Bit ServoR clear
T1_fin: 
    out SREG,stat       ;Statusregister herstellen
    reti

T1_timeMark:
    
    in itemp,TCNT1      ;Counter1 Low einlesen
    add itempL,itemp    ;Addiere zu Counter1
    out OCR1A,itempL    ;ins Compare1 Register schreiben
    ret

main:
    sbrc sysTime,1      ;wenn Bit 1 gesetzt
    rjmp ma_setled      ;setze Leds
    cbi PORTB,BlinkLED  ;sonst loesche ledS
    rjmp mainEye
ma_setled:
    sbi PORTB,BlinkLED  ;LED Spain leuchtet
mainEye:
    cp sysTime,eyeWait
    brlo main
    clr sysTime
    lpm eyeData,Z
    cpi eyeData,0
    breq eyeVonAnfang
    adiw Z,1
    mov eyeWait,eyeData
    swap eyeWait
    andi eyeWait,$0f
    mov temp2,eyeData
    asr temp2
    asr temp2
    andi temp2,3
    ldi XH,high(serPos)
    ldi XL,low(serPos)
    add XL,temp2
    ld temp,X
    mov servoL,temp
    mov temp2,eyeData
    andi temp2,3
    ldi XH,high(serPos)
    ldi XL,low(serPos)
    add XL,temp2
    ld temp,X
    mov servoR,temp
    rjmp main
eyeVonAnfang:
    rcall ladeMoves
    rjmp main

wait:
    ldi temp,100    ;aeusseren Zaehler laden: 80 bei 4MHz
wl1:
    ldi temp2,250   ;inneren Zaehler laden
wl2:
    nop
    dec temp2       ;temp2--
    brne wl2        ;Sprung wenn nicht null
    dec temp        ;temp--
    brne wl1        ;Sprung wenn nicht null
    ret

ladeMoves:
    sbis PINB,selfTest
    rjmp normalMoves
    ldi ZH,high(testMoves*2)
    ldi ZL,low(testMoves*2)
    ret
normalMoves:
    ldi ZH,high(eyeMoves*2)
    ldi ZL,low(eyeMoves*2)
    ret
serPositionen: .db servoMi*0.75,servoMi*0.85,servoMi*1.0,servoMi*1.15,0 ; Servo Positionen
testMoves: .db $45,$4a,$45,$40,$4f,$60,$65,$6f,0
eyeMoves: .db $25,$2a,$2f,$23,$24,$3a,$44,$22,$51,$32,$21,$15,$12,$3c,$3a,$2d,$1c,$1d,$1c,$1d,$1c,0

.dseg ;Datenbereich
    serPos: .byte 4