Specifications

81
LDAA PWDTY2 ;LDAA SPEED OF MOTOR A
CMPA #$65 ;IF MOTOR A IS SLOWER THAN $C5
BMI FASTB ;SPEED UP MOTOR B
SUBA #$01 ;IF MOTOR A IS FASTER THAN $C5
STAA PWDTY2 ;SLOW MOTOR A SLIGHTLY
BRA DONE ;EXIT
FASTB LDAA PWDTY0 ;|
ADDA #$01 ;|SPEED B UP ONE
STAA PWDTY0 ;|
BRA DONE
;--------------------------------------
B2FAST JSR NEWLINE ;CASE MOTOR B IS FASTER THAN A
LDAA #'b'
JSR TXBYTE ;TX BYTE THROUGH SERIAL (DEBUG)
LDAA PWDTY0 ;LDAA WITH SPEED OF MOTOR B
CMPA #$65 ; |
BMI FASTA ; | IF MOTOR B GOING TOO SLOW, SPD
UP A
SUBA #$01 ;|
STAA PWDTY0 ;|SLOW MOTOR B SLIGHTLY
BRA DONE
FASTA LDAA PWDTY2 ;|
ADDA #$01 ;| SPEED A UP BY ONE
STAA PWDTY2 ;|
DONE MOVB #$00, COUNT ;|
MOVB #$00, ODMA ;| CLEAR COUNTERS
MOVB #$00, ODMB ;|
RTS ;RETURN AND WAIT FOR NEXT
COMPARE
;*****************KEY WAKE UP PORT G ***************************************
;ADDS 1 TO COUNTER FOR CORRESPONDING MOTOR
;****SENSOR G 0************
KWGISR BRCLR KWIFG,$01,NOTG0 ;CHECK BIT 0
LDAA ODMB ;IF MOTOR B, LDA WITH ODOM COUNT
B
ADDA #$01 ;ADD 1 TO ODMB COUNT
STAA ODMB ;STORE BACK TO ODMB