*********
* Program ILLOP_2 (6811)
*
* Illustrates the use of the illegal op code trap to implement functions
* that a user who is not familiar with assembly might write to perform
* various functions.
*
* User writes code a RBSCT - robot section
* Each "high level task" begins with illegal op code 18 00
* followed by two arguements; the task and an argument.
*
* In this example only two tasks are defined
*	00 XX	  delay for time XX
*	01 XX	  wink LED XX times
*
* This could be extended into a whole robot language - say where 4 args
* are used.
*
*	04 01 45 03 turn motor 1, cw at speed 45, 3 steps
*	09 D0 50 00 say phrase at D0 50, 00 is dummy.
*
* or you could get fancy.
*	14 01 45 03 turn motor as above while doing next task
*	19 D0 50 00 say phrase at D0 50 while doing next
*	20 01 02 A3 suspend processes 1 and 2 if sound above A3
*	25 03 80 A3 go forward 3 instructions if sound above A3
*
* Lots of room for creativity.	A real nice senior project.
*
* P. H. Anderson, 15 Oct 90, 22 Feb 91; 5 Nov 91; 15 Jan 93
**********

PSCT		EQU $C000
DSCT		EQU $D000
IDSCT		EQU $D300

RBSCT		EQU $D500   /* place where user writes code */

STACKTP 	EQU $0045
REG_BASE	EQU $1000
ILLOP_VECT	EQU $00F7

DDRC	EQU $07
PORTB	EQU $04       * output
PORTC	EQU $03       * input

	ORG PSCT

	LDS #STACKTP
	LDY #REG_BASE

	CLR DDRC,Y	* although switches are not used
	BCLR PORTB,Y #%10000000	     * zero LED 7

	JSR TASK_SEQ	* user routine

	SWI	* end of main program

ILLOP_INT_SERV

	STY PTR1	* save X and Y
	STX PTR2
	TSX		* load X with SP+1
	LDY 07,X	* Y now contains old program counter
	LDAA 02,Y	* arg 1
	STAA ARG_1
	LDAA 03,Y	* arg 2
	STAA ARG_2
	INY		* Y is now adr after 18 00 XX YY
	INY
	INY
	INY
	STY 07,X
	LDY PTR1	* restore X and Y
	LDX PTR2

* now determine and do the task
	LDAA ARG_1
	CMPA #$00
	BNE ARND_1
	BSR DELAY
	BRA DONE
ARND_1
	CMPA #$01
	BNE ARND_2
	BSR WINK
	BRA DONE
ARND_2
DONE
	RTI

WINK
* winks LED 7, the number of times specified
WINK_TOP
	TST ARG_2
	BEQ WINK_DONE
	BSET PORTB,Y #%10000000       * wink LED
	BSR TIME
	BCLR PORTB,Y #%10000000
	BSR TIME
	DEC ARG_2
	BRA WINK_TOP
WINK_DONE
	RTS

DELAY
* delays for time specified
DELAY_TOP
	TST ARG_2
	BEQ DELAY_DONE
	BSR TIME
	DEC ARG_2
	BRA DELAY_TOP
DELAY_DONE
	RTS


TIME
	PSHX
	LDX #50000
L1
	DEX
	BNE L1
	PULX
	RTS

	ORG ILLOP_VECT

	JMP ILLOP_INT_SERV

	ORG DSCT

ARG_1		RMB 1
ARG_2		RMB 1
PTR1		RMB 2
PTR2		RMB 2

	ORG RBSCT

TASK_SEQ
* this would be written by a user
* note that 18 00 is an illegal op code
* this is then followed by two arguements
* the specific task and a modifier

	FCB $18, $00	*illegal op code
	FCB $01 	* wink LED
	FCB $09 	* nine times
	FCB $18, $00	* illegal op code
	FCB $00 	* delay
	FCB $10
	FCB $18, $00	* wink LED 10 times
	FCB $01
	FCB 10
	RTS