Specifications

4.6.7 COMP Compare Subroutine

The entire purpose of the COMP subroutine is to compare the speeds of each motor and

ensure they run in parallel. This ensures straight line driving which was one of the

major project guidelines. This function is called by the main loop every time the

odometer for motor A reaches its defined value. When this occurs, it writes a new line

through the serial port then compares ODMA (odometer of motor A) with ODMB

(odometer of motor B). If ODMA is larger than ODMB then motor A is spinning too

fast, therefore the subroutine branches to the label ‘A2FAST’ where the motor speeds

are adjusted. Likewise if ODMB is larger than ODMA the subroutine branches to the

label ‘B2FAST’.

A2FAST works on a simple rule. Before simply reducing the duty cycle in the PWDTY

Without this check, the robot would eventually slow to a stop. If the duty cycle is lower

than this threshold, in this case $65, instead of slowing Motor A it will instead increase

the speed of motor B slightly. Likewise the routine B2FAST works on the same

threshold, if the duty cycle drops below the threshold, it will speed motor A instead of

slowing motor B. Of course if the duty cycle is above the threshold it will simply

reduce the speed of the respective motor.

;*************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)

LDAA PWDTY2 ;LDAA SPEED OF MOTOR A

CMPA #$65 ;IF MOTOR A IS SLOWER THAN $65

BMI FASTB ;SPEED UP MOTOR B