' {$STAMP BS2}
' {$PBASIC 2.5}

' Sean Ulrich
' 4-11-2006



' -------------------------------------------------------------------------
' I/O Definitions
' -------------------------------------------------------------------------

Xin             PIN     8                       ' X input from Memsic 2125
Yin             PIN     9                       ' Y input from Memsic 2125
HB25            PIN     15                      ' I/O Pin For HB-25

' -------------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------------

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

PosNeg          VAR     Word
test            VAR     Word

index           VAR     Word                    ' Counter For Ramping
' -------------------------------------------------------------------------
' Initialization
' -------------------------------------------------------------------------

Setup:
  PAUSE 250                                     ' let DEBUG window open
  DEBUG "Memsic 2125 Accelerometer", CR
  DEBUG "-------------------------"

  DO : LOOP UNTIL HB25 = 1            ' Wait For HB-25 Power Up
    LOW HB25                          ' Make I/O Pin Output/Low
    PAUSE 5                           ' Wait For HB-25 To Initialize
    PULSOUT HB25, 750                 ' Stop Motor 1

' -------------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------------


DO
GOSUB Calcs
GOSUB Tilt_Motor
'PAUSE 100                          ' update about 10x/second
LOOP



' -------------------------------------------------------------------------
' Subroutines
' -------------------------------------------------------------------------

' Memsic Subroutines-------------------------------------------------------

Calcs:
    GOSUB Read_Tilt                             ' reads G-force and Tilt

    ' display results
    SEROUT 15,84,[22]
    SEROUT 15,84,[12,"Memsic 2125",148,"X ",(xTilt.BIT15 * 13 + " " ),
          DEC ABS xTilt, DegSym," ","Y ", (yTilt.BIT15 * 13 + " "),DEC ABS yTilt, DegSym," ","tilt",CR]

    DEBUG CRSRXY, 0, 3
    DEBUG "X Input...  ",
          DEC (xRaw / 500), ".", DEC3 xRaw, " ms",
          CLREOL, CR,
          "G Force... ", (xmG.BIT15 * 13 + " "),
          DEC (ABS xmG / 1000), ".", DEC3 (ABS xmG), " g",
          CLREOL, CR,
          "x Tilt Raw... ",  DEC ABS xTilt,
          CLREOL, CR

          PosNeg = xTilt.BIT15 * 13                'indentify whether Pos or Neg
          IF PosNeg = 0 THEN                             '0 is Pos, 13 is Neg
             DEBUG "the angle is Positive", CLREOL, CR
          ENDIF

    DEBUG "X Tilt.... ", (xTilt.BIT15 * 13 + " "),
          DEC ABS xTilt, DegSym,
          CLREOL, CR,
          "Pos or Neg... ", DEC PosNeg, CLREOL

    DEBUG CRSRXY, 0, 10
    DEBUG "Y Input...  ",
          DEC (yRaw / 500), ".", 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
     DEBUG CRSRXY, 0, 15, "                       "
     RETURN



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




' Tilt Motor Subroutines---------------------------------------------------------

Tilt_Motor:                                'tilt seat based on the angle
    IF xTilt = 0 THEN
      PULSOUT HB25, 750                     ' Stop Motor 1
    ELSEIF PosNeg = 13 THEN
'      FOR index = 0 TO 250                ' Ramp Up To Full Speed
        PULSOUT HB25, 900 '- index          ' Motor 1 Forward
        PAUSE 1
        DEBUG CRSRXY, 0, 15, "push actuator out"
'      NEXT
    ELSEIF PosNeg = 0 THEN
'       FOR index = 0 TO 250                ' Ramp Up To Full Speed
        PULSOUT HB25, 600 '+ index          ' Motor 1 Forward
        PAUSE 1
        DEBUG CRSRXY, 0, 15, "pull actuator in"
'      NEXT
    ENDIF
RETURN 