Homing Routine

macro_3.sbp

' HOMING ROUTINE
' RYAN STURMER
' WRITTEN FOR FABMO PLATFORM FOR A HANDIBOT

&approach_speed_fast = 1.0
&approach_speed_slow = 0.25
&search_distance_x = -8.0
&search_distance_y = -6.0
&backoff_x = 0.25
&backoff_y = 0.25
&backoff_z = 0.25

'Save the XY speed
&saved_speed = %(71)

' Make sure we're off the switches
IF %(53) = 1 THEN GOSUB backoff_all

' Home the X-axis
VS,&approach_speed_fast
GOSUB approach_x
GOSUB backoff_x
VS,&approach_speed_slow
GOSUB approach_x
ZX
GOSUB backoff_x

' Home the Y-axis
VS,&approach_speed_fast
GOSUB approach_y
GOSUB backoff_y
VS,&approach_speed_slow 
GOSUB approach_y
ZY
GOSUB backoff_y

' Restore the XY speed
VS,&saved_speed
END

' SUBROUTINES BELOW HERE

approach_x:
   ON INPUT(3,0) RETURN
   MX,&search_distance_x
   ' FAIL if we got here
   END
   
approach_y:
   ON INPUT(3,0) RETURN
   MY,&search_distance_y
   'FAIL if we got here also
   END
   
backoff_x:
   JX,%(1) + &backoff_x
   RETURN

backoff_y:
   JY,%(2) + &backoff_y
   RETURN

backoff_all:
   J2,%(1)+&backoff_x,%(2)+&backoff_y
   RETURN