451 lines
17 KiB
Python
451 lines
17 KiB
Python
import time
|
|
from collections import defaultdict
|
|
from typing import *
|
|
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
|
|
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
|
|
|
|
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: 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_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_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)
|
|
|
|
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 = np.ceil(length/self.curve_section)
|
|
|
|
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_micros = self.calculate_feedrate_delay(self.feedrate)
|
|
else:
|
|
self.feedrate_micros = self.get_max_speed()
|
|
|
|
self.move(self.feedrate_micros)
|
|
|
|
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
|
|
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(self.get_max_speed())
|
|
|
|
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.move(self.get_max_speed())
|
|
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_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) -> int:
|
|
index = instruction.find(key)
|
|
tmp = instruction[index+1:].split(' ')[0]
|
|
return int(tmp)
|
|
|
|
|