diff --git a/GCode_Interpreterdc.py b/GCode_Interpreterdc.py index 0cdeb8c..10d9423 100644 --- a/GCode_Interpreterdc.py +++ b/GCode_Interpreterdc.py @@ -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")