diff --git a/.gitignore b/.gitignore index b7319cf..fee4340 100644 --- a/.gitignore +++ b/.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 diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex index 613092b..cbf8653 100644 --- a/MXfastStageDoc/MXfastStage.tex +++ b/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.\\ -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.\\ +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 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=... .\\ -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=...\\ +\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO] +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=... .\\ +\end{tcolorbox} + +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 diff --git a/PBInspect1.pbi b/PBInspect1.pbi index 34aa3ce..322c7cd 100644 --- a/PBInspect1.pbi +++ b/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", - null - ], - [ - "Motor[5].ServoOut", - null - ], - [ - "Motor[6].ServoOut", - null - ], - [ - "Motor[7].ServoOut", - null - ], - [ - "Motor[8].ServoOut", - null - ], - [ - "Gate3[1].Chan[0].UserFlag", + "Motor[2].iqCmd", null ] ], diff --git a/matlab/docSim.slx b/matlab/docSim.slx new file mode 100644 index 0000000..ae34cb9 Binary files /dev/null and b/matlab/docSim.slx differ diff --git a/matlab/sample.slx b/matlab/sample.slx index 68f5ce1..bca6194 100644 Binary files a/matlab/sample.slx and b/matlab/sample.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index d9af8a6..b8134f6 100644 --- a/matlab/simFxFyStage.m +++ b/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]}; diff --git a/python/usr_code/Makefile b/python/usr_code/Makefile index 398d002..4810b39 100644 --- a/python/usr_code/Makefile +++ b/python/usr_code/Makefile @@ -122,8 +122,8 @@ Debug: all end;\ exit(0);" -usrcode.c: ../../matlab/ssc1.mat ../../matlab/ssc2.mat ../MXTuning.py - cd ..;./MXTuning.py -m512 +#usrcode.c: ../../matlab/ssc1.mat ../../matlab/ssc2.mat ../MXTuning.py +# cd ..;./MXTuning.py -m512 install: all scp usralgo.ko root@$(PPMAC):/tmp diff --git a/python/usr_code/usrcode.c b/python/usr_code/usrcode.c new file mode 100644 index 0000000..68a0d78 --- /dev/null +++ b/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; +} + diff --git a/qt/ESB_MX_exp.ui b/qt/ESB_MX_exp.ui index dd84f86..e33c9e4 100644 --- a/qt/ESB_MX_exp.ui +++ b/qt/ESB_MX_exp.ui @@ -451,6 +451,48 @@ P=$(P),M=MOT_GIR_W false;false;false;false;false;false;false;false;false;false;false;false;false;false;false;false + + + + 510 + 580 + 151 + 30 + + + + -ESB_MX EVR VME + + + + 0 + 0 + 0 + + + + + 89 + 126 + 225 + + + + EVR + + + G_EVR_main.ui + + + SYS=SAR-CVME-ESBMX1,DEVICE=EVR0,FF=VME-300 + + + caRowColMenu::Menu + + + false;false;false;false;false;false;false;false;false;false;false;false;false;false;false;false + +