Help please with Assembly (PBPro user)

[ScaleRobotics]

Here is some code to determine tilt using at least a two axis accelerometer. It uses trig.inc
This does not do the a/d conversions, or pulse measurement conversions, that would have to be added.

define OSC 20
'code working with PIC-MT-USB
include "TRIG.inc"

main:
define       LED     PortD.3

DEFINE LCD_DREG PORTD             ; Set LCD Data port                B C   D
DEFINE LCD_DBIT 4                 ; Set starting Data bit (0 or 4)   4 0
DEFINE LCD_RSREG PORTD            ; Set LCD Register Select port     A A
DEFINE LCD_RSBIT 0                ; Set LCD Register Select bit      3 2
DEFINE LCD_EREG PORTD             ; Set LCD Enable port              A A
DEFINE LCD_EBIT 2                 ; Set LCD Enable bit               1 5
DEFINE LCD_BITS 4                 ; Set LCD bus size (4 or 8 bits)   4 4
DEFINE LCD_LINES 2                ; Set number of lines on LCD       2 2
define LCD_RWREG PORTD
define LCD_RWBIT 1
ADCON1=15

TRISA=%00000011                         ' Set PORTA  0-1-3 used for AD
TRISB=%00000000                         ' Set PortB
TRISC=%00000000  
TRISD=%00000100  
'       Vdd     5 volts
'       Vss     Ground
'       Vo      20K potentiometer (or ground)
'       DB0-3   No connect

a   var word         
xresult var word      'result of accelerometer sensor in x axis
yresult var word      'result of accelerometer sensor in y axis
zresult var word      'result of accelerometer sensor in z axis
correct var bit        'able to keep equation the same, and flip angle
xdir    var bit          'shows direction of x tilt
ydir    var bit          'shows direction of y tilt
zdir    var bit          'shows direction of z tilt


Lcdout $fe, 1   ' Clear LCD screen
Pause 500       ' Wait for LCD to startup
toggle portD.3
                                     
xresult = 553                       'is 41 away from center
zresult = 297                       'is 215 away from center
'to try it on your calculator tan^-1(41/215) = 10.796543 degrees
                                    'xresult is acceleration allong the x axis
if xresult > 512 then               'get difference from neutral to gravity reading
    xresult = xresult - 512         'since we are using 10 bit result, half is 512
    xdir = 0
else                                'accelerometer reads 512 with no gravity
    xresult = 512 - xresult         'and above or below depending on direction tilt
    xdir = 1
endif

if yresult > 512 then               'yresult is acceleration allong the y axis
    yresult = yresult - 512
    ydir = 0
else
    yresult = 512 - yresult
    ydir = 1
endif

if zresult > 512 then               'zresult is acceleration allong the z axis
    zresult = zresult - 512
    zdir = 0
else
    zresult = 512 - zresult
    zdir = 1
endif

if xresult < zresult then           'if x/z is < 1 then x/z
    xresult = xresult * 30000       'we will have to correct angle if this is case
    y = div32 zresult
    correct = 0
else
    zresult = zresult * 30000       'else z/x to keep our equation within limits
    y = div32 xresult               
    correct = 1
endif
    x = 30000                       'multiply this by same value 30000 as above
call atan2
if correct = 0 then y = 9000 - y    '90.00 degrees - y = corrected angle

lcdout $FE,1,#y/100,".",#y//100," degrees"            'y = angle 1080 = 10.80 degrees
lcdout $FE,$C0,"x=",#xdir," y=",#ydir," z=",#zdir
end

here is the location of the trig.inc file

http://www.picbasic.co.uk/forum/atta...4&d=1234739854

[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

[ScaleRobotics]

After seeing some odd behavior while rotating the accelerometer, I eventually noticed that in the formula I used, an assumption is made that rotation is ideal around the y axis. So as long as you have x rotation but not y, it works, or y rotation, but not x, it works. But if you have a combination of both, it did not give you correct values.

Time for a new equation. The good news was, the same data sheet that showed me the "ideal" equation had an example of the equation I wanted.



http://www.freescale.com/files/senso...ote/AN3461.pdf

This equation required longs, and a few other adjustments. It seems to be pretty accurate, though I have not come up with an easy way to test it.
I have included TRIGL.INC, which has a minor edit to run with pbpl.



Here is the meat of the code:

solveangle:
'**********Solve for x angle ********************************************
if xresult < yresult + zresult then 
    dummy = 5000   'when x is smaller the following equation works best
    xlong = (dummy * xresult) /(sqr((yresult*yresult) + (zresult*zresult)))
else  'keeps results accurate at near 0 and near 90 degree values
    dummy =30      'when x is larger the following equation works best
    xlong = (dummy * xresult*xresult) / ((yresult*yresult)+(zresult*zresult))
endif
y = xlong
x = dummy 'Assign x and y values for atan2 function
call atan2
xangle = 9000 - y

if yresult < xresult + zresult then 
    dummy = 5000  'when y is smaller the following eqauation works best
    ylong = (dummy * yresult) / (sqr((xresult*xresult) + (zresult*zresult)))
else
    dummy =30 'when y is larger the following equation works best
    ylong = (dummy * yresult*yresult) / ((xresult*xresult)+(zresult*zresult))
endif 
y = ylong
x = dummy '
call atan2
yangle = 9000 - y