Direction to steer problem

W9GFO · 2007-04-23 22:08

Hi all,

This has been giving me headaches, I thought I had it figured out but nooooo. There has got to be a simple way to do this but it escapes me.

I have a direction I need to go (0 to 255)

I have the heading which I am traveling (0 to 255 )

Lastly, I have the Deviation, amount that I am off course (0 to 128)

I need to figure out which way to turn!

It's obvious to me when looking at the numbers which way it should turn but
putting that into a formula has baffled me.

I've tried several formulas but I can't get one to work with all possibilities.

Rich H

Unsoundcode · 2007-04-24 02:35

This may seem a little cumbersome but it will give you the shortest turn from direction of travel to destination direction.

IF compass_heading <128 THEN
heading =1
ELSE
heading =3
ENDIF
IF dir_of_travel <128 THEN
dir =3
ELSE
dir=4
ENDIF
turn_value=heading+dir
SELECT turn_value
CASE 4,7
· IF dir_of_travel > compass_heading THEN
· GOSUB turn_left
· ELSE
· GOSUB turn_right
· ENDIF
CASE 5,6
· dir_of_travel=dir_of_travel-128
· IF dir_of_travel > compass_heading THEN
· GOSUB turn_right
· ELSE
· GOSUB turn_left
· ENDIF
ENDSELECT

Jeff T.

W9GFO · 2007-04-24 04:30

Thanks Jeff, but it is not working for me. Below is a test program where you can enter heading and direction and it will return
how far off course and direction to steer.

I changed some names so that I can follow it better with what I have already written.



' {$STAMP BS2}
' {$PBASIC 2.5}

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

servo        PIN     12

' -----[noparse][[/noparse] Variables ]-------------------------------------------------------

d            VAR      Nib      ' "dir"
h            VAR      Nib      ' "heading"
heading      VAR      Byte     ' "compas_heading"
dir          VAR      Byte     ' "dir_to_destination"
workVal      VAR      Word     ' divisor for reducing distances and other stuff
dev          VAR      Byte     ' brads off course
steerDir     VAR      Bit      ' turn left or right? 0 = right

' -----[noparse][[/noparse] Initialization ]--------------------------------------------------

Initialize:
  PAUSE 250              ' let DEBUG open
  DEBUG CLS              ' clear the screen
  dev = 0                ' so that we start with servo centered
  GOSUB steer            '

' -----[noparse][[/noparse] Program Code ]----------------------------------------------------

Main:
DO
  GOSUB Navigate
  GOSUB Steer
  PAUSE 10000             'gives time to look at answer
  DEBUG CLS
LOOP

END

'------[noparse][[/noparse] Subroutines ]-----------------------------------------------------

Navigate:
  GOSUB Get_Head
  GOSUB Get_Dir
    DEBUG "Dir to go ", DEC dir,  CLREOL,CR
    DEBUG "Heading = ", DEC heading,  CLREOL,CR
  GOSUB Get_Dev
    DEBUG "Off course ", DEC dev,  CLREOL,CR
    IF steerDir = 0 THEN
    DEBUG "Steer Right",CR
    ELSE
    DEBUG "Steer Left",CR
    ENDIF
RETURN

'--------------------------------------------------------------------------

Steer:
  workval = 0
  FOR workval = 1 TO 15
  IF steerDir = 0 THEN                        'use dev for amount and steerDir for direction
  PULSOUT servo, 750 + (dev*4)MAX(1100)
  PAUSE 20
  ELSE
  PULSOUT servo, 750 - (dev*4)MIN(400)
  PAUSE 20
  ENDIF
  NEXT

RETURN

'--------------------------------------------------------------------------

Get_Head:
  DEBUG "Enter Heading "
  DEBUGIN DEC heading
RETURN

'--------------------------------------------------------------------------

 Get_Dir:
  DEBUG "Enter Direction "
  DEBUGIN DEC dir
RETURN
'--------------------------------------------------------------------------

Get_Dev:

  dev = ABS(heading - dir)
  dev = dev MAX(256-dev)

  IF heading < 128 THEN
    h = 1
    ELSE
    h = 3
  ENDIF
    IF dir < 128 THEN
    d = 3
    ELSE
    d = 4
  ENDIF

  workVal = h + d

  SELECT workVal
  CASE 4,7
    IF dir > heading THEN
      steerDir = 0
      ELSE
      steerDir = 1
    ENDIF
  CASE 5,6
  dir = dir - 128
    IF dir > heading THEN
      steerDir = 0
      ELSE
      steerDir = 1
    ENDIF
  ENDSELECT
RETURN

'------------[noparse][[/noparse] END ]-------------------------------------------------------

Bruce Bates · 2007-04-24 09:24

Rich -

You may fnd this course-to-steer calculator helpful. Just look at the math behind it:
http://www.sailingusa.info/course_to_steer.htm

This next comment is intended to be helpful although it may not sound that way. First, you need to learn the proper terminology for (dead reckoning) navigation. Second, your incorrect usage of said terminology may well be dring you nuts. Lastly, you are making this way more complicated than it actaully is.

Break it down into smaller pieces instead on one universal formula that covers every course correction factor known to man, and then add the peices together. Start with something like: If my outbound course is nnn degrees, what is my return course? You may then want to add a return course correction based on the calculated error on the outbound course. Then look at waypoints and corrections therein, etc. Build on a set of basic formulas.

Presuming this is a land-mobile platform, just be glad you don't have to add in current, wind and magnetic deviation considerations, along with hull drag, instrumentation, and Murphy errors

Good luck with it.

Regards,

Bruce Bates

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

Unsoundcode · 2007-04-24 13:43

