From f7b942be71c79dc206baba3e58c18b1620b75a5f Mon Sep 17 00:00:00 2001 From: mpboyer Date: Thu, 9 Jan 2025 17:13:34 +0100 Subject: [PATCH] Arduino Style framework for our toy cnc --- GCode_Interpreterdc.py | 289 +++++++++++++++++++++++++++++++++++++++++ main.py | 7 +- 2 files changed, 295 insertions(+), 1 deletion(-) create mode 100644 GCode_Interpreterdc.py diff --git a/GCode_Interpreterdc.py b/GCode_Interpreterdc.py new file mode 100644 index 0000000..ccba089 --- /dev/null +++ b/GCode_Interpreterdc.py @@ -0,0 +1,289 @@ +import time +from collections import defaultdict +from typing import * +import numpy as np + + +Point = NewType("point", List[float, float, float]) + +# We will assume everything is up to documentation. +class GCodeToMotors: + # Hardcoded Values for Our Machine + CONTROL = defaultdict(lambda t: False) # Figure out how to initialize this. + + X_STEP_PIN = 8 + X_DIR_PIN = 9 + X_MIN_PIN = 4 + X_MAX_PIN = 2 + X_ENABLE_PIN = 15 + + Y_STEP_PIN = 10 + Y_DIR_PIN = 11 + Y_MIN_PIN = 3 + Y_MAX_PIN = 5 + Y_ENABLE_PIN = 15 + + Z_STEP_PIN = 12 + Z_DIR_PIN = 13 + Z_MIN_PIN = 7 + Z_MAX_PIN = 6 + Z_ENABLE_PIN = 15 + + X_STEPS_PER_INCH = x_units = 4800 + X_STEPS_PER_MM: float = 188.97 + X_MOTOR_STEPS: float = 200 + + Y_STEPS_PER_INCH = y_units = 4800 + Y_STEPS_PER_MM: int = 188.97 + Y_MOTOR_STEPS: int = 200 + + Z_STEPS_PER_INCH = z_units = 4800 + Z_STEPS_PER_MM: float = 188.97 + Z_MOTOR_STEPS: int = 200 + + FAST_XY_FEEDRATE: int = 100 + FAST_Z_FEEDRATE: int = 100 + + CURVE_SECTION_INCHES = curve_section = .019685 + CURVE_SECTION_MM: float = .5 + + SENSORS_INVERTING: bool = False + + x_direction: int = 1 + y_direction: int = 1 + z_direction: int = 1 + + 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.] + + feedrate: float = 0. + feedrate_micros: int = 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: abs(t[0] - t[1]), zip(self.target_units, self.current_units))) + self.delta_steps = list(map(lambda t: abs(t[0] - t[1]), zip(self.target_steps, self.current_steps))) + + 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.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() + + def calculate_feedrate_delay(self, feedrate: float) -> float: + distance: float = np.linalg.norm(self.delta_units) + master_steps: float = max(self.delta_steps) + + # Compute delay between steps in microseconds + return ((distance * 600000000.) / feedrate) / master_steps + + def get_max_speed(self) -> float: + if self.delta_steps[2] > 0: + return self.calculate_feedrate_delay(self.FAST_Z_FEEDRATE) + return self.calculate_feedrate_delay(self.FAST_XY_FEEDRATE) + + def move(self, micro_delay: float): + max_delta = max(self.delta_steps) + x_counter = -max_delta/2 + y_counter = -max_delta/2 + z_counter = -max_delta/2 + + if micro_delay >= 16386: + milli_delay = micro_delay / 1000 + else: + milli_delay = 0 + + x_can_step = self.can_step(self.X_MIN_PIN, self.X_MAX_PIN, self.current_steps[0], self.target_steps[0], self.x_direction) + y_can_step = self.can_step(self.Y_MIN_PIN, self.Y_MAX_PIN, self.current_steps[1], self.target_steps[1], self.y_direction) + z_can_step = self.can_step(self.Z_MIN_PIN, self.Z_MAX_PIN, self.current_steps[2], self.target_steps[2], self.z_direction) + + while x_can_step or y_can_step or z_can_step: + x_can_step = self.can_step( + self.X_MIN_PIN, self.X_MAX_PIN, self.current_steps[0], self.target_steps[0], self.x_direction + ) + y_can_step = self.can_step( + self.Y_MIN_PIN, self.Y_MAX_PIN, self.current_steps[1], self.target_steps[1], self.y_direction + ) + z_can_step = self.can_step( + self.Z_MIN_PIN, self.Z_MAX_PIN, self.current_steps[2], self.target_steps[2], self.z_direction + ) + + if x_can_step: + x_counter += self.delta_steps[0] + if x_counter > 0: + self.step(self.X_STEP_PIN, self.X_DIR_PIN, self.x_direction) + x_counter -= max_delta + + if self.x_direction: + self.current_steps[0] += 1 + else: + self.current_steps[0] -= 1 + + if y_can_step: + y_counter += self.delta_steps[1] + if y_counter > 0: + self.step(self.Y_STEP_PIN, self.Y_DIR_PIN, self.y_direction) + y_counter -= max_delta + + if self.y_direction: + self.current_steps[1] += 1 + else: + self.current_steps[1] -= 1 + + if z_can_step: + z_counter += self.delta_steps[2] + if z_counter > 0: + self.step(self.Z_STEP_PIN, self.Z_DIR_PIN, self.z_direction) + z_counter -= max_delta + + if self.z_direction: + self.current_steps[2] += 1 + else: + self.current_steps[2] -= 1 + + if milli_delay > 0: + time.sleep(milli_delay*1e-3) + else: + time.sleep(micro_delay*1e-6) + + self.current_units = self.target_units.copy() + self.calculate_deltas() + + + def can_step(self, min_pin: int, max_pin: int, current: float, target: float, direction: bool): + if target == current: + return False + elif self.CONTROL[min_pin] and not direction: # TODO: IMPLEMENT CONTROL ON POSITION + return False + elif self.CONTROL[max_pin] and direction: + return False + return True + + def step(self, pinA: int, pinB: int, direction: bool): + pinA = bytes(pinA) + pinB = bytes(pinB) + direction = bytes(direction) + match (direction << 2 | self.CONTROL[pinA] << 1 | self.CONTROL[pinB]): # TODO: IMPLEMENT SPEED CONTROL + case 0, 5: + self.CONTROL[pinA] = True + case 1, 7: + self.CONTROL[pinB] = False + case 2, 4: + self.CONTROL[pinB] = True + case 3, 6: + self.CONTROL[pinA] = False + time.sleep(5e-6) + + + def instruction_to_velocities(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_position(fp[0], fp[1], fp[2]) + if has_command('G', instruction): + if code == 1: + self.feedrate = search_string('F', instruction) + if self.feedrate > 0: + self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate) + else: + self.feedrate_micros = self.get_max_speed() + else: + self.feedrate_micros = self.get_max_speed() + else: + 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) + + return + + def execute(self, gcode): + velocities = [] + for instruction in gcode: + velocities.append(self.instruction_to_velocities(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) -> int: + index = instruction.find(key) + tmp = instruction[index+1:].split(' ')[0] + return int(tmp) + + diff --git a/main.py b/main.py index 70f22cb..7eee8b2 100644 --- a/main.py +++ b/main.py @@ -3,6 +3,10 @@ import math from typing import List, Optional class SVGToGCodeConverter: + """ + General SVG to GCode converter, parametrized with the available functions. + """ + def __init__(self, supported_g_functions: List[str]): """Initialize the converter with the supported G-functions. @@ -18,7 +22,8 @@ class SVGToGCodeConverter: gcode += f" F{feedrate}" return gcode - def move_to_gcode(self, x: float, y: float) -> str: + @staticmethod + def move_to_gcode(x: float, y: float) -> str: return f"G0 X{x:.4f} Y{y:.4f}" def line_to_gcode(self, start: complex, end: complex) -> str: