Repository navigation
Expand file tree
/
Copy pathtwo_axis.py
More file actions
195 lines (162 loc) · 7.44 KB
/
Copy pathtwo_axis.py
File metadata and controls
195 lines (162 loc) · 7.44 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
183
184
185
186
187
188
189
190
191
192
193
194
"""Coordinate two linear motors as a planar stage."""
from . import Linear
from . import linear
import math
import time
class TwoAxisStage:
"""Coordinate two :class:`Linear` motors as an XY stage."""
def __init__(self, controller, address_x="0", address_y="1", debug=False):
"""Initialize the X and Y motor objects.
Parameters
----------
controller : BaseSerialController
Controller shared by both motors.
address_x : str, default="0"
Address of the X-axis motor.
address_y : str, default="1"
Address of the Y-axis motor.
debug : bool, default=False
Retained for API compatibility; motor debug output is disabled here.
"""
self.xmotor = Linear(controller=controller, address=address_x, debug=False)
self.ymotor = Linear(controller=controller, address=address_y, debug=False)
## Setting and getting positions
def go_to_point(self, xdistance, ydistance, step_units=False, speed = 0.5, waiting=False):
"""Move both axes to an absolute XY point.
Parameters
----------
xdistance, ydistance : float or int
Target coordinates in micrometers, or steps when ``step_units`` is true.
step_units : bool, default=False
Interpret coordinates as controller steps instead of micrometers.
speed : float, default=0.5
Reserved speed parameter retained for API compatibility.
waiting : bool, default=False
Wait for both axes and restore their original settings.
"""
if not step_units:
xsteps = linear.distance_to_steps(xdistance,
threadpitch=self.xmotor.threadpitch,
steps_per_rev=self.xmotor.microsteps_per_rev)
ysteps = linear.distance_to_steps(ydistance,
threadpitch=self.ymotor.threadpitch,
steps_per_rev=self.ymotor.microsteps_per_rev)
else:
xsteps = xdistance
ysteps = ydistance
xstart = self.xmotor.get_position()
ystart = self.ymotor.get_position()
deltax = xsteps-xstart
deltay = ysteps-ystart
settings1 = self.xmotor.get_settings()
settings2 = self.ymotor.get_settings()
print("Settings1: ", settings1)
print("Settings2: ", settings2)
angle = math.atan2(deltay,deltax)
v = math.sqrt(math.pow(settings1[1],2)+math.pow(settings2[1],2))
a = math.sqrt(math.pow(settings1[2],2)+math.pow(settings2[2],2))
speedx = abs(v*math.cos(angle))
speedy = abs(v*math.sin(angle))
accelx = abs(a*math.cos(angle))
accely = abs(a*math.sin(angle))
if speedx < 50:
speedx = 50
accelx = 200
elif speedy < 50:
speedy = 50
accely = 200
print("New Speedx: ", speedx, "New Speedy: ", speedy)
# Set the calculated velocities
self.xmotor.set_maxvelocity(vmax=int(speedx))
self.ymotor.set_maxvelocity(vmax=int(speedy))
# Set the calculated accelerations
self.xmotor.set_maxacceleration(amax=int(accelx))
self.ymotor.set_maxacceleration(amax=int(accely))
# DO THE MOVEMENT
self.xmotor.move_relative(pos=deltax, wait=False)
self.ymotor.move_relative(pos=deltay, wait=False)
if waiting is True:
while self.is_moving():
time.sleep(0.1)
# Set back the original speed and accelaration
self.xmotor.set_maxvelocity(vmax=settings1[1])
self.ymotor.set_maxvelocity(vmax=settings2[1])
self.xmotor.set_maxacceleration(amax=settings1[2])
self.ymotor.set_maxacceleration(amax=settings2[2])
def move(self, xdistance, ydistance, step_units=False, waiting=False):
"""Move both axes by a relative XY displacement.
Parameters
----------
xdistance, ydistance : float or int
Relative displacement in micrometers, or steps when ``step_units`` is true.
step_units : bool, default=False
Interpret displacements as controller steps instead of micrometers.
waiting : bool, default=False
Wait for both axes and restore their original settings.
"""
if not step_units:
xsteps = linear.distance_to_steps(xdistance,
threadpitch=self.xmotor.threadpitch,
steps_per_rev=self.xmotor.microsteps_per_rev)
ysteps = linear.distance_to_steps(ydistance,
threadpitch=self.ymotor.threadpitch,
steps_per_rev=self.ymotor.microsteps_per_rev)
else:
xsteps = xdistance
ysteps = ydistance
settings1 = self.xmotor.get_settings()
settings2 = self.ymotor.get_settings()
angle = math.atan2(ysteps,xsteps)
v = math.sqrt(math.pow(settings1[1],2)+math.pow(settings2[1],2))
a = math.sqrt(math.pow(settings1[2],2)+math.pow(settings2[2],2))
speedx = abs(v*math.cos(angle))
speedy = abs(v*math.sin(angle))
accelx = abs(a*math.cos(angle))
accely = abs(a*math.sin(angle))
# Set the calculated velocities
self.xmotor.set_maxvelocity(vmax=int(speedx))
self.ymotor.set_maxvelocity(vmax=int(speedy))
# Set the calculated accelerations
self.xmotor.set_maxacceleration(amax=int(accelx))
self.ymotor.set_maxacceleration(amax=int(accely))
# DO THE MOVEMENT
self.xmotor.move_relative(pos=xsteps, wait=False)
self.ymotor.move_relative(pos=ysteps, wait=False)
if waiting is True:
while self.is_moving():
time.sleep(0.1)
# Set back the original speed and accelaration
self.xmotor.set_maxvelocity(vmax=settings1[1])
self.ymotor.set_maxvelocity(vmax=settings2[1])
self.xmotor.set_maxacceleration(amax=settings1[2])
self.ymotor.set_maxacceleration(amax=settings2[2])
def get_distance(self, distance):
"""Return the current X and Y positions in micrometers.
Parameters
----------
distance : object
Unused compatibility parameter.
Returns
-------
tuple of float
``(x_distance, y_distance)`` in micrometers.
"""
posx = self.xmotor.get_position()
posy = self.ymotor.get_position()
distancex = linear.steps_to_distance(posx,
threadpitch=self.xmotor.threadpitch,
steps_per_rev=self.xmotor.microsteps_per_rev)
distancey = linear.steps_to_distance(posy,
threadpitch=self.ymotor.threadpitch,
steps_per_rev=self.ymotor.microsteps_per_rev)
return distancex, distancey
def is_moving(self):
"""Check whether either axis is moving.
Returns
-------
bool
``True`` when at least one motor is moving.
"""
xmove = self.xmotor.get_status()
ymove = self.ymotor.get_status()
return xmove or ymove