*
* 6811ste.asm:  version with shaft Encoder support
*
*		assumes variables "lt_count", "rt_count", and "encdstat"
*		are defined in zero page
*
* start of code
*
* (1) perform initialization of stack, interrupts, system
* (2) code for system interrupt
*

* equates for shaft encoder sensor ports
LTENCDR	EQU	0
RTENCDR	EQU	1

ENCDRLO	EQU	25
ENCDRHI	EQU	40

	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 shaft encoders
	STD	lt_count
	STD	rt_count
	STAA	encdstat

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

* shaft encoder software:  run at 500 Hz
	LDAA	st_lo+1
	ANDA	#1
	BEQ	pwm

	LDX	#BASE

* do left encoder
do_left	LDAA	#LTENCDR
	STAA	ADCTL,X
	BRCLR	ADCTL,X,$80,*
	LDAA	ADR1,X		; get answer
	LDAB	encdstat
	ANDB	#1
	BEQ	lt_lk_hi	; look for rising edge
lt_lk_lo CMPA	#ENCDRLO
	BHS	do_right	; no action on left
lt_gotit LDX	lt_count
	INX
	STX	lt_count
	LDAA	encdstat
	EORA	#1
	STAA	encdstat
	BRA	do_right
lt_lk_hi CMPA	#ENCDRHI
	BHS	lt_gotit
*
do_right LDX	#BASE
	LDAA	#RTENCDR
	STAA	ADCTL,X
	BRCLR	ADCTL,X,$80,*
	LDAA	ADR1,X
	LDAB	encdstat
	ANDB	#2
	BEQ	rt_lk_hi
rt_lk_lo CMPA	#ENCDRLO
	BHS	endrexit
rt_gotit LDX	rt_count
	INX
	STX	rt_count
	LDAA	encdstat
	EORA	#2
	STAA	encdstat
	BRA	endrexit
rt_lk_hi CMPA	#ENCDRHI
	BHS	rt_gotit
*
endrexit 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

	LDD	speed4
	LSLD
	BCC	pwmoff4
	ADDD	#1
	XGDX			* get output byte into D; save new speed in X
	EORA	#%10000000	* toggle motor 4 ctrl bit
	BRA	pwmset4
pwmoff4	XGDX
	ORAA	#%10000000	* set motor 4 ctrl bit
pwmset4	XGDX
	STD	speed4

	XGDX			* get control bits into A
	EORA	#$F0		* toggle speed bits
	STAA	PORTB		* do it

	RTI

*
* PA3: beeper interrupt
beepi	EQU	*
	LDX	#BASE
	LDD	beeptone
	ADDD	TI4O5,X
	STD	TI4O5,X
	BCLR	TFLG1,X,%11110111
	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