BIT1    EQU     %00000010
BIT0    EQU     %00000001
BIT4	EQU	%00010000
BIT6	EQU	%01000000
INV5	EQU	%11011111
INV6	EQU	$10111111

STACK   EQU     $0041
BASE    EQU     $1000
TOC1   	EQU	$1016
TOC2    EQU     $1018    
TOC3	EQU	$101A
TOC4	EQU	$101C
TCTL1   EQU     $1020    
TCTL2	EQU	$1021
TMSK1   EQU     $1022   
TFLG1   EQU     $1023    
TFLG2	EQU	$1025
TMSK2   EQU     $1024    
BAUD    EQU     $102B
SCCR1   EQU     $102C
SCCR2   EQU     $102D
SCDR    EQU     $102F
SCSR    EQU     $102E
TCNT    EQU     $100E
PACTL	EQU	$1026 
PORTD	EQU	$1008 
DDRD	EQU	$1009

SENSOR	EQU	$6000

LIGHTS	EQU	$7000
ON	EQU	$10101010
OFF	EQU	$00000000

MOT_S	EQU	$6000
FFD	EQU	%11110000
REAR_M	EQU	%11111100
RL_M	EQU	%11111110
RR_M	EQU	%11111101
FRONT_M	EQU	%11110011
FL_M	EQU	%11111011
FR_M	EQU	%11110111
LIFT_M	EQU	%11101111
SHIFT_M	EQU	$11011111
***************************************************
* GLOBAL VARIABLES
***************************************************
	ORG	$C000
ON_M	RMB	2	; ON TIME OF MOTOR
OFF_M	RMB	2	; OFF TIME OF MOTOR
*			; ON+OFF=$9800 FOR 51.4 Hz
SWITCH2	RMB	1 	; USE TO SWITCH BETWEEN ON AND OFF
SWITCH3 RMB	1	
***************************************************
* LOCATOR TABLE
*		SPEED		DIRECTION
* MOTOR1	PA6(OC2)	PD4	
* MOTOR2	PA5(OC3)	PD5
***************************************************

*--------------------------------------------------
* INTERRUPT VECTORS
*--------------------------------------------------  

        ORG     $00DC
        JMP     MOTOR1

        ORG     $00D9
        JMP     MOTOR2

*        ORG     $FFE4                   ;SIM11
*        FDB     OC3_ISR

*        ORG     $FFE6                   ;SIM11
*        FDB     OC2_ISR
        
***************************************************
* MAIN PROGRAM
***************************************************
        ORG     $8000
	LDS	#STACK
	LDX	#BASE
*--------------------------------------------------
* INITIALIZE
*--------------------------------------------------
* TURN OFF ALL MOTORS
	LDAA	#$FF		
	STAA	MOT_S

* SET PORTD TO OUTPUTS		
	LDAA	#$FF
	STAA	DDRD

* OC2 AND OC3 OUTPUT CLEARED (MOTOR DRIVERS SPEED)
        LDAA    #%10100000      
	STAA	TCTL1

* SET MOTOR DIRECTIONS (?BIT5=LEFT ?BIT4=RIGHT)
	LDAA	#%00	;00110000
	STAA	PORTD

* SET ON AND OFF TIMES AND SWITCH
	LDD	#$8C00		; SET ON TIME  
	STD	ON_M	
	LDD	#$1000		; SET OFF TIME 
	STD	OFF_M
	LDAA	#$01		; INITIALIZE SWITCH TO 1
	STAA	SWITCH2
	STAA	SWITCH3

*--------------------------------------------------
* INTERRUPT ENABLES SETTINGS
*--------------------------------------------------
* ENABLE INTERRUPTS	
        LDAA    #%01100000      ;ENABLES OC2 AND OC3 INTERRUPT (MOTOR DRIVERS)
	STAA	TMSK1       

        CLI			;ENABLE INTERRUPTS
***************************************************
* MAIN BODY
***************************************************
*--------------------------------------------------
* DRIVE FORWARD
*--------------------------------------------------
	LDAA	#$00
*AGAIN	LDAA	#REAR_M	; TURN ON REAR MOTORS ONLY
	STAA	MOT_S
HERE    BRA     HERE

* CONTROL SPEED AND DIRECTION
POLL	LDAA	SENSOR		; POLL FRONT FOR HIT
	ORAA	%00000011
	BEQ	POLL
	LDAA	SENSOR		; CHECK FRONT LEFT
	ORAA	%00000001
	BEQ	RIGHT
	LDAA	#FR_M		; HIT ON LEFT FRONT TURN ON RIGHT REAR MOTOR ONLY
	STAA	MOT_S	
