' memsic2125_wpd_enabled.bs2
'
' THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF
' ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO
' THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
' PARTICULAR PURPOSE.
'
' Copyright (c) Microsoft Corporation. All rights reserved
'
' {$STAMP BS2}
' {$PBASIC 2.5}
'
' =========================================================================

' -----[ Program Description ]---------------------------------------------
'
' Read the pulse outputs from a Memsic 2125 accelerometer and converts to
' G-force
'
' g = ((t1 / 10 ms) - 0.5) / 12.5%
'
' www.memsic.com


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

Xin             PIN     8                       ' X input from Memsic 2125
Yin             PIN     9                       ' Y input from Memsic 2125


' -----[ Constants ]-------------------------------------------------------

' Set scale factor for PULSIN

#SELECT $STAMP
  #CASE BS2, BS2E
    Scale       CON     $200                    ' 2.0 us per unit
  #CASE BS2SX
    Scale       CON     $0CC                    ' 0.8 us per unit
  #CASE BS2P
    Scale       CON     $0C0                    ' 0.75 us per unit
  #CASE BS2PE
    Scale       CON     $1E1                    ' 1.88 us per unit
#ENDSELECT

HiPulse         CON     1                       ' measure high-going pulse
LoPulse         CON     0

' -----[ Variables ]-------------------------------------------------------

xRaw            VAR     Word                    ' pulse from Memsic 2125
xmG             VAR     Word                    ' g force (1000ths)

yRaw            VAR     Word
ymG             VAR     Word

'disp            VAR     Byte                    ' displacement (0.0 - 0.99)


' Below lines are WPD additions

SensorID  VAR   Byte  'Sensor identifier = 1 for memsic
ElementSize VAR Byte  'Size (in bytes) of each element
ElementCount  VAR   Byte  'Count of elements in packet

NewInterval VAR  Word  'New interval requested by user
Interval  VAR   Word  'Interval value utlized by firmware

SensorID = 6
ElementSize = 1
ElementCount = 6      '6-bytes for g-force

LFD    CON $10        'Linefeed character

Interval = 100         '.10 of a second interval
NewInterval = 100

' -----[ Program Code ]----------------------------------------------------

Main:
    GOSUB Read_G_Force                'reads G-force
    GOSUB RetrieveInterval            'Retrieve units data

    Timeout:
    SEROUT 16, 16468, [DEC1 SensorID, DEC1 ElementSize, DEC1 ElementCount, DEC1(xmG.BIT15), DEC1(ABS xmG/1000),DEC1(ABS xmG/10),DEC1(ymG.BIT15),DEC1(ABS ymG/1000),DEC1(ABS ymG/10),DEC5 Interval,LFD]
    GOTO Main


    RetrieveInterval:
    SERIN 16, 16468, Interval, Timeout, [DEC NewInterval]   'Retrieve interval
    IF NewInterval >= 10 AND NewInterval <= 60000 THEN
      Interval = NewInterval
    ENDIF

' -----[ Subroutines ]-----------------------------------------------------

Read_G_Force:
  PULSIN Xin, HiPulse, xRaw                     ' read pulse output
  xRaw = xRaw */ Scale                          ' convert to uSecs
  xmG = ((xRaw / 10) - 500) * 8                 ' calc 1/1000 g
  PULSIN Yin, HiPulse, yRaw
  yRaw = yRaw */ Scale
  ymG = ((yRaw / 10) - 500) * 8
  RETURN