Hi Rich, just a little insight into my line of thought . Direction headed and direction needed·to go in their simplest form can each have two possible values,·east of zero·or west of zero. That gives 4 permutations E-E,E-W,W-E and W-W. Depending on which combination it is the value is held in turn_value.

·You can calculate the error knowing this information, for example if the direction headed is E40 and you want to be at E60 the error is 60-40=20 left.·If direction headed was E80 then the error would be 20 right, right or left is dependent on whether heading is greater or·less than direction needed to go·.

If one value is east and one value is west our existing code already tells us which way we need to turn but calculating the error is slightly different, for example if we need to be at a heading of W250 and we are travelling toward E10 the equation would be

255-(heading-direction)=255-(250-10)=error 15 left.

the code I posted only tells you which way to turn which is what you originally asked for, but it also contains enough information to calculate the amount of error·of the direction you want to turn. It requires an error calculator for each of the four possible scenarios

Jeff T

CCraig · 2007-04-24 14:23

Here's some I did. I can't find my complete notes, but I think it works.

DC = desired course
H = heading

angle = ((360 - DC) + H)
if angle > 360 then angle = angle - 360
if angle < 180 then angle = left turn angle
if angle > 180 then angle = (360 - angle) right turn angle

You'll have to use a word.

HTH, Chris

W9GFO · 2007-04-24 16:08

Hi guys,

Maybe I am not properly implementing your suggestions, I can't get these to work.

Please look at the following program and let me know if I am doing something weird. I have tried to make it a bit easier
to read and condensed it a bit.

If I enter 193 for heading and 64 for direction it tells me that I am off course by 127 and I should steer right (good).

If I enter 191 for heading and 64 for direction it tells me that I am off course by 127 (good) and that I should steer right (bad).

' {$STAMP BS2}
' {$PBASIC 2.5}

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

servo        PIN     12

' -----[noparse][[/noparse] Variables ]-------------------------------------------------------

angle        VAR      Word
heading      VAR      Byte     ' "compas_heading"
dir          VAR      Byte     ' "dir_to_destination"
workVal      VAR      Word     ' divisor for reducing distances and other stuff
dev          VAR      Byte     ' brads off course
steerDir     VAR      Bit      ' turn left or right? 0 = right

' -----[noparse][[/noparse] Initialization ]--------------------------------------------------

Initialize:
  PAUSE 250              ' let DEBUG open
  DEBUG CLS              ' clear the screen
  dev = 0
  GOSUB steer            '

' -----[noparse][[/noparse] Program Code ]----------------------------------------------------

Main:
DO
  GOSUB Steer             ' so that we start with servo centered

  DEBUG CLS
  DEBUG "Enter Heading "
  DEBUGIN DEC heading
  DEBUG "Enter Direction "
  DEBUGIN DEC dir
  DEBUG "Dir to go ", DEC dir,  CLREOL,CR
  DEBUG "Heading = ", DEC heading,  CLREOL,CR
  dev = ABS(heading - dir)              ' tells us how far off course we are
  dev = dev MAX(256-dev)
  DEBUG "Off course ", DEC dev,  CLREOL,CR


'**************************************************************************
'------------- *** enter direction to turn solution here *** --------------

  angle = ((256 - dir) + heading)
  IF angle > 256 THEN angle = angle - 256
  IF angle < 128 THEN angle = 1 'left
  IF angle > 128 THEN angle = 0' (256 - angle)  'right

  DEBUG ? angle

'--------------------------------------------------------------------------
'**************************************************************************


  IF steerDir = 0 THEN
    DEBUG "Steer Right",CR              ' tells us which way to steer
    ELSE
    DEBUG "Steer Left",CR
  ENDIF
  GOSUB Steer
  PAUSE 8000             ' gives time to look at answer
LOOP

'--------------------------------------------------------------------------

Steer:
  workval = 0
  FOR workval = 1 TO 15
  IF steerDir = 0 THEN                   'use dev for amount and steerDir for direction
  PULSOUT servo, 750 + (dev*4)MAX(1100)
  PAUSE 20
  ELSE
  PULSOUT servo, 750 - (dev*4)MIN(400)
  PAUSE 20
  ENDIF
  NEXT

RETURN

'------------[noparse][[/noparse] END ]-------------------------------------------------------

CCraig · 2007-04-25 14:11

Check your math.
193 and 64
and
191 and 64

both can't be 127

Chris

W9GFO · 2007-04-25 15:01

Chris, I think they can be the same.

dev = ABS(heading - dir)
dev = dev MAX(256-dev)

In Binary radians, where 0 is North, 64 is East, 128 South and 192 West, the difference between (191 and 64) and (193 and 64) is both 127 binary radians.

Rich H

Unsoundcode · 2007-04-25 22:34

HI Rich, I wish I knew more about calculating angles·you would think·this could be done with the example Chris gave, it certainly looks compact. Anyway I looked at the example I posted and as you said there were several situations where it threw out bad values. I have another (cumbersome) offering·and I set up a small debug routine to check it with, it seemed good to me.

Jeff T.

CCraig · 2007-04-26 01:50

Rich,

'

*** enter direction to turn solution here ***

angle = ((256 - dir) + heading) (256-64) + 193 = 385
IF angle > 256 THEN angle = angle - 256 385 - 256 = 129
IF angle < 128 THEN angle = 1 'left
IF angle > 128 THEN angle = 0' (256 - angle) 'right 256 - 129 = 127 right turn

'

*** enter direction to turn solution here ***

angle = ((256 - dir) + heading) (256-64) + 191 = 383
IF angle > 256 THEN angle = angle - 256 383 -256 = 127
IF angle < 128 THEN angle = 1 'left 127 left turn
IF angle > 128 THEN angle = 0' (256 - angle) 'right

I'm must be missing it.

HTH, Chris

Direction to steer problem

Comments