*Author: John Ferrara
*Title:  Robot Control 2.0 (RC10.asm)

ADCTL	EQU	$1030
ADR1	EQU	$1031
ADR2    EQU     $1032
ADR3    EQU     $1033
ADR4    EQU     $1034
TCNT    EQU     $100E  ; TCNT High byte
TFLG2   EQU     $1025  ; Contains RTIF flag
TMSK2   EQU     $1024    ; RTII enable flag
PACTL   EQU     $1026  ; RTI Timer control
BAUD    EQU     $102B  ; BAUD rate control register to set the BAUD rate
SCCR1   EQU     $102C  ; Serial Communication Control Register-1
SCCR2   EQU     $102D  ; Serial Communication Control Register-2
SCSR    EQU     $102E  ; Serial Communication Status Register
SCDR    EQU     $102F  ; Serial Communication Data Register
TOC2    EQU    $1018
TOC3    EQU     $101A
PORTA   EQU     $1000
PORTD   EQU     $1008
DDRD    EQU     $1000
TCTL1      EQU     $1020
TMSK1	EQU	$1022
TFLG1	EQU	$1023
OPTION	EQU	$1039
BASE	EQU	$1000
BIT76   EQU     %11000000
BIT54	EQU	%00110000
BIT7    EQU     %10000000
BIT6    EQU     %01000000
BIT5	EQU	%00100000
BIT4	EQU	%00010000
BIT3    EQU     %00001000
BIT1    EQU    %00000010
BIT2    EQU     %00000100
INV5	EQU	%11011111

EOS     EQU     $04    ; User-defined End Of String (EOS) character
CR      EQU     $0D    ; Carriage Return Character
LF      EQU     $0A    ; Line Feed Character
ESC     EQU     $1B    ; Escape Charracter
*
************************************************************************
* Initialize Interrupt Jump Vectors
************************************************************************
*
*RTI-INTERRUPT: USED FOR MULTITASKING

        ORG      $00EB
        JMP      RTI_ISR

*OC2-INTERRUPT: USED FOR SERVO CONTROL

	ORG     $00DC
	JMP     OC2_ISR         


*OC3-INTERRUPT: USED FOR MOTOR SPEED CONTROL

        ORG     $00D9
        JMP     OC3_ISR

*IRQ-INTERRUPT: USED FOR THE "SOFT" RESET OF THE EVBU

        ORG     $00EE
        JMP     IRQ_ISR

*
************************************************************************
* Define Strings and Reserve Variable memory space for system use
*   such as CPT, DSPT, CurPID, etc.
********************************************************************
        ORG $8000

*MULTITASKING VARIABLES***********************************************************
CPT	RMB 2
	RMB 2 
	RMB 2
	RMB 2
	RMB 2 
	RMB 2
	RMB 2
	RMB 2
        RMB 2
        RMB 2

DSPT    FDB $80FF
        FDB $81FF
        FDB $82FF
        FDB $83FF
        FDB $84FF
        FDB $85FF
        FDB $86FF
        FDB $87FF
        FDB $88FF
        FDB $89FF

MaxProc FCB $09
CurrPID	RMB 1
temp    FCB $00
TEMP2	RMB 2
STACK	RMB 2

*MOTOR-CONTROL VARIABLES***********************************************************

OC3Period       FDB     10000   ;20ms period for the servo control pulses
OC3HIGH         RMB     2
OC3LOW          RMB     2

OC3_CHANGE	RMB	2
SPEED_BOOST     FDB     6000
DIRECTION	RMB	1
DIR_PREV        RMB     1

*SERVO AND OBSTACLE AVOIDANCE VARIABLES***********************************************

OC2Period	FDB	40000	;20ms period for the servo control pulses
OC2HIGH		RMB	2
OC2LOW		RMB	2
OC2_CHANGE	RMB	2

IR_TABLE
RF_IR		RMB	1
LF_IR		RMB	1
MF_IR           RMB     1
TRF_IR		RMB	1
TLF_IR		RMB	1

CURR_MAX	RMB	1
MAX_IR		RMB	1

RF_INIT_IR	RMB	1
LF_INIT_IR      RMB     1
MF_INIT_IR      RMB     1
TRF_INIT_IR	RMB	1
TLF_INIT_IR	RMB	1

RF_OLD          RMB     1
LF_OLD          RMB     1

*FUZZY LOGIC TABLES***********************************************

T_INDEX         RMB     1
TURN_FOUND      RMB     2

