-ish fini pour moi

GCode_Interpreterdc.py

@@ -6,6 +6,29 @@ import numpy as np
 
 Point = NewType("point", List[float, float, float])
 
+
+# noinspection PyPep8Naming
+def bezier_length(x0, y0, I, J, P, Q, X, Y, num_points=1000):
+
+    def dx(t):
+        return -3 * (1 - t)**2 * x0 + 3 * ((1 - t)**2 - 2 * t * (1 - t)) * (x0 + I) + 3 * (2 * t * (1 - t) - t**2) * (x0 + P) + 3 * t**2 * X
+
+    def dy(t):
+        return -3 * (1 - t)**2 * y0 + 3 * ((1 - t)**2 - 2 * t * (1 - t)) * (y0 + J) + 3 * (2 * t * (1 - t) - t**2) * (y0 + Q) + 3 * t**2 * Y
+
+    t_values = np.linspace(0, 1, num_points)
+    length = 0
+
+    for i in range(1, len(t_values)):
+        t1, t2 = t_values[i - 1], t_values[i]
+        dx1, dy1 = dx(t1), dy(t1)
+        dx2, dy2 = dx(t2), dy(t2)
+        segment_length = np.sqrt((dx1**2 + dy1**2) + (dx2**2 + dy2**2)) * (t2 - t1) / 2
+        length += segment_length
+
+    return length
+
+
 # We will assume everything is up to documentation.
 class GCodeToMotors:
     # Hardcoded Values for Our Machine
@@ -66,6 +89,10 @@ class GCodeToMotors:
     feedrate: float = 0.
     feedrate_micros: int = 0
 
+    is_g5_block: bool = False
+    prev_g5_p: float = 0.
+    prev_g5_q: float = 0.
+
     @staticmethod
     def to_steps(steps_per_unit: float, units: float) -> float:
         return steps_per_unit * units
@@ -205,7 +232,7 @@ class GCodeToMotors:
         time.sleep(5e-6)
 
 
-    def instruction_to_velocities(self, instruction: str) -> Optional[List[float]]:
+    def instruction_converter(self, instruction: str) -> Optional[List[float]]:
         if instruction[0] == "/":
             return None
         fp: Point = [0., 0., 0.]
@@ -289,7 +316,7 @@ class GCodeToMotors:
                         step = step if (code == 3) else steps - step
                         newPoint[0] = center[0] + radius * np.cos(angleA + angle * (step / steps))
                         newPoint[1] = center[1] + radius * np.sin(angleA + angle * (step / steps))
-                        self.set_position(newPoint[0], newPoint[1], fp[2])
+                        self.set_target(newPoint[0], newPoint[1], fp[2])
 
                         if self.feedrate > 0:
                             self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
@@ -301,6 +328,54 @@ class GCodeToMotors:
                 case 4:
                     time.sleep(search_string('P', instruction) * 1e-3)
 
+                case 5:
+                    if not self.is_g5_block:
+                        control_1 = [0., 0., 0.]
+                        control_1[0] = search_string('I', instruction) + self.current_units[0]
+                        control_1[1] = search_string('J', instruction) + self.current_units[1]
+
+                    else:
+                        control_1 = [0., 0., 0.]
+                        control_1[0] = -self.prev_g5_p + self.current_units[0]
+                        control_1[1] = -self.prev_g5_q + self.current_units[1]
+
+                    control_2 = [0., 0., 0.]
+                    self.prev_g5_p = search_string('P', instruction)
+                    self.prev_g5_q = search_string('Q', instruction)
+                    control_2[0] = self.prev_g5_p + self.current_units[0]
+                    control_2[1] = self.prev_g5_q + self.current_units[1]
+
+                    length = bezier_length(self.current_units[0], self.current_units[1], *control_1, *control_2, fp[0], fp[1])
+                    steps = np.ceil(length/self.curve_section)
+
+                    newPoint = [0., 0., 0.]
+                    for t in [i / steps for i in range(1, steps + 1)]:
+                        newPoint[0] = (1 - t) ** 3 * self.current_units[0] + 3 * (1 - t) ** 2 * t * control_1[0] + 3 * (
+                                    1 - t) * t ** 2 * control_2[0] + t ** 3 * fp[0]
+                        newPoint[1] = (1 - t) ** 3 * self.current_units[1] + 3 * (1 - t) ** 2 * t * control_1[1] + 3 * (
+                                    1 - t) * t ** 2 * control_2[1] + t ** 3 * fp[1]
+                        self.set_target(newPoint[0], newPoint[1], fp[2])
+
+                        if self.feedrate > 0:
+                            self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
+                        else:
+                            self.feedrate_micros = self.get_max_speed()
+
+                        self.move(self.feedrate_micros)
+
+                case 5.1:
+                    raise NotImplementedError("PAS DE SPLINE QUADRATIQUE J'AI LA FLEMME")
+
+                case 5.2, 5.3:
+                    raise NotImplementedError("Experimental")
+
+                case 6:
+                    # Not canonical, but parabolas maybe
+                    pass
+
+                case 7:
+                    # Not canonical, but ellipses
+
                 case 20:
                     self.x_units = self.X_STEPS_PER_INCH
                     self.y_units = self.Y_STEPS_PER_INCH
@@ -343,14 +418,15 @@ class GCodeToMotors:
                     self.set_target(0., 0., 0.)
                     self.move(self.get_max_speed())
 
-
+                case _:
+                    raise ValueError("No Associated GCode Implemented/Known")
 
         return
 
     def execute(self, gcode):
         velocities = []
         for instruction in gcode:
-            velocities.append(self.instruction_to_velocities(instruction))
+            velocities.append(self.instruction_converter(instruction))
         return velocities
 
 

main.py

@@ -53,6 +53,7 @@ class SVGToGCodeConverter:
                 gcode_lines.append(self.point_to_gcode(x, y))
             return gcode_lines
 
+    # Following two are not canonical, I don't know where I found those lol
     def ellipse_to_gcode(self, start: complex, end: complex, center: complex, rx: float, ry: float, rotation: float, clockwise: bool) -> str:
         if "G7" not in self.supported_g_functions:
             raise NotImplementedError("Ellipse support requires G7 function.")