; Ansteuerung der Motoren über Port D
; Enable des L293D wird per PWM Angesteuert, bisher noch keine Sensoren für Geschwindigkeitsfeedback
; Deshalb immer Vollgas
; dazu werden die Bitmuster für Vorwärts, Rückwärts, Rechts und linksdrehung definiert.
; Auswertung der IR Sensoren an Port A




RamStart     EQU  $0040         ; ab hier 256 bytes Ram
RAMEnd       EQU  $023f         ; bis hier hin
RomStart     EQU  $8000         ; Hier fängt der flash bereich an.
VectorStart  EQU  $FFDC         ; Interrupt Vektoren

CONF1BITS    EQU  %11111111     ; COP disabled

; Bitmuster für Port D Motorsteuerung

VOR          EQU  %00111010        ; Beide motoren Vorwärts
RUECK        EQU  %00110101        ; Beide rückwärts
LINKS        EQU  %00111001        ; Rechts vor, Links rück
RECHTS       EQU  %00110110        ; Links vor Rechts rück
STOP         EQU  %00000000        ; Motoren aus
                                   ; Bit 5 und 6 evtl nicht nötig weil jetzt PWM


; Bitmuster für Port A Frontdetektoren

SW_LINKS     EQU  %01000000        ; Detektoren sind Low aktiv
SW_RECHTS    EQU  %00100000
SW_BEIDE     EQU  %01100000
IR_RECHTS    EQU  %00000100
IR_LINKS     EQU  %00010000
IR_MITTE     EQU  %00001000
IR_ALLE      EQU  %00011100
IR_MIRE      EQU  %00001100
IR_MILI      EQU  %00011000

; für A/D wandler

COCO.        EQU 7                 ; Conversion Complete Bit7 in ADSCR

$Include 'gpregs.inc'              ; Registerdefinitionen ( von P & E Micro )

      org RamStart

sensval    ds   1
del1       ds   1
del2       ds   1
del3       ds   1
voltage    ds   1
zustand    ds   1

      org RomStart                 ; unteres Romende

main_init:
      ldhx #RAMEnd+1               ; Stackpointer ans obere Ramende
      txs
      mov #CONF1BITS,CONFIG1       ; Config Register setzen

****** I/O Port Setup *******

      mov #0,PORTD                 ; Damit die kiste nicht einfach losrennt
      mov #%11001111,DDRD          ; Port D bit 0 - 3 als Output, bit 6,7 als Output weil NC
                                   ; Bit 4 und 5 durch TIM mit PWM belegt


      lda DDRA                     ; PORTA Als Input für Frontsensoren
      and #%10000011               ; Monitormode Bits nicht anfassen
      sta DDRA                     ;
      lda DDRC                     ; Portc bit 0 als Input für heckschalter
      ora #%00011100               ; Bit 2,3,4 für LEDS, Low aktiv
      sta DDRC
      mov #%00011100,PORTC         ; LEDs erstmal ausschalten
      LDA SPCR                     ; SPI abschalten, PORT D aktiv
      AND #$FD
      STA SPCR
                                   ; ein paar Ramzellen initialisieren
      CLR sensval                  ; für IR zustand
      clr del1                     ; benutzt in delay
      clr del2                     ; benutzt in delay
      clr del3                     ; benutzt in delay



****** PWM Setup *********

      mov #$33,T1SC                ; Timer Stop, Timer Reset, Prescaler = Busclk / 8
                                   ; Channel 0 Rechts Channel 1 Links
      mov #$03,T1MODH              ; Modulo High
      mov #$78,T1MODL              ; Modulo Low
      mov #$03,T2MODH              ; Modulo High
      mov #$78,T2MODL              ; Modulo Low
                                   ; ungefähr 10 kHz

      mov #$03,T1CH0H              ; Duty Cycle
      mov #$77,T1CH0L              ; erstmal 100 %
      mov #$03,T1CH1H
      mov #$77,T1CH1L
      mov #%00011011,T1SC0         ; Disable OC Interrupt, Unbuffered PWM MS0A, MS0A = 1
                                   ; ELS0B, ELS0A 10 Clear Output Line on Compare TOV0 =1 Toggle on Overflow
                                   ; CH0MAX = 0 disable max 100% Duty Cycle
      mov #%00011011,T1SC1         ; Disable OC Interrupt, Unbuffered PWM MS0A, MS0A = 1
                                   ; ELS0B, ELS0A 10 Clear Output Line on Compare TOV0 =1 Toggle on Overflow
                                   ; CH0MAX = 0 disable max 100% Duty Cycle
      bclr 5,T1SC                  ; Timer Status und Control, Timer bzw PWM starten

******  A/D Setup *****

      mov #%01000000,ADCLK         ; bei 5MHz Bustakt geteilt durch 4 = 1,25 MHz
      bclr 0,DDRB                  ; PTB0 ist der A/D Port
      mov #%00100000,ADSCR         ; Kontinuierlich samplen
      mov #$ff,voltage


warten:                            ; warten bis taster gedrückt

      lda PORTC                    ; taster Port C Bit 0
      and #%00000001
      beq weiter                   ; wenn gedrückt dann los
      bra warten                   ; sonst abwarten und Tee trinken

weiter:
       mov #%00011000,PORTC        ; Die LEDs hochzählen vor dem losfahren
       lda #8                      ; insgesamt ca 5 sekunden countdown
       jsr delay
       mov #%00010100,PORTC
       lda #8
       jsr delay
       mov #%00010000,PORTC
       lda #8
       jsr delay
       mov #%00001100,PORTC
       lda #8
       jsr delay
       mov #%00001000,PORTC
       lda #8
       jsr delay
       mov #%00000100,PORTC
       lda #8
       jsr delay
       mov #%00000000,PORTC
       lda #8
       jsr delay
       mov #%00011100,PORTC



start:
      lda #VOR                     ; Vorwärts fahren
      sta PORTD
      ldhx #$0377                  ; PWM auf Vollgas
      sthx T1CH0H                  ; Rechts
      sthx T1CH1H                  ; und links

test:
      jsr voltcheck                ; prüfen ob die Motoren noch über 8 V Spannng haben
                                   ; sonst stoppen

      lda PORTA                    ; Sensoren abfregen
      sta PORTC                    ; ausgeben auf LEDs
      and #%01111100               ; monitorbits ausblenden
      cmp #%01111100
      beq test                     ; alles aus OK

      sta sensval                  ; wert sichern
      lda #STOP
      sta PORTD                    ; erstmal stoppen


      lda sensval                  ; wert zurückholen
      and #SW_BEIDE                ; nur die schalter bits übriglassen
      and #SW_LINKS                ; linker schalter 0
      bne m_sw1                    ; momentan immer als wenn beide
      jmp beide_sw
m_sw1:
      lda sensval                  ; wert wiedderholen
      and #SW_RECHTS               ; schalter rechts übriglassen
      bne m_sw2                    ; nein dan nächster Sensor
      jmp beide_sw                 ; wenn 0 dann wieder wie beide

m_sw2:
      lda sensval                  ; wert zurückholen
      and #SW_BEIDE                ; beide schalter
      bne m_ir1                    ; nein dann IR Testen
      jmp beide_sw

m_ir1:
      lda sensval                  ; originalwert zurückholen
      and #IR_ALLE                 ; nur IR bits übriglassen
      bne m_ir2
      jmp alle_ir                  ; bei null alle 3 IR

m_ir2:
      lda sensval                  ; wert zurückholen
      and #IR_MIRE                 ; mitte und rechts übriglassen
      bne m_ir3
      jmp ir_mr                    ; bei null mitte rechts

m_ir3:
      lda sensval                  ; wert zurückholen
      and #IR_RECHTS               ; nur rechts übriglassen
      bne m_ir4
      jmp ir_rech                  ; bei null ir_rech

m_ir4:
      lda sensval
      and #IR_MILI                 ; mitte und links übriglassen
      bne m_ir5
      jmp ir_ml                    ; bei null ir_mittelinks

m_ir5:
      lda sensval                  ; wert zurückholen
      and #IR_LINKS                ; nur links übriglassen
      bne m_ir6
      jmp ir_lin                   ; bei null ir_lin

m_ir6:
      lda sensval                  ; wert zurückholen
      and #IR_MITTE                ; Nur Mitte
      bne m_ir7                    ; nö, dann weiter
      jmp alle_ir                  ; sonst als wenn alle aktiv sind

m_ir7:
      jmp start                    ; hier sollte er nie landen




beide_sw:


      lda #RUECK                   ; erstmal zurücksetzen
      sta PORTD
      lda #0d
      jsr delay
                                   ; ein bisschen fahren

      lda #RECHTS                  ; dann nach rechts abdrehen
      sta PORTD
      lda #0d
      jsr delay
      mov #0,sensval               ; und alles auf 0 Setzen
      jmp start

alle_ir:

      lda #RECHTS                  ; einfach nach rechts abdrehen
      sta PORTD

w_alle:
      lda PORTA                    ; portA holen
      sta PORTC                    ; und an den LEDs ausgeben
      and #IR_ALLE                 ; nur IR bits übriglassen
      cmp #IR_ALLE                 ; alle 1 ?
      beq w_alle                   ; ja, dann weiter testen und fahren
      lda #02
      jsr delay                    ; noch bisschen weiter
      jmp start                    ; sonst vorwärts



ir_rech:

      lda #LINKS                   ; einfach nach links abdrehen
      sta PORTD
w_rech:
      lda PORTA                    ; port einlesen
      sta PORTC                    ; und aud LEDs ausgeben
      and #IR_RECHTS               ; nur rechts übriglassen
      beq w_rech                   ; null dann weitertesten und fahren
      lda #02
      jsr delay
      lda PORTA                    ; links jetzt 0 ?
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_LINKS
      bne raus_r
      lda #04                      ; dann noch etwas weiter drehen
      jsr delay
raus_r:
      jmp start                    ; und wieder vorwärts

ir_mr:

      lda #LINKS                   ; einfach nach links abdrehen
      sta PORTD
w_mire:
      lda PORTA                    ; port einlesen
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_RECHTS               ; nur rechts übriglassen
      beq w_mire                   ; null dann weitertesten und weiterfahren
      lda #02
      jsr delay
      lda PORTA                    ; links jetzt 0 ?
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_LINKS
      bne raus_mr
      lda #04                      ; dann noch etwas weiter drehen
      jsr delay
raus_mr:
      jmp start                    ; und wieder vorwärts


ir_lin:

      lda #RECHTS                  ; nach rechts abdrehen
      sta PORTD
w_lin:
      lda PORTA                    ; porta lesen
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_LINKS                ; nur links übriglassen
      beq w_lin                    ; bei null weiter abfragen
      lda #02
      jsr delay
      lda PORTA                    ; porta holen
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_RECHTS               ; rechts jetzt 0 ?
      bne raus_l
      lda #04                      ; dann noch etwas weiter drehen
      jsr delay

raus_l:

      jmp start                    ; und wieder vorwärts

ir_ml:

      lda #RECHTS                  ; nach rechts abdrehen
      sta PORTD
w_mili:
      lda PORTA                    ; porta lesen
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_LINKS                ; nur links übriglassen
      beq w_mili                   ; bei null weiter abfragen
      lda #02
      jsr delay
      lda PORTA                    ; porta holen
      sta PORTC                    ; auf LEDs ausgeben
      and #IR_RECHTS               ; rechts jetzt 0 ?
      bne raus_ml
      lda #04                      ; dann noch etwas weiter drehen
      jsr delay

raus_ml:

      jmp start                    ; und wieder vorwärts

del_l:
      lda #04
      jsr delay

voltcheck:


       lda PORTD                  ; Fahrtrichtung speichern
       sta zustand
sample:
      brclr COCO.,ADSCR,*         ; Warten bis Conversion Complete
      lda ADR                     ; wert laden
      add voltage                 ; gespeicherten wert hinzuaddieren
      rora                        ; durch 2 teilen
      sta voltage                 ; und sichern
      cmp #$7f                    ; wenn die Spannung einmal auf halbe Nennspannung
                                  ; einbricht, dann ist schluß
      bhi vc_end                  ; Spannung größer dann zum ende springen

      lda #STOP
      sta PORTD
vc_blink
      mov #%00000000,PORTC        ; LEDs an Portc Blinken lassen
      lda #8
      jsr delay
      mov #%00011100,PORTC
      lda #8
      jsr delay      
      bra vc_blink                ; stehenbleiben

vc_end:
      LDA zustand
      STA PORTD                   ; weiterfahren

      rts

delay:                            ; Warteschleife ca 80 ms

      sta $43                     ; 3

delay1:
     inc del1                     ; 4
     bne delay1                   ; 3
     inc del2                     ; 4
     bne delay1                   ; 3
     dec del3                     ; 4
     bne delay1                   ; 3
     rts                          ; 4


**************************************************************
* DUMMY_ISR - Dummy Interrupt Service Routine.               *
*             Just does a return from interrupt.             *
**************************************************************
dummy_isr:

       rti           ; return


**************************************************************
* Vectors                                                    *
*                                                            *
**************************************************************
   org  VectorStart

        dw  dummy_isr    ; Time Base Vector
        dw  dummy_isr    ; ADC Conversion Complete
        dw  dummy_isr    ; Keyboard Vector
        dw  dummy_isr    ; SCI Transmit Vector
        dw  dummy_isr    ; SCI Receive Vector
        dw  dummy_isr    ; SCI Error Vector
        dw  dummy_isr    ; SPI Transmit Vector
        dw  dummy_isr    ; SPI Receive Vector
        dw  dummy_isr    ; TIM2 Overflow Vector
        dw  dummy_isr    ; TIM2 Channel 1 Vector
        dw  dummy_isr    ; TIM2 Channel 0 Vector
        dw  dummy_isr    ; TIM1 Overflow Vector
        dw  dummy_isr    ; TIM1 Channel 1 Vector
        dw  dummy_isr    ; TIM1 Channel 0 Vector
        dw  dummy_isr    ; PLL Vector
        dw  dummy_isr    ; IRQ1 Vector
        dw  dummy_isr    ; SWI Vector
        dw  main_init    ; Reset Vector