DIFF_IR         FCB     4
                FCB     7
                FCB     13
                FCB     20

TURN_SMOOTH     FDB     200
                FDB     300
                FDB     500
                FDB     600

DIFF_SPEED	FCB	85
		FCB	95
		FCB	105
		FCB	115

SMOOTH_SPEED
		FDB	4500
		FDB	3500
		FDB	2700
		FDB	2000

*cOMMAND ARBITRATOR VARIABLES***********************************************

EXEC_HORN	RMB	1
EXEC_AVOID 	RMB	1
EXEC_LANE	RMB	1

*LANE FOLLOWING SENSORS***********************************************

RIGHT_LIGHT     RMB     1
LEFT_LIGHT      RMB     1
LEFT_INIT_L     RMB     1
RIGHT_INIT_L    RMB     1

RIGHT           RMB     1
LEFT            RMB     1
RL_OLD		RMB	1
LL_OLD		RMB	1

*RESET VARIABLES:"SOFT" REBOOT***********************************************
STARTUP         RMB     1

****************************************************************************
****************************************************************************

        ORG     $9000
SOFT_RESET
        LDS     #$41  ;Initialize Stack Pointer
	LDAA	#0
	STAA	STARTUP
	
        JSR     INIT_MULTITASK
        TYS
        JSR     InitRTI

        JSR     INIT_DIRECTION
        JSR     INIT_OC

        CLI

	BRA	WAIT_START

STOP_RESET
	LDS	#$41
	
WAIT_START
	LDAA	STARTUP
	CMPA	#1
	BNE	WAIT_START
        
Main    LDS     #$0041  ;Initialize Stack Pointer
	SEI

        JSR     INIT_ANALOG
	JSR	INIT_PORTS

        JSR     INIT_IR
	JSR	INIT_SPEED
        JSR     INIT_DIRECTION
        JSR     INIT_LIGHTS
        JSR     INIT_MULTITASK

        TYS     ;initialize stack pointer

        LDX     #CONTROL_RESTART
        JSR     Spawn

        LDX     #READ_SENSORS
        JSR     Spawn

*        LDX     #ARBITRATE_MOTOR
*        JSR     Spawn
        
        LDX     #CONTROL_MOTOR
        JSR     Spawn

        LDX     #CONTROL_SERVO
        JSR     Spawn

        LDX     #AVOID_OBSTACLES
        JSR     Spawn

	LDX	#HEINOUS_NOISE
	JSR	Spawn

        LDX     #FOLLOW_LANE
        JSR     Spawn

 	CLI

        LDAA    #1
        STAA    EXEC_AVOID
        STAA    EXEC_HORN
        LDAA    #0
        STAA    EXEC_LANE

h       bra     h

Command_Arbit 
        LDAA    #0
        STAA    EXEC_AVOID
        STAA    EXEC_HORN

        LDAA    #1
        STAA    EXEC_LANE                       
        LDD     #2000
        STD     OC3_CHANGE

        JSR     Delay

R_1    
        LDAA    RIGHT_LIGHT
        ANDA    #$80
        BNE     R_1

        LDAA    LEFT_LIGHT
        ANDA    #$80
        BNE     R_1

        LDAA    #1
        STAA    EXEC_AVOID
        STAA    EXEC_HORN
        LDAA    #0
        STAA    EXEC_LANE

        LDD     #2200
        STD     OC3_CHANGE

        JSR     Delay

R_2     
        LDAA    RIGHT_LIGHT
        ANDA    #$80
        BNE     R_2

        LDAA    LEFT_LIGHT
        ANDA    #$80
        BNE     R_2

        LDAA    #0
        STAA    EXEC_AVOID
        STAA    EXEC_HORN

        LDAA    #1
        STAA    EXEC_LANE                       

E_DEMO
        LDAA    RIGHT_LIGHT
        ANDA    #$80
        BNE     E_DEMO

        LDAA    LEFT_LIGHT
        ANDA    #$80
        BNE     E_DEMO

        SWI


********************************************************************
********************************************************************
*MULTITASKING PROCESSES***************************************
********************************************************************

********************************************************************
*SUBROUTINE: CONTROL_RESTART
*PURPOSE: CONTROLS THE SOFTWARE RESTART OF THE PROCESSOR
*********************************************************************
CONTROL_RESTART
	LDAA	STARTUP
	CMPA	#1
	BEQ	END_RESTART

        JSR     INIT_MULTITASK
        JMP	STOP_RESET

END_RESTART
	BRA	CONTROL_RESTART

