Repository navigation
Expand file tree
/
Copy pathlinear.py
More file actions
182 lines (145 loc) · 5.32 KB
/
Copy pathlinear.py
File metadata and controls
182 lines (145 loc) · 5.32 KB
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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
"""Module for linear stages. """
from . import CustomMotor
class Linear(CustomMotor):
"""Stepper motor wrapper that converts between steps and linear distance."""
def __init__(self, controller, address="0", debug=False):
"""Initialize a linear motor with a 1 mm/revolution thread pitch."""
super().__init__(controller=controller, address=address, debug=debug)
self.threadpitch = 1.0 # mm/rev
self.wait_movement = False
## Setting and getting positions
def move_to(self, distance):
"""Move to an absolute linear position.
Parameters
----------
distance : float
Target distance in micrometers.
Returns
-------
bool or None
Movement state returned by :meth:`CustomMotor.move_absolute`.
"""
steps = distance_to_steps(distance,threadpitch=self.threadpitch,steps_per_rev=self.microsteps_per_rev)
status = self.move_absolute(pos=steps, wait=self.wait_movement)
return status
def move_distance(self, distance):
"""Move by a relative linear distance.
Parameters
----------
distance : float
Signed displacement in micrometers.
Returns
-------
bool or None
Movement state returned by :meth:`CustomMotor.move_relative`.
"""
steps = distance_to_steps(distance,threadpitch=self.threadpitch,steps_per_rev=self.microsteps_per_rev)
status = self.move_relative(pos=steps, wait=self.wait_movement)
return status
def get_distance(self):
"""Return the current linear position in micrometers.
Returns
-------
float
Current position converted from controller steps.
"""
pos = self.get_position()
distance = steps_to_distance(pos,threadpitch=self.threadpitch,steps_per_rev=self.microsteps_per_rev)
return distance
def jog(self, direction="forward", speed=0.0):
"""Set velocity-mode jogging speed in a selected direction.
Parameters
----------
direction : {"backward", "forward"}, default="forward"
Jogging direction.
speed : float, default=0.0
Jog speed in micrometers per second.
Returns
-------
tuple or None
Current motor status, or ``None`` for an invalid direction.
"""
speed_steps = int(distance_to_steps(speed,threadpitch=self.threadpitch,steps_per_rev=self.microsteps_per_rev))
if speed_steps > 2047:
speed_steps = 2047
if direction in ["backward", "forward"]:
if direction == "backward":
self.set_targetvelocity(speed=-1*speed_steps)
status = self.get_status()
return status
else:
self.set_targetvelocity(speed=speed_steps)
status = self.get_status()
return status
else:
return None
# Set the speed and acceleration
def set_speed(self, speed, threadpitch=1.0, steps_per_rev=3200):
"""Set maximum speed and acceleration from a linear speed.
Parameters
----------
speed : float
Maximum speed in micrometers per second.
threadpitch : float, default=1.0
Screw pitch in millimeters per revolution.
steps_per_rev : int, default=3200
Controller steps per revolution.
"""
step_speed = int(speed/(threadpitch*1000)*steps_per_rev)
self.set_maxvelocity(vmax=step_speed)
self.set_maxacceleration(amax=4*step_speed)
# Helper functions
def steps_to_distance(steps, threadpitch=1.0, steps_per_rev=3200):
"""Convert controller steps to linear distance.
Parameters
----------
steps : int or float
Number of controller steps.
threadpitch : float, default=1.0
Screw pitch in millimeters per revolution.
steps_per_rev : int, default=3200
Controller steps per revolution.
Returns
-------
float
Distance in micrometers.
"""
distance = steps/steps_per_rev*threadpitch*1000
return distance
def distance_to_steps(distance, threadpitch=1.0, steps_per_rev=3200):
"""Convert linear distance to an integer step count.
Parameters
----------
distance : float
Distance in micrometers.
threadpitch : float, default=1.0
Screw pitch in millimeters per revolution.
steps_per_rev : int, default=3200
Controller steps per revolution.
Returns
-------
int
Truncated controller step count.
"""
revs = distance / (threadpitch*1000)
steps = int(revs*steps_per_rev)
# moved_dist = steps/steps_per_rev*threadpitch*1000
return steps
def calc_threadpitch(steps, distance, steps_per_rev=3200):
"""Calculate screw pitch from measured steps and distance.
Parameters
----------
steps : int or float
Number of controller steps measured.
distance : float
Measured distance in micrometers.
steps_per_rev : int, default=3200
Controller steps per revolution.
Returns
-------
float
Screw pitch in millimeters per revolution.
"""
revs = steps/steps_per_rev
threadpitch = distance/revs/1000
return threadpitch