toward optimized cotroller parameters

MXfastStageDoc/MXfastStage.tex

@@ -782,9 +782,24 @@ Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1]
 \subsection{The reality of the state space controller}
 
 The state space controller assumes that the system is observable and controlable. The bode plot shows a flat amplitude at low frequencies, which makes the feeling, that the system is observable and controlable. But in fact the reason of that flat amplitude is friction (section \ref{sec:friction}). The viscode damping is also negligable.\\
-This results to the fact that $F=m \cdot \ddot{x}$ consists of really 2 integrators. But an integrator $\frac{1}{s}$ is neighter observable and controlable. Therefore we have to check, how to implement an optimal controller for such a system.
+This results to the fact that $F=m \cdot \ddot{x}$ consists of really 2 integrators. But an integrator $\frac{1}{s}$ is neighter observable and controlable. Therefore we have to check, how to implement an optimal controller for such a system.\\
+A controller consists in a feed forward and a feed back transfer function.
+The overall transfer functions y/u and y/e are: ... Setting H(s)=s results in a overall transfer function of y/u=... and y/e=... .\\
+Simulating all this in MATLAB results in unstable system because of the derivate, but with a discrete differentiator it becomes stable again.\\
+The optimal loop would be Kaff=(bode value of integrator), Kvff=Kvfb=Kafb=0.
+Kp values is the speed to attenuate error values.\\
+This is stable as long as Kp=0. With a Kp value, the system starts to osscilate instable. Stability can be achived again with e.g. Kvff=Kvfb=...\\
 
+Looking at the Standard Delta Tau controller, one sees, that it has exactly the feed forward and feedback loop. So after all the measurements, we can calculate the optimal Kfff and Kaff values.
+Kp and Ki values will attenuate the errors. Kvff=Kvfb makes the system stable at hither Kp values.
 
+%   Motor 1: 19.8Hz 0dB
+%   K=(19.8*2*np.pi)**2
+%   Ts=5kHz=.2ms
+%   Kaff = 1/(Ts*Ts*K) = 1/((19.8*2*np.pi)**2/5000/5000) = 1615.2877200403302
+
+%   Motor 2:11.84Hz 0dB
+%   Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804
 
 \vspace{1pc}
 

matlab/identifyFxFyStage.m

@@ -270,6 +270,29 @@ function motCell=identifyFxFyStage(mode)
     mot.ss_q.Name='simplified mechanics, no current loop, no resonance';
     chkCtrlObsv(mot.ss_q,'ss_q fyStage');
 
+    % TESTS
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> actVel
+    %             |          3|-------> actPos
+    %             +-----------+
+    s=tfq.InputName{1};tfq.InputName{1}='iqMeas';
+    mot.ss_cq=connect(tfc,tfq,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_cq.Name='simplified mechanics, current loop, no resonance';
+    tfq.InputName{1}=s;%restore
+    
+    % TESTS
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> actVel
+    %             |          2|-------> actPos
+    %             +-----------+
+    s=tf1.InputName{1};tf1.InputName{1}='iqCmd';
+    mot.ss_qr=connect(tfq,tf1,'iqCmd',{'actVel','actPos'});
+    mot.ss_qr.Name='simplified mechanics, no current loop, resonance';
+    tfp.InputName{1}=s;%restore
+
+    
+    
     %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
     %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
     plotBode(mot)

matlab/simFxFyStage.m

@@ -13,12 +13,20 @@ function [pb]=simFxFyStage(mot)
   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')
-    pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
+    %pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
+    pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
+    %19.8Hz 0dB K=(19.8*2*np.pi)**2=15477.1 Ts=5kHz=.2ms
+    %Kaff = 1/(Ts*Ts*K) = 1/((19.8*2*np.pi)**2/5000**2) = 1615.2877200403302
+    %Kfff=100
   else
     %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
     %!motor(mot=2,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=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
     pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
+    %11.84Hz 0dB K=(11.84*2*np.pi)**2=5534.3 Ts=5kHz=.2ms
+    %Kaff = 1/(Ts*Ts*K) = 1/((11.84*2*np.pi)**2/5000**2) = 4517.278506241803
+    %Kfff=100
+
   end
   pb.ss_plt=mot.ss_plt;  pb.sel={3,[3]};
   %pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]};
@@ -26,6 +34,7 @@ function [pb]=simFxFyStage(mot)
   %pb.ss_plt=mot.ss_d1; pb.sel={3,[3]};
   %pb.ss_plt=mot.ss_1; pb.sel={2,[2]};
   %pb.ss_plt=mot.ss_p; pb.sel={2,[2]};
+  %pb.ss_plt=mot.ss_q; pb.sel={2,[2]};
   
   %mdlName='stage_closed_loop';
   %open(mdlName)

python/MXTuning.py

@@ -206,8 +206,8 @@ class MXTuning(Tuning):
         denq=np.poly1d([1,0,0])
 
         #force->velocity (simplified)
-        #reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
-        numv=np.poly1d([19.8*2*np.pi])
+        #reiner integrator: 11.84Hz=0dB -> k=frq*2*pi
+        numv=np.poly1d([11.84*2*np.pi])
         denv=np.poly1d([1,0])