POLL2	LDAA	SENSOR
	ORAA	%00000010	; CHECK FRONT RIGHT
	BEQ	POLL2
	JMP	LIFT
RIGHT	LDAA	#FL_M		; HIT ON RIGHT FRONT TURN ON LEFT REAR MOTOR ONLY
	STAA	MOT_S
POLL3	LDAA	SENSOR
	ORAA	%00000001	; CHECK FRONT LEFT
	BEQ	POLL3
	JMP	LIFT
*--------------------------------------------------
* LIFTING BODY FRONT
*--------------------------------------------------
LIFT	LDAA	#LIFT_M		; LIFT FRONT
	STAA	MOT_S
*CONTROL SPEED AND DIRECTION
POLL4	LDAA	SENSOR
	ORAA	%00100011	; CHECH FOR MAX, LEFT FRONT AND RIGHT FRONT
	BEQ	POLL4
*DELAY 
*--------------------------------------------------
* ELEVATED DRIVE
*--------------------------------------------------
	LDAA	#REAR_M		; REAR WHEEL DRIVE FORWARDS
	STAA	MOT_S	
*CONTROL SPEED AND DIRECTION
POLL5	LDAA	SENSOR
	ANDA	%00001100	; CHECK REAR LEFT AND RIGHT FOR RELEASE
	BNE	POLL5
*DELAY
*--------------------------------------------------
* SHIFT WEIGHT FORWARD
*--------------------------------------------------
	LDAA	#SHIFT_M		; SHIFRT WEIGHT FORWARD
	STAA	MOT_S
*CONTROL SPEED AND DIRECTION
POLL6	LDAA	SENSOR		; CHECK IF AT FRONT
	ORAA	%10000000
	BEQ	POLL6	
*--------------------------------------------------
* LIFTING BODY REAR
*--------------------------------------------------
	LDAA	#LIFT_M		; LIFT REAR
	STAA	MOT_S
*CONTROL SPEED AND DIRECTION
POLL7	LDAA	SENSOR		; CHECK IF DONE
	ORAA	%00010000
	BEQ	POLL7
*--------------------------------------------------
* DRIVE ONTO STEP
*--------------------------------------------------
	LDAA	#FFD		; 4-WHEEL DRIVE
	STAA	MOT_S
*CONTROL SPEED AND DIRECTION
* DELAY UNTIL ON STAIR
	JMP	AGAIN
***************************************************
* INTERRUPT SERVICE ROUTINES
***************************************************
*--------------------------------------------------
* MOTOR 1
*--------------------------------------------------   
MOTOR1	LDX   	#BASE       
	BCLR	TFLG1-BASE,X INV6 	;CLEAR FLAG 
	LDAA	SWITCH2			; IS SWITCH2 ON OR OFF
	BEQ	SET2			; IF OFF SET
CLEAR2	LDD	TOC2			; 
	ADDD	OFF_M
	STD	TOC2			; ADD ON TIME TO TOC2
	BCLR	TCTL1-BASE,X BIT6	; OUTPUT=0
	LDAA	#$00
	STAA	SWITCH2			; TURN SWITCH2 OFF
	BRA	RT_OC2
SET2	LDD	TOC2
	ADDD	ON_M 
	STD	TOC2			; ADD OFF TIME TO TOC2 
	BSET	TCTL1-BASE,X BIT6	; OUTPUT=1   
	LDAA	#$01			; TURN SWITCH2 ON
	STAA	SWITCH2
RT_OC2	RTI
*--------------------------------------------------
* MOTOR 2                 
*--------------------------------------------------
MOTOR2  LDX	#BASE        
        BCLR    TFLG1-BASE,X INV5  	;CLEAR FLAG 
	LDAA	SWITCH3			; IS SWITCH ON OR OFF
	BEQ	SET3			; IF OFF SET
CLEAR3	LDD	TOC3			; 
	ADDD	OFF_M
	STD	TOC3			; ADD ON TIME TO TOC3
	BCLR	TCTL1-BASE,X BIT4	; OUTPUT=0
	LDAA	#$00
	STAA	SWITCH3			; TURN SWITCH3 OFF
	BRA	RT_OC3
SET3	LDD	TOC3
	ADDD	ON_M 
	STD	TOC3			; ADD OFF TIME TO TOC3 
	BSET	TCTL1-BASE,X BIT4	; OUTPUT=1  	
	LDAA	#$01			; TURN SWITCH3 ON
	STAA	SWITCH3     
RT_OC3   RTI
****************