********************************************************************
*SUBROUTINE: READ_SENSORS
*PURPOSE: READS THE ANALOG PORTS CONTAINING IR_SENSORS
*********************************************************************

READ_SENSORS
	LDAA    #$30	;FIRST, READ THE FIRST FOUR ANALOG PORTS(ALL IR)
	STAA	ADCTL

WAIT_SENS
	LDAA	ADCTL	;WAIT FOR 4-CONVERSIONS TO COMPLETE
	ANDA	#BIT7
	BEQ	WAIT_SENS

        LDAA    #$30    ;CLEAR THE CONVERSIONS FLAG
	STAA	ADCTL

        LDAA    RF_IR
        STAA    RF_OLD

        LDAA    ADR1    ;GET THE SENSOR READINGS AND PUT INTO GLOBAL
	STAA	RF_IR	;VARIABLES

        LDAA    LF_IR
        STAA    LF_OLD

        LDAA    ADR2
	STAA	LF_IR

        LDAA    ADR3
        STAA    MF_IR

	LDAA	ADR4
	STAA	TRF_IR

	LDAA	#$34
	STAA	ADCTL

*NOW, GET THE NEXT FOUR ANALOG PORT READINGS

WAIT_SENS2
	LDAA	ADCTL	;WAIT FOR 4-CONVERSIONS TO COMPLETE
	ANDA	#BIT7
	BEQ	WAIT_SENS2

        LDAA    #$34    ;CLEAR THE CONVERSIONS FLAG
	STAA	ADCTL

        LDAA    ADR1    ;GET THE SENSOR READINGS AND PUT INTO GLOBAL
	STAA	TLF_IR	;VARIABLES
	
        LDAA    ADR2
	LDAB	RIGHT_LIGHT
	STAB	RL_OLD
	STAA	RIGHT_LIGHT

        LDAA    ADR3
	LDAB	LEFT_LIGHT
	STAB	LL_OLD
        STAA    LEFT_LIGHT

	JMP	READ_SENSORS


********************************************************************
*SUBROUTINE: FOLLOW_LANE
*PURPOSE: FOLLOWS THE YELLOW LANE
*********************************************************************
FOLLOW_LANE
        LDAA    EXEC_LANE
        CMPA    #1
        BNE     FOLLOW_LANE

        LDAA    RIGHT_LIGHT
        SUBA    LEFT_LIGHT
        CMPA    #7
        BGT     LEFT_LANE

        LDAA    LEFT_LIGHT
        SUBA    RIGHT_LIGHT
        CMPA    #7
        BGT     RIGHT_LANE

        BRA     BETWEEN_LANES

RIGHT_LANE
        LDD     #3600
        STD     OC2_CHANGE
        JMP     FOLLOW_LANE

LEFT_LANE
        LDD     #2400
        STD     OC2_CHANGE
        JMP     FOLLOW_LANE

BETWEEN_LANES
        LDD     #3000
        STD     OC2_CHANGE
        JMP     FOLLOW_LANE

********************************************************************
*SUBROUTINE: FOLLOW_HUMAN
*PURPOSE: FOLLOWS A HUMAN WITH AN IR EMITTER
*********************************************************************
FOLLOW_HUMAN
        LDAA    TRF_IR
        CMPA    TLF_IR
        BGT     HUMAN_RIGHT

HUMAN_LEFT
        LDD     #3400
        STD     OC2_CHANGE
        BRA     FOLLOW_HUMAN

HUMAN_RIGHT
        LDD     #2600
        STD     OC2_CHANGE
        BRA     FOLLOW_HUMAN

********************************************************************
*SUBROUTINE: FIND_LIGHT
*PURPOSE: MOVES INTO THE LIGHT, CARROL-ANNE
*********************************************************************
FIND_LIGHT
        LDAA    RIGHT_LIGHT
        LSRA
        LDAB    LEFT_LIGHT
        LSRB
        SBA
        BGT     LIGHT_RIGHT

LIGHT_LEFT
        LDD     #3400
        STD     OC2_CHANGE
        BRA     END_LIGHT

LIGHT_RIGHT
        LDD     #2600
        STD     OC2_CHANGE

END_LIGHT
        JMP     FIND_LIGHT

********************************************************************
*SUBROUTINE: AVOID_OBSTACLES
*PURPOSE: AVOIDS ONCOMING CARS
*********************************************************************
AVOID_OBSTACLES

	LDAA    EXEC_AVOID
        CMPA    #1
        BNE     AVOID_OBSTACLES

