refactor calibration process, some dummy commands support

cnc/config.py

@@ -1,27 +1,42 @@
-# Hardware limitations config
+# ------------------------------------------------------------------------------
-STEPPER_PULSE_LENGTH_US = 2
+# Hardware config.
-STEPPER_MAX_ACCELERATION_MM_PER_S2 = 3000  # for all axis, mm per sec^2
 
-MAX_VELOCITY_MM_PER_MIN_X = 30000  # mm per min
+# Maximum velocity for each axis in millimeter per minute.
-MAX_VELOCITY_MM_PER_MIN_Y = 24000  # mm per min
+MAX_VELOCITY_MM_PER_MIN_X = 24000
-MAX_VELOCITY_MM_PER_MIN_Z = 120    # mm per min
+MAX_VELOCITY_MM_PER_MIN_Y = 15000
-MAX_VELOCITY_MM_PER_MIN_E = 1500   # mm per min
+MAX_VELOCITY_MM_PER_MIN_Z = 600
+MAX_VELOCITY_MM_PER_MIN_E = 1500
+# Average velocity for endstop calibration procedure
+CALIBRATION_VELOCITY_MM_PER_MIN = 300
 
+# Stepper motors steps per millimeter for each axis.
 STEPPER_PULSES_PER_MM_X = 100
 STEPPER_PULSES_PER_MM_Y = 100
 STEPPER_PULSES_PER_MM_Z = 400
 STEPPER_PULSES_PER_MM_E = 150
 
-# invert axises direction
+# Invert axises direction, by default(False) high level means increase of
+# position. For inverted(True) axis, high level means decrease of position.
 STEPPER_INVERTED_X = True
 STEPPER_INVERTED_Y = False
 STEPPER_INVERTED_Z = False
 STEPPER_INVERTED_E = True
 
+# Invert zero end stops switches. By default(False) low level on input pin means
+# that axis in zero position. For inverted(True) end stops, high level means
+# zero position.
+ENDSTOP_INVERTED_X = True
+ENDSTOP_INVERTED_Y = True
+ENDSTOP_INVERTED_Z = True
+
+# Workplace physical size.
 TABLE_SIZE_X_MM = 200
 TABLE_SIZE_Y_MM = 200
 TABLE_SIZE_Z_MM = 220
 
+# Mixed settings.
+STEPPER_PULSE_LENGTH_US = 2
+STEPPER_MAX_ACCELERATION_MM_PER_S2 = 3000  # for all axis, mm per sec^2
 SPINDLE_MAX_RPM = 10000
 EXTRUDER_MAX_TEMPERATURE = 250
 BED_MAX_TEMPERATURE = 100
@@ -33,7 +48,9 @@ BED_PID = {"P": 5.06820175723,
            "I": 0.0476413193519,
            "D": 4.76413193519}
 
-# Pins config
+# ------------------------------------------------------------------------------
+# Pins configuration.
+
 STEPPER_STEP_PIN_X = 16
 STEPPER_STEP_PIN_Y = 20
 STEPPER_STEP_PIN_Z = 21
@@ -55,7 +72,9 @@ ENDSTOP_PIN_X = 12
 ENDSTOP_PIN_Y = 6
 ENDSTOP_PIN_Z = 5
 
-# Hardware behavior config
+# ------------------------------------------------------------------------------
+#  Behavior config
+
 # Run command immediately after receiving and stream new pulses, otherwise
 # buffer will be prepared firstly and then command will run.
 # Before enabling this feature, please make sure that board performance is

cnc/gmachine.py

@@ -37,7 +37,7 @@ class GMachine(object):
     def release(self):
         """ Return machine to original position and free all resources.
         """
-        self.home()
+        self.safe_zero()
         self._spindle(0)
         for h in self._heaters:
             self._heaters[h].stop()
@@ -112,7 +112,6 @@ class GMachine(object):
                             1.0 / STEPPER_PULSES_PER_MM_E)
         if delta.is_zero():
             return
-        velocity_per_axis = abs(delta) * (velocity / delta.length())
         self.__check_delta(delta)
 
         logging.info("Moving linearly {}".format(delta))
@@ -239,14 +238,25 @@ class GMachine(object):
         # save position
         self._position = self._position + circle_end + linear_delta
 
