;
; Pulse Width Modulation Control routine.
;
; Inputs: 
;
;	
;
; Outputs: Control signals on PORTD
;
; Uses:	
; 
;
; Define our output bits for the motor
;
.equ	MOTOR	= PORTD

.equ	Servo_1	= PD0		; Motor control bits
.equ	Servo_2	= PD1		; Port D(2) = external interrupt
.equ	Servo_3	= PD3
.equ	Servo_4	= PD5

.set	DDRD_INIT = DDRD_INIT | 1<<Servo_1 | 1<<Servo_2 | 1<<Servo_3 | 1<<Servo_4
;
; Pulse Width Modulation limits
;
.equ	Servo_min	= 07
.equ	Servo_max	= 17

;* Code
;
; High Speed logic for generating the servo control pulse.
;
; Do this for four servo ports.  The counters are reset via
; a process running at 50 hz.
;
; THis is called at 100 khz rate
; consumes 20 cycles when active, 10 cycles when idle
;
Servo_Control:
	tst	SC_Count
	brne	sc1

	cbi	MOTOR, servo_1
	cbi	MOTOR, servo_2
	cbi	MOTOR, servo_3
	cbi	MOTOR, servo_4

	ret
sc1:
	dec	SC_Count

	cp	SC_Count, servo1
	brsh	sc2
	sbi	MOTOR, servo_1
sc2:
	cp	SC_Count, servo2
	brsh	sc3
	sbi	MOTOR, servo_2
sc3:
	cp	SC_Count, servo3
	brsh	sc4
	sbi	MOTOR, servo_3
sc4:
	cp	SC_Count, servo4
	brsh	sc5
	sbi	MOTOR, servo_4
sc5:
	ret
;
; Servo Process.  Sets pulse width and then sleeps for 20 milliseconds
;
Servo:
	ldi	E_Temp, 20
	mov	B_Delay, E_Temp
	
	ldi	E_Temp, 200
	mov	SC_Count, E_Temp

	ret