'{$STAMP BS2}
'{$PBASIC 2.5}

' DS1620 3-Wire Thermometer
'''''''''''''''''''''''''''''''''''''''''''''''''''
' BS2                                   DS1620    '
'                                                 '
' Pin2 (term 7) -------------------- DQ (term 1)  '
' Pin1 (term 6) -------------------- CLk (term 2) '
' Pin0 (term 5) -------------------- RST (term 3) '
'''''''''''''''''''''''''''''''''''''''''''''''''''

' Sharp IR Distance Sensor
'''''''''''''''''''''''''''''''''''''''''''''''''''
' Pin 2 of DIRRS+  to  Pin 12 of BS2              '
'''''''''''''''''''''''''''''''''''''''''''''''''''

' Memsic 2125 Tilt Sensor
'''''''''''''''''''''''''''''''''''''''''''''''''''
' Pin 8 BS2 to x output on 2125                   '
' Pin 9 BS2 to y output on 2125                   '
'''''''''''''''''''''''''''''''''''''''''''''''''''


'VARIABLES'
    'DS1620 Temp'
        o_byte VAR Byte         ' byte sent to DS1620
        i_9_bit VAR Word        ' reaw temperature data from 1620
        T_C VAR Word
        n VAR Byte              ' index
        times VAR Byte          ' number of measurements
        DIRS = $f0ff
        IN CON 0
        OUT CON 1
        RST CON 0
        CLK CON 1
        DQ CON 2
        DQ_OUT VAR OUT2
        DQ_IN VAR IN2
        DQ_DIR VAR DIR2
    'Memsic 2125
        Xin PIN 8 ' X input from Memsic 2125
        Yin PIN 9 ' Y input from Memsic 2125
        HiPulse CON 1 ' measure high-going pulse
        LoPulse CON 0
        DegSym CON 176 ' degrees symbol
        xRaw VAR Word ' pulse from Memsic 2125
        xmG VAR Word ' g force (1000ths)
        xTilt VAR Word ' tilt angle
        yRaw VAR Word
        ymG VAR Word
        yTilt VAR Word
        disp VAR Byte ' displacement (0.0 - 0.99)
        angle VAR Byte ' tilt angle
    'Sharp Dist'
        DIST  VAR Word
        datain  CON 12



''''''MAIN FUNCTION''''''
START:
      ''''''read dist''''''
      SERIN datain,188,[WAIT(%10101010),DIST]

      ''''''read tilt''''''
      GOSUB Read_Tilt ' reads G-force and Tilt

      ''''''read temp''''''
      GOSUB config_1620       ' configure 1620
      GOSUB start_convert     ' continuous conversion
      GOSUB meas
      T_C = i_9_bit/2
      GOSUB stop_convert      ' stop conversion process to save power

      ''''''display''''''
      'sensor values stored in DEC variables 'dist', 'T_C', ABS 'xTilt', and ABS 'yTilt'
      DEBUG "Distance = ", DEC dist, CR, "Temperature = ", DEC T_C, CR
      DEBUG "X Tilt.... ", (xTilt.BIT15 * 13 + " "), DEC ABS xTilt, DegSym, CLREOL, CR
      DEBUG "Y Tilt.... ", (yTilt.BIT15 * 13 + " "), DEC ABS yTilt, DegSym, CLREOL
      END
      STOP





'''''DS1620 subroutines'''''
config_1620:            ' send command $0c followed by $02 to configure
                        ' for CPU mode

        LOW RST
        HIGH CLK
        HIGH RST        ' initiate by bringing RST high

        o_byte = $0c
        GOSUB out_byte
        o_byte = $02
        GOSUB out_byte
        LOW RST
        PAUSE 200       ' give some time for the internal programming
        RETURN

start_convert:           ' send command $ee to cause continuous conversion
        HIGH CLK
        HIGH RST
        o_byte = $ee
        GOSUB out_byte
        LOW RST
        RETURN

meas:                    ' send command $aa to read data and then read
        HIGH CLK
        HIGH RST
        o_byte = $aa
        GOSUB out_byte
        GOSUB get_9_bits
        LOW RST
        RETURN

stop_convert:            ' send command $22 to stop the conversions
        HIGH CLK
        HIGH RST
        o_byte = $22
        GOSUB out_byte
        LOW RST
        RETURN

out_byte:                ' outputs byte in o_byte, beginning with least
                        ' significant bit
        DQ_DIR=OUT
        FOR n = 0 TO 7
           DQ_OUT = o_byte & $01     'least significant bit
           LOW CLK
          ' DEBUG DEC DQ_OUT
           HIGH CLK
           o_byte = o_byte >> 1      'shift data 1 place right
        NEXT
        DEBUG 13
        RETURN

get_9_bits:              ' reads 9 bits from 1620 beginning with least
                        ' significant bit
        DQ_DIR = IN
        i_9_bit = 0
        FOR n = 0 TO 8
           LOW CLK
           i_9_bit = (i_9_bit >> 1) | (DQ_IN << 8)
           'DEBUG DEC DQ_IN
           HIGH CLK
        NEXT
        DQ_DIR=OUT
       'DEBUG 13
        RETURN





'''''Memsic 2125 subroutines'''''
Read_G_Force:
    PULSIN Xin, HiPulse, xRaw ' read pulse output
    xmG = ((xRaw / 5) - 500) * 8 ' convert to 1/1000 g
    PULSIN Yin, HiPulse, yRaw
    ymG = ((yRaw / 5) - 500) * 8
    RETURN

Read_Tilt:
    GOSUB Read_G_Force
    ' restrict displacement to unit circle (0.0 - 1.0)
    disp = ABS xmG / 10 MAX 100 ' x displacement
    GOSUB Arcsine
    xTilt = angle * (-2 * xmG.BIT15 + 1) ' fix sign
    disp = ABS ymG / 10 MAX 100 ' y displacement
    GOSUB Arcsine
    yTilt = angle * (-2 * ymG.BIT15 + 1) ' fix sign
    RETURN

' Trig routines courtesy Tracy Allen, PhD. (www.emesystems.com)
Arccosine:
    disp = disp */ 983 / 3 ' normalize input to 127
    angle = 63 - (disp / 2) ' approximate angle
    DO ' find angle
    IF (COS angle <= disp) THEN EXIT
    angle = angle + 1
    LOOP
    angle = angle */ 360 ' convert brads to degrees
    RETURN
Arcsine:
    GOSUB Arccosine
    angle = 90 - angle
    RETURN