diff --git a/config/example-delta.cfg b/config/example-delta.cfg
index 71ad750a..b6a25fdb 100644
--- a/config/example-delta.cfg
+++ b/config/example-delta.cfg
@@ -123,12 +123,3 @@ radius: 50
 #horizontal_move_z: 5
 #   The height (in mm) that the head should be commanded to move to
 #   just prior to starting a probe operation. The default is 5.
-#samples: 1
-#   The number of times to probe each point.  The probed z-values will
-#   be averaged. The default is to probe 1 time.
-#samples_result: average
-#   One can choose median or average between probes samples
-#   The default is average.
-#sample_retract_dist: 2.0
-#   The distance (in mm) to retract between each sample if sampling
-#   more than once. The default is 2mm.
diff --git a/config/example-extras.cfg b/config/example-extras.cfg
index b0f8cdf3..2b0bdb1c 100644
--- a/config/example-extras.cfg
+++ b/config/example-extras.cfg
@@ -30,6 +30,15 @@
 #   triggers. This parameter must be provided.
 #speed: 5.0
 #   Speed (in mm/s) of the Z axis when probing. The default is 5mm/s.
+#samples: 1
+#   The number of times to probe each point. The probed z-values will
+#   be averaged. The default is to probe 1 time.
+#sample_retract_dist: 2.0
+#   The distance (in mm) to lift the toolhead between each sample (if
+#   sampling more than once). The default is 2mm.
+#samples_result: average
+#   The calculation method when sampling more than once - either
+#   "median" or "average". The default is average.
 #activate_gcode:
 #   A list of G-Code commands (one per line; subsequent lines
 #   indented) to execute prior to each probe attempt. This may be
@@ -72,6 +81,9 @@
 #y_offset:
 #z_offset:
 #speed:
+#samples:
+#sample_retract_dist:
+#samples_result:
 #   See the "probe" section for information on these parameters.
 
 
@@ -102,15 +114,6 @@
 #horizontal_move_z: 5
 #   The height (in mm) that the head should be commanded to move to
 #   just prior to starting a probe operation. The default is 5.
-#samples: 1
-#   The number of times to probe each point.  The probed z-values
-#   will be averaged.  The default is to probe 1 time.
-#samples_result: average
-#   One can choose median or average between probes samples
-#   The default is average.
-#sample_retract_dist: 2.0
-#   The distance (in mm) to retract between each sample if
-#   sampling more than once.  Default is 2mm.
 
 
 # Mesh Bed Leveling. One may define a [bed_mesh] config section
@@ -147,15 +150,6 @@
 #horizontal_move_z: 5
 #   The height (in mm) that the head should be commanded to move to
 #   just prior to starting a probe operation. The default is 5.
-#samples: 1
-#   The number of times to probe each point.  The probed z-values
-#   will be averaged.  The default is to probe 1 time.
-#samples_result: average
-#   One can choose median or average between probes samples
-#   The default is average.
-#sample_retract_dist: 2.0
-#   The distance (in mm) to retract between each sample if
-#   sampling more than once.  Default is 2mm.
 #bed_radius:
 #   Defines the radius to probe for round beds.  Note that the radius
 #   is relative to the nozzle's origin, if using a probe be sure to
@@ -283,15 +277,6 @@
 #horizontal_move_z: 5
 #   The height (in mm) that the head should be commanded to move to
 #   just prior to starting a probe operation. The default is 5.
-#samples: 1
-#   The number of times to probe each point.  The probed z-values
-#   will be averaged.  The default is to probe 1 time.
-#sample_retract_dist: 2.0
-#   The distance (in mm) to retract between each sample if
-#   sampling more than once.  Default is 2mm.
-#samples_result: median
-#   One can choose median or average between screw probes
-#   The default is average.
 #screw_thread: CW-M3
 #   The type of screw used for bed level, M3, M4 or M5 and the
 #   direction of the knob used to level the bed, clockwise decrease
@@ -325,15 +310,6 @@
 #horizontal_move_z: 5
 #   The height (in mm) that the head should be commanded to move to
 #   just prior to starting a probe operation. The default is 5.
