Arduino Style framework for our toy cnc

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)
+
+

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: