;   File Name: MagicRaceCar.asm
;   Authors: Andreas Søgaard Berthou, Jacob Kjærby Christensen & Tobias Jessen
;   Content: Magic sleek code to race car
    list        p=16f684        ; list directive to define processor
    #include    <P16F684.inc>       ; processor specific variable definitions
    __CONFIG    _CP_OFF & _CPD_OFF & _BOD_OFF & _PWRTE_ON & _WDT_OFF & _INTRC_OSC_NOCLKOUT & _MCLRE_ON & _FCMEN_OFF & _IESO_OFF
;***** VARIABLE DEFINITIONS
w_temp      EQU 0x7E            ; variable used for context saving
status_temp EQU 0x7F            ; variable used for context saving
AN1R        EQU 0x21
xref        EQU 0x22            ; brugt til referenceværdien
xthresh     EQU 0x23            ; brugt til threshholdværdien
;**********************************************************************
    ORG     0x000               ; processor reset vector
    goto        INIT            ; go to beginning of program


    ORG     0x004               ; interrupt vector location
    movwf       w_temp          ; save off current W register contents
    movf        STATUS,w        ; move status register into W register
    movwf       status_temp     ; save off contents of STATUS register



INIT
    ;Variable

    ; Setup for controller
    BSF STATUS,RP0              ; Goto bank 1
    CLRF    ANSEL
    CLRF    TRISA
    BCF STATUS,RP0              ; Goto bank 0
    MOVLW   B'00000111'
    MOVWF   CMCON0

    ; Setup og start timer 2
    CLRF    TMR2                ; Nulstil Timer 2 registeret
    BSF STATUS,RP0              ; Skift til Bank 1
    MOVLW   D'195'              ; S?t Period registeret
    MOVWF   PR2                 ;
    BCF STATUS,RP0              ; Skift til Bank 0
    MOVLW   B'01111111'         ; Indstil post- og pre-scaler
    MOVWF   T2CON               ; og start timer 2

    ; PWM PWM PWM PWM
    ; PWM frequency
    BSF STATUS,RP0
    MOVLW   D'128'
    MOVWF   PR2
    BCF STATUS,RP0              ;Sørger for at PWM duty-cycle er som den skal være

    ; PWM starting cycle
    CLRF    CCPR1L              ; Set duty cycle = 0%
    MOVLW   B'00001100'         ; All P1A,B,C,D = high
    MOVWF   CCP1CON

    ; PWM output mode
    BSF STATUS,RP0
    BCF TRISC, 5                ; Bit 2 = output

    ;Input pins
    BSF TRISC,0                 ;C1
    BSF TRISC,1                 ;C2
    BSF ANSEL,5                 ;C1=ANALOG
    BSF ANSEL,4                 ;C0=analog
    BCF STATUS,RP0

    ; Clear timer
    CLRF    TMR2

    ; Enable timer
    BSF T2CON, TMR2ON

    ;Setup ADC
    BSF STATUS,RP0              ; Bank 1
    MOVLW   b'00100000'         ; Cycle
    MOVWF   ADCON1              ; Set cycle in ADCON1
    BCF STATUS,RP0              ; Bank 0
    BSF ADCON0, 0               ; Turns on AD conversion
    BSF ADCON0, 2
    BSF ADCON0, 4               ; Sets CHS to 101 for AN5 input

    GOTO    tomgang
tomgang
    BSF ADCON0,GO               ;start ADC
testlys BTFSC   ADCON0,GO       ;vent på ADC er færdig
    GOTO    testlys
    MOVF    ADRESH,W            ;Flyt ADC-værdien ind i W
    SUBLW   d'30'               ;Træk 30 fra W (Hvor meget lyset skal under)
    BTFSS   STATUS,C            ;Tjek om det går under 0
    GOTO    tomgang             ;Hvis ikke, prøv ADC igen
    CLRF    ADCON0              ;Ellers sæt AN4 som ADC pin
    MOVLW   b'00010001'         ;^^
    MOVWF   ADCON0              ;^^
    BSF     ADCON0, GO          ;vv
setref  BTFSC   ADCON0,GO       ;Start ADC på magneten
    GOTO    setref              ;Vent på den er færdig
    MOVF    ADRESH,W            ;Flyt ADC-værdien i W
    MOVWF   xref                ;Flyt W til 'xref' (referenceværdien for magneten)
    MOVLW   d'2'                ;Træk 2 fra xref
    SUBWF   xref,0              ;Flyt det nye resultat over i W(Så der er op til 2 ud af 255 i usikkerhed til små fejlkilder)
    MOVWF   xthresh             ;FLyt det derefter over i xthresh (Threshhold-værdien)
    GOTO    drive               ;Begynd at teste til at køre

lowspeed                        ;Flytter 40 over i dutycycle som hastighed
    MOVLW   d'40'
    CLRF    CCPR1L
    MOVWF   CCPR1L
    GOTO    drive

fullspeed                       ;Flytter 50 ind i duty-cycle som hastighed
    MOVLW   d'50'
    CLRF    CCPR1L
    MOVWF   CCPR1L
    GOTO    drive
drive
    BSF ADCON0,GO               ;Test ADC
getmag  BTFSC   ADCON0,GO       ;Vent på den er færdig
    GOTO    getmag              ;Vent på den er færdig
    MOVF    ADRESH,W            ;Flyt værdien ind i W
    SUBWF   xthresh,0           ;Træk W fra xthresh
    BTFSC   STATUS,C            ;Tjek om målt ADC-værdi er større end threshholdværdien
    GOTO    lowspeed            ;Hvis den er lavere, sæt lav speed
    GOTO    fullspeed           ;Ellers sæt høj speed

    GOTO    drive               ;Test ny værdi

    END                         ; directive 'end of program'