mainly document changes

.gitignore

@@ -18,7 +18,7 @@ old/
 Module.symvers
 modules.order
 
-python/usr_code/usrcode.c
+#python/usr_code/usrcode.c
 python/usr_code/usrcode.h
 python/usr_code/.tags
 python/usr_code/usr_code.layout

MXfastStageDoc/MXfastStage.tex

@@ -234,10 +234,14 @@ Motor 1,2: $K_{fff}\approx100$\\
 \subsubsection{using advanced Deltatau Servo Loop}
 
 For now only following settings of the servo loop are used:Kp, Kvfb, Ki, Kvff, Kaff, MaxInt.\\
-The standard PID servo loop has some additional features, that were not yet used, especially the polynomial filters.
+The standard PID servo loop (figure\ref{fig:deltatau_std_ctrl}) has some additional features, that were not yet used, especially the polynomial filters.
 
+\begin{figure}[h!]
+\center
 \includegraphics[scale=.2]{/home/zamofing_t/Documents/doc-ext/DeltaTau/ServoBlockDiag.png}
-\\
+\caption{position dependant friction of motor 2}
+\label{fig:deltatau_std_ctrl}
+\end{figure}
 
 \cite[293]{PMACusr} shows details about the standard servo loop but unfortunately there are errors in documentation and the explanations are not really helpful. Hopefully the implementation is correct.\\
 
@@ -781,17 +785,43 @@ Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1]
 \FloatBarrier
 \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.\\
+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 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 the stage consists of really 2 integrators and behaves without friction roughly like $F=m \cdot \ddot{x}$. But an integrator $\frac{1}{s}$ is neighter observable nor 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=... .\\
+\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
-Simulating all this in MATLAB results in unstable system because of the derivate, but with a discrete differentiator it becomes stable again.\\
+Assume a plant consists of a integrator $\frac{1}{s}$ 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=... .\\
-The optimal loop would be Kaff=(bode value of integrator), Kvff=Kvfb=Kafb=0.
+\end{tcolorbox}
-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=...\\
+Simulating a single integrator plant in MATLAB works. But with 2 integrators the system is unstable because of the two derivate elements. With a discrete differentiator it becomes stable again. In fact a derivate element is a very critical element.\\
+
+\includegraphics[scale=.45]{FF_FB_ctrl.png} 
+The optimal parameters for a pure feed forward systems are calculated
+in equation \ref{eq:calc_Kaff}.
+
+\def\ccdot{\negthinspace\cdot\negthinspace}
+
+\begin{equation}
+\begin{aligned}
+&DesPosFB=DesVelFF=0,\quad posErrFB=velErrFB=accErrFB=0\\
+&\text{Motor 1: 19.8Hz 0dB, Ts=5kHz=.2ms,}\quad
+k=(19.8 \ccdot 2 \ccdot \pi)^2\\
+&Kaff = 1/(Ts \ccdot Ts \ccdot k) = 1/((19.8 \ccdot 2 \ccdot \pi)^2/5000^2) = 1615.2\\
+&\text{Motor 2:11.84Hz 0dB, Ts=5kHz=.2ms,}\quad
+k=(11.84 \ccdot 2 \ccdot \pi)^2\\
+&Kaff = 1/(Ts \ccdot Ts \ccdot k) = 1/((11.84 \ccdot 2 \ccdot \pi)^2/5000^2) = 4517.28\\
+\end{aligned}\label{eq:calc_Kaff}
+\end{equation}
+
+To correct errors, posErrFB is increased. But in a discrete system, the regulation becomes instable, because the actPos always lags the desPos for one sample. To avoid instability, a fiter can attenuate this problem.\\
+
+The Standard Delta Tau controller (figure \ref{fig:deltatau_std_ctrl}), shows avery similar feed forward and feedback loop structure with additional filters. 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, but setting the filter B seems to be more appropriate.\\
+
+
+
+%   Motor 2:11.84Hz 0dB
+%   Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804
 
-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

PBInspect1.pbi

@@ -69,30 +69,6 @@
         "Motor[2].idCmd",
         null
       ],
