Compass module and how to make 355 + 10 = 5 (or 5-10 = 355)

SteveWoodrough · 2009-03-08 03:03

I have a routine in my current project that aligns the BOT Heading (I've defined Heading as the direction the BOT is pointed) to a selected Azimuth (I've defined Azimuth as the users desired BOT Heading.· Heading is where its pointed, Azimuth is where I want it to go.· Once an Azimuth is input, the Bot looks at the compass, determines the Heading and pivots right or left to align the heading to the desired Azimuth.· If the Heading is 80 degrees and the desired Azimuth 90 the bot turns right.

The routine works fine for any selected Azimuth that is not close to 0/360.·

The trouble for Azimuths close 0 or 360 is my logic breaks down.· How does one do the math so that the 355 is recognized as being 10 degrees from 5 degrees the same as 15 degrees is 10 degrees from 5 degrees?

Certainly SOMEONE has solved the problem already....
Thanks...Steve

··· '---[noparse][[/noparse]AlignHeading]

Aligns BOT Heading to desired Azimuth
AlignHeading:
· Speed = speed / 10······························ ' Slows down servo movements while finding azimuth
·DO

· GOSUB Compass····································· ' Get current Heading from Compass module
· IF Heading > Azimuth+10 THEN: GOSUB LeftPivot····· ' If the Heading is greater than Azimuth· by 10 degrees turn Left
· ELSEIF Heading < Azimuth-10 THEN: GOSUB RightPivot ' If the Heading is less than Azimuth· by 10 degrees turn Right
· ENDIF
LOOP UNTIL Heading > Azimuth-10 AND Heading < Azimuth+10· ' Loop stops once BOT Heading is between +/- 10 Degrees of desired
· Speed = speed * 10···································· ' Returns BOT speed to normal
RETURN

·

SRLM · 2009-03-08 03:26

Yep, I've solved this problem in the link below. The program as a whole takes GPS coordinates and does calculations, but you'll just want to look at the end of the program.

http://forums.parallax.com/showthread.php?p=768034

Does this do what you need?

SteveWoodrough · 2009-03-08 04:02

I'm reading it but not following the logic.· I'm so dense I've been known to stop neutrinos!

Walk me though this please.· I'm stuck at: ·
IF(theta1 > (180 + theta1)) THEN······

'how can theta 1 be greater than the same value plus 180?··Is theta 1 in brads (which could be the answer to the problem) Not likely since you are adding 180 which would logically be degrees....
··· theta1 = theta1 + 360

' likewise this gives a peculiar value unless the units are somewhere converted back to brads.

The more I think about this the more I am convinced that the most elegant solution is to·do all computation work in brads and save the conversion·from and to degrees for user interface only (since we, at least I, don't think in terms or radians but rollover math becomes automatic)

I would like to better understand your code.
Thanks...Steve

'

CalculateDesiredHeading

'This section recieves two variables in the form of
'degree values 0-360. One is the desired heading as given
'by _GetGPSHeading (above), the other is given by the compass
'Theta1 is compass heading, theta2 is GPS heading
_CalculateDesiredHeading:
· 'get the current heading
· GET 39, Word theta1
· 'theta2 is already given by previous method
· 'DEBUG "Enter the Current heading", CR
· 'DEBUGIN DEC3 theta1
· IF(theta1 > (180 + theta1)) THEN
··· theta1 = theta1 + 360
· ELSE
··· IF(theta1>(180 + theta2)) THEN
····· theta2 = theta2 + 360
··· ENDIF
· ENDIF
· IF(theta1 > theta2) THEN
··· LOOKDOWN theta1 - theta2, >=[noparse][[/noparse]55,40,20,0], headingGPS
··· headingGPS = headingGPS + 1
· ELSE
··· LOOKDOWN theta2 - theta1, >=[noparse][[/noparse]55, 40, 20, 0], headingGPS
··· headingGPS = 7 - headingGPS
· ENDIF
· DEBUG DEC1 ? headingGPS, CR, CR
· PUT 48, headingGPS
RETURN

MrBi11 · 2009-03-08 04:14

haven't tested it to confirm with PBASIC but Modulus would be the answer:

if (heading - Azimuth // 360) > 10 then ......

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Smile ... It increases your face value!

Mike Green · 2009-03-08 05:17

The problem you're having is that the measurement system is circular and 0 is the same as 360. For that matter, 10 is the same as 370. You compute result = (heading - Azimuth) // 360, then you take the minimum of result and 360 - result. In the case you mentioned, heading is 355 and Azimuth is 5. You have result = (355 - 5) // 360 = 350. If this is negative, you add 360. You then take the minimum of 350 and 360-350 = 10 to get the value 10.

If things were reversed, you'd have heading = 5 and Azimuth = 355. You have result = (5 - 355) // 360 = -350 and add 360 to get 10. You take the minimum of 10 and 360-10 = 350 to get the value 10.

SRLM · 2009-03-08 05:40

Wow. I feel embarrassed for writing IF(0 > 180). I suppose I should have included more documentation too. Anyway, I rewrote it much more clearly and in a much more useful manner:

theta1     VAR Word
theta2     VAR Word
dir        VAR Bit '0 = left, 1 = right -- desired heading is to the dir of current heading
difference VAR Byte 'Difference between desired heading and current heading

'This loop is to test the function
DO
  DEBUG CR, "Enter the Current heading", CR
  DEBUGIN DEC3 theta1
  DEBUG CR, "Enter the Desired heading", CR
  DEBUGIN DEC3 theta2
  GOSUB _CalculateDesiredHeading
  DEBUG CR, DEC3 ? difference, CR, DEC1 ? dir, CR
LOOP

'This function calculates a direction to turn and an amount required to turn.
'This is intended for use in mobile robots or turrets.
_CalculateDesiredHeading:
  IF(theta1 >= 360) THEN theta1 = theta1 // 360 'Adjust Range
  IF(theta2 >= 360) THEN theta2 = theta2 // 360 'Adjust Range

  IF(theta1 > theta2)THEN
    IF((theta1-theta2)> 180) THEN 'Want to go right (over 0 degree mark)
      dir = 1
      difference = (360 - theta1) + theta2
    ELSE 'Want to go left
      dir = 0
      difference = theta1 - theta2
    ENDIF
  ELSE 'theta2 > theta1
    IF((theta2-theta1) > 180) THEN 'Want to go left (over 0 degree mark)
      dir = 0
      difference = (360 - theta2) + theta1
    ELSE 'Want to go right
      dir = 1
      difference = theta2 - theta1
    ENDIF
  ENDIF
RETURN

Post Edited (SRLM) : 3/8/2009 5:45:55 AM GMT

Beau Schwabe · 2009-03-08 06:04

SteveWoodrough,

Here is a way to scale the DEG so that it "wraps around" (i.e. 360 equals 0) to do what you want...

Value1  VAR   Word
Value2  VAR   Word

Temp    VAR   Word


'Example1:
Value1 = 355
Value2 = 10

Value1 = Value1*/46604          ' Scale 0-359 to 0-65535
Value2 = Value2*/46604          ' Scale 0-359 to 0-65535

Temp = (Value1 + Value2)/182    ' Scale 0-65535 to 0-359

DEBUG DEC Temp,CR


'Example2:
Value1 = 5
Value2 = 10

Value1 = Value1*/46604          ' Scale 0-359 to 0-65535
Value2 = Value2*/46604          ' Scale 0-359 to 0-65535

Temp = (Value1 - Value2)/182    ' Scale 0-65535 to 0-359

DEBUG DEC Temp,CR

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Post Edited (Beau Schwabe (Parallax)) : 3/8/2009 6:20:27 AM GMT

SteveWoodrough · 2009-03-10 03:06

Beau,
Thank You.· I'll try this out tonight.· One question:
You suggest a value of 46604, however, Page 101 of Smart Sensors and Applications suggests a value of 46733 for a similar conversion.· Am I splitting hairs or is there some detail I'm missing?

Best Regards
Steve

Beau Schwabe · 2009-03-10 06:11

SteveWoodrough,

I knew you were going to ask that..

Using 46733 is equivalent to multiplying the Deg value by 182.5508 and was derived by taking 65536 and dividing it by 359 which works out to...

182.5515 ...· multiplying 182.5515 by 256 gives us 46733 .... Note: 182.5515 is the desired value·.... 182.5508 is the actual value due to rounding.

... I simply used 360 instead of 359 ...· 65536 divided by 360 works out to ...

182.0444 ... multiplying 182.0444 by 256 gives us 46603 ( <-- a little testing indicated that I needed to bump the value up by 1 ) ... rounding error since the 'desired' value of 182.0444 actually works out to 182.0430 when going the other way (46603 / 256 =·182.0430)·.· Bumping this value up to 46604 works out to 182.0469.

Using·46733 will cause a value of 360 to not·properly translate to 0 during the 16-bit roll-over.·

359 * 182.5508 = 65535
360 * 182.5508 = 65718 = 182 (<-after the 16-bit roll-over)

Instead, if you use 46604, a value of 360 will properly translate to 0

359 * 182.0469 = 65534
360 * 182.0469 = 65536 = 0 (<-after the 16-bit roll-over)

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

SteveWoodrough · 2009-03-21 02:37

I don't think I phrased my original question or explained my challenge well or I am not understanding Mike's, MrBi's and Beau's concepts.

I understand scaling, and how to make the 16 bit values wrap around but I'm having trouble understanding how the suggested modulus operator solution·// is going to work.

The logic I'm unsuccesfully using is:

If Heading > Azimuth + 10 then Turn Left
·Else if Heading < Azimuth - 10 then Turn Right

LOOP UNTIL Heading > (Azimuth-10) and Heading < (Azimuth+10)

If my Azimuth (desired direction) is 90 degrees and the heading is 105 then the result is a left turn. Good

If my Azimuth (desired direction) is 90 degrees and the heading is 75 then the result is a right turn. Good

However, if my Azimuth (desired direction) is 0 degrees and the heading is 345 (the same 15 deg from desired)·then the result is a left turn ALL the way around.· And if the BOT overshoots to 359 degrees heading·the only recovery is again a long slow turn to the left.· Seems to me there ought to be a clever way to write the steering logic to accomodate the special case when the Azimuth is close to 0.

Thanks for your help and consideration.·

Jonathan · 2009-03-21 03:38

Here is a snip I used to do this. Its been a while, but I do believe this worked. It's in Spin, but the math is the same.

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

NumDegrees has the number of degrees needed to turn and whichDir tells you whether to turn clockwise or counter-clockwise.

HTH

Jonathan

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

FearTurtles · 2009-03-21 16:37

Would this work? New to programing and wanted to try to figure this out.


Bearing VAR Word                          'Bots current heading
ADJDeg VAR Word                         'Requested degrees of adjustment
Heading VAR Word                          'Next heading
Dir_Trn  VAR Bit                             'Determines what direction to turn

Bearing = 355
ADJDeg = 10

                                                     'Figure out the direction to turn
IF ADJDeg > 0 Then                        'Basically if it is the + or - that determines which way to turn
   Dir_Trn = 0                                 'This woud be helpful if you needed to take sensor readings in an area greater then 180 degrees 
ELSEIF ADJDeg < 0 THEN            
   Dir_Trn = 1


'Now to figure out how far to turn.


Heading = Bearing + ADJDeg           'Heading = 365   If ADJDeg was a negitive number then it would subtract
  IF Heading = 0 then                       'here we are avoiding the use of 0 deg
     Heading = 360
  ENDIF

  IF Heading > 360 Then                   'This will determine if it passed 0              
    Heading = Heading - 360              'Sets heading to 5
  ELSEIF Heading < 1 Then                
    Heading = SDEC ? ABS Heading    ' this changes converts heading from a negitive to positive
    Heading = 359 - Heading.             'This completes Figuring out the needed new heading -1 because we are not counting 0
  ENDIF

Post Edited (FearTurtles) : 3/24/2009 3:18:22 PM GMT

FearTurtles · 2009-03-21 16:40

Wow looks like I needed to some how format that better. Sorry.

SRLM · 2009-03-23 06:59

@steve

What you can do is to test to see if the ten degree band lies on the 0 degree boundary, and make a special case for if it does. The (revised) code that I posted does that.

Modulus just returns the remainder (r) of an integer division.

@Turtles

You can format your code nicely with the [noparse][[/noparse] code ] tag. As for what would work, again, take a look at the code I posted. It sets a bit and an amount.

FearTurtles · 2009-03-24 14:54

Thanks SRLM, So you're saying that my code won't work? Well Accually I took another look at my code and saw that I needed Words and not Bytes. LOL

Post Edited (FearTurtles) : 3/24/2009 3:19:19 PM GMT

Compass module and how to make 355 + 10 = 5 (or 5-10 = 355)

Comments