1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
|
# Printer stepper support
#
# Copyright (C) 2016 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging
class PrinterStepper:
def __init__(self, printer, config):
self.printer = printer
self.config = config
self.mcu_stepper = self.mcu_enable = self.mcu_endstop = None
self.step_dist = config.getfloat('step_distance')
self.inv_step_dist = 1. / self.step_dist
self.max_velocity = config.getfloat('max_velocity')
self.max_accel = config.getfloat('max_accel')
self.max_jerk = 0.
self.homing_speed = config.getfloat('homing_speed', 5.0)
self.homing_positive_dir = config.getboolean(
'homing_positive_dir', False)
self.homing_retract_dist = config.getfloat('homing_retract_dist', 5.)
self.homing_stepper_phases = config.getint('homing_stepper_phases')
self.homing_endstop_phase = config.getint('homing_endstop_phase')
endstop_accuracy = config.getfloat('homing_endstop_accuracy')
self.homing_endstop_accuracy = None
if self.homing_stepper_phases:
if endstop_accuracy is None:
self.homing_endstop_accuracy = self.homing_stepper_phases//2 - 1
elif self.homing_endstop_phase is not None:
self.homing_endstop_accuracy = int(math.ceil(
endstop_accuracy * self.inv_step_dist / 2.))
else:
self.homing_endstop_accuracy = int(math.ceil(
endstop_accuracy * self.inv_step_dist))
if self.homing_endstop_accuracy >= self.homing_stepper_phases/2:
logging.info("Endstop for %s is not accurate enough for stepper"
" phase adjustment" % (self.config.section,))
self.homing_stepper_phases = None
self.position_min = config.getfloat('position_min', 0.)
self.position_endstop = config.getfloat('position_endstop')
self.position_max = config.getfloat('position_max')
self.need_motor_enable = True
def set_max_jerk(self, max_jerk):
self.max_jerk = max_jerk
def build_config(self):
max_error = self.config.getfloat('max_error', 0.000050)
step_pin = self.config.get('step_pin')
dir_pin = self.config.get('dir_pin')
jc = self.max_jerk / self.max_accel
inv_max_step_accel = self.step_dist / self.max_accel
min_stop_interval = (math.sqrt(3.*inv_max_step_accel + jc**2)
- math.sqrt(inv_max_step_accel + jc**2)) - max_error
min_stop_interval = max(0., min_stop_interval)
mcu = self.printer.mcu
self.mcu_stepper = mcu.create_stepper(
step_pin, dir_pin, min_stop_interval, max_error)
enable_pin = self.config.get('enable_pin')
if enable_pin is not None:
self.mcu_enable = mcu.create_digital_out(enable_pin, 0)
endstop_pin = self.config.get('endstop_pin')
if endstop_pin is not None:
self.mcu_endstop = mcu.create_endstop(endstop_pin, self.mcu_stepper)
def motor_enable(self, move_time, enable=0):
if (self.mcu_enable is not None
and self.mcu_enable.get_last_setting() != enable):
mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
self.mcu_enable.set_digital(mcu_time, enable)
self.need_motor_enable = True
def prep_move(self, move_time, sdir):
mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
self.mcu_stepper.set_next_step_dir(mcu_time, sdir)
if self.need_motor_enable:
self.motor_enable(move_time, 1)
self.need_motor_enable = False
return (mcu_time, self.mcu_stepper)
def enable_endstop_checking(self, move_time, step_time):
mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
self.mcu_endstop.home(mcu_time, step_time)
return self.mcu_endstop
def get_homed_position(self):
if not self.homing_stepper_phases:
return self.position_endstop
pos = self.mcu_endstop.get_last_position()
pos %= self.homing_stepper_phases
if self.homing_endstop_phase is None:
logging.info("Setting %s endstop phase to %d" % (
self.config.section, pos))
self.homing_endstop_phase = pos
return self.position_endstop
delta = (pos - self.homing_endstop_phase) % self.homing_stepper_phases
if delta >= self.homing_stepper_phases - self.homing_endstop_accuracy:
delta -= self.homing_stepper_phases
elif delta > self.homing_endstop_accuracy:
logging.error("Endstop %s incorrect phase (got %d vs %d)" % (
self.config.section, pos, self.homing_endstop_phase))
return self.position_endstop
return self.position_endstop + delta * self.step_dist
|
