# Code for handling cartesian (standard x, y, z planes) moves # # Copyright (C) 2016 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging, time import lookahead, stepper, homing # Common suffixes: _d is distance (in mm), _v is velocity (in # mm/second), _t is time (in seconds), _r is ratio (scalar between # 0.0 and 1.0) StepList = (0, 1, 2, 3) class CartKinematics: def __init__(self, printer, config): steppers = ['stepper_x', 'stepper_y', 'stepper_z', 'stepper_e'] self.steppers = [stepper.PrinterStepper(printer, config.getsection(n)) for n in steppers] self.stepper_pos = [0, 0, 0, 0] def build_config(self): for stepper in self.steppers: stepper.build_config() def set_position(self, newpos): self.stepper_pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5) for i in StepList] def get_max_xy_speed(self): max_xy_speed = min(s.max_step_velocity*s.step_dist for s in self.steppers[:2]) max_xy_accel = min(s.max_step_accel*s.step_dist for s in self.steppers[:2]) return max_xy_speed, max_xy_accel def get_max_speed(self, axes_d, move_d): # Calculate max speed and accel for a given move velocity_factor = min( [self.steppers[i].max_step_velocity * self.steppers[i].step_dist / abs(axes_d[i]) for i in StepList if axes_d[i]]) accel_factor = min( [self.steppers[i].max_step_accel * self.steppers[i].step_dist / abs(axes_d[i]) for i in StepList if axes_d[i]]) return velocity_factor * move_d, accel_factor * move_d def get_max_e_speed(self): s = self.steppers[3] return s.max_step_velocity*s.step_dist, s.max_step_accel*s.step_dist def home(self, toolhead, axis): # Each axis is homed independently and in order homing_state = homing.Homing(toolhead, self.steppers) # XXX for a in axis: homing_state.plan_home(a) return homing_state def motor_off(self, move_time): for stepper in self.steppers: stepper.motor_enable(move_time, 0) def move(self, move_time, move): inv_accel = 1. / move.accel inv_cruise_v = 1. / move.cruise_v for i in StepList: new_step_pos = int(move.pos[i]*self.steppers[i].inv_step_dist + 0.5) steps = new_step_pos - self.stepper_pos[i] if not steps: continue self.stepper_pos[i] = new_step_pos sdir = 0 if steps < 0: sdir = 1 steps = -steps clock_offset, clock_freq, so = self.steppers[i].prep_move( sdir, move_time) step_dist = move.move_d / steps step_offset = 0.5 # Acceleration steps #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel accel_clock_offset = move.start_v * inv_accel * clock_freq accel_sqrt_offset = accel_clock_offset**2 accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2 accel_steps = move.accel_r * steps step_offset = so.step_sqrt( accel_steps, step_offset, clock_offset - accel_clock_offset , accel_sqrt_offset, accel_multiplier) clock_offset += move.accel_t * clock_freq # Cruising steps #t = pos/cruise_v cruise_multiplier = step_dist * inv_cruise_v * clock_freq cruise_steps = move.cruise_r * steps step_offset = so.step_factor( cruise_steps, step_offset, clock_offset, cruise_multiplier) clock_offset += move.cruise_t * clock_freq # Deceleration steps #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel) decel_clock_offset = move.cruise_v * inv_accel * clock_freq decel_sqrt_offset = decel_clock_offset**2 decel_steps = move.decel_r * steps so.step_sqrt( decel_steps, step_offset, clock_offset + decel_clock_offset , decel_sqrt_offset, -accel_multiplier) class Move: def __init__(self, toolhead, pos, move_d, axes_d, speed, accel): self.toolhead = toolhead self.pos = tuple(pos) self.move_d = move_d self.axes_d = axes_d self.accel = accel self.junction_max = speed**2 self.junction_delta = 2.0 * move_d * accel self.junction_start_max = 0. def calc_junction(self, prev_move): # Find max start junction velocity using approximated # centripetal velocity as described at: # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/ if not prev_move.move_d: return junction_cos_theta = -((self.axes_d[0] * prev_move.axes_d[0] + self.axes_d[1] * prev_move.axes_d[1]) / (self.move_d * prev_move.move_d)) if junction_cos_theta > 0.999999: return junction_cos_theta = max(junction_cos_theta, -0.999999) sin_theta_d2 = math.sqrt(0.5*(1.0-junction_cos_theta)); R = self.toolhead.junction_deviation * sin_theta_d2 / (1. - sin_theta_d2) self.junction_start_max = min( R * self.accel, self.junction_max, prev_move.junction_max) def process(self, junction_start, junction_end): # Determine accel, cruise, and decel portions of the move junction_cruise = self.junction_max inv_junction_delta = 1. / self.junction_delta accel_r = (junction_cruise-junction_start) * inv_junction_delta decel_r = (junction_cruise-junction_end) * inv_junction_delta cruise_r = 1. - accel_r - decel_r if cruise_r < 0.: accel_r += 0.5 * cruise_r decel_r = 1.0 - accel_r cruise_r = 0. junction_cruise = junction_start + accel_r*self.junction_delta self.accel_r, self.cruise_r, self.decel_r = accel_r, cruise_r, decel_r # Determine the move velocities and time spent in each portion start_v = math.sqrt(junction_start) cruise_v = math.sqrt(junction_cruise) end_v = math.sqrt(junction_end) self.start_v, self.cruise_v, self.end_v = start_v, cruise_v, end_v accel_t = 2.0 * self.move_d * accel_r / (start_v + cruise_v) cruise_t = self.move_d * cruise_r / cruise_v decel_t = 2.0 * self.move_d * decel_r / (end_v + cruise_v) self.accel_t, self.cruise_t, self.decel_t = accel_t, cruise_t, decel_t # Generate step times for the move next_move_time = self.toolhead.get_next_move_time() self.toolhead.kin.move(next_move_time, self) self.toolhead.update_move_time(accel_t + cruise_t + decel_t) STALL_TIME = 0.100 class ToolHead: def __init__(self, printer, config): self.printer = printer self.reactor = printer.reactor self.kin = CartKinematics(printer, config) self.max_xy_speed, self.max_xy_accel = self.kin.get_max_xy_speed() self.junction_deviation = config.getfloat('junction_deviation', 0.02) dummy_move = Move(self, [0.]*4, 0., [0.]*4, 0., 0.) self.move_queue = lookahead.MoveQueue(dummy_move) self.commanded_pos = [0., 0., 0., 0.] # Print time tracking self.buffer_time_high = config.getfloat('buffer_time_high', 5.000) self.buffer_time_low = config.getfloat('buffer_time_low', 0.150) self.move_flush_time = config.getfloat('move_flush_time', 0.050) self.motor_off_delay = config.getfloat('motor_off_time', 60.000) self.print_time = 0. self.print_time_stall = 0 self.motor_off_time = self.reactor.NEVER self.flush_timer = self.reactor.register_timer(self.flush_handler) def build_config(self): self.kin.build_config() # Print time tracking def update_move_time(self, movetime): self.print_time += movetime flush_to_time = self.print_time - self.move_flush_time self.printer.mcu.flush_moves(flush_to_time) def get_next_move_time(self): if not self.print_time: self.print_time = self.buffer_time_low + STALL_TIME curtime = time.time() self.printer.mcu.set_print_start_time(curtime) self.reactor.update_timer(self.flush_timer, self.reactor.NOW) return self.print_time def get_last_move_time(self): self.move_queue.flush() return self.get_next_move_time() def reset_motor_off_time(self, eventtime): self.motor_off_time = eventtime + self.motor_off_delay def reset_print_time(self): self.move_queue.flush() self.printer.mcu.flush_moves(self.print_time) self.print_time = 0. self.reset_motor_off_time(time.time()) self.reactor.update_timer(self.flush_timer, self.motor_off_time) def check_busy(self, eventtime): if not self.print_time: # XXX - find better way to flush initial move_queue items if self.move_queue.queue: self.reactor.update_timer(self.flush_timer, eventtime + 0.100) return False buffer_time = self.printer.mcu.get_print_buffer_time( eventtime, self.print_time) return buffer_time > self.buffer_time_high def flush_handler(self, eventtime): if not self.print_time: self.move_queue.flush() if not self.print_time: if eventtime >= self.motor_off_time: self.motor_off() self.reset_print_time() self.motor_off_time = self.reactor.NEVER return self.motor_off_time print_time = self.print_time buffer_time = self.printer.mcu.get_print_buffer_time( eventtime, print_time) if buffer_time > self.buffer_time_low: return eventtime + buffer_time - self.buffer_time_low self.move_queue.flush() if print_time != self.print_time: self.print_time_stall += 1 self.dwell(self.buffer_time_low + STALL_TIME) return self.reactor.NOW self.reset_print_time() return self.motor_off_time def stats(self, eventtime): buffer_time = 0. if self.print_time: buffer_time = self.printer.mcu.get_print_buffer_time( eventtime, self.print_time) return "print_time=%.3f buffer_time=%.3f print_time_stall=%d" % ( self.print_time, buffer_time, self.print_time_stall) # Movement commands def get_position(self): return list(self.commanded_pos) def set_position(self, newpos): self.move_queue.flush() self.commanded_pos[:] = newpos self.kin.set_position(newpos) def _move_with_z(self, newpos, axes_d, speed): self.move_queue.flush() move_d = math.sqrt(sum([d*d for d in axes_d[:3]])) # Limit velocity and accel to max for each stepper kin_speed, kin_accel = self.kin.get_max_speed(axes_d, move_d) speed = min(speed, self.max_xy_speed, kin_speed) accel = min(self.max_xy_accel, kin_accel) # Generate and execute move move = Move(self, newpos, move_d, axes_d, speed, accel) move.process(0., 0.) def _move_only_e(self, newpos, axes_d, speed): self.move_queue.flush() kin_speed, kin_accel = self.kin.get_max_e_speed() speed = min(speed, self.max_xy_speed, kin_speed) accel = min(self.max_xy_accel, kin_accel) move = Move(self, newpos, abs(axes_d[3]), axes_d, speed, accel) move.process(0., 0.) def move(self, newpos, speed, sloppy=False): axes_d = [newpos[i] - self.commanded_pos[i] for i in StepList] self.commanded_pos[:] = newpos if axes_d[2]: self._move_with_z(newpos, axes_d, speed) return move_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2) if not move_d: if axes_d[3]: self._move_only_e(newpos, axes_d, speed) return # Common xy move - create move and queue it speed = min(speed, self.max_xy_speed) move = Move(self, newpos, move_d, axes_d, speed, self.max_xy_accel) move.calc_junction(self.move_queue.prev_move()) self.move_queue.add_move(move) def home(self, axis): return self.kin.home(self, axis) def dwell(self, delay): self.get_last_move_time() self.update_move_time(delay) def motor_off(self): self.dwell(STALL_TIME) last_move_time = self.get_last_move_time() self.kin.motor_off(last_move_time) self.dwell(STALL_TIME) logging.debug('; Max time of %f' % (last_move_time,))