*        LDAA    PORTD
*        ANDA    #BIT3
*        BNE     GO_REVERSE_LI

	LDAA	RF_IR
        CMPA    #125
        BGT     GO_REVERSE

        LDAA    LF_IR
        CMPA    #125
        BGT     GO_REVERSE

        LDAA    MF_IR
        CMPA    #125
        BGT     GO_REVERSE

        LDAA    #0
        STAA    DIRECTION

        LDAA    RF_IR
        CMPA    RF_OLD  
        BGT     NEED_TURN

        LDAA    LF_IR
        CMPA    LF_OLD 
        BGT     NEED_TURN

        LDAA    RF_IR
        CMPA    RF_INIT_IR + 5
        BGT     NEED_TURN

        LDAA    LF_IR
        CMPA    LF_INIT_IR + 5
        BGT     NEED_TURN


        BRA     STRAIGHT

NEED_TURN
        LDD     #2500
        STD     OC3_CHANGE

        LDAA    RF_IR
        CMPA    LF_IR 
        BGT     TURN_LEFT

        LDAA    LF_IR
        CMPA    RF_IR + 3
        BGT     TURN_RIGHT

        JMP     AVOID_OBSTACLES
                            
GO_REVERSE       
        LDAA    #1
        STAA    DIRECTION

        LDAA    RF_IR
        CMPA    LF_IR
        BGT     REV_LEFT

REV_RIGHT
        LDD     #3300
        STD     OC3_CHANGE
        BRA     END_REVERSE

REV_LEFT
        LDD     #2700
        STD     OC2_CHANGE

END_REVERSE
        JSR     HONK_DELAY
        JMP     AVOID_OBSTACLES
                
STRAIGHT
        LDD     #3500
        STD     OC3_CHANGE
        
	LDD	#3000
	STD	OC2_CHANGE
        JMP     END_AVOID

TURN_LEFT
        LDAA    RF_IR
        SUBA    LF_IR
        JSR     SEARCH_TABLE_IR

        LDD     #3000
        SUBD    TURN_FOUND

        STD     OC2_CHANGE

        BRA     END_AVOID

TURN_RIGHT
        LDAA    LF_IR
        SUBA    RF_IR
        JSR     SEARCH_TABLE_IR

        LDD     #3000
        ADDD    TURN_FOUND

        STD     OC2_CHANGE
        BRA     END_AVOID

END_AVOID
	JMP	AVOID_OBSTACLES

********************************************************************
*SUBROUTINE: HEINOUS_NOISE
*PURPOSE: HONKS THE CAR HORN
*********************************************************************
HEINOUS_NOISE
	
  	LDAA    EXEC_HORN
        CMPA    #1
        BNE     HEINOUS_NOISE

        LDAA    RF_IR
        CMPA    #100
        BGT     HONK

        LDAA    LF_IR
        CMPA    #100
        BGT     HONK

        BRA     HEINOUS_NOISE

HONK
        LDAA    #$F1
        STAA    $7000

        JSR     HONK_DELAY

        LDAA    #$F0
        STAA    $7000

        JSR     HONK_DELAY

        LDAA    #$F1
        STAA    $7000

        JSR     HONK_DELAY

        LDAA    #$F0
        STAA    $7000
        
        JMP     HEINOUS_NOISE

********************************************************************
*SUBROUTINE: CONTROL_MOTOR
*PURPOSE: aDJUSTS THE SPEED OF THE MOTOR
*********************************************************************

CONTROL_MOTOR
	LDD	OC3_CHANGE
	STD	OC3HIGH
	
	LDD	OC3Period
	SUBD	OC3HIGH
	STD	OC3LOW

	LDAA	DIRECTION
	CMPA	#0
	BEQ	FORWARD

REVERSE	
	LDAA	PORTD	
	ORAA	#BIT2
	STAA	PORTD	
        BRA     END_MOTORCONTROL

FORWARD
	LDAA	PORTD
	ANDA	#$FB
	STAA	PORTD

END_MOTORCONTROL
	JMP	CONTROL_MOTOR

********************************************************************
*SUBROUTINE: ARBITRATE_MOTOR
*PURPOSE: CONTROLS THE TRANSITION SPEED OF THE MOTOR
*********************************************************************
ARBITRATE_MOTOR
	LDAA	EXEC_LANE
	CMPA	#1
	BNE	CHANGE_SPEED
	
	LDD	#2000
	STD	OC3_CHANGE
	BRA	ARBITRATE_MOTOR	

