Periscope.py

import numpy as np
import matplotlib.pyplot as plt

import sys
sys.path.append('../pyAPT/')
from pyAPT import Z825B

'''
config file:
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

deltaz_Mz:                motor position at local origin Mz
deltaz_Cz:                motor position at local origin Cz

sync_camera:              boolean




measurement file (n.a)
'''

class Periscope:
    def __init__(self, config, home = False):
        import time
        time.sleep(0.01)
        try:
            self.connect_motors()
            print ('Connnection with motors sucessful!')
        except:
            print ('Connection to motors failed. Close all programs that'+' use the motors or restart the controllers')
            raise   
        if home:
            self.home()
            
        self.set_config(config)
        
        
    def set_config(self,config):
        self.pos_local_origin_g = np.array([0.,0.,0.])
        
        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']
        
        pos_l = np.array([0.,0.,0.])
        
        pos_l[0] = config['pos_l_x']
        pos_l[1] = config['pos_l_y']
        pos_l[2] = config['pos_l_z']
        
        self.deltaz_Mz = config['deltaz_Mz']
        self.deltaz_Cz = config['deltaz_Cz']
        
        self.set_bounds()
        
        self.sync_camera = config['sync_camera']
        
        self.mAbs_l(pos_l, sync_camera = self.sync_camera)
        
    def get_config(self):
        config = {}
        
        pos_g = self.get_position()                           
        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])
                                   
        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])   
                                   
        config['deltaz_Mz'] = float(self.get_deltaz_Mz())
        config['deltaz_Cz'] = float(self.get_deltaz_Cz())
        
        config['sync_camera'] = self.sync_camera
        


        return config
        
    def get_position(self, kind = 'global'):
        pos_g = np.array([-self.Px.position(), self.Py.position(), self.Pz.position()])
        if kind == 'global':
            return pos_g
        if kind == 'local':
            return pos_g - self.pos_local_origin_g
    
    def get_deltaz_Mz(self):
        return self.Pz.position() - 25 + self.Mz.position()
    
    def get_deltaz_Cz(self):
        return self.Pz.position() - 25 + self.Cz.position() 
    
    def set_bounds(self):
        self.xmin = -25
        self.ymin = 0
        self.xmax = 0
        self.ymax = 25
        self.zmax = min((25 + min((self.deltaz_Mz, self.deltaz_Cz)), 25))
        self.zmin = max((max((self.deltaz_Mz, self.deltaz_Cz)), 0))
        
    def connect_motors(self):
        self.Mz = Z825B(83829619)    #Z825B(83835879) 
        self.Pz = Z825B(83832249)
        self.Py = Z825B(83832219)
        self.Px = Z825B(83835886)
        
        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)
        self.motors = [self.Px, self.Py, self.Pz, self.Mz, self.Cz]

    def get_pos_motor(self):
        return np.array([motor.position() for motor in self.motors])
    def home(self):
        for motor in self.motors:
            motor.home()
    def set_local_origin(self):
        self.pos_local_origin_g = self.get_position()
        self.pos_motor_local_origin = self.get_pos_motor()
        set_bounds()
    
    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):
        assert (self.Px.move(-dx)), 'Motor Px failed'
        
        
    def _myRel(self, dy):
        assert (self.Py.move(dy)), 'Motor Py failed'
        
    def _mzRel(self, dz):
        assert (self.Pz.move(dz)), 'Motor Pz failed'
        assert (self.Mz.move(-dz)), 'Motor Mz failed'
        
    def _mCamRel(self, dz):
        assert (self.Cz.move(-dz)), 'Motor Cz failed'
    
    def mRel(self, dpos, camera_sync = False):
        dx, dy, dz = list(dpos)
        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)])
        self.position_g = [-Px.position(), Py.position(), Pz.position()]

    def _mxAbs(self, x):
        assert (self.Px.goto(-x)), 'Motor Px failed'
        
        
    def _myAbs(self, y):
        assert (self.Py.goto(y)), 'Motor Py failed'
        
    def _mzAbs(self, z):
        
        deltaz =  -self.deltaz_Mz
        
        assert (self.Pz.goto(z)), 'Motor Pz failed'
        assert (self.Mz.goto(25 - z - deltaz)), 'Motor Mz failed'
        
    def _mCamAbs(self, z):
        deltaz =  -self.deltaz_Cz
        assert (self.Cz.goto(25 - z - deltaz)), 'Motor Cz failed'
    
    def mAbs(self, pos_g, sync_camera = False):
        
        self.check_bounds(pos_g)
        
        x,y,z = list(pos_g)
        
        if sync_camera:
            print ('c')
            self._mCamAbs(z)
        
        print ('x')
        self._mxAbs(x)
        print ('y')
        self._myAbs(y)
        print ('z')
        self._mzAbs(z)
             
        
    def mAbs_l(self, pos_l, sync_camera = False):
        
        self.mAbs(pos_l + self.pos_local_origin_g, sync_camera = sync_camera)