diff --git a/cnc/config.py b/cnc/config.py index 3f8e551..7351e4b 100644 --- a/cnc/config.py +++ b/cnc/config.py @@ -81,3 +81,8 @@ ENDSTOP_PIN_Z = 5 # Before enabling this feature, please make sure that board performance is # enough for streaming pulses(faster then real time). INSTANT_RUN = True + +# If this parameter is False, error will be raised on command with velocity more +# than maximum velocity specified here. If this parameter is True, velocity +# Would be decreased(proportional for all axises) to fit the maximum velocity. +AUTO_VELOCITY_ADJUSTMENT = True diff --git a/cnc/pulses.py b/cnc/pulses.py index 9aefc49..3bbc35a 100644 --- a/cnc/pulses.py +++ b/cnc/pulses.py @@ -30,6 +30,7 @@ class PulseGenerator(object): In the same way circular or other interpolation can be implemented based this class. """ + AUTO_VELOCITY_ADJUSTMENT = AUTO_VELOCITY_ADJUSTMENT def __init__(self, delta): """ Create object. Do not create directly this object, inherit this @@ -47,6 +48,31 @@ class PulseGenerator(object): self._2Vmax_per_a = 0.0 self._delta = delta + def _adjust_velocity(self, velocity_mm_sec): + """ Automatically decrease velocity to all axises proportionally if + velocity for one or more axises is more then maximum velocity for axis. + :param velocity_mm_sec: input velocity. + :return: adjusted(decreased if needed) velocity. + """ + if not self.AUTO_VELOCITY_ADJUSTMENT: + return velocity_mm_sec + k = 1.0 + if velocity_mm_sec.x * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_X: + k = min(k, MAX_VELOCITY_MM_PER_MIN_X + / velocity_mm_sec.x / SECONDS_IN_MINUTE) + if velocity_mm_sec.y * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_Y: + k = min(k, MAX_VELOCITY_MM_PER_MIN_Y + / velocity_mm_sec.y / SECONDS_IN_MINUTE) + if velocity_mm_sec.z * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_Z: + k = min(k, MAX_VELOCITY_MM_PER_MIN_Z + / velocity_mm_sec.z / SECONDS_IN_MINUTE) + if velocity_mm_sec.e * SECONDS_IN_MINUTE > MAX_VELOCITY_MM_PER_MIN_E: + k = min(k, MAX_VELOCITY_MM_PER_MIN_E + / velocity_mm_sec.e / SECONDS_IN_MINUTE) + if k != 1.0: + logging.warning("Out of speed, multiply velocity by {}".format(k)) + return velocity_mm_sec * k + def _get_movement_parameters(self): """ Get parameters for interpolation. This method have to be reimplemented in parent classes and should calculate 3 parameters. @@ -231,8 +257,8 @@ class PulseGeneratorLinear(PulseGenerator): distance_mm = abs(delta_mm) # type: Coordinates # velocity of each axis distance_total_mm = distance_mm.length() - self.max_velocity_mm_per_sec = distance_mm * ( - velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_mm) + self.max_velocity_mm_per_sec = self._adjust_velocity(distance_mm * ( + velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_mm)) # acceleration time self.acceleration_time_s = (self.max_velocity_mm_per_sec.find_max() / STEPPER_MAX_ACCELERATION_MM_PER_S2) @@ -456,71 +482,87 @@ class PulseGeneratorCircular(PulseGenerator): e2 = delta.e * delta.e if self._plane == PLANE_XY: self._iterations_3rd = abs(delta.z) * STEPPER_PULSES_PER_MM_Z - l = math.sqrt(arc * arc + delta.z * delta.z + e2) - if l == 0: + full_length = math.sqrt(arc * arc + delta.z * delta.z + e2) + if full_length == 0: self._velocity_3rd = velocity else: - self._velocity_3rd = abs(delta.z) / l * velocity + self._velocity_3rd = abs(delta.z) / full_length * velocity self._third_dir = math.copysign(1, delta.z) elif self._plane == PLANE_YZ: self._iterations_3rd = abs(delta.x) * STEPPER_PULSES_PER_MM_X - l = math.sqrt(arc * arc + delta.x * delta.x + e2) - if l == 0: + full_length = math.sqrt(arc * arc + delta.x * delta.x + e2) + if full_length == 0: self._velocity_3rd = velocity else: - self._velocity_3rd = abs(delta.x) / l * velocity + self._velocity_3rd = abs(delta.x) / full_length * velocity self._third_dir = math.copysign(1, delta.x) elif self._plane == PLANE_ZX: self._iterations_3rd = abs(delta.y) * STEPPER_PULSES_PER_MM_Y - l = math.sqrt(arc * arc + delta.y * delta.y + e2) - if l == 0: + full_length = math.sqrt(arc * arc + delta.y * delta.y + e2) + if full_length == 0: self._velocity_3rd = velocity else: - self._velocity_3rd = abs(delta.y) / l * velocity + self._velocity_3rd = abs(delta.y) / full_length * velocity self._third_dir = math.copysign(1, delta.y) else: raise ValueError("Unknown plane") self._iterations_e = abs(delta.e) * STEPPER_PULSES_PER_MM_E # Velocity splits with corresponding distance. - if l == 0: + if full_length == 0: circular_velocity = velocity self._e_velocity = velocity else: - circular_velocity = arc / l * velocity - self._e_velocity = abs(delta.e) / l * velocity + circular_velocity = arc / full_length * velocity + self._e_velocity = abs(delta.e) / full_length * velocity + if self._plane == PLANE_XY: + self.max_velocity_mm_per_sec = self._adjust_velocity( + Coordinates(circular_velocity, circular_velocity, + self._velocity_3rd, self._e_velocity)) + circular_velocity = min(self.max_velocity_mm_per_sec.x, + self.max_velocity_mm_per_sec.y) + self._velocity_3rd = self.max_velocity_mm_per_sec.z + elif self._plane == PLANE_YZ: + self.max_velocity_mm_per_sec = self._adjust_velocity( + Coordinates(self._velocity_3rd, circular_velocity, + circular_velocity, self._e_velocity)) + circular_velocity = min(self.max_velocity_mm_per_sec.y, + self.max_velocity_mm_per_sec.z) + self._velocity_3rd = self.max_velocity_mm_per_sec.x + elif self._plane == PLANE_ZX: + self.max_velocity_mm_per_sec = self._adjust_velocity( + Coordinates(circular_velocity, self._velocity_3rd, + circular_velocity, self._e_velocity)) + circular_velocity = min(self.max_velocity_mm_per_sec.z, + self.max_velocity_mm_per_sec.x) + self._velocity_3rd = self.max_velocity_mm_per_sec.y + self._e_velocity = self.max_velocity_mm_per_sec.e self._r_div_v = radius / circular_velocity self._e_dir = math.copysign(1, delta.e) - if self._plane == PLANE_XY: - self.max_velocity_mm_per_sec = Coordinates(circular_velocity, - circular_velocity, - self._velocity_3rd, - self._e_velocity) - elif self._plane == PLANE_YZ: - self.max_velocity_mm_per_sec = Coordinates(self._velocity_3rd, - circular_velocity, - circular_velocity, - self._e_velocity) - elif self._plane == PLANE_ZX: - self.max_velocity_mm_per_sec = Coordinates(circular_velocity, - self._velocity_3rd, - circular_velocity, - self._e_velocity) self.acceleration_time_s = (self.max_velocity_mm_per_sec.find_max() / STEPPER_MAX_ACCELERATION_MM_PER_S2) - if l == 0: + if full_length == 0: self.linear_time_s = 0.0 self.max_velocity_mm_per_sec = Coordinates(0, 0, 0, 0) elif STEPPER_MAX_ACCELERATION_MM_PER_S2 * self.acceleration_time_s \ - ** 2 > l: + ** 2 > full_length: self.acceleration_time_s = \ - math.sqrt(l / STEPPER_MAX_ACCELERATION_MM_PER_S2) + math.sqrt(full_length / STEPPER_MAX_ACCELERATION_MM_PER_S2) self.linear_time_s = 0.0 - v = l / self.acceleration_time_s - self.max_velocity_mm_per_sec = Coordinates(v, v, v, v) + v = full_length / self.acceleration_time_s + if self.max_velocity_mm_per_sec.x > 0.0: + self.max_velocity_mm_per_sec.x = v + if self.max_velocity_mm_per_sec.y > 0.0: + self.max_velocity_mm_per_sec.y = v + if self.max_velocity_mm_per_sec.z > 0.0: + self.max_velocity_mm_per_sec.z = v + if self.max_velocity_mm_per_sec.e > 0.0: + self.max_velocity_mm_per_sec.e = v else: - linear_distance_mm = l - self.acceleration_time_s ** 2 \ + linear_distance_mm = full_length - self.acceleration_time_s ** 2 \ * STEPPER_MAX_ACCELERATION_MM_PER_S2 - self.linear_time_s = linear_distance_mm / velocity + self.linear_time_s = linear_distance_mm / math.sqrt( + circular_velocity ** 2 + self._velocity_3rd ** 2 + + self._e_velocity ** 2) @staticmethod def __angle(a, b): diff --git a/tests/test_gmachine.py b/tests/test_gmachine.py index 52d7beb..578c86d 100644 --- a/tests/test_gmachine.py +++ b/tests/test_gmachine.py @@ -5,6 +5,7 @@ from cnc.gmachine import * from cnc.coordinates import * from cnc.heater import * from cnc.pid import * +from cnc.config import * class TestGMachine(unittest.TestCase): @@ -64,8 +65,6 @@ class TestGMachine(unittest.TestCase): self.assertEqual(m.position(), Coordinates(1, 2, 3, 4)) self.assertRaises(GMachineException, m.do_command, GCode.parse_line("G1F-1")) - self.assertRaises(GMachineException, - m.do_command, GCode.parse_line("G1X100F999999")) self.assertRaises(GMachineException, m.do_command, GCode.parse_line("G1X-1Y0Z0")) self.assertRaises(GMachineException, @@ -74,6 +73,7 @@ class TestGMachine(unittest.TestCase): m.do_command, GCode.parse_line("G1X0Y0Z-1")) def test_feed_rate(self): + PulseGenerator.AUTO_VELOCITY_ADJUSTMENT = False m = GMachine() self.assertRaises(GMachineException, m.do_command, GCode.parse_line("G1X1F-1")) @@ -87,6 +87,8 @@ class TestGMachine(unittest.TestCase): + str(MAX_VELOCITY_MM_PER_MIN_Z))) m.do_command(GCode.parse_line("G1E100F" + str(MAX_VELOCITY_MM_PER_MIN_E))) + self.assertRaises(GMachineException, + m.do_command, GCode.parse_line("G1X0F999999")) s = "G1X0F" + str(MAX_VELOCITY_MM_PER_MIN_X + 1) self.assertRaises(GMachineException, m.do_command, GCode.parse_line(s)) s = "G1Y0F" + str(MAX_VELOCITY_MM_PER_MIN_Y + 1) @@ -95,6 +97,11 @@ class TestGMachine(unittest.TestCase): self.assertRaises(GMachineException, m.do_command, GCode.parse_line(s)) s = "G1E0F" + str(MAX_VELOCITY_MM_PER_MIN_E + 1) self.assertRaises(GMachineException, m.do_command, GCode.parse_line(s)) + PulseGenerator.AUTO_VELOCITY_ADJUSTMENT = True + m.do_command(GCode.parse_line("G1X10Y10Z10F9999999999999999999")) + m.do_command(GCode.parse_line("G2I0.1F9999999999999999999")) + m.do_command(GCode.parse_line("G2I10F9999999999999999999")) + PulseGenerator.AUTO_VELOCITY_ADJUSTMENT = AUTO_VELOCITY_ADJUSTMENT def test_g2_g3(self): m = GMachine()