' =========================================================================
'
'   File....... Ping4.BS2
'   Purpose.... Reads four Ping sensors in round-robin fashion
'   Author..... Jon Williams -- Parallax, Inc.
'   E-mail..... jwilliams@parallax.com
'   Started....
'   Updated.... 27 JUL 2005
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

Ping1           PIN     5
Ping2           PIN     5
Ping3           PIN     7
Ping4           PIN     8

PingBase        CON     5                       ' for use in Get_Sonar


' -----[ Constants ]-------------------------------------------------------

#SELECT $STAMP
  #CASE BS2, BS2E
    Trigger     CON     5                       ' trigger pulse = 10 uS
    Scale       CON     $200                    ' raw x 2.00 = uS
  #CASE BS2SX, BS2P, BS2PX
    Trigger     CON     13
    Scale       CON     $0CD                    ' raw x 0.80 = uS
  #CASE BS2PE
    Trigger     CON     5
    Scale       CON     $1E1                    ' raw x 1.88 = uS
#ENDSELECT

RawToIn         CON     889                     ' 1 / 73.746 (with **)
RawToCm         CON     2257                    ' 1 / 29.034 (with **)

IsHigh          CON     1                       ' for PULSOUT
IsLow           CON     0


' -----[ Variables ]-------------------------------------------------------

pingIdx         VAR     Nib                     ' sensor index
thePing         VAR     Nib                     ' sensor pin
rawDist         VAR     Word                    ' raw measurement
inches          VAR     Word(4)


' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Reset:


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Get_Sonar                             ' read selected sensor
    pingIdx = pingIdx + 1 // 4                  ' update index

    ' put robot logic here



  LOOP

  END


' -----[ Subroutines ]-----------------------------------------------------

' Reads selected Ping sensor (0..3)
' -- put sensor number into "pingIdx"

Get_Sonar:
  thePing = PingBase + pingIdx                  ' convert to I/O pin
  LOW thePing                                   ' make pulse 0-1-0
  PULSOUT thePing, Trigger                      ' activate sensor
  PULSIN  thePing, IsHigh, rawDist              ' measure echo pulse
  rawDist = rawDist */ Scale                    ' convert to uS
  rawDist = rawDist / 2                         ' remove return trip
  inches(pingIdx) = rawDist ** RawToIn          ' convert to inches
  RETURN 