usr_servo code generation

python/MXTuning.py

@@ -15,6 +15,8 @@ Modes:
 4 plot the full bode recording
 5 plot the full bode recording with an approximation model
 6 plot all raw acquired data files
+7 custom chirp test
+8 generate observer code (after files generated with matlab)
 
      -> check https://github.com/klauer/ppmac for fast data gathering server which supports
         phase gathering -> not yet compiling: /home/zamofing_t/Documents/prj/SwissFEL/PowerBrickInspector/ppmac/fast_gather
@@ -201,6 +203,90 @@ class MXTuning(Tuning):
       ax.semilogx(f, phase,'-k',lw=1)  # Bode phase plot
       # tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
 
+    def usr_servo_gen_code(self,fn='/tmp/ssc1.mat'):
+      import scipy.io,re
+      #the file ssc[1|2].mat has been generated with matlab:
+      #[mot1, mot2]=identifyFxFyStage();
+      #[pb]=simFxFyStage(mot1);
+      #[ssc]=StateSpaceControlDesign(mot1);
+      mat=scipy.io.loadmat(fn)
+      motid=int(re.search('(\d)\.mat',fn).group(1))
+      A=mat['Aoz']
+      B=mat['Boz']
+      C=mat['Coz']
+      D=mat['Doz']
+      V=mat['V']
+      u=('DesPos','IqMeas','IqVolts','ActPos')
+      y=('obsvOut',)
+      progSample='''double usr_servo_ctrl_{motid}(MotorData *Mptr)
+{{
+  pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001;  //lowpass of Position error
+  return pshm->ServoCtrl(Mptr);
+}}'''.format(motid=motid)
+
+      prog='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr)
+{{
+  //x[n+1]=A*x[n]+B*u
+  //y=C*x[n]+D*x[n]
+  //u=[{u}].T
+  double x[{A.shape[0]}];  //new state
+  static double _x[{A.shape[0]}]={{0,0,0,0}}; //old state
+  //double {u}; // input values
+  double {y},iqCmd; // output values
+  double maxDac=Mptr->MaxDac;
+'''.format(motid=motid,A=A,xInit=','.join('0'*A.shape[0]),u=', '.join(u),y=', '.join(y))
+      s='  //input values\n'
+      for i in range(len(u)):
+        s+='  double {u}=Mptr->{u};\n'.format(u=u[i])
+      prog+=s+'''\n
+  if (Mptr->ClosedLoop)
+  {
+    return iqCmd;
+  }
+  else
+  {
+    Mptr->Servo.Integrator=0.0;
+    return 0.0;
+  }
+'''
+
+      s='  //x[n+1]=A*x[n]+B*u;\n'
+      for i in range(A.shape[0]):
+        s+='  x[%d]='%i
+        for j in range(A.shape[1]):
+          s+='%+28.22g*_x[%d]'%(A[i,j],j)
+        for j in range(B.shape[0]):
+          s+='%+28.22g*%s'%(B[i,j],u[j])
+        s+=';\n'
+      prog+=s+'\n'
+
+      s='  //y=C*x[n]+D*x[n];\n'
+      for i in range(C.shape[0]):
+        s+='  %s='%y[i]
+        for j in range(C.shape[1]):
+          s+='%+28.22g*_x[%d]'%(C[i,j],j)
+        s+=';\n'
+      prog+=s+'\n'
+
+
+      prog+='''  iqCmd=DesPos*{V}-{y};
+  if (iqCmd>maxDac)
+  {{
+    iqCmd=maxDac;
+  }}
+  else
+  {{
+    if (iqCmd<-maxDac)
+    {{
+      iqCmd=-maxDac;
+    }}
+  }}
+  return iqCmd;
+}}'''.format(V=V[0,0],y=y[0])
+
+      hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr);
+EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
+      return (hdr,prog)
 
 def bode(mdl):
   w,mag,phase = signal.bode(mdl,1000)
@@ -250,6 +336,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
 
     #plt.ion()
     args.host='MOTTEST-CPPM-CRM0573'
+    args.host=None
     if args.host is None:
       comm=gt=None
     else:
@@ -342,10 +429,42 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
       tune.custom_chirp(motor=1,minFrq=100,maxFrq=3000,amp=10,tSec=5,mode=0,file='/tmp/cst_chirp0.npz')
       #tune.custom_chirp(motor=2,minFrq=1,maxFrq=1000,tSec=5,mode=1,file='/tmp/cst_chirp1.npz')
       #tune.custom_chirp(motor=1,minFrq=1,maxFrq=3000,tSec=5,mode=2,file='/tmp/cst_chirp2.npz')
-
+    elif mode==8: #generater code
+      #before this can be done, the observer controller has to be designed with matlab:
+      #s.a.ESB_MX/matlab/Readme.md
+      #clear;
+      #clear global;
+      #close all;
+      #[mot1,mot2]=identifyFxFyStage();
+      #[pb]=simFxFyStage(mot1);
+      #[ssc]=StateSpaceControlDesign(mot1);
+      #[pb]=simFxFyStage(mot2);
+      #[ssc]=StateSpaceControlDesign(mot2);
+      #after this go to: python/usr_code and call make to build the controller
+      #to activate the controller checkout: PBTools/pbtools/usr_servo_phase
+      base=os.path.dirname(__file__)
+      (hdr1,prog1)=tune.usr_servo_gen_code('/tmp/ssc1.mat')
+      (hdr2,prog2)=tune.usr_servo_gen_code('/tmp/ssc2.mat')
+      fn_ct=os.path.join(base,'usr_code/usrcode_template.c')
+      fn_ht=os.path.join(base,'usr_code/usrcode_template.h')
+      fnc=os.path.join(base,'usr_code/usrcode.c')
+      fnh=os.path.join(base,'usr_code/usrcode.h')
+      s=open(fn_ht).read()
+      s=s.replace('<usr_header>',hdr1+'\n\n'+hdr2)
+      fh=open(fnh,'w')
+      fh.write(s)
+      fh.close()
+      print(fnh+' generated.')
+      s=open(fn_ct).read()
+      s=s.replace('<usr_code>',prog1+'\n\n'+prog2)
+      fh=open(fnc,'w')
+      fh.write(s)
+      fh.close()
+      print(fnc+' generated.')
+      print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample')
     plt.show()
   #------------------ Main Code ----------------------------------
-  #ssh_test()
+  #ssh_test()'/tmp/usrcode.c'
   ret=parse_args()
   exit(ret)