-    def home(self):
+    def safe_zero(self, x=True, y=True, z=True):
-        """ Move head to park position
+        """ Move head to zero position safely.
+        :param x: boolean, move X axis to zero
+        :param y: boolean, move X axis to zero
+        :param z: boolean, move X axis to zero
         """
-        d = Coordinates(0, 0, -self._position.z, 0)
+        if z:
-        self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z)
+            d = Coordinates(0, 0, -self._position.z, 0)
-        d = Coordinates(-self._position.x, -self._position.y, 0, 0)
+            self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z)
-        self._move_linear(d, min(MAX_VELOCITY_MM_PER_MIN_X,
+        if x and not y:
-                                 MAX_VELOCITY_MM_PER_MIN_Y))
+            self._move_linear(Coordinates(-self._position.x, 0, 0, 0),
+                              MAX_VELOCITY_MM_PER_MIN_X)
+        elif y and not x:
+            self._move_linear(Coordinates(0, -self._position.y, 0, 0),
+                              MAX_VELOCITY_MM_PER_MIN_X)
+        elif x and y:
+            d = Coordinates(-self._position.x, -self._position.y, 0, 0)
+            self._move_linear(d, min(MAX_VELOCITY_MM_PER_MIN_X,
+                              MAX_VELOCITY_MM_PER_MIN_Y))
 
     def position(self):
         """ Return current machine position (after the latest command)
@@ -367,7 +377,13 @@ class GMachine(object):
         elif c == 'G21':  # switch to mm
             self._convertCoordinates = 1.0
         elif c == 'G28':  # home
-            self.home()
+            axises = gcode.has('X'), gcode.has('Y'), gcode.has('Z')
+            if axises == (False, False, False):
+                axises = True, True, True
+            self.safe_zero(*axises)
+            hal.join()
+            if not hal.calibrate(*axises):
+                raise GMachineException("failed to calibrate")
         elif c == 'G53':  # switch to machine coords
             self._local = Coordinates(0.0, 0.0, 0.0, 0.0)
         elif c == 'G90':  # switch to absolute coords
@@ -432,6 +448,15 @@ class GMachine(object):
             answer = "X:{} Y:{} Z:{} E:{}".format(p.x, p.y, p.z, p.e)
         elif c is None:  # command not specified(for example, just F was passed)
             pass
+        # commands below are added just for compatibility
+        elif c == 'M82':  # absolute mode for extruder
+            if not self._absoluteCoordinates:
+                raise GMachineException("Not supported, use G90/G91")
+        elif c == 'M83':  # relative mode for extruder
+            if self._absoluteCoordinates:
+                raise GMachineException("Not supported, use G90/G91")
+        elif c == 'M84':  # disable motors
+            pass  # do not do anything
         else:
             raise GMachineException("unknown command")
         # save parameters on success

cnc/hal.py

@@ -2,8 +2,7 @@
 # Imported module contains functions for hardware access fo some board/SoC.
 # List of HAL methods that should be implemented in each module:
 #    def init():
-#        """ Initialize GPIO pins and machine itself, including calibration if
+#        """ Initialize GPIO pins and machine itself.
-#            needed. Do not return till all procedure is completed.
 #        """
 #        do_something()
 #
@@ -52,6 +51,15 @@
 #        """
 #        return measure()
 #
+#    def calibrate(x, y, z):
+#        """ Move head to home position till end stop switch will be triggered.
+#        Do not return till all procedures are completed.
+#        :param x: boolean, True to calibrate X axis.
+#        :param y: boolean, True to calibrate Y axis.
+#        :param z: boolean, True to calibrate Z axis.
+#        :return: boolean, True if all specified end stops were triggered.
+#        """
+#        return do_something()
 #
 #    def move(generator):
 #        """ Move head to according pulses in PulseGenerator.
@@ -95,6 +103,8 @@ if 'get_extruder_temperature' not in locals():
     raise NotImplementedError("hal.get_extruder_temperature() not implemented")
 if 'get_bed_temperature' not in locals():
     raise NotImplementedError("hal.get_bed_temperature() not implemented")
+if 'calibrate' not in locals():
+    raise NotImplementedError("hal.calibrate() not implemented")
 if 'move' not in locals():
     raise NotImplementedError("hal.move() not implemented")
 if 'join' not in locals():

cnc/hal_raspberry/hal.py

@@ -18,8 +18,7 @@ STEP_PIN_MASK_E = 1 << STEPPER_STEP_PIN_E
 
 
 def init():
