; Servo-Motor-Routinen fürs Hauptprogramm sonne

; Port und Pin, an den Servo angeschlossen ist
SERVOPORT   EQU    PORTC
SERVODDPORT EQU    DDRC
SERVOPIN    EQU    5

; Zeiten als Vielfaches von 0.008 ms für Ausgabedauer von 0 und 1
TIME0H       EQU   $0D
TIME0L       EQU   $AC
TIME1RIGHTH  EQU   $00 ; Auslenkung nach rechts
TIME1RIGHTL  EQU   $38
TIME1LEFTH   EQU   $01 ; Auslenkung nach links
TIME1LEFTL   EQU   $20
FREQ_MAX     EQU    30 ; Anzahl der Impulse für maximale Bewegung
FREQ_STEP    EQU     1 ; Anzahl der Impulse für einen Schritt

WERTEBEREICH EQU   $0120-$0038      ; !!! ANZUPASSEN
ANZRETADR    EQU   2                ; Anzahl Byte, die durch Return-Adr. der Aufrufhierarchie entstehen

; Servo-Initialisierung: Digitales Output-Pin festlegen
SERVOINIT:
        bset SERVOPIN,SERVODDPORT
        rts

; richtet Servo aus
; erwartet ein Byte auf dem Stack, das auch wieder heruntergenommen werden muß
; Es gibt die Orientierung an: $00-$FF: $00=ganz rechts
SERVO:
        ; Wertebereich $00-$FF herunterrechnen auf maximale Spanne
        lda ANZRETADR+1,SP          ; Wert oberhalb Returnadresse
        ldx #WERTEBEREICH
        mul
        pshx
        lda     modus
        ; modus=2 ?
        cbeqa   #2,SERVO01
        ; modus!=2 !
        lda #FREQ_MAX
        bra SERVO1
SERVO01:
        lda #FREQ_STEP
SERVO1:
        psha
        ; Modus behandeln
        lda     modus
        ; modus=0 ?
        cbeqa   #0,SERVO2       ; branch if modus=0
        cbeqa   #2,SERVO2       ; branch if modus=2
        ; modus=1 !
        ; ADC abfragen, dabei evtl. Bewegung abbrechen
        jsr ADC
        lda     right
        cbeq    left,SERVO5     ; branch if left=right
SERVO2:
        ; 1 ausgeben und warten
        bset SERVOPIN,SERVOPORT     ; Servoport auslesen und Pin auf 1 setzen
        lda #%00010100              ; kein TOIE, PLL/16=0.008ms
        sta T1SC
        lda #TIME1RIGHTL
        add 2,SP
        psha
        lda #TIME1RIGHTH
        adc #0
        sta T1MODH
        pula
        sta T1MODL
SERVO3:
        sta     COPCTL              ; streichele Watchdog
        brclr 7,T1SC,SERVO3     ; Bit 7 T1SC testen
        ; 0 ausgeben und warten
        bclr SERVOPIN,SERVOPORT     ; Servoport auslesen und Pin auf 0 setzen
        lda #%00010100              ; kein TOIE, PLL/16=0.008ms
        sta T1SC
        lda #TIME0H
        sta T1MODH
        lda #TIME0L
        sta T1MODL
SERVO4:
        sta     COPCTL              ; streichele Watchdog
        brclr 7,T1SC,SERVO4     ; Bit 7 T1SC testen
        ; Schleifenzähler
        pula
        deca
        bpl SERVO1
SERVO5:
        pula                        ; angepaßte Orientierung vom Stack nehmen
        rts
