calculating Rotation

[Nakia]

Hi all,

just wondering if anyone has already done the math required to convert the output of the rotation rate sensor (with time stamp) into
a degrees value? don't want to re invent the wheel if the math is already out there..

I am needing to embed a digital style level into one part of my App hence this request?

Thanks for any assistance in advanced.

[sturgis]

Here are a few functions that might help, specifically rToDegrees and dToRadians. The others might come in handy at some point.. Or not.

Code: Select all

## takes 2 points, px,py starting, px2, py2, ending and returns vector offsets
function pointsToVec px,py,px2,py2
   return px2-px,py2-py
end pointsToVec


##Return the length of the vector.
##vX and vY are the offset distances from the starting point
## then uses pythag Theorem to calc the distance
function vLength vX,vY
   return sqrt((vX * vX) + (vY * vY))
end vLength

## Takes vector as the offset distance between points for x and y
## calculates the radians using atan2 * 180 / pi
function vToDegrees vX, vY
   return atan2(vx,vy) * 180/pi
end vToDegrees

## takes degrees and changes to radians
function dToRadians pDegrees
   return pDegrees / 180 * pi
end dToRadians

## takes radians and converts to degrees
function rToDegrees pRadians
   return pRadians * 180 / pi
end rToDegrees

## takes length and the angle in radians and returns vector as x,y offsets
function aLenToVector pLen pAngle
   return pLen * sin(pAngle),pLen * cos(pAngle)
end aLenToVector

## Get unit vector.
## Returns vector offsets whos vector length = 1 based on vector x and vector y
## don't understand purpose yet but here it is
## make sure if vector length is 0, handle the error. 
function uVector vX,vY
   if  vLength(vx,vy) is not 0 then
      return vx/vLength(vx,vy) ,vy/vLength(vx,vy)
   else
      return 0,0
   end if
end uVector

## return left and right normals
function LRNormals vx,vy
   return -vy,vx,vy,-vx
end LRNormals

## add 2 vectors
function vAdd vx,vy,vx2,vy2
   return vx + vx2,vy + vy2
end vAdd


## find the dot product of 2 vectors
## positive dp means the vectors are additive
## negative means they're subtractive
## IE positive they go generaly the same direction
## negative they go generally the opposite direction. 
## 0 = perpendicular movement
function dotProd vx,vy,vx2,vy2
   return vx*vx2 + vy * vy2
end dotProd


##Projection-- still learning so not sure yet where this falls into things. 
function vProj vX,vY,vX2,vY2
   return dotProd(vx,vy,vx2,vy2) * vx2,dotProd(vx,vy,vx2,vy2) * vy2
end vProj



