Code:
loop:
zave = 0
xave = 0
yave = 0
for nn = 0 to 5 'takes average of nn adc results
ADCON0.2 = 0
ADCON0.3 = 0 'Channel select bit CHS1(should already be a 0)
gosub adconvert
zresult = adval
zave = zave + zresult
ADCON0.2 = 1
ADCON0.3 = 0 'Channel select bit CHS1(should already be a 0)
gosub adconvert
xresult = adval
xave = xave + xresult
ADCON0.2 = 0
ADCON0.3 = 1 'Channel select bit CHS1(should already be a 0)
gosub adconvert
yresult = adval
yave = yave + yresult
next nn
zresult = zave/6 'divide averaged sum by nn
xresult = xave/6
yresult = yave/6
@ PrintVar 16,0, _zresult
@ PrintVar 16,1, _xresult
@ PrintVar 16,2, _yresult
if startbit = 0 then 'calibrate routine - only does at first startup
'first hold horizontal for first set of calibration for x and y
xcal = xresult
ycal = yresult
@ PrintStr 0,0, "HOLD VERTICAL UNTIL MESSAGE DISSAPEARS"
'then hold vertical to calibrate z sensor
pause 5000
ADCON0.2 = 0
ADCON0.3 = 0 'Channel select bit CHS1(should already be a 0)
gosub adconvert
zresult = adval
zcal = zresult
gosub Lcd_Clear
@ PrintStr 0,0, "Z= " ;display on Nokia 3310
@ PrintStr 0,1, "X= "
@ PrintStr 0,2, "Y= "
@ PrintVar 48,0, _zcal
@ PrintVar 48,1, _xcal
@ PrintVar 48,2, _ycal
startbit =1
endif
if xresult > xcal then 'get difference from neutral to gravity reading
xresult = xresult - xcal 'since we are using 10 bit result, should be around 512
xdir = 0
else 'accelerometer reads 512 with no gravity
xresult = xcal - xresult 'and above or below depending on direction tilt
xdir = 1
endif
if yresult > ycal then 'yresult is acceleration allong the y axis
yresult = yresult - ycal
ydir = 0
else
yresult = ycal - yresult
ydir = 1
endif
if zresult > zcal then 'zresult is acceleration allong the z axis
zresult = zresult - zcal
zdir = 0
else
zresult = zcal - zresult
zdir = 1
endif
angletosolve = xresult
gosub solveangle
adval = y/100
@ PrintStr 0,5, " X DEGREES"
if xdir = 1 then
@ PrintStr 0,5, "U" 'tilted upward from level
else
@ PrintStr 0,5, "D" 'tilted down
endif
@ PrintVar 8,5, _adval
angletosolve = yresult
gosub solveangle
If zdir = 0 then
@ PrintStr 0,3, "INVERTED" 'upside down
else
@ PrintStr 0,3, "NORMAL " 'right side up
endif
adval = y/100
@ PrintStr 0,4, " Y DEGREES"
if ydir = 1 then
@ PrintStr 0,4, "L" 'tilted left
else
@ PrintStr 0,4, "R" 'tilted right
endif
@ PrintVar 8,4, _adval
goto loop
adconvert: ADCON0.1 = 1 'Start ADC Conversion
notdone: pause 5
if ADCON0.1 = 1 Then notdone 'wait for low on bit-2 of ADCON0
adval.highbyte = ADRESH 'move HIGH byte of result to adval
adval.lowbyte = ADRESL 'move LOW byte of result to adval
'Pause 10 'Wait .1 second
return
solveangle:
'**********Solve for x angle ********************************************
if angletosolve < zresult then 'if x/z is < 1 then x/z
dummy = angletosolve * 10000 'we will have to correct angle if this is case
y = div32 zresult
correct = 0
else
dummy = zresult * 10000 'else z/x to keep our equation within limits
y = div32 angletosolve
correct = 1
endif
x = 10000 'multiply this by same value 10000 as above
call atan2
if correct = 0 then y = 9000 - y '90.00 degrees - y = corrected angle
'************************************************************************
return
Bookmarks