Some math help

Jonathan · 2007-06-28 14:52

Hi All,

I am, as I often am, stumped on a little piece of math. I'm hoping that someone can help me out here, what I need to do is pretty simple, but I juts can't get it to work.

I am working on getting my robot to follow a compass heading. Heading is a integer from 0 to 359 obtained from the compass. Bearing is the desired direction to follow. What I want to do is compare bearing Vs heading and come up wit two pieces of data.

a) How many degrees do we need to turn to face the bearing? This allows us to turn quickly if the bearing is 15 degrees or greater than heading, then turn slowly when we approach the correct heading.

Which direction is is fastest (closest) to turn. THe formula below is supposed to return a 0 for CCW and a 1 for CW.

What I have been trying is here:

numDegrees:= || (bearing - heading + (540//360)-180)
whichDir := (bearing - heading+ 360) / 360

What I can't do is get it to cross the 0 degree point properly. If not crossing 0 it works fine.

Any math whizzes out there?

Thanks!

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

John Abshier · 2007-06-28 15:15

b = final bearing
cb = current bearing
ds = shortest distance

diff = abs(b-cb)
if diff < 180 ds = diff else ds = 360 - diff

if diff < 180 and b > cb turn right
if diff < 180 and b < cb turn left
if diff > 180 and b > cb turn left
if diff > 180 and b < cb turn right

return trip if B < 180 opposite = b + 180 else oppostie = b - 180

rokicki · 2007-06-28 17:48

Heh, this is going to be a fun thread.

diff = b - cb
if diff < 180
   diff += 360
if diff > 180
   diff -= 360

if diff < 0 turn left by -diff
if diff > 0 turn right by diff

Jonathan · 2007-06-29 13:29

Thanks for the help folks. I feel that there is a one line, no *if* solution, but I guess I'll give up and try the alternatives you all so kindly posted.

Thanks!

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Lawson · 2007-06-29 14:10

diff = b - cb
if diff < 180
   diff += 360
if diff > 180
   diff -= 360

if diff < 0 turn left by -diff
if diff > 0 turn right by diff

Rokicki I think there's a typo in the above code.

It should probably be

diff = b - cb
if diff < -180      '<-- notice the negative sign here
   diff += 360
if diff > 180
   diff -= 360

if diff < 0 turn left by -diff
if diff > 0 turn right by diff

Rokicki's code restricts 'diff' to the range of -180 to 180 degrees. Conveniently, this range is perfect to feed into a servo loop. (basically what the last two 'If' statements do)

rokicki · 2007-06-29 16:57

Thanks for the correction; you are absolutely correct.

RinksCustoms · 2007-06-29 17:18

var
long    right_rate 'read-only
long    left_rate  'read-only
pub follow_heading(des_brg,cur_brg) | fsrl
  
    fsrl := ((des_brg /360)  - (cur_brg/360)) * 360 
    right_rate := 0
    left_rate := 0
  case fsrl
    (fsrl => -1 or fsrl <= 1): ' ±1° deviation tolerance (can be omited)
      abort
    (fsrl <= 180 and fsrl >90):
      right_rate := right_rate + 8
    (fsrl <90 and fsrl =>0):
      right_rate := right_rate + 4
    (fsrl => 0):
      right_rate := right_rate + 2
    (fsrl => -180 and fsrl < -90):
      left_rate := left_rate + 8
    (fsrl > -90 and fsrl <0):
      left_rate := left_rate + 4
    (fsrl < 0):
      left_rate := left_rate + 2

Try this method in your code, it should:

Solve your crossing zero problem
give the shortest turning time to the new heading
and supply you with a variable turning rate in both directions

A single line formula, and a small case statement

Please give this code a try, i'd like to know if it works well/works at all. It'd come in handy for a R/C project of mine involving a small·r/c airplane, possibly a small r/c helicopter.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Definetly a E³ (Electronics Engineer Extrodinare!)
"I laugh in the face of imposible,... not because i know it all, ... but because I don't know well enough!"

Graham Stabler · 2007-06-29 17:35

fsrl := ((des_brg /360) - (cur_brg/360)) * 360

is the same as

fsrl := des_brg - cur_brg

And does not deal with the zero crossing.

Jonathan · 2007-06-29 20:30

Thanks for all the help. Here is what I have going so far. Rinks, I couldn't get yours to work.

I finally setup a simple test program. I found that the whichDir statement *is* working properly. The following code *seems* to work under all conditions, but I would like if someone else tried it.

whichDir   := (bearing - heading+ 360) / 360
numDegrees := || (bearing - heading)
if numDegrees > 180
  numDegrees := 360 - numdegrees

See anything obvious I am missing?

Thanks!

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Jonathan · 2007-06-29 20:45

Correction: The whichDir statement IS NOT working properly. The numDegrees is.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Graham Stabler · 2007-06-30 00:47

There is no really elegant way to do this, Rokicki has the best idea but you can do it without if statements. The code attached works for all cases EXCEPT for when the required movement is 180 degrees. There is absolutely no reason to do it this way but I had to get it out of my system and you may find some of the basic concepts interesting:


   cb := 10
   b  := 15
   diff := b-cb                                        ' Difference between angles, positive if CW move needed
   abdiff := ||(b-cb)                                  ' Absolute angular distance


   ' This is a way to compare the absolute difference with 180
   whichway := ((abdiff) - 180) / (||((abdiff)-180))   ' 1 if abs(diff)>180, -1 if abs(diff)<180
   
   ' Produce a value that varies depending on direction of required rotation (assuming at first does not cross zero)
   direction := diff/(abdiff)                           ' 1 CW, -1 if CCW  
   direction := -1 * direction * whichway              ' Reverses direction if difference > 180 (i.e. other direction better)

   ' Now works out distance to travel (effectively takes abs of diff from 360 if == 1) 
   distance := ||(abdiff-((whichway+1)*180))

Tracy Allen · 2007-06-30 15:57

It seems like deja vu, like going in circles, and I wouldn't want to miss out on the fun. Jonathan, I think you were pretty close with the first formula you posted (but no gold ring), and that you are right, it should be a relatively simple formula to subtract two angles, deceptively so.

Here is my suggestion:

rotation := ((bearing - heading + 540) // 360) - 180

where the desired bearing and the current heading are in degrees from 0 to 359, and the rotation is degrees from -180 to +179. The direction is the sign bit.

Let me explain why it works (assuming it does work--I'm open to counterexamples!)? It depends on a particular shift of the coordinate system.

The first graph below shows in red the rotation required when the current heading has a particular value near 100 degrees, as a function of the desired bearing on the x axis from 0 to 360. Note that the rotation required is zero when the desired heading equals the current bearing. If the desired bearing is for example 170 degrees, then it takes a positive rotation of +70 degrees. Very simple graph, slope 1. Should be easy, right? The hard part comes with the discontinuity, which in this graph occurs at 280 degrees, and where the rotation required suddenly jumps from +180 to -180. The gymnastics required in the formulae suggested here have been wrapped up in how to account for the discontinuity. Note the extension of the red line on the lower left.

The second graph shows the same red line, but with the coordinate system shifted to the left. Now it is really easy, because there is no discontinuity! The coordinates are shifted by the amount that puts the current bearing precisely at 180 degrees, and the shift is
(Yj' = 180 - Yj).
The same coordinate shift is applied to the desired bearing,
Yk' = Yk + (180 - Yj)
That could have negative values, so to put it within the interval 0 to 360 we write:
Yk' = (Yk + (180 - Yj) + 360) mod 360
... = (Yk - Yj + 540) mod 360
Then to find the rotation, we have to subtract the shifted value of Yj', which is precisely and by choice 180 degrees.
Yk' = (Yk + (180 - Yj) + 360) mod 360 - 180

In spin:
rotation := ((Yk - Yj + 540) // 360) - 180

Note that 540// 360=180, so you are really back to rotation = Yk-Yj. But the weird thing is that the legerdemain moves it to the only frame of reference that zaps the discontinuity.

attachment.php?attachmentid=47979

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

RinksCustoms · 2007-06-30 16:05

Grahm,
Now that i think about it, your right about the formula, i ran it through my test calculations and it worked the same when the result was passed through the case statement.

Johnathan,
I appologize that my block 'o code didn't work, as i would have had to develope a simulation object to test this code.
It was just a bit of brainstorming i did in about 15 minutes.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Definetly a E³ (Electronics Engineer Extrodinare!)
"I laugh in the face of imposible,... not because i know it all, ... but because I don't know well enough!"

CardboardGuru · 2007-06-30 16:11

I had a go, and gave up on producing an elegant solution. The problem is the lack of an unsigned modulus (//) operator. Subtracting one number from another and having the answer wrap around in the 0 to 359 range should be a straightforward subtract and an unsigned modulus.

CMPSUB is good for doing this kind of stuff stuff in ASM.

Jonathan · 2007-06-30 19:00

Tracy,

You were right in your PM, I HAD misplaced the brackets on the numDegrees formula. One thing about your statement above is that it can return a negative number, so I changed it to:

numDegrees := || (((heading-bearing+540)//360)-180)

And it seems to work perfectly. Thanks so much, I was going crazy because I was sure I had it working before in Spin, and it was working on a calculator.

As to the direction to turn, I am still trying to understand your post. I still have a feeling that the original should work, or at least be able to made to work. I can't make it go on the calculator though, so it is more than just a bracket issue. However I just need to get something working and get past this issue, so I'm going to continue to try making your example work.

Rinks,

Hey, no worries! thanks for taking a stab at it, and I by no means expect a warranty along with any suggestions that you are kind enough to offer!

Thanks for all the help, folks! These things always stump me. As soon as I crack whichDir I can·get back to the meat of the matter.

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Tracy Allen · 2007-06-30 19:43

Hi Jonaathan, just as the numDegrees is the absolute value of rotation, direction is its sign:

rotation := (((heading - bearing + 540) // 360) - 180)
whichDir := rotation >> 31    ' 1 for CW, 0 for CCW or none
numDegrees := || rotation  ' 0 to 180 in whichDirection

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Jonathan · 2007-06-30 19:43

Tracy,

Re-reading your post I realized that you had intentionally left out the ABS because the sign bit is the direction neede to turn. Duh! Anyway, I just did this, and it seems to work!!

numDegrees := (((heading - bearing + 540)//360)-180)
whichDir := numdegrees >> 31
numDegrees := || numdegrees

Whoo hoo! Thanks!

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Tracy Allen · 2007-06-30 19:46

It looks like we crossed pens in the last email! Exactly. Whoo hoo, or

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Jonathan · 2007-06-30 19:46

haha, we crossed posts on that, I had just figgered that out [noparse]:)[/noparse]

Thanks again

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Larry · 2007-06-30 20:17

I did this for a roboMagellan robot a few years ago. I pulled out my old code and the formula I had was

steerAngle= ( ( 540+Bearing) - Heading)) // 360

which yields a number between ) and 360, where 0 was steering hard right, 360 was steering hard left. and 180 was straight ahead.

I was Ackerman steering with a servo, so I wanted a number that was continous, not one with a positive and negative term.

A pulseout value for a servo could be as simple as (570+SteerAngle) but will probably need a little scaling. (180 gives a value of 750, which was straight ahead)
You'll Probably have to do some PID P term at least, to adjust to your robot's geometry.

If you are skid steering, steer right for numbers under 180 whith something like (180-SteerAngle) and for numbers over 180, steer left with (Steerangle-180)

In both these cases the left and right might be reversed in the formulas, depending on your robot's geometry.

Hope this helps

whoops, can't copy my own formulas. I think it's right now.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

Post Edited (Larry) : 6/30/2007 8:54:53 PM GMT

Graham Stabler · 2007-06-30 21:05

It would have helped if I had noticed the // command in the origional formula because I didn't know the command existed. I feel a bit less stupid now [noparse]:)[/noparse]

Graham

Tracy Allen · 2007-07-01 20:17

This is a different problem, also involving computation on a circle. I have an anemometer, wind vane, and during an interval of time it records whether or not the vane has been pointing in which direction. The circle is broken up into 32 possible directions, so the result is stored in a long with ones for each direction detected and zeros otherwise. So at the end of an interval of time it might end up as:
rose = %1010000000000000000000000000111

The subsequent calculation has to determine the spread of values, which in in the above case would be 6, because the circle wraps around from msb to lsb and the hole counts as part of the spread. Actually, any number of holes count as part of the spread, and the object is to compute the minimum over the circle. Here is the snippet I came up to do that. I still wonder if it could be done without the repeat. Can anyone think of a way? The repeat rotates the current value of rose into all 32 positions and tracks the minimum value returned by the encode operator |<.

]PRI Spread(rose)
result := |< rose
repeat 32
  result := |< rose <# result  'index highest bit and and track minimum
  rose <-= 1    ' rotate msb to lsb

There is a class of problems on a circle like this. In robotics it might be to find the widest open gap detected by a rotating turrent.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Tracy Allen
www.emesystems.com

Jonathan · 2007-07-02 15:51

Tracy, All,

I can already see some applications for the formula in your last post. So too with Larry's. Of course, I can't think of any way to do it without the repeat. But do let us know if it comes to you in a stroke of brilliance.

Jonathan

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
www.madlabs.info - Home of the Hydrogen Fuel Cell Robot

Some math help

Comments