' This program demonstrates how to use multiple SRF04 sonar's with the OOPic controller
' The important note is in the use of the oTimer. The oTimer is a hardware object,
' specifically Timer1 in the PIC chip. There is only one oTimer so we can't DIM
' more than one EchoTimer. Instead we just use one and move the link to its operate
' control for each sonar each time we take a reading.
' Further sonar's are added by DIMing some more I/O lines and adding new ReadS() Subs

Dim SLeft as new oWord                      ' left sonar reading
Dim SRight as new oWord                     ' right sonar reading
Dim EchoTimer as new oTimer                 ' object to time the returning echo
Dim TmrControl as new oGate(1)              ' used to connect the sonar echo pin to
                                            ' the timer operate control
Dim SLeftTrigger as new oDio1               ' Trigger pulse for left sonar
Dim SLeftEcho as new oDio1                  ' Echo input from left sonar
Dim SRightTrigger as new oDio1              ' Trigger pulse for right sonar
Dim SRightEcho as new oDio1                 ' Echo input from right sonar

Sub Main()
  SonarSetup                                ' setup I/O pins, oTimer and TmrControl
  Do
    ReadSLeft                               ' get reading from left sonar
    OOPic.Delay = 3                         ' wait 30mS for "ping" to fade away
    ReadSRight
    OOPic.Delay = 3

' Data is now in SLeft and SRight variables
' Add code here to do whatever you wish with the data

  Loop
End Sub 'Main

Sub ReadSLeft()
  TmrControl.Input1.Link(SLeftEcho.value)   ' set oTimer operate control to left echo pin   
  EchoTimer.value=0                         ' Start timing from zero count
  SLeftTrigger.value=1                      ' pulse the sonar trigger input
  SLeftTrigger.value=0
  While (SLeftEcho.value=1)                 ' wait for end of echo pulse
  Wend
  SLeft=EchoTimer.value/92                  ' convert reading to inches (divide by 36 for cm)
End Sub 'ReadSLeft

Sub ReadSRight()
  TmrControl.Input1.Link(SRightEcho.value)  ' set oTimer operate control to right echo pin  
  EchoTimer.value=0                         ' Start timing from zero count
  SRightTrigger.value=1                     ' pulse the sonar trigger input
  SRightTrigger.value=0
  While (SRightEcho.value=1)                ' wait for end of echo pulse
  Wend
  SRight=EchoTimer.value/92                 ' convert reading to inches (divide by 36 for cm)
End Sub 'ReadSRight

Sub SonarSetup()
  EchoTimer.ExtClock=cvOff                  ' use 5MHz internal clock for timer
  EchoTimer.PreScale=3                      ' divide 5MHz internal clock by 8
  SLeftTrigger.IOLine=6                     ' Pin 17 on 40Way Connector
  SLeftTrigger.Direction=cvOutput
  SLeftTrigger.value=0
  SRightTrigger.IOLine=9                    ' Pin 18 on 40Way Connector
  SRightTrigger.Direction=cvOutput
  SRightTrigger.value=0
  SLeftEcho.IOLine=7                        ' Pin 19 on 40Way Connector
  SLeftEcho.Direction=cvInput
  SRightEcho.IOLine=8                       ' Pin 20 on 40Way Connector
  SRightEcho.Direction=cvInput
  TmrControl.Output.Link(EchoTimer.Operate) ' Note - the input link is set in each 
  TmrControl.Operate=cvTrue                 ' ReadSLeft|ReadSRight subroutine
End Sub 'SonarSetup