Help please with Assembly (PBPro user)

[ScaleRobotics]

There was a bug in my above code, which (with my hardware) showed up between 40 and 50 degrees tilt. I was multiplying with too large a number that either a word in TRIG.INC, or a 31 bit div32 could handle. Changing the div32 equations from 30,000 to 10,000 did the trick. With a 3 axis sensor, it must be calibrated horizontal (for x and Y) and then in a position that puts Z in a horizontal position. Here is some code that worked pretty well for me using Olimex's PIC_LCD3310 with the built in MMA7260 g sensor. The math gives degreess out in a dddd format which really equals dd.dd. However, with a 10 bit result from ADC, the math is accurate to about 0.3 degrees. This version calibrates x and y, and then z, when it is first turned on, and averages 6 samples. It also shows left, right, up and down tilt, as well as whether it is inverted or not.

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