PDA

View Full Version : accelerometer code/servo



Let's Go!
08-04-2008, 07:36 PM
all the examples parallax offers re: accelerometer are games and reading values, but no real life examples. can anyone share some code that moves a servo, from either the 2 or 3 axis hitachi accelerometor?

stamptrol
08-04-2008, 11:10 PM
The accel examples get a number based on the movement of the accel chip.

Examples for the servos take a number and move the servo.

Loop to beginning.


Show us your code and we'll talk you through the snags.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tom Sisk

http://www.siskconsult.com
·

Let's Go!
08-05-2008, 12:07 AM
tom,



the below is calculating g force and tilt angle, and now i want to make servo respond and maintain a level condition of a platform.· jim



' ================================================== =======================
'
'·· File...... MEMSIC2125-Dual.BS2
'·· Purpose... Memsic 2125 Accelerometer Dual-Axis Demo
'·· Author.... (C) 2003-2004 Parallax, Inc -- All Rights Reserved
'·· E-mail.... support@parallax.com (mailto:support@parallax.com)
'·· Started...
'·· Updated... 07 SEP 2004
'
'·· {$STAMP BS2}
'·· {$PBASIC 2.5}
'
' ================================================== =======================


' -----[ Program Description ]---------------------------------------------
'
' Read the pulse outputs from a Memsic 2125 accelerometer and converts to
' G-force and tilt angle.
'
' g = ((t1 / 10 ms) - 0.5) / 12.5%
'
' Tilt = ARCSIN(g)
'
' Refer to Memsic documentation (AN-00MX-007.PDF) for details on g-to-tilt
' conversion and considerations.
'
' www.memsic.com (http://www.memsic.com)


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

Xin············ PIN···· 8······················ ' X input from Memsic 2125
Yin············ PIN···· 9······················ ' Y input from Memsic 2125


' -----[ Constants ]-------------------------------------------------------

' Set scale factor for PULSIN

#SELECT $STAMP
· #CASE BS2, BS2E
··· Scale······ CON···· $200··················· ' 2.0 us per unit
· #CASE BS2SX
··· Scale······ CON···· $0CC··················· ' 0.8 us per unit
· #CASE BS2P
··· Scale······ CON···· $0C0··················· ' 0.75 us per unit
· #CASE BS2PE
··· Scale······ CON···· $1E1··················· ' 1.88 us per unit
#ENDSELECT

HiPulse········ CON···· 1······················ ' measure high-going pulse
LoPulse········ CON···· 0

DegSym········· CON···· 176···················· ' degrees symbol


' -----[ Variables ]-------------------------------------------------------

xRaw··········· VAR···· Word··················· ' pulse from Memsic 2125
xmG············ VAR···· Word··················· ' g force (1000ths)
xTilt·········· VAR···· Word··················· ' tilt angle

yRaw··········· VAR···· Word
ymG············ VAR···· Word
yTilt·········· VAR···· Word

disp··········· VAR···· Byte··················· ' displacement (0.0 - 0.99)
angle·········· VAR···· Byte··················· ' tilt angle


' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Setup:
· PAUSE 250···································· ' let DEBUG window open
· DEBUG "Memsic 2125 Accelerometer", CR,
······· "-------------------------"


' -----[ Program Code ]----------------------------------------------------

Main:
· DO
··· GOSUB Read_Tilt···························· ' reads G-force and Tilt

··· ' display results

··· DEBUG CRSRXY, 0, 3
··· DEBUG "X Input...· ",
········· DEC (xRaw / 1000), ".", DEC3 xRaw, " ms",
········· CLREOL, CR,
········· "G Force... ", (xmG.BIT15 * 13 + " "),
········· DEC (ABS xmG / 1000), ".", DEC3 (ABS xmG), " g",
········· CLREOL, CR,
········· "X Tilt.... ", (xTilt.BIT15 * 13 + " "),
········· DEC ABS xTilt, DegSym, CLREOL

··· DEBUG CRSRXY, 0, 7
··· DEBUG "Y Input...· ",
········· DEC (yRaw / 1000), ".", DEC3 yRaw, " ms",
········· CLREOL, CR,
········· "G Force... ", (ymG.BIT15 * 13 + " "),
········· DEC (ABS ymG / 1000), ".", DEC3 (ABS ymG), " g",
········· CLREOL, CR,
········· "Y Tilt.... ", (yTilt.BIT15 * 13 + " "),
········· DEC ABS yTilt, DegSym, CLREOL

··· PAUSE 200·································· ' update about 5x/second
· LOOP
· END


' -----[ Subroutines ]-----------------------------------------------------

Read_G_Force:
· PULSIN Xin, HiPulse, xRaw···················· ' read pulse output
· xRaw = xRaw */ Scale························· ' convert to uSecs
· xmG = ((xRaw / 10) - 500) * 8················ ' calc 1/1000 g
· PULSIN Yin, HiPulse, yRaw
· yRaw = yRaw */ Scale
· ymG = ((yRaw / 10) - 500) * 8
· RETURN


Read_Tilt:
· GOSUB Read_G_Force
· disp = ABS xmG / 10 MAX 99··················· ' x displacement
· GOSUB Arcsine
· xTilt = angle * (-2 * xmG.BIT15 + 1)········· ' fix sign
· disp = ABS ymG / 10 MAX 99··················· ' y displacement
· GOSUB Arcsine
· yTilt = angle * (-2 * ymG.BIT15 + 1)········· ' fix sign
· RETURN


' Trig routines courtesy Tracy Allen, PhD. (www.emesystems.com (http://www.emesystems.com))

Arccosine:
· disp = disp */ 983 / 3······················· ' normalize input to 127
· angle = 63 - (disp / 2)······················ ' approximate angle
· DO··········································· ' find angle
··· IF (COS angle <= disp) THEN EXIT
··· angle = angle + 1
· LOOP
· angle = angle */ 360························· ' convert brads to degrees
· RETURN


Arcsine:
· GOSUB Arccosine
· angle = 90 - angle
· RETURN

stamptrol
08-06-2008, 02:18 AM
Two quick points:

1. After you've calculated tilt and g-force, you'll have to know whether they are nominal (and therefore need no correction) or whether they're too big or too small.
So, at the very least you'll have to compare the measured values with the normal or ideal value (maybe a pot could be used with RCTIME to allow a variable setpoint) . That difference will be used to send the servo the pulsewidth required to bring the vessel on course.

2. Most servos need updating about every 20 mSec or so in order to hold position. Note in your main loop that you have a pause of 200 mSec which means you'll need an off board servo controller to tend to the servos while the program loops. You may also be able to loop faster.

Keep at it.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tom Sisk

http://www.siskconsult.com
·