-#samples: 1
-#   The number of times to probe each point.  The probed z-values
-#   will be averaged.  The default is to probe 1 time.
-#samples_result: average
-#   One can choose median or average between probes samples
-#   The default is average.
-#sample_retract_dist: 2.0
-#   The distance (in mm) to retract between each sample if
-#   sampling more than once.  Default is 2mm.
 
 
 # Moving gantry leveling using 4 independently controlled Z motors.
@@ -374,13 +350,6 @@
 #max_adjust: 4
 #   Saftey limit if an ajustment greater than this value is requested
 #   quad_gantry_level will abort.
-#samples: 1
-#   Number of probe samples per point. The defaut is 1
-#samples_result: average
-#   One can choose median or average between probes samples
-#   The default is average.
-#sample_retract_dist: 2.0
-#   Distance in mm to retract the probe between samples. Default is 2.
 
 
 # In a multi-extruder printer add an additional extruder section for
diff --git a/config/kit-voron2-250mm.cfg b/config/kit-voron2-250mm.cfg
index 73257af6..278d4299 100644
--- a/config/kit-voron2-250mm.cfg
+++ b/config/kit-voron2-250mm.cfg
@@ -158,6 +158,10 @@ x_offset: 0.0
 y_offset: 25.0
 z_offset: 0.00
 speed: 2.0
+samples: 4
+#   Number of times to probe a point
+sample_retract_dist: 6.0
+#   How far to retract (in mm) from the probe point for multi-probe samples
 
 [fan]
 # Print cooling fan
@@ -231,10 +235,6 @@ points:
 #   Probe points
 speed: 200
 horizontal_move_z: 6
-samples: 4
-#   Number of times to probe a point
-sample_retract_dist: 6.0
-#   How far to retract (in mm) from the probe point for multi-probe samples
 
 [display]
 # RepRapDiscount 128x64 Full Graphic Smart Controller
diff --git a/config/kit-zav3d-2019.cfg b/config/kit-zav3d-2019.cfg
index 502d0645..01beadcb 100644
--- a/config/kit-zav3d-2019.cfg
+++ b/config/kit-zav3d-2019.cfg
@@ -123,12 +123,12 @@ x_offset: 39
 y_offset: 11
 z_offset: 0.9
 pin_up_touch_mode_reports_triggered: false
+samples: 2
+sample_retract_dist: 3.0
 
 [bed_mesh]
 speed: 100
 horizontal_move_z: 5
-samples: 2
-sample_retract_dist: 3.0
 min_point: 30,30
 max_point: 150,150
 probe_count: 3,3
diff --git a/config/printer-tevo-flash-2018.cfg b/config/printer-tevo-flash-2018.cfg
index 169704bd..8c1adf8c 100644
--- a/config/printer-tevo-flash-2018.cfg
+++ b/config/printer-tevo-flash-2018.cfg
@@ -103,6 +103,8 @@ control_pin: ar11
 x_offset: 0
 y_offset: 18
 z_offset: 1.64
+samples: 3
+sample_retract_dist: 5
 
 # The homing_override section modifies the default G28 behavior
 [homing_override]
@@ -117,8 +119,6 @@ gcode:
 
 # Mesh Bed Leveling.
 [bed_mesh]
-samples: 3
-sample_retract_dist: 5
 min_point: 5,0
 max_point: 230,210
 probe_count: 9,9
diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py
index 84bf5c9d..e47bdf63 100644
--- a/klippy/extras/probe.py
+++ b/klippy/extras/probe.py
@@ -29,6 +29,13 @@ class PrinterProbe:
         else:
             pconfig = config.getsection('printer')
             self.z_position = pconfig.getfloat('minimum_z_position', 0.)
+        # Multi-sample support (for improved accuracy)
+        self.samples = config.getint('samples', 1, minval=1)
+        self.sample_retract_dist = config.getfloat(
+            'sample_retract_dist', 2., above=0.)
+        self.samples_result = config.getchoice('samples_result',
+                                               {'median': 0, 'average': 1},
+                                               default='average')
         # Register z_virtual_endstop pin
         self.printer.lookup_object('pins').register_chip('probe', self)
         # Register PROBE/QUERY_PROBE commands
@@ -49,9 +56,6 @@ class PrinterProbe:
         return self.mcu_probe
     def get_offsets(self):
         return self.x_offset, self.y_offset, self.z_offset
