Periscope.py 6.8 KB
Newer Older
Nick Sauerwein's avatar
Nick Sauerwein committed
1 2 3
import numpy as np
import matplotlib.pyplot as plt

Nick Sauerwein's avatar
Nick Sauerwein committed
4 5 6
import sys
sys.path.append('../pyAPT/')
from pyAPT import Z825B
Nick Sauerwein's avatar
Nick Sauerwein committed
7

8 9
'''
config file:
10 11 12 13 14 15 16 17
pos_l_x:                  current local coordinate x
pos_l_y:                  current local coordinate y
pos_l_z:                  current local coordinate z

pos_g_origin_x:           global x coordinate at local origin
pos_g_origin_y:           global y coordinate at local origin
pos_g_origin_z:           global z coordinate at local origin

18 19
deltaz_Mz:                motor position at local origin Mz
deltaz_Cz:                motor position at local origin Cz
20 21 22 23 24

sync_camera:              boolean



25 26 27

measurement file (n.a)
'''
Nick Sauerwein's avatar
Nick Sauerwein committed
28 29

class Periscope:
30
    def __init__(self, config, home = False):
31
        import time
Nick Sauerwein's avatar
Nick Sauerwein committed
32
        time.sleep(0.01)
33 34 35 36
        try:
            self.connect_motors()
            print ('Connnection with motors sucessful!')
        except:
37
            print ('Connection to motors failed. Close all programs that'+' use the motors or restart the controllers')
38 39 40 41 42
            raise   
        if home:
            self.home()
            
        self.set_config(config)
43
        
44 45
        
    def set_config(self,config):
46
        self.pos_local_origin_g = np.array([0.,0.,0.])
47 48 49 50 51
        
        self.pos_local_origin_g[0] = config['pos_g_origin_x']
        self.pos_local_origin_g[1] = config['pos_g_origin_y']
        self.pos_local_origin_g[2] = config['pos_g_origin_z']
        
52
        pos_l = np.array([0.,0.,0.])
53 54 55 56 57
        
        pos_l[0] = config['pos_l_x']
        pos_l[1] = config['pos_l_y']
        pos_l[2] = config['pos_l_z']
        
58 59 60 61
        self.deltaz_Mz = config['deltaz_Mz']
        self.deltaz_Cz = config['deltaz_Cz']
        
        self.set_bounds()
62
        
63
        self.sync_camera = config['sync_camera']
64
        
65
        self.mAbs_l(pos_l, sync_camera = self.sync_camera)
66 67 68
        
    def get_config(self):
        config = {}
69 70
        
        pos_g = self.get_position()                           
71 72 73
        config['pos_g_origin_x'] = float(self.pos_local_origin_g[0])
        config['pos_g_origin_y'] = float(self.pos_local_origin_g[1])
        config['pos_g_origin_z'] = float(self.pos_local_origin_g[2])
74
                                   
75 76 77
        config['pos_l_x'] = float(pos_g[0] - self.pos_local_origin_g[0])
        config['pos_l_y'] = float(pos_g[1] - self.pos_local_origin_g[1])
        config['pos_l_z'] = float(pos_g[2] - self.pos_local_origin_g[2])   
78
                                   
79 80
        config['deltaz_Mz'] = float(self.get_deltaz_Mz())
        config['deltaz_Cz'] = float(self.get_deltaz_Cz())
81 82 83 84
        
        config['sync_camera'] = self.sync_camera
        

85 86 87 88

        return config
        
    def get_position(self, kind = 'global'):
Nick Sauerwein's avatar
Nick Sauerwein committed
89
        pos_g = np.array([-self.Px.position(), self.Py.position(), self.Pz.position()])
90 91 92 93
        if kind == 'global':
            return pos_g
        if kind == 'local':
            return pos_g - self.pos_local_origin_g
94 95
    
    def get_deltaz_Mz(self):
Nick Sauerwein's avatar
Nick Sauerwein committed
96
        return self.Pz.position() - 25 + self.Mz.position()
97 98
    
    def get_deltaz_Cz(self):
Nick Sauerwein's avatar
Nick Sauerwein committed
99
        return self.Pz.position() - 25 + self.Cz.position() 
100
    
101
    def set_bounds(self):
Nick Sauerwein's avatar
Nick Sauerwein committed
102 103 104 105
        self.xmin = -25
        self.ymin = 0
        self.xmax = 0
        self.ymax = 25
106 107
        self.zmax = min((25 + min((self.deltaz_Mz, self.deltaz_Cz)), 25))
        self.zmin = max((max((self.deltaz_Mz, self.deltaz_Cz)), 0))
108 109
        
    def connect_motors(self):
110
        self.Mz = Z825B(83829619)    #Z825B(83835879) 
Nick Sauerwein's avatar
Nick Sauerwein committed
111 112 113
        self.Pz = Z825B(83832249)
        self.Py = Z825B(83832219)
        self.Px = Z825B(83835886)
114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
        
        class dummmotor():
            def __init__(self):
                print ('dummmotor used')
                self.pos = 0.
            def position(self):
                return self.pos
            
            def goto(self,z):
                self.pos = z
                return True
                
            def home(self):
                self.pos = 0.
                return True
        
        self.Cz = dummmotor()#Z825B(83829619)
