Specifications

;JSR TXBYTE ;(DEBUG)

;MOVB #$00,PWDTY0 ;|

;MOVB #$00,PWDTY2 ;|STOP MOTORS (DRIVING FORWARDS)

;JSR DELAY ;WAIT DELAY PERIOD

;MOVB #$A0,PWDTY1 ;|

;MOVB #$A0,PWDTY3 ;|REVERSE STRAIGHT

WFCLR BRCLR PORTA,$02,OUTL ;IF LEFT NOT SET BRA TO OUTL

;BRCLR PORTA,$04,OUTR ;IF RIGHT NOT SET BRA TO OUTR

BRA WFCLR ;IF L&R SET, BRA WFCLR. WAIT FOR L

OR R TO CLEAR

OUTL JSR TURNL ;TURN L WHEN L SENSOR IS CLEAR

OUTR JSR TURNR ;TURN R WHEN R SENSOR IS CLEAR

JSR DELAY

RTS ;RETURN TO MAIN LOOP

;*************DELAY*********************************************************

;CODE TO ADD A DELAY TO EASE THE SWITCHING OF MOTORS

;GIVES APPROX 2 SEC DELAY. CAN SHORTEN TIME BY INCREASING INITIAL Y VALUE

DELAY LDY #$FFDC ;INITIAL VALUE FOR Y

DELA LDX #$0000 ;INITIAL CONDITION OF X

DEL INX ;INCREMENT X BY 1

BNE DEL ;IF Z NOT = 0 BRANCH TO DELAY

INY

BNE DELA

RTS

;*************COMPARE SPEEDS**************************************************

;CODE TO COMPARE MOTOR A AND B SPEEDS AND CONTROL H BRIDGE

COMP JSR NEWLINE ;START NEW LINE ON SERIAL PORT

LDAA ODMA ;LOAD MOTOR A COUNT INTO ACC A

CMPA ODMB ;COMPARE MOTOR A WITH MOTOR B

BPL A2FAST ;IF MOTOR A FASTER THAN MOTOR B

BMI B2FAST :IF MOTOR A SLOWER THAN MOTOR B

;-------------------------------------

A2FAST JSR NEWLINE ;CASE MOTOR A IS FASTER THAN B

LDAA #'a'

JSR TXBYTE ;TX BYTE THROUGH SERIAL (DEBUG)