-    """ Initialize GPIO pins and machine itself, including calibration if
+    """ Initialize GPIO pins and machine itself.
-        needed. Do not return till all procedures are completed.
     """
     gpio.init(STEPPER_STEP_PIN_X, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(STEPPER_STEP_PIN_Y, rpgpio.GPIO.MODE_OUTPUT)
@@ -30,8 +29,8 @@ def init():
     gpio.init(STEPPER_DIR_PIN_Z, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(STEPPER_DIR_PIN_E, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(ENDSTOP_PIN_X, rpgpio.GPIO.MODE_INPUT_PULLUP)
-    gpio.init(ENDSTOP_PIN_X, rpgpio.GPIO.MODE_INPUT_PULLUP)
+    gpio.init(ENDSTOP_PIN_Y, rpgpio.GPIO.MODE_INPUT_PULLUP)
-    gpio.init(ENDSTOP_PIN_X, rpgpio.GPIO.MODE_INPUT_PULLUP)
+    gpio.init(ENDSTOP_PIN_Z, rpgpio.GPIO.MODE_INPUT_PULLUP)
     gpio.init(SPINDLE_PWM_PIN, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(FAN_PIN, rpgpio.GPIO.MODE_OUTPUT)
     gpio.init(EXTRUDER_HEATER_PIN, rpgpio.GPIO.MODE_OUTPUT)
@@ -41,60 +40,6 @@ def init():
     gpio.clear(EXTRUDER_HEATER_PIN)
     gpio.clear(BED_HEATER_PIN)
 
-    # calibration
-    # TODO remove this from hal and rewrite, check if there is a special g
-    # command for this
-    gpio.set(STEPPER_DIR_PIN_X)
-    gpio.set(STEPPER_DIR_PIN_Y)
-    gpio.set(STEPPER_DIR_PIN_Z)
-    pins = STEP_PIN_MASK_X | STEP_PIN_MASK_Y | STEP_PIN_MASK_Z
-    dma.clear()
-    dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
-    st = time.time()
-    max_pulses_left = int(1.2 * max(STEPPER_PULSES_PER_MM_X,
-                                    STEPPER_PULSES_PER_MM_Y,
-                                    STEPPER_PULSES_PER_MM_Z) *
-                          max(TABLE_SIZE_X_MM,
-                              TABLE_SIZE_Y_MM,
-                              TABLE_SIZE_Z_MM))
-    try:
-        while max_pulses_left > 0:
-            if (STEP_PIN_MASK_X & pins) != 0 and gpio.read(ENDSTOP_PIN_X) == 0:
-                pins &= ~STEP_PIN_MASK_X
-                dma.clear()
-                dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
-            if (STEP_PIN_MASK_Y & pins) != 0 and gpio.read(ENDSTOP_PIN_Y) == 0:
-                pins &= ~STEP_PIN_MASK_Y
-                dma.clear()
-                dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
-            if (STEP_PIN_MASK_Z & pins) != 0 and gpio.read(ENDSTOP_PIN_Z) == 0:
-                pins &= ~STEP_PIN_MASK_Z
-                dma.clear()
-                dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
-            if pins == 0:
-                break
-            dma.run(False)
-            # limit velocity at ~10% of top velocity
-            time.sleep((1 / 0.10) / (min(MAX_VELOCITY_MM_PER_MIN_X,
-                                         MAX_VELOCITY_MM_PER_MIN_Y,
-                                         MAX_VELOCITY_MM_PER_MIN_Z,
-                                         MAX_VELOCITY_MM_PER_MIN_E)
-                                     / 60 * max(STEPPER_PULSES_PER_MM_X,
-                                                STEPPER_PULSES_PER_MM_Y,
-                                                STEPPER_PULSES_PER_MM_Z)))
-            max_pulses_left -= 1
-            if st is not None:
-                if time.time() - st > 2:
-                    logging.critical("Calibration still in progress. Check if "
-                                     "machine is moving.\nPress Ctrl+C to "
-                                     "cancel calibration and proceed as is.")
-                    st = None
-        if pins != 0:
-            logging.critical("Calibration has failed. You may proceed, but be "
-                             "careful.")
-    except KeyboardInterrupt:
-        logging.critical("Calibration has canceled by user. Be careful.")
-
 
 def spindle_control(percent):
     """ Spindle control implementation.
@@ -154,6 +99,78 @@ def get_bed_temperature():
     return thermistor.get_temperature(BED_TEMPERATURE_SENSOR_CHANNEL)
 
 
