	org 	8000h
        jmp     8200h

	$MOD51GB

;Pulse width signals
PWM_MOT         EQU     R0
PWM_STR         EQU     R1

;Timer counter
CompCount      EQU     R2

;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     080H

;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

;IR Values
Threshold       EQU     100
CounterFlag     EQU     P5.7
Right           EQU     P5.6
Left            EQU     P5.5
Reverse         EQU     P5.4
Stop            EQU     P5.3

        org     802bh
        jmp     T2_ISR

        org     8100h
        db      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

        call    T2_Init         ;Initialize T2
        call    Print
        db      'Timer 2 is initialized\n',0,

        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      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
        clr     CounterFlag
        clr     Right
        clr     Left
        clr     Reverse
        mov     CompCount,#0
        setb    STP_GO           ;start motor
        call    PRINT
        db      'Forward from IR\n',0,
stp_rev2:
        jmp     Scan


Front:
        call    Print
        db      'front',0,
        clr     C             ; Clear carry bit
        mov     A,AD2         ; get the left front IR
        mov     R6,A          ; save value of Left Front
        subb    A,#Threshold  ; subtract threshold from value
        jc      L_low
        clr     c             ;left IR > threshold is right?
        mov     A,AD3
        mov     R7,A          ;save the value of Right Front
        subb    a,#Threshold
        jc      VeerRight     ;right is < threshold so veer right

L_low:  clr     C             ;clear carry
        mov     A,R7          ;get the right front IR
        subb    A,#Threshold  ;subtract threshold from value
        jc      Scan         ;jump if both are lower than threshold
VeerLeft:
        mov     A,R7          ;put right IR in A
        clr     c
        subb    A,R6          ;Right_IR - Left_IR -> A
        mov     R6,#5        ;if difference between the two is
        subb    A,R6          ;less than 10 -> backup
        jc      for2rev
        mov     CCAP1H,#Full_Left
        call    Print
        db      'VeerLeft',0,
        setb    CounterFlag
        setb    Left
        jmp     Scan

VeerRight:
        mov     A,R6          ;put Left IR value in A
        clr     c
        subb    A,R7          ;Left_IR - Right_IR -> A
        mov     R7,#5        ;if difference between the two is
        subb    A,R7          ;less than 10 -> backup
        jc      for2rev
        mov     CCAP1H,#Full_Right
        call    Print
        db      'VeerRight',0,
        setb    CounterFlag
        setb    Right
        jmp     Scan

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    CounterFlag
        setb    Reverse
        setb    STP_GO                  ;start motor
        call    PRINT
        db      'Backing from IR\n',0,
stp_rev0:
        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

wait:   jmp     wait

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: push    Acc
        push    PSW
        push    07
        push    06
        push    05
        push    04
        push    03
        push    02
        push    01
        push    00

        clr     TF2             ;Clear the T2 interrupt flag
        clr     TR2             ;Stop the timer
        mov     TH2,#0B7h       ;Load the timer with the value
        mov     TL2,#0CBh       ;needed for a 10msec interval
        setb    TR2             ;turn timer back on
        jnb     CounterFlag,end_isr
        inc     CompCount       ;Increment the process counter
        clr     c
        mov     A,CompCount
        subb    A,#200
        jc      end_isr
        jnb     Right,isr1
        mov     CCAP1H,Straight
;        call    Print
;        db      'Straight by timer',0,
        mov     CompCount,#0
        clr     Right
        clr     CounterFlag
        jmp     end_isr

isr1:   jnb     Left,isr2
        mov     CCAP1H,Straight
;        call    Print
;        db      'Straight by timer',0,
        mov     CompCount,#0
        clr     Left
        clr     CounterFlag
        jmp     end_isr

isr2:   jnb     Reverse,end_isr
        jnb     STP_GO,stp_rev3  ;already stopped
        jnb     FOR_REV,stp_rev3 ;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
        clr     CounterFlag
        clr     Right
        clr     Left
        clr     Reverse
        mov     CompCount,#0
        setb    STP_GO           ;start motor
;        call    PRINT
;        db      'Forward from IR\n',0,
stp_rev3:
        jmp     Scan

        setb    STP_GO           ;start motor
end_isr:
        pop     00
        pop     01
        pop     02
        pop     03
        pop     04
        pop     05
        pop     06
        pop     07
        pop     PSW
        pop     Acc
        reti

        ;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)



	end
