From 5b9c0de25e399a027e4b84cad8e257a0d2e4c38d Mon Sep 17 00:00:00 2001 From: Nikolay Khabarov <2xl@mail.ru> Date: Sun, 2 Jul 2017 00:08:15 +0300 Subject: [PATCH] make calibration safer --- cnc/gmachine.py | 8 +-- cnc/hal_raspberry/hal.py | 115 ++++++++++++++++++++++++--------------- tests/test_gmachine.py | 4 +- 3 files changed, 76 insertions(+), 51 deletions(-) diff --git a/cnc/gmachine.py b/cnc/gmachine.py index e9bc4ac..0cab36b 100644 --- a/cnc/gmachine.py +++ b/cnc/gmachine.py @@ -244,9 +244,6 @@ class GMachine(object): :param y: boolean, move X axis to zero :param z: boolean, move X axis to zero """ - if z: - d = Coordinates(0, 0, -self._position.z, 0) - self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z) if x and not y: self._move_linear(Coordinates(-self._position.x, 0, 0, 0), MAX_VELOCITY_MM_PER_MIN_X) @@ -256,7 +253,10 @@ class GMachine(object): 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)) + MAX_VELOCITY_MM_PER_MIN_Y)) + if z: + d = Coordinates(0, 0, -self._position.z, 0) + self._move_linear(d, MAX_VELOCITY_MM_PER_MIN_Z) def position(self): """ Return current machine position (after the latest command) diff --git a/cnc/hal_raspberry/hal.py b/cnc/hal_raspberry/hal.py index b32b54c..9f61d2c 100644 --- a/cnc/hal_raspberry/hal.py +++ b/cnc/hal_raspberry/hal.py @@ -99,24 +99,30 @@ 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: +def __calibrate_private(x, y, z, invert): + if invert: + stepper_inverted_x = not STEPPER_INVERTED_X + stepper_inverted_y = not STEPPER_INVERTED_Y + stepper_inverted_z = not STEPPER_INVERTED_Z + endstop_inverted_x = not ENDSTOP_INVERTED_X + endstop_inverted_y = not ENDSTOP_INVERTED_Y + endstop_inverted_z = not ENDSTOP_INVERTED_Z + else: + stepper_inverted_x = STEPPER_INVERTED_X + stepper_inverted_y = STEPPER_INVERTED_Y + stepper_inverted_z = STEPPER_INVERTED_Z + endstop_inverted_x = ENDSTOP_INVERTED_X + endstop_inverted_y = ENDSTOP_INVERTED_Y + endstop_inverted_z = ENDSTOP_INVERTED_Z + if stepper_inverted_x: gpio.clear(STEPPER_DIR_PIN_X) else: gpio.set(STEPPER_DIR_PIN_X) - if STEPPER_INVERTED_Y: + if stepper_inverted_y: gpio.clear(STEPPER_DIR_PIN_Y) else: gpio.set(STEPPER_DIR_PIN_Y) - if STEPPER_INVERTED_Z: + if stepper_inverted_z: gpio.clear(STEPPER_DIR_PIN_Z) else: gpio.set(STEPPER_DIR_PIN_Z) @@ -130,47 +136,66 @@ def calibrate(x, y, z): 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) + max_size = max(max_size, TABLE_SIZE_Z_MM * STEPPER_PULSES_PER_MM_Z) 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 + delay = int(1000000 / 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 + 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() + if pins == 0: + return True + last_pins = pins + # Prepare chunk with 1 second buffer. It is needed to make sure + # that if program unexpectedly stops, dma will continue work not + # long then this buffer time. + dma.clear() + generate = 1000000 + while generate > 0: 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() + dma.add_delay(delay) + generate -= delay + STEPPER_PULSE_LENGTH_US + dma.finalize_stream() + if not dma.is_active(): + dma.run(False) return False +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 not __calibrate_private(x, y, z, True): # move from endstop switch + return False + return __calibrate_private(x, y, z, False) # move to endstop switch + + def move(generator): """ Move head to specified position :param generator: PulseGenerator object. @@ -247,7 +272,7 @@ def move(generator): if not is_ran and instant and current_cb is None: if k - k0 > 100000: # wait at least 100 ms is uploaded nt = time.time() - st - ng = (k - k0)/ 1000000.0 + ng = (k - k0) / 1000000.0 if nt > ng: logging.warn("Buffer preparing for instant run took more " "time then buffer time" diff --git a/tests/test_gmachine.py b/tests/test_gmachine.py index 73afb7c..e640656 100644 --- a/tests/test_gmachine.py +++ b/tests/test_gmachine.py @@ -75,8 +75,8 @@ class TestGMachine(unittest.TestCase): m = GMachine() self.assertRaises(GMachineException, m.do_command, GCode.parse_line("G1X1F-1")) - c = "G1X1F" + str(MIN_VELOCITY_MM_PER_MIN - 0.0000001) - self.assertRaises(GMachineException, m.do_command, GCode.parse_line(c)) + cl = "G1X1F" + str(MIN_VELOCITY_MM_PER_MIN - 0.0000001) + self.assertRaises(GMachineException, m.do_command, GCode.parse_line(cl)) m.do_command(GCode.parse_line("G1X100F" + str(MAX_VELOCITY_MM_PER_MIN_X))) m.do_command(GCode.parse_line("G1Y100F"