-      [
-        "Motor[3].idCmd",
-        null
-      ],
-      [
-        "Motor[4].idCmd",
-        null
-      ],
-      [
-        "Motor[5].idCmd",
-        null
-      ],
-      [
-        "Motor[6].idCmd",
-        null
-      ],
-      [
-        "Motor[7].idCmd",
-        null
-      ],
-      [
-        "Motor[8].idCmd",
-        null
-      ],
       [
         "Motor[1].ServoOut",
         null
@@ -102,31 +78,11 @@
         null
       ],
       [
-        "Motor[3].ServoOut",
+        "Motor[1].iqCmd",
         null
       ],
       [
-        "Motor[4].ServoOut",
+        "Motor[2].iqCmd",
-        null
-      ],
-      [
-        "Motor[5].ServoOut",
-        null
-      ],
-      [
-        "Motor[6].ServoOut",
-        null
-      ],
-      [
-        "Motor[7].ServoOut",
-        null
-      ],
-      [
-        "Motor[8].ServoOut",
-        null
-      ],
-      [
-        "Gate3[1].Chan[0].UserFlag",
         null
       ]
     ],

matlab/simFxFyStage.m

@@ -21,11 +21,12 @@ function [pb]=simFxFyStage(mot)
   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;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
+    %pb.MaxInt=200 %200mA should be enought to fix static errors
 
   end
   pb.ss_plt=mot.ss_plt;  pb.sel={3,[3]};

python/usr_code/Makefile

@@ -122,8 +122,8 @@ Debug: all
        end;\
        exit(0);"
 
-usrcode.c: ../../matlab/ssc1.mat ../../matlab/ssc2.mat ../MXTuning.py
+#usrcode.c: ../../matlab/ssc1.mat ../../matlab/ssc2.mat ../MXTuning.py
-	cd ..;./MXTuning.py -m512
+#	cd ..;./MXTuning.py -m512
 
 install: all
 	scp usralgo.ko root@$(PPMAC):/tmp

python/usr_code/usrcode.c

@@ -0,0 +1,255 @@
+//---------------------------------------------------------------------------
+//  Project PowerPMAC Firmware
+//  Delta Tau Data Systems, Inc.
+//  Copyright  2007. All Rights Reserved.
+//
+//  SUBSYSTEM:			User Servo Driver
+//  FILE:         		usrcode.c
+//  TEMPLATE AUTHOR:		Henry Bausley
+//
+//  OVERVIEW
+//  ~~~~~~~~
+//  This file is where exportable user code can be placed.
+//  To make a function callable as a user servo do three steps
+//
+//  1.)  Prototye the function user_func(void ,void );
+//  2.)  Export the function  EXPORT_SYMBOL(user_func);
+//  3.)  Make sure useralgo.ko has been loaded with projpp.ini
+//
+//--------------------------------------------------------------------------------
+#include "usrcode.h"
+//----------------------------------------------------------------------------------
+// pp_proj.h is the C header for accessing PMAC Global, CSGlobal, Ptr vars
+// _PPScriptMode_ for Pmac Script like access global & csglobal
+// global Mypvar - access with "Mypvar"
+// global Myparray(32) - access with "Myparray(i)"
+// csglobal Myqvar - access with "Myqvar(i)" where "i" is Coord #
+// csglobal Myqarray(16) - access with "Myqvar(i,j)" where "j" is index
+// _EnumMode_ for Pmac enum data type checking on Set & Get global functions
+// Example
+// global Mypvar
+// csglobal Myqvar
+// "SetGlobalVar(Myqvar, data)" will give a compile error because its a csglobal var.
+// "SetCSGlobalVar(Mypvar, data)" will give a compile error because its a global var.
+//------------------------------------------------------------------------------------
+#define _PPScriptMode_	// uncomment for Pmac Script type access
+// #define _EnumMode_			// uncomment for Pmac enum data type checking on Set & Get global functions
+
+#include "pp_proj.h"
+#ifdef __KERNEL__
+// Kernal mode can't have paths with spaces and long names
+//#include "../../PMACSC~1/GLOBAL~1/asharedwithcapp.pmh"
+#else
+//#include "../../PMAC Script Language/Global Includes/asharedwithcapp.pmh"
+#endif
+
+extern struct SHM        *pshm;  // Pointer to shared memory
+extern volatile unsigned *piom;  // Pointer to I/O memory
+extern void              *pushm; // Pointer to user memory
+
+double obsvr_servo_ctrl_1(MotorData *Mptr)
+{
+  //x[n+1]=A*x[n]+B*u
+  //y=C*x[n]+D*x[n]
+  //u=[DesPos, IqMeas, ActVel, ActPos].T
+  //y=[obsvOut].T
+
+  //double x[4];  //new state
+  //static double _x[4]={0,0,0,0}; //old state
+
+  //input values
+  double DesPos=Mptr->DesPos;
+  double IqMeas=Mptr->IqMeas;
+  double ActVel=Mptr->ActVel;
+  double ActPos=Mptr->ActPos;
+
+  //double obsvOut; // output values
+  double kpPosErr,iqCmd,iqCmdOrig; // output values
+  //double maxDac=Mptr->MaxDac; //other stuff
+  double desAcc,Kfff,posErr;
+  static double itg=0; //integrator
+  static double prevDesVel=0; //integrator
+  if (Mptr->ClosedLoop)
+  {
+    //Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
+    posErr=DesPos-ActPos; //should be equal to Mptr->PosError
+    kpPosErr=posErr*Mptr->Servo.Kp;
+    itg+=kpPosErr*Mptr->Servo.Ki;
+    if(itg>Mptr->Servo.MaxInt)
+      itg=Mptr->Servo.MaxInt;
+    else if(itg<-Mptr->Servo.MaxInt)
+      itg=-Mptr->Servo.MaxInt;
+
+    desAcc=Mptr->DesVel-prevDesVel;
+
+    if(Mptr->DesVel>0)
+      Kfff=Mptr->Servo.Kfff;
+    else
+      Kfff=-Mptr->Servo.Kfff;
+
+    iqCmd= +kpPosErr
+           -Mptr->Servo.Kvfb*Mptr->ActVel
+           +Mptr->Servo.Kvff*Mptr->DesVel
+           +itg
+           +Mptr->Servo.Kaff*desAcc
+           +Kfff;
+
+    prevDesVel=Mptr->DesVel;
+    iqCmdOrig=pshm->ServoCtrl(Mptr);
+
+    //return iqCmd;
+    pshm->P[2010]=DesPos;
+    pshm->P[2011]=IqMeas;
+    pshm->P[2012]=ActVel;
+    pshm->P[2013]=ActPos;
+    //pshm->P[2014]=_x[0];
+    //pshm->P[2015]=_x[1];
+    pshm->P[2016]+=iqCmdOrig-iqCmd;
+    pshm->P[2017]=iqCmdOrig;
+    pshm->P[2018]=iqCmd;
+
+    return iqCmd;
+  }
+  else
+  {
+    Mptr->Servo.Integrator=0.0;
+    return 0.0;
+  }
+}
+
+double obsvr_servo_ctrl_2(MotorData *Mptr)
+{
+  //x[n+1]=A*x[n]+B*u
+  //y=C*x[n]+D*x[n]
+  //u=[DesPos, IqMeas, ActVel, ActPos].T
+  //y=[obsvOut].T
+
+  //double x[4];  //new state
+  //static double _x[4]={0,0,0,0}; //old state
+
+  //input values
+  double DesPos=Mptr->DesPos;
+  double IqMeas=Mptr->IqMeas;
+  double ActVel=Mptr->ActVel;
+  double ActPos=Mptr->ActPos;
+
+  //double obsvOut; // output values
+  double kpPosErr,iqCmd,iqCmdOrig; // output values
+  //double maxDac=Mptr->MaxDac; //other stuff
+  double desAcc,Kfff,posErr;
+  static double itg=0; //integrator
+  static double prevDesVel=0; //integrator
+  if (Mptr->ClosedLoop)
+  {
+    //Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
+    posErr=DesPos-ActPos; //should be equal to Mptr->PosError
+    kpPosErr=posErr*Mptr->Servo.Kp;
+    itg+=kpPosErr*Mptr->Servo.Ki;
+    if(itg>Mptr->Servo.MaxInt)
+      itg=Mptr->Servo.MaxInt;
+    else if(itg<-Mptr->Servo.MaxInt)
+      itg=-Mptr->Servo.MaxInt;
+
+    desAcc=Mptr->DesVel-prevDesVel;
+
+    if(Mptr->DesVel>0)
+      Kfff=Mptr->Servo.Kfff;
+    else
+      Kfff=-Mptr->Servo.Kfff;
+
+    iqCmd= +kpPosErr
+           -Mptr->Servo.Kvfb*Mptr->ActVel
+           +Mptr->Servo.Kvff*Mptr->DesVel
+           +itg
+           +Mptr->Servo.Kaff*desAcc
+           +Kfff;
+
+    prevDesVel=Mptr->DesVel;
+    iqCmdOrig=pshm->ServoCtrl(Mptr);
+
+    //return iqCmd;
+    pshm->P[2020]=DesPos;
+    pshm->P[2021]=IqMeas;
+    pshm->P[2022]=ActVel;
+    pshm->P[2023]=ActPos;
+    //pshm->P[2024]=_x[0];
+    //pshm->P[2025]=_x[1];
+    pshm->P[2026]+=iqCmdOrig-iqCmd;
+    pshm->P[2027]=iqCmdOrig;
+    pshm->P[2028]=iqCmd;
+
+    return iqCmd;
+  }
+  else
+  {
+    Mptr->Servo.Integrator=0.0;
+    return 0.0;
+  }
+}
+
+void user_phase_sample( struct MotorData *Mptr)
+{
+}
+
+double usr_servo_sample(MotorData *Mptr)
+{
+  pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001;  //lowpass of Position error
+  return pshm->ServoCtrl(Mptr);
+}
+
+//double user_pid_ctrl( struct MotorData *Mptr)
+//{
+//  //double *p;
+//  //p = pushm;
+//  //return 0;
+//  double ctrl_out;
+//  if (Mptr->ClosedLoop) {
+//    // Compute PD terms
+//    ctrl_out=Mptr->Servo.Kp*Mptr->PosError-Mptr->Servo.Kvfb*Mptr->ActVel;
+//    Mptr->Servo.Integrator+=Mptr->PosError*Mptr->Servo.Ki; // I term
+//    ctrl_out+=Mptr->Servo.Integrator;
+//    // Combine
+//    if (ctrl_out>2000)ctrl_out=2000;
+//    if (ctrl_out<-2000)ctrl_out=-2000;
+//    return ctrl_out;
+//  }
+//  else {
+//    Mptr->Servo.Integrator=0.0;
+//    return 0.0;
+//  }
+//}
+
+void CaptCompISR(void)
+{
+  unsigned *pUnsigned = pushm;
+  *pUnsigned = *pUnsigned + 1;
+}
+
+double GetLocal(struct LocalData *Ldata,int m)
+{
+  return *(Ldata->L + Ldata->Lindex + m);
+}
+
+void SetLocal(struct LocalData *Ldata,int m,double value)
+{
+  *(Ldata->L + Ldata->Lindex + m) = value;
+}
+
+double *GetLocalPtr(struct LocalData *Ldata,int m)
+{
+  return (Ldata->L + Ldata->Lindex + m);
+}
+
+double CfromScript(double cfrom_type, double arg2, double arg3, double arg4, double arg5, double arg6, double arg7, struct LocalData *Ldata)
+{
+  int icfrom_type = (int) cfrom_type;
+  double *C, *D, *L, *R, rtn; // C, D, R - only needed if doing Kinmatics
+  
+  C = GetCVarPtr(Ldata);  // Only needed if doing Kinmatics
+  D = GetDVarPtr(Ldata);  // Only needed if doing Kinmatics
+  L = GetLVarPtr(Ldata);  // Only needed if using Ldata or Kinmatics
+  R = GetRVarPtr(Ldata);  // Only needed if doing Kinmatics
+  rtn = -1.0;
+  return rtn;
+}
+

qt/ESB_MX_exp.ui

@@ -451,6 +451,48 @@ P=$(P),M=MOT_GIR_W</string>
     <string>false;false;false;false;false;false;false;false;false;false;false;false;false;false;false;false</string>
    </property>
   </widget>
+  <widget class="caRelatedDisplay" name="caRelatedDisplay_12">
+   <property name="geometry">
+    <rect>
+     <x>510</x>
+     <y>580</y>
+     <width>151</width>
+     <height>30</height>
+    </rect>
+   </property>
+   <property name="label">
+    <string notr="true">-ESB_MX EVR VME</string>
+   </property>
+   <property name="foreground">
+    <color>
+     <red>0</red>
+     <green>0</green>
+     <blue>0</blue>
+    </color>
+   </property>
+   <property name="background">
+    <color>
+     <red>89</red>
+     <green>126</green>
+     <blue>225</blue>
+    </color>
+   </property>
+   <property name="labels">
+    <string>EVR</string>
+   </property>
+   <property name="files">
+    <string>G_EVR_main.ui</string>
+   </property>
+   <property name="args">
+    <string>SYS=SAR-CVME-ESBMX1,DEVICE=EVR0,FF=VME-300</string>
+   </property>
+   <property name="stackingMode" stdset="0">
+    <enum>caRowColMenu::Menu</enum>
+   </property>
+   <property name="removeParent" stdset="0">
+    <string>false;false;false;false;false;false;false;false;false;false;false;false;false;false;false;false</string>
+   </property>
+  </widget>
  </widget>
  <customwidgets>
   <customwidget>