CHANGE_SPEED
	LDAA	DIRECTION
	CMPA	DIR_PREV
	BEQ	SAME_DIR

	LDD	SPEED_BOOST
	STD	OC3_CHANGE
	JSR	HONK_DELAY

SAME_DIR
        LDAA    DIRECTION
	STAA	DIR_PREV
	
	LDAA	#3
	LDY	#MAX_IR
	LDX	#IR_TABLE
	JSR	FIND_MAX

	LDAA	MAX_IR
	LDX	#DIFF_SPEED
	LDY	#SMOOTH_SPEED
        JSR     SEARCH_TABLE_MOT
	
	CMPA	#1
	BNE	SLOWEST_SPEED
	
	STX	OC3_CHANGE
	BRA	END_ARBMOT
	
SLOWEST_SPEED
	LDX	#1900
	STX	OC3_CHANGE	

END_ARBMOT
	JMP	ARBITRATE_MOTOR


********************************************************************
*SUBROUTINE: CONTROL_SERVO
*PURPOSE:ADJUSTS THE POSITION OF THE SERVO
*********************************************************************

CONTROL_SERVO
	LDD	OC2_CHANGE
	STD	OC2HIGH
	
	LDD	OC2Period
	SUBD	OC2HIGH
	STD	OC2LOW

	JMP	CONTROL_SERVO

******************************INITIALIZATION CODES*******************************8

********************************************************************
*SUBROUTINE: INIT_MULTITASK
*PURPOSE: INITIALIZES THE CPT AND STACK POINTERS
*********************************************************************
	
INIT_MULTITASK
        LDAB    #$00
        LDX     #CPT  
        LDAA    #$0A

Zero    LDY     #$0     ;this code zeros-out the CPT(table)
	STY	0,X
	INX
        INX
        INCB
	CBA
	BNE	Zero

	LDAA	#$00	;initialize the starting PID		
	STAA	CurrPID
		
	LDX	#DSPT
	LDY	0,X
	
	LDX     #CPT	;setup the initial stack pointer for the first
        STY     0,X	;...process
        INY

 	LDAA    #BIT6	;clear the RTI-flag so the next process is not
        STAA    TFLG2	;immediately interruted
                     
	RTS

********************************************************************
*SUBROUTINE: SET_ANALOG
*PURPOSE: CONFIGURES THE A/D SYSTEM FOR LATER USE
*********************************************************************

INIT_ANALOG
	LDAA	#$80	;POWER-UP A/D-SYSTEM
	STAA	OPTION

	LDAA	#40	
WAIT_AN
        DECA            ;WAIT FOR CHARGE PUMP TO STABILIZE
        BNE     WAIT_AN

	RTS	

********************************************************************
*SUBROUTINE: INIT_IR
*PURPOSE: SETS-UP INITIAL IR CONDITIONS (SELF-CALIBRATION)
*********************************************************************

INIT_IR
	LDAA    #$30
	STAA	ADCTL

WAIT_IR	LDAA	ADCTL	;WAIT FOR 4-CONVERSIONS TO COMPLETE
	ANDA	#BIT7
	BEQ	WAIT_IR

        LDAA    #$30    ;CLEAR THE CONVERSIONS FLAG
	STAA	ADCTL

        LDAA    ADR1    ;GET THE SENSOR READINGS AND PUT INTO GLOBAL
	STAA	RF_INIT_IR	;VARIABLES
	
        LDAA    ADR2
	STAA	LF_INIT_IR

        LDAA    ADR3
        STAA    MF_INIT_IR
	
	LDAA	ADR4
	STAA	TRF_INIT_IR

	LDAA    #$34
	STAA	ADCTL
     	
WAIT_IR2
	LDAA	ADCTL	;WAIT FOR 4-CONVERSIONS TO COMPLETE
	ANDA	#BIT7
	BEQ	WAIT_IR2

        LDAA    #$34    ;CLEAR THE CONVERSIONS FLAG
	STAA	ADCTL

        LDAA    ADR1    ;GET THE SENSOR READINGS AND PUT INTO GLOBAL
	STAA	TLF_INIT_IR	;VARIABLES

	RTS


INIT_LIGHTS
	LDAA    #$34
	STAA	ADCTL
     	
