add missing files, fix coordinate transformation

swissmx.py

@@ -20,7 +20,50 @@ bitmask for simulation:
 
 """
 import logging
-logging.basicConfig(level=logging.DEBUG, format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s ')
+class col:
+  d='\033[0m'  # default
+  r='\033[31m'  # red
+  g='\033[32m'  # green
+  y='\033[33m'  # yellow
+  rr='\033[91m'  # red(bright)
+  gg='\033[92m'  # green(bright)
+  yy='\033[93m'  # yellow(bright)
+  b='\033[1m'  # bold
+  u='\033[4m'  # underline
+
+
+class logHandler(logging.StreamHandler):
+  def __init__(self):
+    logging.StreamHandler.__init__(self)
+
+  def emit(self, record):
+    '''override function of base class'''
+    try:
+      msg=self.format(record)
+      # print(record.__dict__)
+      if record.levelno<=10:
+        c=col.g
+      elif record.levelno<=20:
+        c=col.y
+      elif record.levelno<=30:
+        c=col.yy
+      elif record.levelno<=40:
+        c=col.r
+      else:
+        c=col.rr+col.b
+      msg=c+msg+col.d
+      stream=self.stream
+      stream.write(msg+self.terminator)
+      self.flush()
+    except RecursionError:
+      raise
+    except Exception:
+      self.handleError(record)
+
+
+# logging.basicConfig(level=logging.DEBUG, format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s ')
+logging.basicConfig(level=logging.INFO, format='%(levelname)s:%(module)s:%(lineno)d:%(funcName)s:%(message)s',
+                    handlers=[logHandler()])
 
 logging.getLogger('PyQt5.uic').setLevel(logging.INFO)
 logging.getLogger('requests').setLevel(logging.INFO)
@@ -33,6 +76,18 @@ logging.getLogger('zoom').setLevel(logging.INFO)
 logging.getLogger('pbtools.misc.pp_comm').setLevel(logging.INFO)
 _log = logging.getLogger("swissmx")
 #_log.setLevel(logging.INFO)
+class col:
+  d = '\033[0m'  #default
+  r = '\033[31m' #red
+  g = '\033[32m' #green
+  y = '\033[33m' #yellow
+  rr= '\033[91m' #red(bright)
+  gg= '\033[92m' #green(bright)
+  yy= '\033[93m' #yellow(bright)
+  b = '\033[1m' #bold
+  u = '\033[4m' #underline
+
+
 
 import time
 class timestamp():
@@ -2041,7 +2096,7 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
       fidScl=param.pop('fidScl',.02)
       fiducial=param.pop('fiducial',((.1, .1), (.1, 2.7), (10.3, .1), (10.3, 2.7)))
       fiducial=np.array(fiducial)
-      gp=psi_device.shapepath.GenPath(); gp.swissmx_points(ofs=ofs, width=width, flipy=True)
+      gp=psi_device.shapepath.GenPath(); gp.swissmx(ofs=ofs, width=width, flipy=True)
       sz=gp.points.max(0)+np.array(ofs)*2
       go=UsrGO.Path((fx-sz[0]/2, fy-sz[1]/2), sz, gp.points, fiducial, fidScl,**param)
     elif idx==6:
@@ -2050,7 +2105,7 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
       fidScl=param.pop('fidScl',.02)
       fiducial=param.pop('fiducial',((.1, .1), (.1, 2.2), (10.3, .1), (10.3, 2.2)))
       fiducial=np.array(fiducial)
-      gp=psi_device.shapepath.GenPath(); gp.swissfel_points(ofs=ofs, width=width,flipy=True,**param)
+      gp=psi_device.shapepath.GenPath(); gp.swissfel(ofs=ofs, width=width,flipy=True,**param)
       sz=gp.points.max(0)+np.array(ofs)*2
       go=UsrGO.Path((fx-sz[0]/2, fy-sz[1]/2), sz, gp.points, fiducial, fidScl)
     else:
@@ -2095,7 +2150,8 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
           sz=np.array(go._dscr['size'])
           fid=fid/sz
           trf=geometry.geometry.least_square_trf(ptFitTrf,fid)
-          print(trf)
+          #_log.debug(f'{trf}')
+          _log.debug('\n  trf='+str(trf).replace('\n','\n      '))
           tr=go.transform()
           tr.setMatrix(1,                 trf[1,0]/trf[0,0], 0,
                        trf[0,1]/trf[1,1], 1,                 0,
@@ -2112,7 +2168,7 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
           sz=go.szOrig
           fid=fid/sz
           trf=geometry.geometry.least_square_trf(ptFitTrf, fid)
-          print(trf)
+          _log.debug('\n  trf='+str(trf).replace('\n','\n      '))
           tr=go.transform()
           tr.setMatrix(1, trf[1, 0]/trf[0, 0], 0,
                        trf[0, 1]/trf[1, 1], 1, 0,
@@ -2193,12 +2249,24 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
         sp.setup_motion(fnPrg=fn+'.prg', mode=4, scale=1., dwell=10, grid=kwargs['grid'],trf=kwargs['trf'])
       try:
         p=geo._fitPlane
+        # TODO: Cleanup
         if code_gen==0:
           # X has inverted sign !
           # Z is in um -> therefore the offset must be multiplied with 1000 ! Z motor has opposite sign !
-          cz=f'{+p[0]:+.18g}X{-p[1]:+.18g}Y{-p[2]*1000:+.18g}'
+          #cz=f'{+p[0]:+.18g}X{-p[1]:+.18g}Y{-p[2]*1000:+.18g}'
+          t=p*np.array((1,-1,-1000))
+          cz=f'{t[0]:+.18g}X{t[1]:+.18g}Y{t[2]:+.18g}'
         else:
-          cz=f'{+p[0]:+.18g}X{-p[1]:+.18g}Y{-p[2]*1000:+.18g}'
+          trf=kwargs['trf'] # grid-coord -> motor-um
+          #(0,0,1)*trf
+          # trf*'gridpos in um' -> motor pos in mm
+          trf2=np.asmatrix(np.identity(3))
+          trf2[:, :2]=trf
+          #p1=(0,0,1)*trf2 # =matrix([[-2376.8,  1376.8,     1. ]]) um
+          trf3=np.matrix( ((p[0],0,0),(-p[1],0,0),(-1000*p[2],0,1)) )
+          #(0, 0, 1)*trf2*trf3
+          t=(trf2*trf3)[:,0].A.ravel()
+          cz=f'{t[0]:+.18g}X{t[1]:+.18g}Y{t[2]:+.18g}'
       except AttributeError:
         cz='0'
         _log.warning('no plane fitting done. z does not move')
@@ -2211,8 +2279,8 @@ Author Thierry Zamofing (thierry.zamofing@psi.ch)
         fy=f'{trf[0,1]:+.18g}X{trf[1,1]:+.18g}Y{trf[2,1]:+.18g}'
 
       _log.debug(f'pts_trf[0,:]={kwargs["pts_trf"][0, :]}')
-      _log.debug((f'points[0,:]={kwargs["points"][0, :]}')
-      _log.debug((f'trf=\n{trf}')
+      _log.debug(f'points[0,:]={kwargs["points"][0, :]}')
+      _log.debug(f'trf=\n{trf}')
 
       sp.setup_coord_trf(fx, fy, cz)  # reset to shape path system