import time from typing import * import numpy as np import controller import hardware Point = NewType("point", List[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: def __init__(self, ctrl, hw_interface): self.CONTROLLER = ctrl self.HARDWARE = hw_interface X_STEPS_PER_INCH = 4800 X_STEPS_PER_MM = x_units = 188.97 X_MOTOR_STEPS: float = 200 Y_STEPS_PER_INCH = 4800 Y_STEPS_PER_MM = y_units = 188.97 Y_MOTOR_STEPS: int = 200 Z_STEPS_PER_INCH = 4800 Z_STEPS_PER_MM = z_units = 188.97 Z_MOTOR_STEPS: int = 200 FAST_XY_FEEDRATE: float = 1 # in m/mn FAST_Z_FEEDRATE: float = 1 CURVE_SECTION_INCHES = curve_section = .019685 CURVE_SECTION_MM: float = .5 SENSORS_INVERTING: bool = False x_direction: int = 1 x_throttle : int = 0 # 0 means no movement, otherwise fast_feedrate/throttle y_direction: int = 1 y_throttle : int = 0 z_direction: int = 1 z_throttle : int = 0 feedrate: float = 0. # In m/mn ctrl_step: float = 1e-5 # in s abs_mode: bool = False current_units: Point = [0., 0., 0.] target_units: Point = [0., 0., 0.] delta_units: Point = [0., 0., 0.] current_steps: Point = [0., 0., 0.] target_steps: Point = [0., 0., 0.] delta_steps: Point = [0., 0., 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 def calculate_deltas(self): self.delta_units = list(map(lambda t: (t[0] - t[1]), zip(self.target_units, self.current_units))) self.current_steps[0] = self.to_steps(self.x_units, self.current_units[0]) self.current_steps[1] = self.to_steps(self.y_units, self.current_units[1]) self.current_steps[2] = self.to_steps(self.z_units, self.current_units[2]) self.target_steps[0] = self.to_steps(self.x_units, self.target_units[0]) self.target_steps[1] = self.to_steps(self.y_units, self.target_units[1]) self.target_steps[2] = self.to_steps(self.z_units, self.target_units[2]) self.delta_steps = list(map(lambda t: (t[0] - t[1]), zip(self.target_steps, self.current_steps))) self.x_direction = (self.target_units[0] >= self.current_units[0]) self.y_direction = (self.target_units[1] >= self.current_units[1]) self.z_direction = (self.target_units[2] >= self.current_units[2]) def set_position(self, x: float, y: float, z: float): self.current_units[0] = x self.current_units[1] = y self.current_units[2] = z self.calculate_deltas() def set_target(self, x: float, y: float, z: float): self.target_units[0] = x self.target_units[1] = y self.target_units[2] = z self.calculate_deltas() # This is somewhat naïve : depending on direction we may be able to go faster def get_max_speed(self) -> float: if self.delta_steps[2] > 0: return self.FAST_Z_FEEDRATE return self.FAST_XY_FEEDRATE # Try to move to target_units at feedrate # We honor the following semantics: # GCodeToMotors translates the GCode to high-level controls/theoretical position and targets # HARDWARE.probe() updates GCodeToMotors with the actual position # CONTROLLER() takes the current state and objective, then makes a movement decision # HARDWARE.realize() applies the current commands to the actual hardware def move(self): print(self.target_units) print(self.feedrate) self.HARDWARE.probe(self) while not self.CONTROLLER(self): # Allow controller to alter self self.HARDWARE.realize(self) print("realized?") time.sleep(self.ctrl_step) self.HARDWARE.probe(self) self.calculate_deltas() def instruction_converter(self, instruction: str) -> Optional[List[float]]: if instruction[0] == "/": return None fp: Point = [0., 0., 0.] code: int = 0 if has_command('G', instruction)\ or has_command('X', instruction)\ or has_command('Y', instruction)\ or has_command('Z', instruction): code = search_string('G', instruction) match code: case 0 | 1 | 2 | 3: if self.abs_mode: if has_command('X', instruction): fp[0] = search_string('X', instruction) else: fp[0] = self.current_units[0] if has_command('Y', instruction): fp[1] = search_string('Y', instruction) else: fp[1] = self.current_units[1] if has_command('Z', instruction): fp[2] = search_string('Z', instruction) else: fp[2] = self.current_units[2] else: fp[0] = self.current_units[0] + search_string('X', instruction) fp[1] = self.current_units[1] + search_string('Y', instruction) fp[2] = self.current_units[2] + search_string('Z', instruction) case _: pass match code: case 0 | 1: self.set_target(fp[0], fp[1], fp[2]) if has_command('G', instruction) and code == 1: self.feedrate = search_string('F', instruction) if self.feedrate == 0: self.feedrate = self.get_max_speed() self.move() case 2 | 3: center = [0., 0., 0.] center[0] = search_string('I', instruction) + self.current_units[0] center[1] = search_string('J', instruction) + self.current_units[1] aX = self.current_units[0] - center[0] aY = self.current_units[1] - center[1] bX = fp[0] - center[0] bY = fp[1] - center[1] if code == 2: # If in fucked up anti-trigonometric direction angleA = np.atan2(bY, bX) angleB = np.atan2(aY, aX) else: angleA = np.atan2(aY, aX) angleB = np.atan2(bY, bX) if angleB <= angleA: angleB += 2 * np.pi angle = angleB - angleA radius = np.linalg.norm([aX, aY]) length = radius * angle steps = int(length / self.curve_section) + 1 newPoint = [0., 0., 0.] for step in range(1, steps + 1): 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_target(newPoint[0], newPoint[1], fp[2]) if self.feedrate == 0: self.feedrate = self.get_max_speed() self.move() 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 = int(length/self.curve_section) + 1 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 = self.get_max_speed() self.move() case 5.1: raise NotImplementedError("PAS DE SPLINE QUADRATIQUE J'AI LA FLEMME") case 5.2 | 5.3: raise NotImplementedError("Experimental Unimplemented Feature") case 6: # Not canonical, but parabolas maybe pass case 7: # Not canonical, but ellipses pass case 20: self.x_units = self.X_STEPS_PER_INCH self.y_units = self.Y_STEPS_PER_INCH self.z_units = self.Z_STEPS_PER_INCH self.curve_section = self.CURVE_SECTION_INCHES self.calculate_deltas() case 21: self.x_units = self.X_STEPS_PER_MM self.y_units = self.Y_STEPS_PER_MM self.z_units = self.Z_STEPS_PER_MM self.curve_section = self.CURVE_SECTION_MM self.calculate_deltas() case 28: self.set_target(0., 0., 0.) self.move() case 30: fp = [0., 0., 0.] fp[0] = search_string('X', instruction) fp[1] = search_string('Y', instruction) fp[2] = search_string('Z', instruction) if self.abs_mode: if not has_command('X', instruction): fp[0] = self.current_units[0] if not has_command('Y', instruction): fp[1] = self.current_units[1] if not has_command('Z', instruction): fp[2] = self.current_units[2] self.set_target(fp[0], fp[1], fp[2]) else: self.set_target(self.current_units[0] + fp[0], self.current_units[1] + fp[1], self.current_units[2] + fp[2]) self.feedrate = self.get_max_speed() self.move() self.set_target(0., 0., 0.) self.feedrate = self.get_max_speed() self.move() case _: raise ValueError("No Associated GCode Implemented/Known") return def execute(self, gcode): velocities = [] for instruction in gcode: velocities.append(self.instruction_converter(instruction)) return velocities def draw(self, file_path: str): with open(file_path, "r") as gcode_file: gcode = gcode_file.readlines() self.execute(gcode) def velocities_to_positions(self, velocities): return def has_command(key: str, instruction: str) -> bool: return key in instruction def search_string(key: str, instruction: str) -> float: index = instruction.find(key) if(index==-1): return float(0.) tmp = instruction[index+1:].split(' ')[0] return float(tmp)