WAIT_LTS
	LDAA	ADCTL	;WAIT FOR 4-CONVERSIONS TO COMPLETE
	ANDA	#BIT7
        BEQ     WAIT_LTS

        LDAA    #$34    ;CLEAR THE CONVERSIONS FLAG
	STAA	ADCTL

        LDAA    ADR2    ;GET THE SENSOR READINGS AND PUT INTO GLOBAL
        STAA    RIGHT_INIT_L     ;VARIABLES

        LDAA    ADR3
        STAA    LEFT_INIT_L

	RTS
        
********************************************************************
*SUBROUTINE: INIT_PORTS
*PURPOSE: INITIALIZES ALL PORTS TO BE EITHER INPUTS OR OUTPUTS
*********************************************************************

INIT_PORTS
        LDAA    #$F7
        STAA    DDRD

        LDAA    #$F0
        STAA    $7000

        LDAA    #0
        STAA    PORTA  

	RTS


********************************************************************
*SUBROUTINE: INIT_SPEED
*PURPOSE: STARTS THE CAR AT AN INITAL SLOW SPEED
*********************************************************************

INIT_SPEED

        LDD     #3200
        STD     OC3HIGH
        STD     OC3_CHANGE

        LDD     OC3Period
        SUBD    OC3HIGH
        STD     OC3LOW

	RTS
                   
********************************************************************
*SUBROUTINE: INIT_DIRECTION
*PURPOSE: STARTS THE CAR OUT IN FORWARD GOING STRAIGHT
*********************************************************************

INIT_DIRECTION
	LDAA	PORTD
	ANDA	#$FB
	STAA	PORTD     
       
	LDAA	#0
	STAA	DIRECTION
        LDAA    #1
        STAA    DIR_PREV

        LDD     #3000   ;START WITH SERVOS STRAIGHT
	STD	OC2HIGH
        STD     OC2_CHANGE

	LDD	OC2Period
	SUBD	OC2HIGH
	STD	OC2LOW
	
	RTS

********************************************************************
* Subroutine:  InitRTI
* Function:    This routine enables RTIs and sets the RTI rate to
*              32.77ms.
* Input:       None
* Output:      Initializes RTI
********************************************************************
InitRTI LDAA    #$88    ;set the interrupt rate to 32.77ms
        STAA    PACTL
        LDAA	#$40 
	STAA	TMSK2
	RTS		;return to the main

********************************************************************
*SUBROUTINE: INIT_OC
*PURPOSE: INITIALIZES ALL OUTPUT COMPARES
*********************************************************************

INIT_OC

        LDAA    #%10100000   ;CLEAR OC2 AND 0C3 LINES TO ZERO
	STAA    TCTL1

        LDAA    #%01100000   ;ENABLE OC2 AND OC3 INTERRUPT 
	STAA    TMSK1

	RTS

*********************************INTERRUPT SERVICE ROUTINES**************************8

********************************************************************
*SUBROUTINE: OC2_ISR
*PURPOSE: CONTROLS THE OUTPUT COMPARE 2
*********************************************************************

OC2_ISR
	LDAA    TFLG1   ;CHECK FOR LEGAL INTERRUPT
	ANDA    #BIT6   
	BEQ     END_OC2

        LDAA    #BIT6   ;CLEAR THE INTERRUPT
        STAA    TFLG1

	LDAA    TCTL1   ;CHECK IF LAST PULSE WAS HIGH OR LOW
	ANDA    #BIT6
	BEQ     LASTHIGH_OC2

        LDAA    TCTL1    ;SET NEXT PULSE TO BE LOW
        EORA    #$40
        STAA    TCTL1

        LDD     TOC2 ;SET OC2 HIGH TIME
        ADDD    OC2HIGH
	STD     TOC2

        BRA     END_OC2
			
LASTHIGH_OC2
        LDAA    TCTL1    ;SET THE NEXT PULSE TO BE HIGH
        EORA    #$40
	STAA    TCTL1
	
        LDD     TOC2
        ADDD    OC2LOW
	STD     TOC2

END_OC2                 
	RTI            ;RETURN FROM INTERRUPT

********************************************************************
*SUBROUTINE: OC3_ISR
*PURPOSE:CONTROLS OUTPUT COMPARE 3
*********************************************************************
	
OC3_ISR
	LDAA    TFLG1   ;CHECK FOR LEGAL INTERRUPT
        ANDA    #BIT5   
        BEQ     END_OC3

        LDAA    #BIT5   ;CLEAR THE INTERRUPT
        STAA    TFLG1

	LDAA    TCTL1   ;CHECK IF LAST PULSE WAS HIGH OR LOW
        ANDA    #BIT4
	BEQ     LASTHIGH_OC3

        LDAA    TCTL1    ;SET NEXT PULSE TO BE LOW
        EORA    #$10
        STAA    TCTL1

        LDD     TOC3 ;SET OC2 HIGH TIME
        ADDD    OC3HIGH
        STD     TOC3

        BRA     END_OC3
			
