mboyer02/GCode-Generator
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Begun refactoring for control loop

Browse source
This commit is contained in:
Sélène Corbineau 2025-01-12 21:30:53 +01:00
parent 770100ef1c
commit 836cfbd6a6
1 changed files with 44 additions and 122 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

166
GCode_Interpreterdc.py
View file

@ -31,9 +31,11 @@ def bezier_length(x0, y0, I, J, P, Q, X, Y, num_points=1000):
# 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.
def __init__(self, ctrl, hw_interface):
self.CONTROLLER = ctrl
self.HARDWARE = hw_interface
# Hardcoded Values for Our Machine
X_STEP_PIN = 8
X_DIR_PIN = 9
X_MIN_PIN = 4
@ -50,7 +52,7 @@ class GCodeToMotors:
Z_DIR_PIN = 13
Z_MIN_PIN = 7
Z_MAX_PIN = 6
Z_ENABLE_PIN = 15
Z_ENABLE_PIN = 15 # All the above should probably go to HARDWARE
X_STEPS_PER_INCH = x_units = 4800
X_STEPS_PER_MM: float = 188.97
@ -64,8 +66,8 @@ class GCodeToMotors:
Z_STEPS_PER_MM: float = 188.97
Z_MOTOR_STEPS: int = 200
FAST_XY_FEEDRATE: int = 100
FAST_Z_FEEDRATE: int = 100
FAST_XY_FEEDRATE: float = 100 # in mm/s? Maybe we get mm/inch there too
FAST_Z_FEEDRATE: float = 100
CURVE_SECTION_INCHES = curve_section = .019685
CURVE_SECTION_MM: float = .5
@ -73,8 +75,15 @@ class GCodeToMotors:
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
target_feedrate: float = 0 # Target total head speed. For now in mm/s always
abs_mode: bool = False
@ -86,9 +95,6 @@ class GCodeToMotors:
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.
@ -127,111 +133,26 @@ class GCodeToMotors:
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
# 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.calculate_feedrate_delay(self.FAST_Z_FEEDRATE)
return self.calculate_feedrate_delay(self.FAST_XY_FEEDRATE)
return self.FAST_Z_FEEDRATE
return 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
# Try to move to target_units using target_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):
#self.HARDWARE.probe(self)
#while not self.CONTROLLER(self): # Allow controller to alter self
# self.HARDWARE.realize(self)
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.current_units = self.target_units.copy() # Too good to be true
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
@ -274,17 +195,17 @@ class GCodeToMotors:
if code == 1:
self.feedrate = search_string('F', instruction)
if self.feedrate > 0:
self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
self.target_feedrate = self.calculate_feedrate_delay(self.feedrate)
else:
self.feedrate_micros = self.get_max_speed()
self.target_feedrate = self.get_max_speed()
else:
self.feedrate_micros = self.get_max_speed()
self.target_feedrate = self.get_max_speed()
else:
if self.feedrate > 0:
self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
self.target_feedrate = self.feedrate
else:
self.feedrate_micros = self.get_max_speed()
self.move(self.feedrate_micros)
self.target_feedrate = self.get_max_speed()
self.move()
case 2, 3:
center = [0., 0., 0.]
@ -319,11 +240,11 @@ class GCodeToMotors:
self.set_target(newPoint[0], newPoint[1], fp[2])
if self.feedrate > 0:
self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
self.target_feedrate = self.feedrate
else:
self.feedrate_micros = self.get_max_speed()
self.target_feedrate = self.get_max_speed()
self.move(self.feedrate_micros)
self.move()
case 4:
time.sleep(search_string('P', instruction) * 1e-3)
@ -357,11 +278,11 @@ class GCodeToMotors:
self.set_target(newPoint[0], newPoint[1], fp[2])
if self.feedrate > 0:
self.feedrate_micros = self.calculate_feedrate_delay(self.feedrate)
self.target_feedrate = self.feedrate
else:
self.feedrate_micros = self.get_max_speed()
self.target_feedrate = self.get_max_speed()
self.move(self.feedrate_micros)
self.move()
case 5.1:
raise NotImplementedError("PAS DE SPLINE QUADRATIQUE J'AI LA FLEMME")
@ -413,10 +334,11 @@ class GCodeToMotors:
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.target_feedrate = self.get_max_speed()
self.move()
self.set_target(0., 0., 0.)
self.move(self.get_max_speed())
self.target_feedrate = self.get_max_speed()
self.move()
case _:
raise ValueError("No Associated GCode Implemented/Known")