+def calibrate(x, y, z):
+    """ Move head to home position till end stop switch will be triggered.
+    Do not return till all procedures are completed.
+    :param x: boolean, True to calibrate X axis.
+    :param y: boolean, True to calibrate Y axis.
+    :param z: boolean, True to calibrate Z axis.
+    :return: boolean, True if all specified end stops were triggered.
+    """
+    logging.info("hal calibrate, x={}, y={}, z={}".format(x, y, z))
+    if STEPPER_INVERTED_X:
+        gpio.clear(STEPPER_DIR_PIN_X)
+    else:
+        gpio.set(STEPPER_DIR_PIN_X)
+    if STEPPER_INVERTED_Y:
+        gpio.clear(STEPPER_DIR_PIN_Y)
+    else:
+        gpio.set(STEPPER_DIR_PIN_Y)
+    if STEPPER_INVERTED_Z:
+        gpio.clear(STEPPER_DIR_PIN_Z)
+    else:
+        gpio.set(STEPPER_DIR_PIN_Z)
+    pins = 0
+    max_size = 0
+    if x:
+        pins |= STEP_PIN_MASK_X
+        max_size = max(max_size, TABLE_SIZE_X_MM * STEPPER_PULSES_PER_MM_X)
+    if y:
+        pins |= STEP_PIN_MASK_Y
+        max_size = max(max_size, TABLE_SIZE_Y_MM * STEPPER_PULSES_PER_MM_Y)
+    if z:
+        pins |= STEP_PIN_MASK_Z
+        max_size = max(max_size, TABLE_SIZE_Z_MM * TABLE_SIZE_Z_MM)
+    pulses_per_mm_avg = (STEPPER_PULSES_PER_MM_X + STEPPER_PULSES_PER_MM_Y
+                         + STEPPER_PULSES_PER_MM_Z) / 3.0
+    pulses_per_sec = CALIBRATION_VELOCITY_MM_PER_MIN / 60.0 * pulses_per_mm_avg
+    end_time = time.time() + 1.2 * max_size / pulses_per_sec
+    last_pins = ~pins
+    try:
+        while time.time() < end_time:
+            # check each axis end stop twice
+            x_endstop = (STEP_PIN_MASK_X & pins) != 0
+            y_endstop = (STEP_PIN_MASK_Y & pins) != 0
+            z_endstop = (STEP_PIN_MASK_Z & pins) != 0
+            # read each sensor three time
+            for _ in range(0, 3):
+                x_endstop = x_endstop and ((gpio.read(ENDSTOP_PIN_X) == 1)
+                                           == ENDSTOP_INVERTED_X)
+                y_endstop = y_endstop and ((gpio.read(ENDSTOP_PIN_Y) == 1)
+                                           == ENDSTOP_INVERTED_Y)
+                z_endstop = z_endstop and ((gpio.read(ENDSTOP_PIN_Z) == 1)
+                                           == ENDSTOP_INVERTED_Z)
+            if x_endstop:
+                pins &= ~STEP_PIN_MASK_X
+            if y_endstop:
+                pins &= ~STEP_PIN_MASK_Y
+            if z_endstop:
+                pins &= ~STEP_PIN_MASK_Z
+            if pins != last_pins:
+                dma.stop()
+                dma.clear()
+                if pins == 0:
+                    return True
+                dma.add_pulse(pins, STEPPER_PULSE_LENGTH_US)
+                # limit velocity
+                dma.add_delay(int(1000000 / pulses_per_sec))
+                last_pins = pins
+                dma.run(True)
+    except KeyboardInterrupt:
+        dma.stop()
+    return False
+
+
 def move(generator):
     """ Move head to specified position
     :param generator: PulseGenerator object.

cnc/hal_virtual.py

@@ -10,8 +10,7 @@ from cnc.config import *
 
 
 def init():
-    """ Initialize GPIO pins and machine itself, including calibration if
+    """ Initialize GPIO pins and machine itself.
-        needed. Do not return till all procedure is completed.
     """
     logging.info("initialize hal")
 
@@ -63,6 +62,18 @@ def get_bed_temperature():
     return BED_MAX_TEMPERATURE * 0.999
 
 
+def calibrate(x, y, z):
+    """ Move head to home position till end stop switch will be triggered.
+    Do not return till all procedures are completed.
+    :param x: boolean, True to calibrate X axis.
+    :param y: boolean, True to calibrate Y axis.
+    :param z: boolean, True to calibrate Z axis.
+    :return: boolean, True if all specified end stops were triggered.
+    """
+    logging.info("hal calibrate, x={}, y={}, z={}".format(x, y, z))
+    return True
+
+
 # noinspection PyUnusedLocal
 def move(generator):
     """ Move head to specified position.

tests/test_gmachine.py

@@ -32,10 +32,10 @@ class TestGMachine(unittest.TestCase):
         m.release()
         self.assertEqual(m.position(), Coordinates(0, 0, 0, 4))
 
-    def test_home(self):
+    def test_safe_zero(self):
         m = GMachine()
         m.do_command(GCode.parse_line("X1Y2Z3E4"))
-        m.home()
+        m.safe_zero()
         self.assertEqual(m.position(), Coordinates(0, 0, 0, 4))
 
     def test_none(self):