LASTHIGH_OC3
        LDAA    TCTL1    ;SET THE NEXT PULSE TO BE HIGH
        EORA    #$10
        STAA    TCTL1
	
        LDD     TOC3
        ADDD    OC3LOW
        STD     TOC3

END_OC3                 
	RTI            ;RETURN FROM INTERRUPT
********************************************************************
*SUBROUTINE: IRQ_ISR
*PURPOSE:USED FOR THE REBOOT PROCESS
*********************************************************************

IRQ_ISR
	LDAA	STARTUP
	CMPA	#0
	BEQ	GO_SYSTEMS

OFF_SYSTEMS
	LDAA	#0
	STAA	STARTUP
	
        BRA     END_IRQ

GO_SYSTEMS
	LDAA	#1
	STAA	STARTUP

END_IRQ
	RTI


				
*
********************************************************************
* Interrupt Service Routine (ISR): RTI_ISR
* Function:  This ISR services the Real-Time Interrupts.
* This ISR should do the followings:
*  - Clear RTI flag.
*  - Update current SP in CPT[Current PID]
*  - Find next PID
*  - Update CurPID
*  - Load New SP from CPT[Next PID]
********************************************************************
RTI_ISR 
	LDX     #BASE
        LDAA    TFLG2
        ANDA    #BIT6
        BEQ     END_ISR	;check for valid interrupt
 
        LDX	#CPT	;store the current stack pointer to the appropriate
        LDAA    CurrPID	;entry in the CPT, as dictated by CurrPID
        ASLA		;to get the offset into the table must mult. 
        STAA    temp	;CurrPID by 2
        LDAB    temp
        ABX
        TSY
	DEY
        STY     0,X       ;save the current stack pointer of the current process
     
	LDAA    CurrPID
        INCA              ;...to the appropriate slot in the CPT
*                         ;check to see if the process is the last one
FndNxt  CMPA    #$0A      ;is so the next process to be run is process[0]
	BEQ	REFRESH

        LDX     #CPT	;determine the next nonzero entry in the CPT
        STAA    temp	;...this is the stack pointer we need to use
        ASL     temp	;for the next process
        LDAB    temp
        ABX
        INCA
        LDY     0,X       ;I must skip zeroed entried here, since my KILL
	BEQ	FndNxt	  ;does not account for them
	BRA	SET
	
REFRESH LDX     #CPT
        LDY     0,X        ;restart the process poll
        LDAA    #$0
        STAA    CurrPID
	INY
	TYS

        BRA     RT_ISR

SET	INY
	TYS
        DECA                ;transfer the correct processes SP[Proc.] to the SP
        STAA    CurrPID

RT_ISR  LDAA    #BIT6	;clear the RTI-flag so the next process is not
        STAA    TFLG2	;immediately interruted
	
END_ISR RTI
				


********************************************************************
********************************************************************
*SUBROUTINES***************************************
********************************************************************

********************************************************************
*SUBROUTINE: SEARCH_TABLE
*PURPOSE: FINDS THE APPROPRIATE SERVO DIRECTION FOR HIGH-SPEED
*OBSTACLE AVOIDANCE
*INPUT: THE TABLE TO SEARCH IN THE X-REGISTER
*	THE TABLE WITH THE CORRESPONDING ENTRY SEARCHED FOR
*	IN ACCUMULATOR A IS THE VALUE TO MATCH IN THE TABLES
*********************************************************************
SEARCH_TABLE_IR
        LDAB    #0
        STAB    T_INDEX
        LDX     #DIFF_IR

ST_LOOP
        LDAB    0,X
        CBA
        BLT     FOUND_ENTRY

        INX
        LDAB     T_INDEX
        INCB
        STAB    T_INDEX
        CMPB    #4
        BLT     ST_LOOP

        LDD     #700
        STD     TURN_FOUND
        BRA     END_SEARCH

FOUND_ENTRY
        LDAB    T_INDEX
        ASLB
        LDX     #TURN_SMOOTH
        ABX
        LDD     0,X
       
END_SEARCH
        STD     TURN_FOUND       
        RTS

SEARCH_TABLE_MOT