-    cmd_PROBE_help = "Probe Z-height at current XY position"
-    def cmd_PROBE(self, params):
-        self._probe(self.speed)
     def _probe(self, speed):
         toolhead = self.printer.lookup_object('toolhead')
         homing_state = homing.Homing(self.printer)
@@ -71,6 +75,50 @@ class PrinterProbe:
         self.gcode.respond_info("probe at %.3f,%.3f is z=%.6f" % (
             pos[0], pos[1], pos[2]))
         self.gcode.reset_last_position()
+        return pos
+    def _move(self, coord, speed):
+        toolhead = self.printer.lookup_object('toolhead')
+        curpos = toolhead.get_position()
+        for i in range(len(coord)):
+            if coord[i] is not None:
+                curpos[i] = coord[i]
+        try:
+            toolhead.move(curpos, speed)
+        except homing.EndstopError as e:
+            raise self.gcode.error(str(e))
+    def run_probe(self):
+        positions = []
+        for i in range(self.samples):
+            pos = self._probe(self.speed)
+            positions.append(pos)
+            if i < self.samples - 1:
+                # retract
+                liftpos = [None, None, pos[2] + self.sample_retract_dist]
+                self._move(liftpos, self.speed)
+        if self.samples_result == 1:
+            # Calculate Average
+            calculated_value = [sum([pos[i] for pos in positions]) /
+                                self.samples for i in range(3)]
+        else:
+            # Calculate Median
+            sorted_z_positions = sorted([position[2]
+                                         for position in positions])
+            middle = self.samples // 2
+            if (self.samples & 1) == 1:
+                # odd number of samples
+                median = sorted_z_positions[middle]
+            else:
+                # even number of samples
+                median = (sorted_z_positions[middle] +
+                          sorted_z_positions[middle - 1]) / 2
+            calculated_value = [positions[0][0],
+                                positions[0][1],
+                                median]
+        return calculated_value
+    cmd_PROBE_help = "Probe Z-height at current XY position"
+    def cmd_PROBE(self, params):
+        pos = self.run_probe()
+        self.gcode.respond_info("Result is z=%.6f" % (pos[2],))
     cmd_QUERY_PROBE_help = "Return the status of the z-probe"
     def cmd_QUERY_PROBE(self, params):
         toolhead = self.printer.lookup_object('toolhead')
@@ -104,10 +152,9 @@ class PrinterProbe:
             # Move Z to start reading position
             self._move(start_pos, speed)
             # Probe
-            self._probe(speed)
+            pos = self._probe(speed)
             # Get Z value, accumulate value to calculate average
             # and save it to calculate standard deviation
-            pos = toolhead.get_position()
             sum_reads += pos[2]
             probes.append(pos[2])
         # Move Z to start reading position
@@ -135,16 +182,6 @@ class PrinterProbe:
             "probe accuracy results: maximum %.6f, minimum %.6f, "
             "average %.6f, median %.6f, standard deviation %.6f" % (
             max_value, min_value, avg_value, median, sigma))
-    def _move(self, coord, speed):
-        toolhead = self.printer.lookup_object('toolhead')
-        curpos = toolhead.get_position()
-        for i in range(len(coord)):
-            if coord[i] is not None:
-                curpos[i] = coord[i]
-        try:
-            toolhead.move(curpos, speed)
-        except homing.EndstopError as e:
-            raise self.gcode.error(str(e))
     def probe_calibrate_finalize(self, kin_pos):
         if kin_pos is None:
             return
@@ -158,10 +195,8 @@ class PrinterProbe:
     cmd_PROBE_CALIBRATE_help = "Calibrate the probe's z_offset"
     def cmd_PROBE_CALIBRATE(self, params):
         # Perform initial probe
-        self._probe(self.speed)
+        curpos = self.run_probe()
         # Move away from the bed
-        toolhead = self.printer.lookup_object('toolhead')
-        curpos = toolhead.get_position()
         self.probe_calibrate_z = curpos[2]
         curpos[2] += 5.
         self._move(curpos, self.speed)
@@ -240,12 +275,6 @@ class ProbePointsHelper:
         self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.)
         self.speed = self.lift_speed = config.getfloat('speed', 50., above=0.)
         self.probe_offsets = (0., 0., 0.)
