'  file: servos123_16bit_8M.BAS
'  this file initializes and loads registers as necessary to run
'  up to 4 servo motors on the DARC Board.  See each servo init and
'  set up subroutine for instructions on values to send.
'--------------------------------------------------------------

' put in the cal value for your part here (marked on the 8535 case
' most are hex 9F, some 91's and some in between
Osccal = &HA3

$crystal = 8000000

Declare Sub Servos12_init
Declare Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
Declare Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)
Declare Sub Servo3_init
Declare Sub Servo3_set(byval Setting As Byte)
Declare Sub Servo4_init
Declare Sub Servo4_set(byval Setting As Byte)

Dim Servos12_active As Bit
Dim Setting As Byte


'Main
' init the servos
   Call Servos12_init
   Call Servo3_init
   Call Servo4_init

   ' set the servos to run for 5 seconds
   Call Servo1_set(0 , 60)
   Call Servo2_set(0 , 20)
   Call Servo3_set(&H09)
   Call Servo4_set(&H09)
   Wait 5

   ' then turn off the servos
   Call Servo1_set(&H03 , &HFF)
   Call Servo2_set(&H03 , &HFF)
   Call Servo3_set(&Hff)
   Call Servo4_set(&Hff)



End




Sub Servos12_init:

' set bits 5 and 4 for output
Ddrd = Ddrd Or &H30

' set timer 1 for pwm output, prescaler 64
' these settings are for 1 MHz clock
Tccr1a = &HA3
Tccr1b = &H0C

'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

' Here the servos are init'd to be at an "OFF" setting
' hex 03FF gives a 100 % duty cycle pulse.
Ocr1ah = &H03
Ocr1al = &HFF

Ocr1bh = &H03
Ocr1bl = &HFF

Servos12_active = 1

End Sub Servos12_init


Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1bh = Hset
   Ocr1bl = Lset
End Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)


Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1ah = Hset
   Ocr1al = Lset
End Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)


Sub Servo3_set(setting As Byte)
'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

   Ocr2 = Setting
End Sub Servo3_set(setting As Byte)



Sub Servo3_init:
' set bit 7 for output
Ddrd = Ddrd Or &H80

' set timer 2 for pwm output, prescaler 1024
' these settings are for 8 MHz clock
Tccr2 = &H6F

'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

Ocr2 = &HFF
End Sub Servo3_init


Sub Servo4_set(setting As Byte)
'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

   Ocr0 = Setting
End Sub Servo4_set(setting As Byte)



Sub Servo4_init:
' set bit 3 for output
Ddrb.3 = 1

' set timer 2 for pwm output, prescaler 1024
' these settings are for 8 MHz clock
Tccr0 = &H6D

'OCR2 is an 8 bit value that sets the on time or duty cycle.
'Pulse width = (setting + 1) * 130 us.  So a setting of
'0 will give a 130 us pulse.  Setting = 1 gives a 260 us
'pulse.  A setting of 10 will give about 1.5 ms pulse.
'The OFF setting is &HFF.

Ocr0 = &HFF
End Sub Servo4_init