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)