	org 	8000h
        jmp     8200h

	$MOD51GB

;Pulse width signals
PWM_MOT         EQU     R0
PWM_STR         EQU     R1

;Compass data register
CompCount       EQU     R2
DataByte        EQU     R4

;Compass result direction flag
Left_Flag       EQU     P5.7
Right_Flag      EQU     P5.6

;Motor Direction
STP_GO          EQU     P5.1
FOR_REV         EQU     P5.0

;Speeds
FWD_LO          EQU     0C0H
FWD_MED         EQU     0B0H
FWD_HI          EQU     0B0H
REV_LO          EQU     090H

;Steering Directions
Straight        EQU     03fH
Full_Left       EQU     030H
Half_Left       EQU     037H
Qtr_Left        EQU     03AH
Full_Right      EQU     04DH
Half_Right      EQU     045H
Qtr_Right       EQU     042H

;Sonar ranges
Sonar_15        EQU     245
Sonar_12        EQU     208
Sonar_9         EQU     155
Sonar_6         EQU     103
Sonar_3         EQU     51
Sonar_1         EQU     26

;Compass Pins
EOC     equ     P4.0
SS      equ     P4.1
MS      equ     P4.2
RESET   equ     P4.3
SCLK    equ     P4.4
SDO     equ     P4.5
POLL    equ     P4.6
POW     equ     P4.7

;Compass data
Base      equ     081h
Actual_HI equ     000h
Actual_LO equ     001h

Desired_HI  equ     002h
Desired_LO  equ     003h

        org     802bh
        jmp     T2_ISR

        org     8100h
        db      0,0,0,0


        org     8200h
main:
        ;enable A/D operation
        mov     ACON,#010h   

        ;STP_GO is cleared to stop motor
        ;FOR_REV is set to go in reverse

        ;initalize the motor and steering PWM waveforms
        mov     P1,#0FFH        ;must set all bits on port1 to 0
        mov     PWM_MOT,#FWD_LO   ;set motor for 1/8 speed
        mov     CCAP2h,PWM_MOT
        mov     PWM_STR,#03Ch   ;set steering servo to center
        mov     CCAP1h,PWM_STR
        clr     FOR_REV           ;set relays for FORWARD
        clr     STP_GO          ;block PWM signal to motor
        call    PCA_INIT

        ;Set up the compass Inits
        call    Comp_Init       ;Initialize the compass
        call    Print
        db      'The compass is initialized\n',0,
        call    T2_Init         ;Initialize T2
        call    Print
        db      'Timer 2 is initialized\n',0,

;****************************************************************************
        mov     DPH,Base
        mov     DPL,Desired_HI
        mov     A,#0
        movx    @DPTR,A
        inc     DPTR
        mov     A,#05Ah
        movx    @DPTR,A
;****************************************************************************

;        setb    ET2             ;enable Timer 2 interrupt
;        setb    EA              ;enable interrupts

        setb    STP_GO

Scan: call    lngDelay


L_Rear: call    Print
        db      'LR',0,
        clr     C             ; Clear carry bit
        mov     A,AD1         ; get the left rear IR
        subb    A,#100        ; subtract threshold from value
        jc      L_Front       ; jump if not too close

rev2for:
        jnb     STP_GO,stp_rev2  ;already stopped
        jnb     FOR_REV,stp_rev2 ;or going forward
        clr     STP_GO           ;stop motor
        clr     FOR_REV          ;place in forward
        mov     PWM_STR,#Straight ;center wheels
        mov     CCAP1H,PWM_STR    ;
        call    lngDelay
        mov     PWM_MOT,#FWD_LO  ;set motor for 1/8 speed
        mov     CCAP2h,PWM_MOT
        setb    STP_GO           ;start motor
        call    PRINT
        db      'Forward from IR\n',0,
stp_rev2:
        jmp     Scan


L_Front:
        call    Print
        db      'LF',0,
        clr     C             ; Clear carry bit
        mov     A,AD2         ; get the left front IR
        subb    A,#100        ; subtract threshold from value
        jc      R_Front       ; jump if not too close

        jmp     for2rev

R_Front:
        call    Print
        db      'RF',0,
        clr     C             ; Clear carry bit
        mov     A,AD3         ; get the left front IR
        subb    A,#100        ; subtract threshold from value
        jc      Compass_Calc  ; jump if not too close
        

for2Rev:
        jnb     STP_GO,stp_rev0         ;already stopped
        jb      FOR_REV,stp_rev0        ;or backing
        clr     STP_GO                  ;stop motor
        setb    FOR_REV                 ;reverse it
        mov     PWM_STR,#031h           ;turn wheels to right
        mov     CCAP1H,PWM_STR          ;
        call    lngDelay                ;Give it 1/2 a second
        mov     PWM_MOT,#REV_LO         ;set motor for 1/8 speed
        mov     CCAP2h,PWM_MOT
        setb    STP_GO                  ;start motor
        call    PRINT
        db      'Backing from IR\n',0,
stp_rev0:
        jmp     Scan

Compass_Calc:
        call    Compass         ;get differential heading
        mov     A,DPL           ;from DPL  ( < B4 )
        jnz     TurnGo          ;if heading matches goto Scan(sonarsoon???)
        jmp     Scan

TurnGo: clr     C               ;clear carry
        mov     R7,#10          ;load R7 with 10
        subb    A,R7            ;is difference >10?
        jc      undr_10
        mov     A,DPL
        clr     C
        mov     R7,#25
        subb    A,R7            ;is difference >25?
        jc      undr_25
        jb      Right_Flag,Right_Full
Left_Full:
        mov     PWM_STR,#Full_Left
        jmp     comp_str      
Right_Full:
        mov     PWM_STR,#Full_Right
        jmp     comp_str

undr_25:
        jb      Right_Flag,Right_Half
Left_Half:
        mov     PWM_STR,#Half_Left
        jmp     comp_str

Right_Half:
        mov     PWM_STR,#Half_Right
        jmp     comp_str

undr_10:
        jb      Right_Flag,Right_Qtr
Left_Qtr:
        mov     PWM_STR,#Half_Left
        jmp     comp_str
Right_Qtr:
        mov     PWM_STR,#Half_Left
        jmp     comp_str

comp_str:
        mov     CCAP1H,PWM_STR
        jmp     Scan





Sonar:  clr     C               
        mov     A,AD0           ; Get the sonar value
        subb    A,#Sonar_15     ; subtract threshold from value
        jc      Ahd_hlf         ; jump if distance is < 15 ft

        mov     PWM_MOT,#FWD_HI  ;set motor for High speed
        mov     CCAP2h,PWM_MOT
        call    PRINT
        db      'Ahead Full\n',0,

        jmp     Scan

Ahd_hlf:
        clr     C
        mov     A,AD0           ; Get the sonar value
        subb    A,#Sonar_9      ; subtract threshold from value
        jc      Ahd_slow        ; jump if distance is < 9 ft
        cjne    PWM_MOT,#FWD_HI,All_Stop
                                ; If motor is in high gear
                                ; turn it off
       
        mov     PWM_MOT,#FWD_MED  ;set motor for Medium speed
        mov     CCAP2h,PWM_MOT
        call    PRINT
        db      'Ahead Medium\n',0,
        
        jmp     Scan

Ahd_slow:
        clr     C
        mov     A,AD0           ; Get the sonar value
        subb    A,#Sonar_3      ; subtract threshold from value
;***************************************************************************
;        jc      for2rev         ; jump if distance is < 3 ft
;***************************************************************************
        cjne    PWM_MOT,#FWD_HI,All_Stop
                                ; If motor is in high gear
                                ; turn it off
        
        mov     PWM_MOT,#FWD_LO  ;set motor for Low speed
        mov     CCAP2h,PWM_MOT
        call    PRINT
        db      'Ahead Slow\n',0,
        jmp     Scan

All_Stop:
        clr     STP_GO
        jmp     Scan

Compass:
        push    04
        call    load_actual     ;get actual from memory
        call    load_desired    ;get desired from memory
        call    get_actual      ;put actual into DPTR
        call    get_desired     ;put desired into B/Acc
        call    util_subbad     ;Actual-Desired
        jc      Neg             ;if actual is less than desired jump to Neg
        mov     B,#0            ;load B/Acc with 180
        mov     A,#0B4h
        call    util_subbad     ;subtract 180 from DPTR
        jc      Left_Pos        ;if Actual-Desired is <180 jump Left_Pos
Right_Pos:
        setb    Right_Flag      ;set flag for right turn
        call    get_desired     ;put desired into B/Acc
        mov     DPH,#1          ;load DPTR with 360
        mov     DPL,#067h
        call    util_adcbad     ;Add 360 to desired->(DPTR)
        mov     B,R6            ;Load B/Acc with actual
        mov     A,R7            ;
        call    util_subbad     ;get final difference
                                ;put it in DPTR
        jmp     end_sub
       
Neg:    mov     DPH,R3          ;put desired in DPTR
        mov     DPL,R4
        mov     B,R6            ;put actual in B/Acc
        mov     A,R7
        call    util_subbad     ;DPTR = DPTR - B/Acc
        mov     B,#0
        mov     A,#0B4h
        call    util_subbad     ;if Desired-Actual < 180 jump
        jc      Right_Neg
Left_Neg:
        setb    Left_Flag
        mov     B,R6            ;put actual in B/Acc
        mov     A,R7
        mov     DPH,#1          ;add 360 to actual
        mov     DPL,#067h
        call    util_adcbad     ;Actual+360->DPTR
        mov     B,R3            ;Put desired into 
        mov     A,R4            ;B/Acc
        call    util_subbad     ;get final difference
                                ;place it in DPTR
        jmp     end_sub        

