Help please with Assembly (PBPro user)

[ScaleRobotics]

I looked a little more at the assembly code you attached to your post. One drawback, it uses LCD code that is written for 4 mhz crystal which makes it a bit less flexible. This could be edited out and more code written for pbp, to be flexible for use with different pics. By the time you do that, maybe Alain is right, better to use code that is written in picbasic.

I looked at the parallax code, like Alain suggested. They are using a lookup table with a pulsin command. This could be done pretty easily. If you want to make it more exciting, you could use an tan-1 or arctan function. Oh, but picbasic does not have an arctan function you might say.... True, but Darrel helped me get a atan2(y,x) function working, and atan2(y,1) is the same as the tan-1 or arctan function!

What am I talking about?
Well, I did a few searches on angle tilt and got to Freescale Semiconductors AN3461
http://www.freescale.com/files/senso...ote/AN3461.pdf

And they mention using the arctan function to determine the angle:

theta = tan-1 (Ax/Az) and I believe that angle of Y = tan-1(Ay/Az)

A lookup table for the 10 bit a/d converter would be a bit long. And we can have resolution of about .3 degrees with a 10 bit tilt sensor value. (I know your tilt sensor does not work on A/D converter, but rather pulse width, but you could still use this formula).

Here is the atan2 code:
http://www.picbasic.co.uk/forum/showthread.php?t=10528

I will try to show an example in the next couple days.

[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