esrf_spectro_eh2 detector CalcMotor framewk

id26/spectro/esrf_spectro_eh2.py

+from bliss.controllers.motor import CalcController
+
+'''
+
+
+- package: id26.spectro.esrf_spectro_eh2
+  class: EH2SpectroDetector
+  name: hdetector2
+  radius: 1004.0
+  dlong: $dlong
+  azlong: $azlong
+  axes:
+        - name: $hdx2
+          tags: real y
+        - name: $hdz2
+          tags: real z
+        - name: $hdth2
+          tags: real th
+          
+        - name: xdet
+          tags: xpos
+        - name: zdet
+          tags: zpos
+        - name: pitdet
+          tags: pitch
+
+'''
+
+
+class EH2SpectroDetector(CalcController):
+    
+    def __init__(self, *args, **kwargs):
+        CalcController.__init__(self, *args, **kwargs)
+        
+        self.dlong = self.config.get("dlong", None)
+        self.azlong = self.config.get("azlong", None)
+
+        self.dlong_angle  = self.config.get("dlong_angle", float, 68)
+        self.dlong_travel = self.config.get("dlong_travel", float, 1200)
+        self.dlong_base_x = self.config.get("dlong_base_x", float, -21.51)
+        self.dlong_base_z = self.config.get("dlong_base_z", float, 160.25)
+        
+        self.ref_params = HashObjSetting(f"MMSpectroDetector:{self.name}")
+       
+        if self.ref_params.get('dx_offset') is None:
+                print("Warning: dx_offset is None, updating with default value of 150 (mm)")
+                self.ref_params['dx_offset'] = 150
+
+        if self.ref_params.get('dz_offset') is None:
+                print("Warning: dz_offset is None, updating with default value of 150 (mm)")
+                self.ref_params['dz_offset'] = 150
+        
+        if self.ref_params.get('det_deviation_x') is None:
+                print("Warning: det_deviation_x is None, updating with zero.")
+                self.ref_params['det_deviation_x'] = 0  
+
+        if self.ref_params.get('dz_correction') is None:
+                self.ref_params['dz_correction'] = 0  
+
+        if self.ref_params.get('radius') is None:
+                self.ref_params['radius'] = self.config.get('radius', float)
+           
+        if self.ref_params.get('alpha') is None:
+                self.ref_params['alpha'] = self.config.get('alpha', float, 0)
+
+        if self.ref_params.get('a_z_set') is None:
+                self.ref_params['a_z_set'] = self.config.get('a_z_set', float, 0)
+                
+        self._init_meta_data_publishing()
+
+    def _init_meta_data_publishing(self):
+
+        if not self.name:
+            user_warning(
+                "to publish metadata the spectrometer detector controller needs a name in config"
+            )
+            return
+        scan_meta_obj = get_user_scan_meta()
+        scan_meta_obj.instrument.set(self, lambda _: {self.name: self.metadata()})
+
+    def metadata(self):
+        
+        meta_dict = dict()
+
+        for key in self.ref_params.keys():
+                meta_dict.update(
+                        {
+                                key: self.ref_params[key],
+                        }
+            )
+
+        meta_dict["@NX_class"] = "NXspectro"
+
+        return meta_dict
+
+    def calc_to_real(self, positions_dict):
+        real_dict = dict()
+        pitch = positions_dict["pitch"]
+        xpos = positions_dict["xpos"]
+        zpos = positions_dict["zpos"]
+        
+        radius = self.ref_params.get('radius')
+        det_deviation_x = self.ref_params.get('det_deviation_x')
+        alpha = self.ref_params['alpha']
+        a_z_set = self.ref_params['a_z_set']
+            
+        dx,dz,th = self._dx_dz_th_calc (xpos, zpos, pitch)
+        
+        real_dict["y"]  = dx
+        real_dict["z"]  = dz
+        real_dict["th"] = th
+            
+        return real_dict
+
+    def calc_from_real(self, positions_dict):
+
+        th = positions_dict["th"]
+        dx = positions_dict["y"]
+        dz = positions_dict["z"]
+
+        x,z,p = self._x_z_pitch_calc (dx, dz, th)
+        
+        return {"xpos" : x,
+                "zpos" : z,
+                "pitch": p,
+        }
+
+    def _dx_dz_th_calc (self, xpos, zpos, pitch):
+        #TODO WRITE REAL CALCS
+        dx=xpos
+        dz=zpos
+        th=pitch
+        return dx, dz, th
+    
+    def _x_z_pitch_calc (self, dx, dz, dth):
+        #TODO WRITE REAL CALCS
+        x=dx
+        z=dz
+        pitch=dth
+        return x, z, pitch
+
+    
+### OLD MMSPECTROMETER.PY WAY CALCS 
+    
+    def _old_dx_dz_calc(self, Dvec, dlong_pos):
+
+        dx=Dvec[0]
+        dz=Dvec[2]
+        
+#        print("dx abs is {} and dz abs is {}".format(dx, dz))
+        
+        dlong_angle  = math.radians(self.dlong_angle);
+        dlong_base_x = self.dlong_base_x
+        dlong_base_z = self.dlong_base_z -self.azlong.position
+
+#        print("dlong_angle is {} and dlong_base_x is {} and dlong_base_z is {}".format(dlong_angle, dlong_base_x, dlong_base_z))
+
+        dlong_center_x = dlong_pos * math.cos(dlong_angle) + dlong_base_x
+        dlong_center_z = dlong_pos * math.sin(dlong_angle) + dlong_base_z
+
+#        print("dlong_center_x is {} and dlong_center_z is {}".format(dlong_center_x, dlong_center_z))
+
+        dx_offset = self.ref_params.get('dx_offset')
+        dz_offset = self.ref_params.get('dz_offset')
+        dz_correction = self.ref_params.get('dz_correction')
+        
+#        print("dx_offset is {} and dz_offset is {} and dz_correction is {}".format(dx_offset, dz_offset, dz_correction))
+
+        stage_dx_pos = dx - dlong_center_x + dx_offset 
+        stage_dz_pos = dz - dlong_center_z + dz_offset + dz_correction
+
+        return stage_dx_pos, stage_dz_pos
+    
+    
+    def dlong_calc_middle(self, th_1, th_2):
+        ''' 
+        required if dlong is not moved during scans. The scan range is between low and high.
+        A middle position is determined for dlong which is used itself to calculate the motor positions for stage_dx and stage_dz
+        '''
+        
+        th_low = min(th_1, th_2)
+        th_high = max(th_1, th_2)
+        
+        print(f'at low angle {th_low}')
+        pos_lo, dvec_lo =  self._dlong_calc(th_low)
+        print(f'at high angle {th_high}')
+        pos_hi, dvec_hi =  self._dlong_calc(th_high)
+            
+        pos_middle = abs(pos_hi - pos_lo) / 2 + min(pos_hi, pos_lo)
+
+        print(f'dlong at low angle at {pos_lo:.2f}')
+        print(f'dlong at high angle at {pos_hi:.2f}')
+        print(f'dlong middle position is at {pos_middle:.2f}')
+
+        try:
+                _dx_pos_lo, _dz_pos_lo = self._dx_dz_calc(dvec_lo, pos_middle)
+                _dx_pos_hi, _dz_pos_hi = self._dx_dz_calc(dvec_hi, pos_middle)
+
+                def _check_lim (range, value):
+                        if value == min(list(range)+[value]):
+                                return f'- OUT OF RANGE {range}'
+                        if value == max(list(range)+[value]):
+                                return f'- OUT OF RANGE {range}'
+                        return ''
+                
+                print(f'\nFor this middle position of dlong we get:')
+                print(f'stage dx at low angle at {_dx_pos_lo:.2f} {_check_lim(self._tagged["y"][0].limits, _dx_pos_lo)}')
+                print(f'stage dx at high angle at {_dx_pos_hi:.2f} {_check_lim(self._tagged["y"][0].limits, _dx_pos_hi)}')
+                print(f'stage dz at low angle at {_dz_pos_lo:.2f} {_check_lim(self._tagged["z"][0].limits, _dz_pos_lo)}')
+                print(f'stage dz at high angle at {_dz_pos_hi:.2f} {_check_lim(self._tagged["z"][0].limits, _dz_pos_hi)}')
+
+        except RuntimeError as e:
+                print (e.args[0])
+                
+            
+        return pos_middle
+
+
+    def _dlong_calc(self, theta):
+            
+        vecs = _xes_eh2(self.ref_params['radius'], theta, 0, 0, 0, 0)
+        dvec = vecs[1]
+                
+        dx=dvec[0]  
+        dz=dvec[2]
+                
+        dlong_angle  = np.deg2rad (self.dlong_angle)
+        dlong_travel = self.dlong_travel
+        dlong_base_x = self.dlong_base_x
+        dlong_base_z = self.dlong_base_z - self.azlong.position
+
+        dlong_low_limit = self.dlong.low_limit
+        dlong_high_limit = self.dlong.high_limit
+        if dlong_low_limit > dlong_high_limit:
+                dlong_low_limit,  dlong_high_limit = dlong_high_limit ,dlong_low_limit  
+        
+       
+        # make lines for dlong stage and m=-1 through detector point
+        m = np.tan(dlong_angle)
+        b = dlong_base_z-dlong_base_x*m
+        mn = np.tan(np.deg2rad(-45))
+        bn = dz-mn*dx
+
+        # calculate intersection point between dlong and m-1 line: This is where dlong will be driven.
+        dlong_center_x = (bn-b)/(m-mn)
+        dlong_center_z = m*dlong_center_x+b
+
+        dlong_center_x = max (dlong_center_x, dlong_base_x)
+        dlong_center_x = min (dlong_center_x, dlong_travel*np.cos(dlong_angle)+dlong_base_x)
+        dlong_center_z = dlong_base_z if dlong_center_z < dlong_base_x else dlong_center_z
+        dlong_center_z = min(dlong_center_z, dlong_travel*np.sin(dlong_angle) + dlong_base_z)
+
+        dlong_pos = (dlong_center_z-dlong_base_z) / np.sin(dlong_angle) # This may be unnecessary as it is already done before if base is the lowest possible position
+
+        if dlong_pos < dlong_low_limit:
+                print('WARNING: Hitting low software limit on dlong. Recalculating dlong positions.')
+                dlong_pos = dlong_low_limit + 1
+                dlong_center_x=dlong_pos*np.cos(dlong_angle)+dlong_base_x;
+                dlong_center_z=dlong_pos*np.sin(dlong_angle)+dlong_base_z;
+                
+        if dlong_pos > dlong_high_limit:
+                print('WARNING: Hitting high sofware limit on dlong. Recalculating dlong positions.')
+                dlong_pos = dlong_high_limit - 1;
+                dlong_center_x=dlong_pos*np.cos(dlong_angle)+dlong_base_x;
+                dlong_center_z=dlong_pos*np.sin(dlong_angle)+dlong_base_z;
+
+        print(f'dlong distance from base is {dlong_pos:.2f}')
+        print(f'distance x from base is {dlong_center_x:.2f}')
+        print(f'distance z from base is {dlong_center_z:.2f}')
+
+        stage_dx_pos, stage_dz_pos = self._dx_dz_calc(dvec, dlong_pos)
+        
+        print(f'stage dx at {stage_dx_pos:.2f}')
+        print(f'stage dz at {stage_dz_pos:.2f}\n')
+        
+        return dlong_pos, dvec 
+