-        self.samples = config.getint('samples', 1, minval=1)
-        self.sample_retract_dist = config.getfloat(
-            'sample_retract_dist', 2., above=0.)
-        self.samples_result = config.getchoice('samples_result',
-                                               {'median': 0, 'average': 1},
-                                               default='average')
         # Internal probing state
         self.results = []
         self.busy = self.manual_probe = False
@@ -256,15 +285,10 @@ class ProbePointsHelper:
                 "Need at least %d probe points for %s" % (n, self.name))
     def get_lift_speed(self):
         return self.lift_speed
-    def _lift_z(self, z_pos, add=False, speed=None):
+    def _lift_z(self, z_pos, speed):
         # Lift toolhead
         curpos = self.toolhead.get_position()
-        if add:
-            curpos[2] += z_pos
-        else:
-            curpos[2] = z_pos
-        if speed is None:
-            speed = self.lift_speed
+        curpos[2] = z_pos
         try:
             self.toolhead.move(curpos, speed)
         except homing.EndstopError as e:
@@ -272,7 +296,7 @@ class ProbePointsHelper:
             raise self.gcode.error(str(e))
     def _move_next(self):
         # Lift toolhead
-        self._lift_z(self.horizontal_move_z)
+        self._lift_z(self.horizontal_move_z, self.lift_speed)
         # Check if done probing
         if len(self.results) >= len(self.probe_points):
             self.toolhead.get_last_move_time()
@@ -293,38 +317,6 @@ class ProbePointsHelper:
         if self.manual_probe:
             manual_probe.ManualProbeHelper(self.printer, {},
                                            self._manual_probe_finalize)
-    def _automatic_probe_point(self):
-        positions = []
-        for i in range(self.samples):
-            try:
-                self.gcode.run_script_from_command("PROBE")
-            except self.gcode.error as e:
-                self._finalize(False)
-                raise
-            positions.append(self.toolhead.get_position())
-            if i < self.samples - 1:
-                # retract
-                self._lift_z(self.sample_retract_dist, add=True)
-        if self.samples_result == 1:
-            # Calculate Average
-            calculated_value = [sum([pos[i] for pos in positions]) /
-                                self.samples for i in range(3)]
-        else:
-            # Calculate Median
-            sorted_z_positions = sorted([position[2]
-                                         for position in positions])
-            middle = self.samples // 2
-            if (self.samples & 1) == 1:
-                # odd number of samples
-                median = sorted_z_positions[middle]
-            else:
-                # even number of samples
-                median = (sorted_z_positions[middle] +
-                          sorted_z_positions[middle - 1]) / 2
-            calculated_value = [positions[0][0],
-                                positions[0][1],
-                                median]
-        self.results.append(calculated_value)
     def start_probe(self, params):
         # Lookup objects
         self.toolhead = self.printer.lookup_object('toolhead')
@@ -345,12 +337,17 @@ class ProbePointsHelper:
         # Start probe
         self.results = []
         self.busy = True
-        self._lift_z(self.horizontal_move_z, speed=self.speed)
+        self._lift_z(self.horizontal_move_z, self.speed)
         self._move_next()
         if not self.manual_probe:
             # Perform automatic probing
             while self.busy:
-                self._automatic_probe_point()
+                try:
+                    pos = probe.run_probe()
+                except self.gcode.error as e:
+                    self._finalize(False)
+                    raise
+                self.results.append(pos)
                 self._move_next()
     def _manual_probe_finalize(self, kin_pos):
         if kin_pos is None:
diff --git a/test/klippy/screws_tilt_adjust.cfg b/test/klippy/screws_tilt_adjust.cfg
index 7b29e6ec..cc985d19 100644
--- a/test/klippy/screws_tilt_adjust.cfg
+++ b/test/klippy/screws_tilt_adjust.cfg
@@ -56,6 +56,9 @@ max_temp: 130
 sensor_pin: ar30
 control_pin: ar32
 z_offset: 1.15
+samples: 3
+sample_retract_dist: 2.
+samples_result: median
 
 [bed_mesh]
 min_point: 10,10
@@ -83,7 +86,4 @@ screw4: 10,190
 screw4_name: read left screw
 horizontal_move_z: 10.
 speed: 50.
-samples: 3
-sample_retract_dist: 2.
-samples_result: median
 screw_thread: CW-M3