' =========================================================================
'
'   File...... MEMSIC2125-Dual.BS2
'   Purpose... Memsic 2125 Accelerometer Dual-Axis Demo
'   Author.... (C) 2003-2004 Parallax, Inc -- All Rights Reserved
'   E-mail.... support@parallax.com
'   Started...
'   Updated... 07 SEP 2004
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
'
' Read the pulse outputs from a Memsic 2125 accelerometer and converts to
' G-force and tilt angle.
'
' g = ((t1 / 10 ms) - 0.5) / 12.5%
'
' Tilt = ARCSIN(g)
'
' Refer to Memsic documentation (AN-00MX-007.PDF) for details on g-to-tilt
' conversion and considerations.
'
' 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 ]-------------------------------------------------------

#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    Scale       CON     $200                    ' 2.0 us per unit
  #CASE BS2SX, BS2P, BS2PX
    Scale       CON     $0CC                    ' 0.8 us per unit
#ENDSELECT


HiPulse         CON     1                       ' measure high-going pulse
LoPulse         CON     0

DegSym          CON     176                     ' degrees symbol


' -----[ Variables ]-------------------------------------------------------

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


' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Setup:
  PAUSE 250                                     ' let DEBUG window open
  DEBUG "Memsic 2125 Accelerometer", CR,
        "-------------------------"


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Read_Tilt                             ' reads G-force and Tilt

    ' display results

    DEBUG CRSRXY, 0, 3
    DEBUG "X Input...  ",
          DEC (xRaw / 1000), ".", DEC3 xRaw, " ms",
          CLREOL, CR,
          "G Force... ", (xmG.BIT15 * 13 + " "),
          DEC (ABS xmG / 1000), ".", DEC3 (ABS xmG), " g",
          CLREOL, CR,
          "X Tilt.... ", (xTilt.BIT15 * 13 + " "),
          DEC ABS xTilt, DegSym, CLREOL

    DEBUG CRSRXY, 0, 7
    DEBUG "Y Input...  ",
          DEC (yRaw / 1000), ".", DEC3 yRaw, " ms",
          CLREOL, CR,
          "G Force... ", (ymG.BIT15 * 13 + " "),
          DEC (ABS ymG / 1000), ".", DEC3 (ABS ymG), " g",
          CLREOL, CR,
          "Y Tilt.... ", (yTilt.BIT15 * 13 + " "),
          DEC ABS yTilt, DegSym, CLREOL

    PAUSE 200                                   ' update about 5x/second
  LOOP
  END


' -----[ 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


Read_Tilt:
  GOSUB Read_G_Force
  disp = ABS xmG / 10 MAX 99                    ' x displacement
  GOSUB Arcsine
  xTilt = angle * (-2 * xmG.BIT15 + 1)          ' fix sign
  disp = ABS ymG / 10 MAX 99                    ' 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 