Direction to steer problem
W9GFO
Posts: 4,010
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
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
Comments
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.
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 ]-------------------------------------------------------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
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
·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
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
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 ]-------------------------------------------------------193 and 64
and
191 and 64
both can't be 127
Chris
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
Jeff T.
'
*** 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