' srf04_get_sonar.bas
' THis file initializes the SRF04 header to drive the Devantech
' SRF04 Sonar Rangefinder module (for the DARC Board Rev 2).
' Pin assignments at that header:
' 1 - +5V
' 2 - Echo (Port B2)
' 3 - Init (Port A6)
' 4 - Not connected
' 5 - Ground


' 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 = &HXX

$crystal = 8000000

Declare Sub Srf04_init
Declare Sub Srf04_get_distance
Declare Sub Servos12_init

Dim Temp As Bit
Dim Starttime As Integer , Endtime As Integer , Sonartime As Integer
Dim Dist As Integer
Dim Servos12_active As Bit


Temp = 1

Main:
   Call Servos12_init
   Call Srf04_init

   Print "s"

   Do
     Wait 1
     Call Srf04_get_distance
     Print "range is " ; Dist ; " cm"
     Print ""
   Loop Until Temp = 0

End

Sub Srf04_init
   Ddrb.2 = 0
   Ddra.6 = 1

   Portb.2 = 1                                              'set pull up on pin b2

   If Servos12_active = 0 Then Call Servos12_init

End Sub Srf04_init

Sub Srf04_get_distance

   Porta.6 = 0
   Waitms 1

   Porta.6 = 1
   Waitms 1


   Porta.6 = 0
   Starttime = Counter1

   Waitus 210


   While Pinb.2 = 1
   Wend

   Endtime = Counter1
   Porta.6 = 0


   If Endtime > Starttime Then Sonartime = Endtime - Starttime
   If Starttime > Endtime Then
      Sonartime = &H3FF - Starttime
      Sonartime = Sonartime + Endtime
   End If

   Dist = Sonartime * 1.09


   Dist = Dist / 2

   If Dist < 30 Then Dist = Dist - 2


End Sub Srf04_get_range



Sub Servos12_init:

Ddrd = &B11111111

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

'OCR2 sets the on time or duty cycle
'An OCR2 setting of &H16 (hex 16) gives about 1.5 ms
'pulse.  Every increment increases this by about 65 us
'Same for decrement.  OCR2 is default set to hex FF or
'decimal 256 so that the output is a steady high level
'resulting in no motion of the servo
'
Ocr1ah = &H03
Ocr1al = &HFF

Ocr1bh = &H03
Ocr1bl = &HFF

Servos12_active = 1

End Sub Setup_servo12