Okay so I have to be doing something fundamentally stupid! Just not sure what? Any ideas?


DEFINE OSC 20 'Define crystal frequency

Include "Modedefs.bas"
include "hpwm10L.pbp"
Enable Debug

#CONFIG
__CONFIG _CONFIG1H, _HS_OSC_1H & _FSCM_OFF_1H
__CONFIG _CONFIG2H, _WDT_OFF_2H & _WDTPS_8K_2H
__CONFIG _CONFIG2L, _PWRT_ON_2L & _BOR_OFF_2L & _BORV_27_2L
__CONFIG _CONFIG3H, _MCLRE_OFF_3H
__CONFIG _CONFIG4L, _DEBUG_OFF_4L & _LVP_ON_4L & _STVR_ON_4L
__CONFIG _CONFIG5L, _CP0_OFF_5L & _CP1_OFF_5L
__CONFIG _CONFIG5H, _CPB_OFF_5H & _CPD_OFF_5H
__CONFIG _CONFIG6L, _WRT0_OFF_6L & _WRT1_OFF_6L
__CONFIG _CONFIG6H, _WRTC_OFF_6H & _WRTB_OFF_6H & _WRTD_OFF_6H
__CONFIG _CONFIG7L, _EBTR0_OFF_7L & _EBTR1_OFF_7L
__CONFIG _CONFIG7H, _EBTRB_OFF_7H
#ENDCONFIG


'********************* Declaired Pins **************************

INPUT PortB.2 ' X Data is clocked on rising edge of this pin


' ** Variables **

PxRaw VAR Long ' Low part of Pulse from Memsic 2125
WxRaw var Long ' High part of Pluse from Memsic 2125
WxTRaw var long ' Total of High and low Pluse from Memsic 2125
Dcycle var Long ' Duty cycle
xGForce VAR Word ' x axis g force (1000ths)
xTilt VAR Word ' x axis tilt (100ths)
xidx VAR byte ' Table index
xmult VAR Word ' Multiplier - whole part
xfrac VAR Word ' Multiplier - fractional part
xPosNeg var byte ' ositive angle or Negative angle flag

ADCON1 = 255 ' Make everything digital (no analog)

TRISB.2 = 1 ' Make B.2 an input

Read_Force:

PULSIN PORTB.2, 1, WxRaw
PULSIN PORTB.2, 0, PxRaw

WxRaw = WxRaw * 20 ' The high part of the pulse
PxRaw = PxRaw * 20 ' The low part of the pluse

WxTRaw = wxraw + pxraw ' The total time of the pluses

Dcycle = (Pxraw * 10000) / WxTRaw

xGForce = (((WxRaw * 10000)/ WxTRaw) - Dcycle) * 8 'xGForce = ((t1 / t2) - duty) / 12.5%


Read_Tiltx: 'tilt = g x k Select tilt conversion factor based on static G force. Table data derived from Memsic specs.

LOOKDOWN2 ABS xGForce, <=[174, 344, 508, 661, 2000], xidx ' Table is in thousandths g results for 10,20,30,40 and max value degrees
LOOKUP xidx, [57, 58, 59, 60, 62], xmult 'integer lookup
LOOKUP2 xidx, [32768, 10486, 2621, 30802, 22938], xfrac 'gets fractional result
xTilt = xmult * (ABS xGForce / 10) + (xfrac ** (ABS xGForce / 10))

Check_SignX: 'Check the sign of X angle
IF (xGForce.Bit15 = 0) THEN XT_Exit ' if positive, skip correct for g force sign
xPosNeg = 0 ' If xPosNeg = 0 then the angle is negative (-)

XT_Exit:
xPosneg = 1 ' If xPosNeg = 1 then the angle is positive (+)


gosub Read_Force