## pR is radius, the lats and lons are decimal reperesntaions of the start and end point
function calcDist pR,plat1,plon1,plat2,plon2
   ## convert to radians
   put plat1 / 180 * pi into lat1
   put plon1 / 180 * pi into lon1
   put plat2 / 180 * pi into lat2
   put plon2 / 180 * pi into lon2
   
   put lat2 - lat1 into dlat
   put lon2 - lon1 into dlon
   
   put (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2 into a
   put 2 * atan2( sqrt(a),sqrt(1-a)) into c
   
   return pR * c
end calcDist

[Nakia]

Thanks.

These should help.

Seeing as there are a mutlitude of "Digital Level" style Apps available I was expecting someone had done this with LC already but doesn't seem so..
the functions you posted will definately help in getting this task done.

Thanks'

[sturgis]

While not quite what you're talking about, I used accelerationchanged and the functions provided (along with a gameloop to handle some of the screen update stuff) to create a very simple app that allows you to roll a ball around by tilting. It should be pretty easy to do this way. Since you don't really need to worry about simulating inertia or any of the more complicated stuff, all you really need to do is get the appropriate sensor value and position your bubble accordingly.

on accelerationChanged x,y,z
--x, y, z contain changing values. The closer to 0, the value, the closer to horizontal (perpendicular to gravity) each plane.
-- The values are imperfect so you'll want to implement a "fudge factor"
-- If its "close enough" to 0 the plane is flat. If not, position the indicator accordingly.
-- if the value is 1 (close enough), then the plane is in-line with gravity.
-- You will also want to implement a "calibrate" function, since not all devices are created equal.
-- THis way you can have the user calibrate the device so that if true horizontal is NOT 0
-- you can find the real value and adjust in code.
end accelerationChanged

[Nakia]

Hi Again,

Made a little progress with this but seems that if the device is left still on one axis and moved slightly on the other axis
(to get the mobieSensorRead to occur) it seems to grow the number?

If i move the phone and KEEP it moving it seems to work relatively accurately..

Note- I haven't created a method to calibrate (zero) the base value yet as I haven't got past this hurdle

This is what I have so far - note there is some stuff in the first handler that isn't used right now

Looking for any advice!!!

Again, what I am trying to achieve here is make a digital level read out (displaying in degrees) for the z Axis

Code: Select all

on rotationRateChanged pXRate, pYRate, pZRate
   -- Log cumulative rotation rate changes
   if  gRateCum = empty then
      put pZRate *180 / pi into gRateCum
   else
      add pZRate *180 / pi to gRateCum
   end if
   -- tally up samples to calculate adverage 
   if gSampleCount = empty then
      put 1 into gSampleCount
   else
      add 1 to gSampleCount
   end if
   -- calculate an adverage rotation rate change
   if gRateAv = empty then
      put gRateCum into gRateAv
   else
      put gRateCum / gSampleCount into gRateAv
   end if
   --
   updateDisplay
end rotationRateChanged

on updateDisplay
   -- read in sensor value and place contents into tArray
   put mobileSensorReading("rotation rate",true) into tArray
   -- fill old time stamp reference global on first sample ( needs more work)
   if gTimeStampOld[timestamp] = empty then
      put tArray[timestamp] into gTimeStampOld[timestamp]
   end if
   -- calculate the time diff between samples to establish multiplication factor to apply to the gRateCum
   put tArray[timestamp] - gTimeStampOld[timestamp] into tMultFactor
   --Storing the rotation (degrees value)
   put (tMultFactor * gRateCum) into tNewValue
   -- storing this to apply to current value of level
   put the tmText of control "Fld" into tOldValue
   --write data to tmText fld
   set the tmText of control "Fld" to (tOldValue + tNewValue)
   -- emptying the variable to force non cumulation of values
   put empty into gRateCum
   --- storing old timestamp to reference time diff on the next sample
   put tArray[timestamp] into gTimeStampOld[timestamp]
end updateDisplay

[Nakia]

Seems I have everyone stumped here

More investigation appears it seems to be a factor of the round that occurs when the numbers are calculated, but still
not 100% sure...

Investigations continue!!!!

[sturgis]

You might try something like this (untested) instead.. Place it in the card script, create a grc as your level bubble and name it ball. Add 2 lines, a vertical and horizontal and name then vert and hor. Add a field so you can get feedback of x,y,z

Code: Select all

on preopencard
   set the acceleratedrendering of this stack to true -- faster rendering, set the ball to "dynamic"
-- position and size the 2 lines
   set the height of grc "vert" to the height of this card 
   set the loc of grc vert to the loc of this card
   set the width of grc "hor" to the width of this card
   set the loc of grc "hor" to the loc of this card

   if the environment is "mobile" then
      mobileEnableAccelerometer  -- if on mobile, turn on the accelerometer
   end if
end preopencard


on accelerationchanged px,py,pz
   put px,py,pz into field 1   --feedback so you can see what numbers are coming through

-- I believe accelrattionchanged is based on gravities. 0 is freefall, 1 is earth norm.  Its possible to use momentum to go over 1 of course
-- but since the normal range for this will between 0 and 1, you'll want to multiply px and py by a factor. For example if your screen is 100 px tall, a factor of 50 should provide snappy movement.  
   put 50 into tFactor

-- place the ball in the center of the screen, offset by the value of the x and y accelerometer readings * the factor
   set the loc of grc "ball" to ((item 1 of the loc of this card + px * tfactor) ,((item 2 of the loc of this card + py * tfactor) 
end accelerationchanged

This is off the top of my head, so i'm not sure i've thought of everything, and can't test right at the moment. Maybe something in here will help.

EDIT: fixed a probable bug.

[sturgis]

Just realized since this is going to be a level type of reaction you'll want to reverse movements (tilt left, bubble moves right) but this is easy enough. For the 2 direction (plane) level example I described, multiply your numbers by -1 to reverse the movement of the bubble to get the reaction you want. And of course for a single axis, just.. er.. use a single axis. (x, y or z) but treat it the same way.

[Nakia]

Thanks Sturgis,

I will take a look over this but what I really need to do is calculate an accurate rotation in degrees (not degrees/sec as that is acceleration)
Its not the bubble level part I am really interested in but rather the digital display of degrees (like a digital level)

What I need to do is place the device on a surface and set that as my Zero Point then rotate the device (either way) around the
z axis and display the resultant calculated variance in accurate degrees (+/-) from the Zero Point.

The code I posted above is doing this kind of but when the device sits still it seems to climb in the direction of the last sensor reading..

I am thinking its a rounding issue that is accumulating with every update..

[sturgis]

You might look here: accelerometer is the way to go, with rate calculations you'll be shooting in the dark. http://www.hobbytronics.co.uk/accelerometer-info


Also here: http://www.phidgets.com/docs/Accelerometer_Primer

And possibly here: http://www.st.com/internet/com/TECHNICA ... 268887.pdf

Not sure its possible to do on the fly calculations using rotation rate to get this done. You'd need a way to get a solid starting angle and then go from there (as you do I guess) but if you can get a solid starting angle, you can just get the angles.