***********************************************************************
* 6811st.src
*
* define "ENCODER" during make for shaft encoder support
*	assumes variables "lt_count", "rt_count", and "encdstat"
*	are defined in zero page (for shaft encoders)
*
* define "MOTOR_BEEP" during make for beeping through motor port 4
*	motor port 4 mimics beeper
*
***********************************************************************

* start of code
*
* (1) perform initialization of stack, interrupts, system
* (2) code for system interrupt
*


	ORG	$F800
	LDS	#$FF

	LDX     #$1000

* initialize serial port
	LDAA	#$30		; 9600 baud
	STAA	BAUD,X
	BSET	SCCR2,X,$0C	; transmit, receive enable

* turn on analog subsystem
	BSET    OPTION,X,$80

* set up interrupts
* OC4:  1 kHz system interrupt
	LDAA	#%00010000
	STAA	TFLG1,X
	STAA	TMSK1,X

	CLRA
	CLRB

* initialize system time
	STD	st_hi
	STD	st_lo

* initialize button variables
	STAA	irqval
	STD	buttime


* initialize motor control
	STAA	motctrl		* default == OFF
	LDD	#$FFFF
	STD	speed1		* full speed
	STD	speed2
	STD	speed3
	STD	speed4

* initialize beeper
	BSET	PACTL,X;%00001000	; set PA3 for output

* enable interrupts
	CLI

	JMP	main


*
*
*
*
* 1 KHz driver routine
*
*

systemi	EQU	*
	LDX	#BASE		* point to register base

* setup for next interrupt
	LDD	#2000		; 2000 cycles = 1 millisec.
	ADDD	TOC4,X		* add TOC5 to D
	STD	TOC4,X		* store back
	BCLR	TFLG1,X,%11101111	* clear OC4 for next compare

* enable beeper to interrupt us
	BSET	TMSK1,X,%00001000
	CLI

* increment system time
	LDX	st_lo
	INX
	STX	st_lo
	BNE	si_noinc
	LDX	st_hi
	INX
	STX	st_hi

si_noinc EQU	*



* do pulse width modulation, 4 motors
* motor 1 (ls bit of low nybble is dir, ls bit of high nybble is on/off
pwm	LDX	motctrl		* upper nybble is on/off; lower is dir

	LDD	speed1
	LSLD
	BCC	pwmoff1
	ADDD	#1
	XGDX			* get output byte into D; save new speed in X
	EORA	#%00010000	* toggle motor 1 ctrl bit
	BRA	pwmset1
pwmoff1	XGDX
	ORAA	#%00010000	* set motor 1 ctrl bit
pwmset1	XGDX
	STD	speed1

	LDD	speed2
	LSLD
	BCC	pwmoff2
	ADDD	#1
	XGDX			* get output byte into D; save new speed in X
	EORA	#%00100000	* toggle motor 2 ctrl bit
	BRA	pwmset2
pwmoff2	XGDX
	ORAA	#%00100000	* set motor 2 ctrl bit
pwmset2	XGDX
	STD	speed2

	LDD	speed3
	LSLD
	BCC	pwmoff3
	ADDD	#1
	XGDX			* get output byte into D; save new speed in X
	EORA	#%01000000	* toggle motor 3 ctrl bit
	BRA	pwmset3
pwmoff3	XGDX
	ORAA	#%01000000	* set motor 3 ctrl bit
pwmset3	XGDX
	STD	speed3


	XGDX			* get control bits into A
	EORA	#$F0		* toggle speed bits
* leave motor 4 bits from PORTB alone
	ANDA	#%01110111
	LDAB	PORTB
	ANDB	#%10001000
	ABA
	STAA	PORTB		* do it

	RTI

*
* PA3: beeper interrupt
beepi	EQU	*
	LDX	#BASE
	LDD	beeptone
	ADDD	TI4O5,X
	STD	TI4O5,X
	BCLR	TFLG1,X,%11110111
	LDAA	PORTB,X
	EORA	#$08		; toggle direction bit of motor 4
	STAA	PORTB,X
	RTI

*
* IRQ interrupt:  sets irqval to one when interrupt occurs
irqi	EQU	*
	LDD	st_lo
	SUBD	buttime
	CPD	#60
	BLO	irqdone
	LDAA	#1
	STAA	irqval
	LDD	st_lo
	STD	buttime
irqdone	RTI