358 lines
13 KiB
Python
358 lines
13 KiB
Python
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 = 60 # in m/mn
|
|
FAST_Z_FEEDRATE: float = 1
|
|
|
|
CURVE_SECTION_INCHES = curve_section = 4*.019685
|
|
CURVE_SECTION_MM: float = .5*4
|
|
|
|
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('F', instruction) and code == 1:
|
|
self.feedrate = search_string('F', instruction)
|
|
print("set feedrate to", self.feedrate)
|
|
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.arctan2(bY, bX)
|
|
angleB = np.arctan2(aY, aX)
|
|
else:
|
|
angleA = np.arctan2(aY, aX)
|
|
angleB = np.arctan2(bY, bX)
|
|
|
|
if angleB <= angleA:
|
|
angleB += 2 * np.pi
|
|
angle = angleB - angleA
|
|
|
|
if has_command('F', instruction):
|
|
self.feedrate = search_string('F', instruction)
|
|
print("set feedrate to", self.feedrate)
|
|
if self.feedrate == 0:
|
|
self.feedrate = self.get_max_speed()
|
|
|
|
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)
|