From 770100ef1c441615e4de24f8330ef2719c22f9ee Mon Sep 17 00:00:00 2001 From: mpboyer Date: Sun, 12 Jan 2025 13:10:17 +0100 Subject: [PATCH] -ish fini pour moi --- GCode_Interpreterdc.py | 84 ++++++++++++++++++++++++++++++++++++++++-- main.py | 1 + 2 files changed, 81 insertions(+), 4 deletions(-) diff --git a/GCode_Interpreterdc.py b/GCode_Interpreterdc.py index dd71097..0cdeb8c 100644 --- a/GCode_Interpreterdc.py +++ b/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 diff --git a/main.py b/main.py index 7eee8b2..fffd298 100644 --- a/main.py +++ b/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.")