auto velocity

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

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 * (
+        self.max_velocity_mm_per_sec = self._adjust_velocity(distance_mm * (
-            velocity_mm_per_min / SECONDS_IN_MINUTE / distance_total_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)
+            full_length = math.sqrt(arc * arc + delta.z * delta.z + e2)
-            if l == 0:
+            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)
+            full_length = math.sqrt(arc * arc + delta.x * delta.x + e2)
-            if l == 0:
+            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)
+            full_length = math.sqrt(arc * arc + delta.y * delta.y + e2)
-            if l == 0:
+            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
+            circular_velocity = arc / full_length * velocity
-            self._e_velocity = abs(delta.e) / l * 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
+            v = full_length / self.acceleration_time_s
-            self.max_velocity_mm_per_sec = Coordinates(v, v, v, v)
+            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):

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()