usr_servo code generation

matlab/Readme.md

@@ -26,12 +26,10 @@ clear;
 clear global;
 close all;
 [mot1,mot2]=identifyFxFyStage();
-[pb]=simFxFyStage(mot1,1);
-[ssc]=StateSpaceControlDesign(mot1,1);
-
-[mot1,mot2]=identifyFxFyStage();
-[pb]=simFxFyStage(mot2,2);
-[ssc]=StateSpaceControlDesign(mot2,2);
+[pb]=simFxFyStage(mot1);
+[ssc]=StateSpaceControlDesign(mot1);
+[pb]=simFxFyStage(mot2);
+[ssc]=StateSpaceControlDesign(mot2);
 
 
 ```

matlab/StateSpaceControlDesign.m

@@ -1,4 +1,4 @@
-function [ssc]=StateSpaceControlDesign(mot,motid)
+function [ssc]=StateSpaceControlDesign(mot)
   % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
   %
   % builds a state space controller designed for the plant.
@@ -17,18 +17,18 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   % https://www.youtube.com/watch?v=Lax3etc837U
 
   %ss_ol=mot.ssPlt;
-  ss_ol_plt=mot.ssPlt; %real plant (model of real plant)
-  ss_ol_plt.Name='open loop plant';
-  ss_ol_mdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
-  ss_ol_mdl.Name='open loop model';
+  ssPlt=mot.ssPlt; %real plant (model of real plant)
+  ssPlt.Name='open loop plant';
+  ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
+  ssMdl.Name='open loop model';
 
-  [Ap,Bp,Cp,Dp]=ssdata(ss_ol_plt);
-  [Am,Bm,Cm,Dm]=ssdata(ss_ol_mdl);
+  [Ap,Bp,Cp,Dp]=ssdata(ssPlt);
+  [Am,Bm,Cm,Dm]=ssdata(ssMdl);
 
   
   %sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
 
-  figure();h=bodeplot(ss_ol_plt,ss_ol_mdl);
+  figure();h=bodeplot(ssPlt,ssMdl);
   setoptions(h,'IOGrouping','all')
 
   % step answer on open loop:
@@ -37,8 +37,8 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   xp0 = zeros(1,length(Ap));
   xm0 = zeros(1,length(Am));
 
-  [yp,t,x] = lsim(ss_ol_plt,u,t,xp0);
-  [ym,t,x] = lsim(ss_ol_mdl,u,t,xm0);
+  [yp,t,x] = lsim(ssPlt,u,t,xp0);
+  [ym,t,x] = lsim(ssMdl,u,t,xm0);
   figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
   legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
 
@@ -51,44 +51,52 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   % *** space state controller ***
   %
   %place poles for the controller feedback
-  if motid==1
-    %2500rad/s = 397Hz -> locate poles here
-    %6300rad/s = 1027Hz -> locate poles here
-    if length(poles)==4
-      p1=-6300+2800i;
-      p2=-6200+1500i;   
-      P=[p1 p1' p2 p2'];
-    else
-      p1=-3300+2800i;
-      p2=-2700+500i;
-      p3=-2500+10i;
-      %p1=-3300+2800i;
-      %p2=-1500+500i;
-      %p3=-1200+10i;
-      P=[p1 p1' p2 p2' p3 p3'];
-    end
+  use_lqr=0;
+  if use_lqr %use the lqr controller
+    Q=eye(length(ssMdl.A));
+    R=1;
+    [K,P,E]=lqr(ssMdl,Q,R,0);
   else
-    %2500rad/s = 397Hz -> locate poles here
-    %6300rad/s = 1027Hz -> locate poles here
-    if length(poles)==4
-      p1=-6300+2800i;
-      p2=-6200+1500i;   
-      P=[p1 p1' p2 p2'];
+    if mot.id==1
+      %2500rad/s = 397Hz -> locate poles here
+      %6300rad/s = 1027Hz -> locate poles here
+      if length(poles)==4
+        p1=-6300+280i;
+        p2=-6200+150i;   
+        P=[p1 p1' p2 p2'];
+      else
+        p1=-3300+2800i;
+        p2=-2700+500i;
+        p3=-2500+10i;
+        %p1=-3300+2800i;
+        %p2=-1500+500i;
+        %p3=-1200+10i;
+        P=[p1 p1' p2 p2' p3 p3'];
+      end
     else
-      p1=-3300+2800i;
-      p2=-1900+130i;
-      p3=-2900+80i;
-      p4=-2300+450i;
-      p5=-2000+20i;
-      p6=-1500+10i;
-      %p1=-3300+2800i;
-      %p2=-1500+500i;
-      %p3=-1200+10i;
-      P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
+      %2500rad/s = 397Hz -> locate poles here
+      %6300rad/s = 1027Hz -> locate poles here
+      if length(poles)==4
+        p1=-6300+2800i;
+        p2=-6200+1500i;   
+        P=[p1 p1' p2 p2'];
+      else
+        p1=-3300+2800i;
+        p2=-1900+130i;
+        p3=-2900+80i;
+        p4=-2300+450i;
+        p5=-2000+20i;
+        p6=-1500+10i;
+        %p1=-3300+2800i;
+        %p2=-1500+500i;
+        %p3=-1200+10i;
+        P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
+      end
     end
-  end
-  K = place(Am,Bm,P);
-  %K = acker(Am,Bm,Pm);
+    K = place(Am,Bm,P);
+    %K = acker(Am,Bm,Pm);
+  end %if lqr
+
   V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
   %Nbar(2)=1; %the voltage stuff is crap for now
   if length(V)>1
@@ -110,7 +118,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   % *** observer controller ***
   %
   %observer poles-> 5 times farther left than system poles
-  if motid==1
+  if mot.id==1
     op1=(p1*5);
     op2=(p2*5);
     if length(poles)>4
@@ -143,27 +151,27 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   Ct = [ Cm    zeros(size(Cm)) ];
 
   % step answer on closed loop with observer controller:
-  ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
-  figure();lsim(ss_o,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
+  ss_t = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
+  figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
 
   
   % *** disctrete observer controller ***
   %
   Ts=1/5000; % 5kHz
-  ss_od = c2d(ss_o,Ts);
-  ss_od .Name='discrete obsvr ctrl';
+  ss_tz = c2d(ss_t,Ts);
+  ss_tz .Name='discrete obsvr ctrl';
   % step answer on closed loop with disctrete observer controller:
   t = 0:Ts:.05;
-  figure();lsim(ss_od ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
+  figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
 
 
   %plot all bode diagrams of desPos->actPos
   figure();
-  h=bodeplot(ss_cl(3),ss_o(3),ss_od(3));
+  h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3));
   setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
 
   figure();
-  h=pzplot(ss_cl(3),ss_o(3),ss_od(3));
+  h=pzplot(ss_cl(3),ss_t(3),ss_tz(3));
   setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
 
   
@@ -172,15 +180,25 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
   Bo=[Bm L];
   Co=K;
   Do=zeros(size(Co,1),size(Bo,2));
+
+  ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
+   
+  ss_oz = c2d(ss_o,Ts);
+  [Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
   mdlName='observer';
   open(mdlName);
+  
 
+ssPltz = c2d(ssPlt,Ts);
+[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
+  
+ 
   %state space controller
   ssc=struct();
-  for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od","numV","denV"]
+  for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV"]
     ssc=setfield(ssc,k,eval(k));
   end
-
+  save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
 end
 
 

matlab/identifyFxFyStage.m

@@ -101,7 +101,9 @@ function [mot1,mot2]=identifyFxFyStage()
   end
 
   function mot=fyStage()
-    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1);
+    motid=1;
+    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid);
+    mot.id=motid;
     mot.tfc=currstep(mot);
 
     opt=tfestOptions;
@@ -171,7 +173,9 @@ function [mot1,mot2]=identifyFxFyStage()
   end
 
   function mot=fxStage()
-    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2);
+    motid=2;
+    mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid);
+    mot.id=motid;
     currstep(mot);
 
     opt=tfestOptions;

matlab/simFxFyStage.m

@@ -1,4 +1,4 @@
-function [pb]=simFxFyStage(mot,motid)
+function [pb]=simFxFyStage(mot)
   % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
   %
   %loads the current (11.10.2018) controller settings of the
@@ -9,7 +9,7 @@ function [pb]=simFxFyStage(mot,motid)
   Ts=2E-4; % 0.2ms=5kHz
   MaxDac=2011.968;
   MaxPosErr=10000;
-  if motid==1
+  if mot.id==1
     %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
     %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
     Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;