/* This code is Copyright (c) Mike Jackson, 2004
 It is distributed under the GNU general public license, available from http://www.gnu.org/copyleft/gpl.html
 you may use, distribute and modify this code free of charge, so long as the original author (me!) is acknowledged.
*/

#include "include\6811regs.asm"

MAX_SERVO	EQU	0x0F		* this value must be a (2^n) - 1 value, as it is ANDed, not compared to ensure indexes are within bounds.

	ORG	MAIN_START

variable_servovalues:
*				you only need enough values here to store (MAX_SERVO) words.
*				$0BB8 = 3000 = 1.5ms = middle of servo travel

	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8
	FDB	$0BB8

variable_currentservo:
	FCB	$00

variable_servotemp:
	FDB	$0000

subroutine_set_servo_value:
/*				  call this function with D = *struct { INT index, INT value }
				  in other words, D is a pointer to a location in memory where 4 bytes can be found
				  the first byte is ignored (IC4 has problems with CHAR values, but INTs are fine..)
				  the second is the servo index, which is ANDed with MAX_SERVO to make sure it is in bounds
				  the third and fourth form a word, that is used as the number of ticks, or 500ns increments
				  to have output _index_ high for.
				  3000 ticks is about 1.5ms plus a few us for the instructions between reading the clock and
				  saving the output compare value in the interrupt routine below
*/
	XGDX				* put pointer to structure in X
	LDAB	$01,X			* load index into b (1 byte)
	ANDB	#MAX_SERVO		* make sure in bounds
	LDY	$02,X			* load value into y (2 bytes)
	LDX	#variable_servovalues	* load index of array into x
	LSLB				* double index: variable_servovalues is an array of words
	ABX				* add b to x: move to correct value
	LDD	$00,X			* read existing value to return ;)
	STY	$00,X			* store Y in *X: write new value to correct location
	RTS				* return

subroutine_get_servo_value:
*					  just call this function with the index in D - nothing fancy like the above
	LDX	#variable_servovalues	* load index of array into x
	ANDB	#MAX_SERVO
	LSLB				* double index: is a two byte array
	ABX				* add b to x: move to correct value
	LDD	$00,X			* read existing value to return
	RTS				* return

subroutine_initialize_module:
	LDY		#BASE

#include "include\findx.asm"
*					  X now has base of interrupt table

	LDD	#servo_interrupt
	STD	TOC1INT,X		* take over interrupt

	BCLR	TMSK1,Y $80		* disable timer (not sure if this is necessary)

	BSET	PACTL,Y $80		* set to output mode

	BSET	OC1M,Y $80		* enable OC1 mask for PA7 control
	BSET	OC1D,Y $80		* set to 1 on compare - disable outputs

	BCLR	TFLG1,Y $7F		* reset bit in flags
	BSET	TMSK1,Y $80		* enable interrupt

	RTS

servo_interrupt:
/*	at this point, the STROBE/INHIBIT line has been high for a short amount of time, and all the outputs of the 4514 are low.
	we can safely take our time until we re-enable/latch the 4514's outputs, as we're only contributing to "dead time" which is non-critical.
	its nice to try and keep things thin regardless though :)
*/

	LDAB	variable_currentservo	* load (previous) index into B
	INCB				* go to next
	ANDB	#MAX_SERVO		* make sure not out of bounds
	STB	variable_currentservo	* save

/*	this section maps 0000xxxx => xx00xx00, respectively - bits 2,3,6,7 of $7000 are connected to our 4514.
*/
	LSLB				* shift left twice: --nnxxxx => nnxxxx00
	LSLB
	TBA				* copy B into A
	LSLA				* rotate A left twice: --xxxxnn => xxxxnn00
	LSLA
	ANDA	#%11000000		* bitmask A: xx000000
	ANDB	#%00001100		* bitmask B: 0000xx00
	ABA				* combine into xx00xx00

/*	skip/rewrite the above section (and change the value below) if you are not using motor 3&4's output port.
*/
	STA	$7000			* write to motor port

	LDAB	variable_currentservo	* re-load current servo index
	LSLB				* double: 2 byte array
	LDY	#variable_servovalues	* put array base into y
	ABY				* move to relevant index
	LDD	$00,Y			* get current servo value from array

	LDY	#BASE

	BCLR	OC1D,Y $80		* set timer to write a 0 to the out, to set the servo pin +ve...

/*	 the 3 instructions below are the +few us mentioned above, between when the servo out goes high, and the compare register is written.
	 I decided not to compensate, as the delay is negligible - my servos have a higher positional error than this code :)
*/

	ADDD	TCNT			* add offset to current time
	BSET	CFORC,Y $80		* do it now
	STD	TOC1			* set time for servo output to change to negative

	BSET	OC1D,Y $80		* set to 1 on interrupt
	BCLR	TFLG1,Y $7F		* reset bit in flags so we can interrupt again next time round
	RTI				* return from interrupt