*
********************************************************************
* Subroutine: Spawn
* Function:   generates a new process
* Input:      X: starting address of the process
* Output:     A: PID of the process just created, or
*                $FF if no slots are available
* Destroys:   Contents of A register
* Side effects: Creates the initial stack for the process.  This
*               stack must have the process PID in A, and %01000000
*               in CCR.
********************************************************************
Spawn	
	
	PSHY		;put these on the stack so they are not destroyed
	PSHB
	STX	TEMP2
	LDAA	#$00	;intialize the CPT position=0
	LDY	#CPT

FndPlc  LDX     0,Y     ;find the next empty spot in the CPT
        INY             ;POINT TO NEXT ENTRY IN THE TABLE
        INY
        INCA

        CMPA    #11     ;if A = 9 then the table has been completely
        BEQ     FULL	;scanned and there are no zero entries

        CPX     #$0
        BNE     FndPlc

        DECA

        STAA	temp	;store the displacement into the table in temp
        ASL     temp    ;multiply temp by 2=>index into table(CPT)
        LDAB    temp
        LDX     #DSPT   ;get default stack table
        ABX        
        LDY     0,X  ;get appropriate default stack pointer
        LDX     #CPT    ;put into the appropriate CPT slot
	ABX
        STY     0,X

        STAA    temp

        LDY     0,X        ;initialize the stack for the new process
        LDX	TEMP2
	DEY
	STX     0,Y
        DEY
	DEY
	LDX     #$0
        STX     0,Y
        DEY
        DEY
        LDX     #$0
        STX     0,Y
        DEY
        LDAA    temp
        STAA    0,Y
        DEY
        LDAB    #$0
        STAB    0,Y
        DEY
        LDAB    #$40
        STAB    0,Y
	DEY        		;pulled out DEY here
	
	ASL     temp
        LDAB    temp
        LDX     #CPT
        ABX                
        STY     0,X
        
	PULB		;restore variables used for this subroutine
	PULY	
        BRA     RT_RTS

FULL 	PULB
        PULY
        LDAA    #$FF
 	   
RT_RTS  RTS             ; return from subroutine

********************************************************************
* Subroutine: Kill
* Function:   removes a currently active process
* Input:      A: process ID to kill
* Output:     A: process ID just killed
* Destroys:   None
********************************************************************
Kill    PSHB

        STAA    temp
        ASL     temp
        LDAB    temp

	PSHX		;SAVE THESE REGISTERS SOP THEY ARE NOT CHANGED
	PSHY
	
	LDY	#$0	;zero-out the corresponding entry of the table
	LDX	#CPT
        ABX
        STY     0,X

        PULY            ;restor the registers used during the subroutine
	PULX
        PULB
	
	RTS		;return from subroutine
	

*
********************************************************************
*
* Subroutine:  Delay
* Input:       None
* Output:      Provides a delay by simple looping
* Destroys:    None
* Note:        If you're looking to save memory, this function
*              may be rewritten or subsumed by Process since
*              Process is the only routine to call it.
********************************************************************
*
Delay   PSHA             ;
        PSHB             ;
        PSHX             ; Save registers
        PSHY             ;
        LDX    #10    ; Load outer loop counter
Outer   LDY    #10000    ; Load inner loop counter
Inner  
        DEY              ; Decrement inner counter
        BNE    Inner     ; Branch if >0 to inner loop
        DEX              ; Decrement outer counter
        BNE    Outer     ; Branch if >0 to outer loop
        PULY             ; Restore registers
        PULX             ;
        PULB             ;
        PULA             ;
        RTS              ; Return from subroutine


HONK_DELAY
        LDX     #20000
H_LOOP
        DEX
        CPX     #0
        BNE     H_LOOP

        RTS


********************************************************************
*
* Subroutine:  FIND_MAX
* Input:       ARRAY INDEX:X-REGISTER; ARRAY SIZE: A ACCUMULATOR;Y:WHERE TO STORE THE MAX
* Output:      FINDS MAXIMUM OF A GIVEN ARRAY; PASSED IN THE X-REGISTER
* Destroys:    None
********************************************************************
*

FIND_MAX
	LDAB	0,X
	STAB	CURR_MAX
	DECA
MAX_LOOP
	LDAB	0,X
	CMPB	CURR_MAX
	BLT	SAME_MAX
	STAB	CURR_MAX
SAME_MAX
	INX
	DECA
	CMPA	#0
	BNE	MAX_LOOP
	
	LDAB	CURR_MAX
	STAB	0,Y	

	RTS

		
