diff --git a/macros/TEST_SPEED.cfg b/macros/TEST_SPEED.cfg
index 9f36892..589214e 100644
--- a/macros/TEST_SPEED.cfg
+++ b/macros/TEST_SPEED.cfg
@@ -6,7 +6,9 @@
 [gcode_macro TEST_SPEED]
 gcode:
     # Speed
-    {% set speed  = params.SPEED|default(printer.configfile.settings.printer.max_velocity)|int %}
+    {% set speed = params.SPEED|default(printer.configfile.settings.printer.max_velocity)|int %}
+    # Safe speed, for homing and setup
+    {% set safe_speed = params.SAFE_SPEED|default(printer.configfile.settings.printer.max_velocity / 3)|int %}
     # Iterations
     {% set iterations = params.ITERATIONS|default(5)|int %}
     # Acceleration
@@ -50,48 +52,70 @@ gcode:
                 G28 Z
             {% endif %}
         {% endif %} 
-        # Move 50mm away from max position and home again (to help with hall effect endstop accuracy - https://github.com/AndrewEllis93/Print-Tuning-Guide/issues/24)
+        # Move 50mm away from max position and home X Y (to help with hall effect endstop accuracy - https://github.com/AndrewEllis93/Print-Tuning-Guide/issues/24)
         G90
-        G1 X{printer.toolhead.axis_maximum.x-50} Y{printer.toolhead.axis_maximum.y-50} F{30*60}
+        G0 X{printer.toolhead.axis_maximum.x-50} Y{printer.toolhead.axis_maximum.y-50} F{safe_speed*60}
+        G4 P2000 #delay before homing in case StallGuard was triggered and if the printer uses sensorless homing
         M400 # Finish moves - https://github.com/AndrewEllis93/Print-Tuning-Guide/issues/66
         G28 X Y
-        G0 X{printer.toolhead.axis_maximum.x-1} Y{printer.toolhead.axis_maximum.y-1} F{30*60}
+        # Move to corner for position measurement
+        G0 X{printer.toolhead.axis_maximum.x-1} Y{printer.toolhead.axis_maximum.y-1} F{safe_speed*60}
         G4 P1000 
         GET_POSITION
 
     # Go to starting position
-    G0 X{x_min} Y{y_min} Z{bound + 10} F{speed*60}
-
+    G0 X{x_min} Y{y_min} Z{bound + 10} F{safe_speed*60}
+    
     # Set new limits
     SET_VELOCITY_LIMIT VELOCITY={speed} ACCEL={accel} ACCEL_TO_DECEL={accel / 2}
 
     {% for i in range(iterations) %}
-        # Large pattern diagonals
+        # Diagonals
         G0 X{x_min} Y{y_min} F{speed*60}
         G0 X{x_max} Y{y_max} F{speed*60}
         G0 X{x_min} Y{y_min} F{speed*60}
+        G0 X{x_max} Y{y_max} F{speed*60}
+        G0 X{x_min} Y{y_max} F{speed*60}
         G0 X{x_max} Y{y_min} F{speed*60}
         G0 X{x_min} Y{y_max} F{speed*60}
         G0 X{x_max} Y{y_min} F{speed*60}
-        
-        # Large pattern box
+
+        # Box
+        G0 X{x_min} Y{y_min} F{speed*60}
+        G0 X{x_max} Y{y_min} F{speed*60}
+        G0 X{x_min} Y{y_min} F{speed*60}
+        G0 X{x_min} Y{y_max} F{speed*60}
         G0 X{x_min} Y{y_min} F{speed*60}
         G0 X{x_min} Y{y_max} F{speed*60}
         G0 X{x_max} Y{y_max} F{speed*60}
+        G0 X{x_min} Y{y_max} F{speed*60}
+        G0 X{x_max} Y{y_max} F{speed*60}
         G0 X{x_max} Y{y_min} F{speed*60}
-    
-        # Small pattern diagonals
+        G0 X{x_max} Y{y_max} F{speed*60}
+        G0 X{x_max} Y{y_min} F{speed*60}
+
+        # Small diagonals 
         G0 X{x_center_min} Y{y_center_min} F{speed*60}
         G0 X{x_center_max} Y{y_center_max} F{speed*60}
         G0 X{x_center_min} Y{y_center_min} F{speed*60}
+        G0 X{x_center_max} Y{y_center_max} F{speed*60}
+        G0 X{x_center_min} Y{y_center_max} F{speed*60}
         G0 X{x_center_max} Y{y_center_min} F{speed*60}
         G0 X{x_center_min} Y{y_center_max} F{speed*60}
         G0 X{x_center_max} Y{y_center_min} F{speed*60}
-        
-        # Small patternbox
+
+        # Small box
+        G0 X{x_center_min} Y{y_center_min} F{speed*60}
+        G0 X{x_center_max} Y{y_center_min} F{speed*60}
+        G0 X{x_center_min} Y{y_center_min} F{speed*60}
+        G0 X{x_center_min} Y{y_center_max} F{speed*60}
         G0 X{x_center_min} Y{y_center_min} F{speed*60}
         G0 X{x_center_min} Y{y_center_max} F{speed*60}
         G0 X{x_center_max} Y{y_center_max} F{speed*60}
+        G0 X{x_center_min} Y{y_center_max} F{speed*60}
+        G0 X{x_center_max} Y{y_center_max} F{speed*60}
+        G0 X{x_center_max} Y{y_center_min} F{speed*60}
+        G0 X{x_center_max} Y{y_center_max} F{speed*60}
         G0 X{x_center_max} Y{y_center_min} F{speed*60}
     {% endfor %}
 
@@ -108,4 +132,4 @@ gcode:
         GET_POSITION
 
     # Restore previous gcode state (absolute/relative, etc)
-    RESTORE_GCODE_STATE NAME=TEST_SPEED
\ No newline at end of file
+    RESTORE_GCODE_STATE NAME=TEST_SPEED