*  File:   initserv.asm
*  Author: Tom Dickens
*  Date:   June 4, 1995
*
*  Purpose: This file contains the servo initialization routines.
*
*  Subroutines:
*  Init_2_Servos: Initialize the software to drive two servo motors.
*  Init_4_Servos: Initialize the software to drive four servo motors.
*       Arguments:  -none-
*
*=============================================================
*  OK, what we want to do is to get a PWM signal going for a
*  servo motor.  The free-running counter will cycle at
*  500nS * 2^16 = 32.768 mS, which will work nicely
*  (Assuming 8 MHz Xtal).  We want a range from 1 mS to 2 ms
*  high, and the rest low.  Comparing to the counter values,
*  starting at $0000, 1 mS later we have $07D0, 1.5 mS at
*  $0BB8, and 2 mS at $0FA0 (decimal 2000, 3000, and 4000)
*  If we set the servo output(s) high at $0000 of the free-
*  running counter, we can use the 4 timer compare registers
*  to control bits 3, 4, 5, & 6 on port A.  Use the desired
*  value in the timer compare and configure the compare to
*  toggle the port bit.  This will happen automatically;
*  the port bits are set high when the free-running counter
*  rolls over, using OC1.  No interrupts are needed.
*
Init_2_Servos:
   psha
   pshx
   ldx     #$1000          ; set index to control registers
   ldaa    #%01010000
   staa    $20,x   ; set timers 2 & 3 to toggle at count equal
   ldaa    #%01100000
   staa    $0C,x   ; set timers 2 & 3 to set on OC1 overflow
   staa    $0D,x   ; specify data states for timers 2 & 3
   bsr     Set_Forward
   pulx
   pula
   rts
*
Init_4_Servos:
   psha
   pshx
   ldx     #$1000          ; set index to control registers
   ldaa    #%01010101
   staa    $20,x   ; set timers 2, 3, 4 & 5 to toggle at count equal
   ldaa    #%01111000
   staa    $0C,x   ; set timers 2, 3, 4 & 5 to set on OC1 overflow
   staa    $0D,x   ; specify data states for timers 2, 3, 4, & 5.
   bsr     Set_Forward
   pulx
   pula
   rts