Left_Pos:
        setb    Left_Flag       ;set bit for left turn
        mov     DPH,R6          ;put actual in DPTR
        mov     DPL,R7
        mov     B,R3            ;put desired in B/Acc
        mov     B,R4
        call    util_subbad     ;get final difference
                                ;place it in DPTR
        jmp     end_sub         

Right_Neg:
        setb    Right_Flag      ;set bit for right turn
        mov     DPH,R3          ;put desired in DPTR
        mov     DPL,R4
        mov     B,R6            ;put actual in B/Acc
        mov     A,R7
        call    util_subbad     ;get final difference
                                ;place it in DPTR
end_sub:       
        pop     04
        ret

load_actual:
        mov     DPH,#Base
        mov     DPL,#Actual_HI
        movx    A,@DPTR         ;get high byte of compass data
        mov     R6,A            ;store it in R6
        inc     DPTR            ;get low byte of data
        movx    A,@DPTR         ;
        mov     R7,A            ;store low byte in R7
        inc     DPTR            ;
        ret

load_desired:
        mov     DPH,#Base
        mov     DPL,#Desired_HI
        movx    A,@DPTR         ;get desired heading
        mov     R3,A            ;high byte
        inc     DPTR            ;get desired heading
        mov     R4,A            ;low byte
        ret

get_actual:
        mov     DPH,R6          ;move actual into DPTR
        mov     DPL,R7
        push    02
        push    03
        push    04
        push    05
        push    PSW
        call    pint16u
        pop     PSW
        pop     05
        pop     04
        pop     03
        pop     02
        ret

get_desired:
        mov     B,R3            ;move desired into B/Acc
        mov     A,R4
        ret

util_adcbad:
        push    acc                     ; Make Sure Acc Gets Saved
        add     a,dpl                   ; Add 'A' To DPL
        mov     dpl,a                   ; Move Result To DPL
        mov     a,dph                   ; Get DPH
        addc    a,b                     ; Add 'B' To DPH + CY
        mov     dph,a                   ; Move Bck To DPH
        pop     acc                     ; Recover Original 'A'
        ret                             ; Return To Caller

util_subbad:
        push    acc                     ; Make Sure Acc Gets Saved
        clr     c                       ; Clear For SUBB
        xch     a,dpl                   ; Swap
        subb    a,dpl                   ; Subtract
        mov     dpl,a                   ; Move Back To DPL
        mov     a,dph                   ; Get DPH
        subb    a,b                     ; Subtract + CY
        mov     dph,a                   ; Move Back To DPH
        pop     acc                     ; Recover Original 'A'
        ret                             ; Return To Caller



wait:   jmp     wait


Comp_Init:
        ;set all the bits on the compass
        setb    POW             ;turn power off
        setb    POLL            ;set the poll bit
        setb    SS              ;set the slave select bit
        setb    RESET           ;set the NOT Reset line
        setb    SCLK            ;set the shift clock
        setb    SDO             ;setting SDO will set that pin on the
                                ;8051 as an input
        setb    EOC             ;same for EOC

        clr     POW             ;turn power on

        ;Initialize the counter for the compass process controller
        mov     CompCount,#0
        clr     RESET           ;reset compass
        call    Delay10         ;wait 10msecs
        setb    RESET           ;clear reset
        call    lngDelay        ;wait .5secs
        setb    MS              ;set compass as slave
        ret

T2_Init:
        ;load the timer to run the compass
        clr     TR2             ;turn Timer 2 off
        mov     TH2,#0B7h       ;load the T2 high byte
        mov     TL2,#0CBh       ;load the T2 low byte
        setb    TR2             ;turn Timer 2 on
        ret

T2_ISR: 
        $INCLUDE(T2_ISR.INC)

        ;initializes the programmable counter array for the motor
        ;and the steering servo
PCA_INIT:
        mov     CMOD,#006h      ;set PCA frequency to external
        mov     CCAPM1,#42h     ;put PCA module 1 into PWM mode
        mov     CCAPM2,#42h     ;put PCA module 0 into PWM mode
        setb    CR              ;start the PCA clock
        ret

clkdelay: mov     R7,#073h
clk:      djnz    R7,clk
          ret

delay10:  mov     R6,#025h
delay:    mov     R7,#0FFh
delay1:   djnz    R7, delay1    ;wait >10 msec
          djnz    R6, delay
          ret

lngDelay:
        $INCLUDE(lngDelay.inc)

cout:
        $INCLUDE(cout.inc)

PRINT:
        $INCLUDE(PRINT.inc)

SEND_BYTE:
        $INCLUDE(SEND_BYT.inc)

pint16u:
        $INCLUDE(PINT16U.INC)



	end
