        org     000h


	$MOD51GB

;Pulse width signals
PWM_MOT         EQU     R0
PWM_STR         EQU     R1

;Motor Direction
STP_GO          EQU     P5.1
FOR_REV         EQU     P5.0

;Speeds
FWD_LO          EQU     0C0H
FWD_MED         EQU     0A0H
FWD_HI          EQU     080H
REV_LO          EQU     0B0H

;Steering Directions
Straight        EQU     03CH
Full_Left       EQU     031H
Half_Left       EQU     037H
Qtr_Left        EQU     03AH
Full_Right      EQU     04BH
Half_Right      EQU     043H
Qtr_Right       EQU     039H



;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


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

        setb    STP_GO
Scan: call    lngDelay

L_Rear: 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

        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\n',0,
stp_rev2:
        jmp     Scan


L_Front:
        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     forward

R_Front:
        clr     C             ; Clear carry bit
        mov     A,AD3         ; get the left front IR
        subb    A,#100        ; subtract threshold from value
        jc      Sonar         ; jump if not too close

forward:
        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\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
        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

        mov     PWM_MOT,#FWD_MED  ;set motor for Medium speed
        mov     CCAP2h,PWM_MOT
        jmp     Scan

Ahd_slow:
        clr     C
        mov     A,AD0           ; Get the sonar value
        subb    A,#Sonar_3      ; subtract threshold from value
        jc      All_Stop        ; jump if distance is < 3 ft
        
        mov     PWM_MOT,#FWD_LO  ;set motor for Low speed
        mov     CCAP2h,PWM_MOT
        jmp     Scan

All_Stop:
        clr     Stp_Go                  ;Stop the motor
        setb    FOR_REV                 ;reverse it
        mov     PWM_STR,#Full_Right     ;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\n',0,
        jmp     Scan





wait:   jmp     wait

	

        ;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

lngDelay:
        $INCLUDE(lngDelay.inc)

cout:
        $INCLUDE(cout.inc)

PRINT:
        $INCLUDE(PRINT.inc)

SEND_BYTE:
        $INCLUDE(SEND_BYT.inc)



	end
