
include "m8c.inc"
include "memory.inc"
include "PWM16_SERVO.inc"

export SERVO_ISR
export  _SERVO_ISR

export _servonum
export _servowidth

;-----------------------------------------------
; Variable Allocation
;-----------------------------------------------
AREA InterruptRAM (RAM, REL, CON)

_servonum: blk 1
_servowidth: blk 8

AREA UserModules (ROM, REL)
SERVO_ISR:
_SERVO_ISR:
	push a
	push x

; We don't change IDX_PP.
;	IF SYSTEM_LARGE_MEMORY_MODEL
;	      REG_PRESERVE IDX_PP
;    ENDIF

; F=0 on ISR entry
;	M8C_SetBank0

	PWM16_SERVO_Stop_M
	; hardware should be configured as:
	; PWM16, period=0xffff, compare: less than equal
	; interrupt on terminal count
	; clock = (approx) 24Mhz/2.5
	;
	; reset the counter to 0xffff. (period is copied immediately
	; to counter when module is disabled. this also clears the
	; interrupt condition)
	;
	; this will bring the output low if it wasn't already.
	; (important because we're about to connect it to a new output pin--
	; we don't want a glitch!)
    mov   reg[PWM16_SERVO_PERIOD_LSB_REG], 0xff
    mov   reg[PWM16_SERVO_PERIOD_MSB_REG], 0xff
	
	; servonum contains the new servo port to use.
	
	; force all pins to zero
	mov  reg[RDI3LT0], 0x00
	mov  reg[RDI3LT1], 0x00
	
	; branch to the appropriate handler
	cmp [_servonum], 1
	jz  _SERVO_ISR_1
	cmp [_servonum], 2
	jz  _SERVO_ISR_2
	cmp [_servonum], 3
	jz  _SERVO_ISR_3

	; these handlers set the appropriate output pins
_SERVO_ISR_0:
	mov reg[RDI3LT0], 0x03
	jmp _SERVO_ISR_exit;

_SERVO_ISR_1:
	mov reg[RDI3LT0], 0x30
	jmp _SERVO_ISR_exit;

_SERVO_ISR_2:
	mov reg[RDI3LT1], 0x03
	jmp _SERVO_ISR_exit;

_SERVO_ISR_3:
	mov reg[RDI3LT1], 0x30
;	jmp _SERVO_ISR_exit;
	
_SERVO_ISR_exit:
	; connect the correct row to the output
	M8C_SetBank1
	mov a, reg[DBB31OU]
	and a, 0xfc
	or  a, [_servonum]
	mov reg[DBB31OU], a
	
	M8C_SetBank0
	; load the pwm module with the right pulse width
	mov a, [_servonum]
	asl a
	mov x, a
	mov a, [x+_servowidth]
	mov reg[PWM16_SERVO_COMPARE_MSB_REG], a
	mov a, [x+_servowidth+1]
	mov reg[PWM16_SERVO_COMPARE_LSB_REG], a

	; rotate to the next servo
	inc [_servonum]
	and [_servonum], 0x3
	
	PWM16_SERVO_Start_M
	
 ; IF SYSTEM_LARGE_MEMORY_MODEL
 ;     REG_RESTORE IDX_PP
 ;  ENDIF
	
	pop x
	pop a
 
   reti