131 132 133
        self.motors = [self.Px, self.Py, self.Pz, self.Mz, self.Cz]

    def get_pos_motor(self):
134
        return np.array([motor.position() for motor in self.motors])
135 136
    def home(self):
        for motor in self.motors:
Nick Sauerwein's avatar
Nick Sauerwein committed
137
            motor.home()
Nick Sauerwein's avatar
Nick Sauerwein committed
138
    def set_local_origin(self):
139 140 141
        self.pos_local_origin_g = self.get_position()
        self.pos_motor_local_origin = self.get_pos_motor()
        set_bounds()
Nick Sauerwein's avatar
Nick Sauerwein committed
142 143 144 145 146 147 148 149 150 151
    
    def check_bounds(self, position_g):
        assert (position_g[0] >= self.xmin),"x coordinate to low!"
        assert (position_g[0] <= self.xmax),"x coordinate to high!"
        assert (position_g[1] >= self.ymin),"y coordinate to low!"
        assert (position_g[1] <= self.ymax),"y coordinate to high!"
        assert (position_g[2] >= self.zmin),"z coordinate to low!"
        assert (position_g[2] <= self.zmax),"z coordinate to high!"
    
    def _mxRel(self, dx):
Nick Sauerwein's avatar
Nick Sauerwein committed
152
        assert (self.Px.move(-dx)), 'Motor Px failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
153 154 155
        
        
    def _myRel(self, dy):
Nick Sauerwein's avatar
Nick Sauerwein committed
156
        assert (self.Py.move(dy)), 'Motor Py failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
157 158
        
    def _mzRel(self, dz):
Nick Sauerwein's avatar
Nick Sauerwein committed
159 160
        assert (self.Pz.move(dz)), 'Motor Pz failed'
        assert (self.Mz.move(-dz)), 'Motor Mz failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
161 162
        
    def _mCamRel(self, dz):
Nick Sauerwein's avatar
Nick Sauerwein committed
163
        assert (self.Cz.move(-dz)), 'Motor Cz failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
164
    
165 166
    def mRel(self, dpos, camera_sync = False):
        dx, dy, dz = list(dpos)
Nick Sauerwein's avatar
Nick Sauerwein committed
167 168 169 170 171 172 173 174 175 176 177 178
        self.check_bounds(self.position_g + np.array([dx, dy, dz]))
        
        self._mxRel(dx)
        self._myRel(dy)
        self._mzRel(dz)
 
        if camera_sync:
            self._mCamRel(dz)
        
        self.history_g += [self.position_g]
        
        self.position_l += np.array([np.float64(dx), np.float64(dy), np.float64(dz)])
179
        self.position_g = [-Px.position(), Py.position(), Pz.position()]
Nick Sauerwein's avatar
Nick Sauerwein committed
180 181

    def _mxAbs(self, x):
Nick Sauerwein's avatar
Nick Sauerwein committed
182
        assert (self.Px.goto(-x)), 'Motor Px failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
183 184 185
        
        
    def _myAbs(self, y):
Nick Sauerwein's avatar
Nick Sauerwein committed
186
        assert (self.Py.goto(y)), 'Motor Py failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
187 188 189
        
    def _mzAbs(self, z):
        
190
        deltaz =  -self.deltaz_Mz
Nick Sauerwein's avatar
Nick Sauerwein committed
191
        
Nick Sauerwein's avatar
Nick Sauerwein committed
192 193
        assert (self.Pz.goto(z)), 'Motor Pz failed'
        assert (self.Mz.goto(25 - z - deltaz)), 'Motor Mz failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
194 195
        
    def _mCamAbs(self, z):
196
        deltaz =  -self.deltaz_Cz
Nick Sauerwein's avatar
Nick Sauerwein committed
197
        assert (self.Cz.goto(25 - z - deltaz)), 'Motor Cz failed'
Nick Sauerwein's avatar
Nick Sauerwein committed
198
    
199
    def mAbs(self, pos_g, sync_camera = False):
Nick Sauerwein's avatar
Nick Sauerwein committed
200
        
201 202 203
        self.check_bounds(pos_g)
        
        x,y,z = list(pos_g)
Nick Sauerwein's avatar
Nick Sauerwein committed
204
        
205
        if sync_camera:
Nick Sauerwein's avatar
Nick Sauerwein committed
206
            print ('c')
207 208
            self._mCamAbs(z)
        
Nick Sauerwein's avatar
Nick Sauerwein committed
209
        print ('x')
Nick Sauerwein's avatar
Nick Sauerwein committed
210
        self._mxAbs(x)
Nick Sauerwein's avatar
Nick Sauerwein committed
211
        print ('y')
Nick Sauerwein's avatar
Nick Sauerwein committed
212
        self._myAbs(y)
Nick Sauerwein's avatar
Nick Sauerwein committed
213
        print ('z')
Nick Sauerwein's avatar
Nick Sauerwein committed
214
        self._mzAbs(z)
215
             
Nick Sauerwein's avatar
Nick Sauerwein committed
216
        
217
    def mAbs_l(self, pos_l, sync_camera = False):
Nick Sauerwein's avatar
Nick Sauerwein committed
218
        
219
        self.mAbs(pos_l + self.pos_local_origin_g, sync_camera = sync_camera)
Nick Sauerwein's avatar
Nick Sauerwein committed
220 221 222 223 224 225 226 227 228