wip
This commit is contained in:
@@ -194,12 +194,34 @@ $\rightarrow$ The closed loop response becomes bad above 20Hz (motor 1 ca. -10\%
|
|||||||
\FloatBarrier
|
\FloatBarrier
|
||||||
\subsubsection{Friction}
|
\subsubsection{Friction}
|
||||||
|
|
||||||
\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
|
To measure the friction, the stage is moved at slow speed from
|
||||||
Record the friction (=current) at a slow move from +lim to -lim.\\
|
+lim to -lim. The current is proportional to the force.
|
||||||
Analyse the friction depending on the positions and motion directions.\\
|
Additionally the friction is measured depending on the stage positions and motion directions. This is measured at different speed to see if at these velocities the viscose friction is visible.
|
||||||
Do the records and analysis at different speeds.
|
|
||||||
\end{tcolorbox}
|
|
||||||
|
|
||||||
|
\begin{figure}[h!]
|
||||||
|
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/friction10.eps}
|
||||||
|
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/friction11.eps}
|
||||||
|
\caption{position dependant friction of motor 1}
|
||||||
|
\label{fig:mot1_frict}
|
||||||
|
\end{figure}
|
||||||
|
|
||||||
|
\begin{figure}[h!]
|
||||||
|
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/friction20.eps}
|
||||||
|
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/friction21.eps}
|
||||||
|
\caption{position dependant friction of motor 2}
|
||||||
|
\label{fig:mot2_frict}
|
||||||
|
\end{figure}
|
||||||
|
|
||||||
|
Data recording is done with: \verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 16|.\\
|
||||||
|
The function parameters are: \verb|pMin=-10000, pMax=10000, speed=(5,10,20,30,60), cnt=2, period=10|.
|
||||||
|
The recorded data moves from -10000um to 10000um at speed of 5 to 60 mm/s.\\
|
||||||
|
|
||||||
|
Avg current forward: 111.7 Avg current backward: -105.3\\
|
||||||
|
Avg current forward: 95.3 Avg current backward: -130.7\\
|
||||||
|
|
||||||
|
These values should be used for the $K_{fff}$ parameter of the Servo loop:\\
|
||||||
|
|
||||||
|
Motor 1,2: $K_{fff}\approx100$\\
|
||||||
|
|
||||||
\FloatBarrier
|
\FloatBarrier
|
||||||
\subsubsection{using advanced Deltatau Servo Loop}
|
\subsubsection{using advanced Deltatau Servo Loop}
|
||||||
@@ -311,8 +333,7 @@ The inductance of the stage is 2.4 mH.\\
|
|||||||
Nevertheless simulations with \verb|current_loop.slx| showed, that the current loop only works in the discrete domain. In continous domain neither the amplification nor the shape mached.\\
|
Nevertheless simulations with \verb|current_loop.slx| showed, that the current loop only works in the discrete domain. In continous domain neither the amplification nor the shape mached.\\
|
||||||
Therefore the only approach is to use the second order transfer function as approximated in section \ref{sec:measCurStep}.\\
|
Therefore the only approach is to use the second order transfer function as approximated in section \ref{sec:measCurStep}.\\
|
||||||
|
|
||||||
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
|
A further test is to 'turn of' the current loop. This can be done by setting:$IiGain=0, IpfGain=1, IpbGain=-1$.
|
||||||
A further test will be to 'remove' the current loop. This can be done by setting:$IiGain=0, IpfGain=1, IpbGain=-1$.
|
|
||||||
The resulting transfer function is:
|
The resulting transfer function is:
|
||||||
\[
|
\[
|
||||||
\frac{Ipf}
|
\frac{Ipf}
|
||||||
@@ -323,8 +344,16 @@ The resulting transfer function is:
|
|||||||
{\frac{L}{R} s +1}\\
|
{\frac{L}{R} s +1}\\
|
||||||
\\
|
\\
|
||||||
\]
|
\]
|
||||||
This is a $PT_1$ element with a time constant of $\frac{L}{R}=\frac{2.4mH}{8.8\Omega}=0.27ms$. But probably due to additional cables etc. the resistance and therefore also the timeconstant is bigger.
|
This is a $PT_1$ element with a time constant of $\frac{L}{R}=\frac{2.4mH}{8.8\Omega}=0.27ms$. Figures \ref{fig:curr_ol} shows a current step of 3000 \verb|curr_bit| resulting in a current of about 150 \verb|curr_bit|. The rise time matches roughly. So the cable resistance has not a major impact.
|
||||||
\end{tcolorbox}
|
|
||||||
|
\begin{figure}[h!]
|
||||||
|
\centering
|
||||||
|
\includegraphics[scale=.25]{../python/MXTuning/19_01_29/img/curr_step_ol_10.eps}
|
||||||
|
\includegraphics[scale=.25]{../python/MXTuning/19_01_29/img/curr_step_ol_20.eps}
|
||||||
|
\caption{voltage step (current open loop)}
|
||||||
|
\label{fig:curr_ol}
|
||||||
|
\end{figure}
|
||||||
|
|
||||||
|
|
||||||
\subsection{Mechanical model}
|
\subsection{Mechanical model}
|
||||||
|
|
||||||
@@ -610,8 +639,9 @@ Following code calculates parameters for a observer controller, does a simulatio
|
|||||||
\begin{verbatim}
|
\begin{verbatim}
|
||||||
clear;clear global;close all;
|
clear;clear global;close all;
|
||||||
mot=identifyFxFyStage(7);
|
mot=identifyFxFyStage(7);
|
||||||
|
sscType=0
|
||||||
for k =1:2
|
for k =1:2
|
||||||
[ssc]=StateSpaceControlDesign(mot{k});sim('observer');
|
[ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
|
||||||
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
||||||
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
||||||
print(f,sprintf('figures/sim_cl_observer_%d',mot{k}.id),'-depsc');
|
print(f,sprintf('figures/sim_cl_observer_%d',mot{k}.id),'-depsc');
|
||||||
@@ -629,10 +659,10 @@ end
|
|||||||
|
|
||||||
\begin{figure}[h!]
|
\begin{figure}[h!]
|
||||||
\center
|
\center
|
||||||
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_1.eps}
|
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_1.eps}
|
||||||
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_1.eps}\\
|
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode1.eps}\\
|
||||||
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_2.eps}
|
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps}
|
||||||
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_2.eps}
|
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps}
|
||||||
\caption{Observer sim: Motor 1 Motor 2}
|
\caption{Observer sim: Motor 1 Motor 2}
|
||||||
\label{fig:mot_observer_sim}
|
\label{fig:mot_observer_sim}
|
||||||
\end{figure}
|
\end{figure}
|
||||||
@@ -673,11 +703,56 @@ Finally the real time servo code is compliled for the DeltaTau with:\\
|
|||||||
|
|
||||||
\verb|/epics_ioc_modules/ESB_MX/python/usr_code$ make|\\
|
\verb|/epics_ioc_modules/ESB_MX/python/usr_code$ make|\\
|
||||||
|
|
||||||
Following lines in gpasciiCommander will activate the user servo loop code:
|
Following lines in gpasciiCommander will activate the user servo loop code.
|
||||||
|
\vspace{1pc}
|
||||||
|
|
||||||
\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
|
Here the full process:
|
||||||
|
|
||||||
|
\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=MATLAB]
|
||||||
|
\begin{verbatim}
|
||||||
|
clear;clear global;close all;
|
||||||
|
mot=identifyFxFyStage(7);
|
||||||
|
sscType=0;
|
||||||
|
for k =1:2
|
||||||
|
[ssc]=StateSpaceControlDesign(mot{k},sscType);
|
||||||
|
end
|
||||||
|
\end{verbatim}
|
||||||
\end{tcolorbox}
|
\end{tcolorbox}
|
||||||
|
|
||||||
|
\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=Shell]
|
||||||
|
\begin{verbatim}
|
||||||
|
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
|
||||||
|
./MXTuning.py -m512
|
||||||
|
|
||||||
|
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/usr_code
|
||||||
|
make
|
||||||
|
|
||||||
|
cd /home/zamofing_t/Documents/prj/SwissFEL/PBTools/pbtools/usr_servo_phase
|
||||||
|
scp userservo_util userphase_util usrServoSample/usralgo.ko root@SAR-CPPM-EXPMX1:/tmp
|
||||||
|
|
||||||
|
|
||||||
|
\end{verbatim}
|
||||||
|
\end{tcolorbox}
|
||||||
|
|
||||||
|
|
||||||
|
\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=DeltaTau Shell]
|
||||||
|
\begin{verbatim}
|
||||||
|
root@:/opt/ppmac#
|
||||||
|
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -d 1
|
||||||
|
rmmod usralgo
|
||||||
|
insmod /tmp/usralgo.ko
|
||||||
|
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -l 1 usr_servo_ctrl_2
|
||||||
|
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -e 1
|
||||||
|
|
||||||
|
gpascii:
|
||||||
|
Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1]
|
||||||
|
\end{verbatim}
|
||||||
|
\end{tcolorbox}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
\vspace{1pc}
|
\vspace{1pc}
|
||||||
|
|
||||||
\begin{appendix}
|
\begin{appendix}
|
||||||
|
|||||||
@@ -1,6 +1,11 @@
|
|||||||
\newpage
|
\newpage
|
||||||
\section{SCRATCH}
|
\section{SCRATCH}
|
||||||
|
|
||||||
|
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
|
||||||
|
Here is still work to do...
|
||||||
|
\end{tcolorbox}
|
||||||
|
|
||||||
|
|
||||||
\begin{verbatim}
|
\begin{verbatim}
|
||||||
clear;
|
clear;
|
||||||
clear global;
|
clear global;
|
||||||
|
|||||||
@@ -49,8 +49,6 @@
|
|||||||
!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')
|
!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')
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//Stage X Parker MX80L (top stage, mounted on Y stage)
|
//Stage X Parker MX80L (top stage, mounted on Y stage)
|
||||||
//----------------------------------------------------
|
//----------------------------------------------------
|
||||||
//Motor[2].pPhaseEnc -> PowerBrick[0].Chan[1].PhaseCapt.a
|
//Motor[2].pPhaseEnc -> PowerBrick[0].Chan[1].PhaseCapt.a
|
||||||
@@ -64,6 +62,11 @@
|
|||||||
!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
|
!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')
|
!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')
|
||||||
|
|
||||||
|
//Tweaks:
|
||||||
|
//Motor[1].Servo.Kfff=100
|
||||||
|
//Motor[2].Servo.Kfff=100
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
//rot stage
|
//rot stage
|
||||||
//---------
|
//---------
|
||||||
|
|||||||
@@ -41,6 +41,7 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
|
|||||||
%use_lqr: use lqr instead of pole placement
|
%use_lqr: use lqr instead of pole placement
|
||||||
verb=1;
|
verb=1;
|
||||||
use_lqr=0;
|
use_lqr=0;
|
||||||
|
MaxDac=2011.968;
|
||||||
filt_pos_err=Prefilt(mot,2);
|
filt_pos_err=Prefilt(mot,2);
|
||||||
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
|
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
|
||||||
switch mode
|
switch mode
|
||||||
@@ -214,10 +215,19 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
|
|||||||
|
|
||||||
%state space controller
|
%state space controller
|
||||||
ssc=struct();
|
ssc=struct();
|
||||||
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V"]
|
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V","MaxDac"]
|
||||||
ssc=setfield(ssc,k,eval(k));
|
ssc=setfield(ssc,k,eval(k));
|
||||||
end
|
end
|
||||||
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
|
|
||||||
|
mat2py=struct();
|
||||||
|
[ozA,ozB,ozC,ozD]=ssdata(ss_oz);
|
||||||
|
[pos_err_num,pos_err_den]=tfdata(filt_pos_err_z);
|
||||||
|
for k=["Ts","V","MaxDac","ozA","ozB","ozC","ozD","pos_err_num","pos_err_den"]
|
||||||
|
mat2py=setfield(mat2py,k,eval(k));
|
||||||
|
end
|
||||||
|
fn=sprintf('/tmp/ssc%d.mat',mot.id);
|
||||||
|
save(fn,'-struct','mat2py');
|
||||||
|
disp(['saved ' fn]);
|
||||||
end
|
end
|
||||||
|
|
||||||
function pf=Prefilt(mot,mode)
|
function pf=Prefilt(mot,mode)
|
||||||
|
|||||||
@@ -2,7 +2,7 @@
|
|||||||
baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab/figures';
|
baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab/figures';
|
||||||
|
|
||||||
clear;clear global;
|
clear;clear global;
|
||||||
close all;disp('löoad identification data...');
|
close all;disp('load identification data...');
|
||||||
mot=identifyFxFyStage(7);
|
mot=identifyFxFyStage(7);
|
||||||
|
|
||||||
close all;disp('simulate stage closed loop...');
|
close all;disp('simulate stage closed loop...');
|
||||||
@@ -16,21 +16,9 @@ for k =1:2
|
|||||||
end
|
end
|
||||||
|
|
||||||
close all;disp('simulate observer...');
|
close all;disp('simulate observer...');
|
||||||
|
for sscType=0%0:1
|
||||||
for k=1:2
|
for k=1:2
|
||||||
[ssc]=StateSpaceControlDesign(mot{k});sim('observer');
|
[ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
|
||||||
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
|
||||||
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
|
||||||
print(f,sprintf('figures/sim_cl_observer_%d',mot{k}.id),'-depsc');
|
|
||||||
f=bodeSamples(desPos_actPos);
|
|
||||||
print(f,sprintf('figures/sim_cl_observer_bode%d',mot{k}.id),'-depsc');
|
|
||||||
end
|
|
||||||
disp('document figure generation done');close all;
|
|
||||||
|
|
||||||
|
|
||||||
close all;disp('simulate observer with prefilter...');
|
|
||||||
for m =0%0:1
|
|
||||||
for k =1:2
|
|
||||||
[ssc]=StateSpaceControlDesign(mot{k},m);sim('observer');
|
|
||||||
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
|
||||||
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
|
||||||
print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc');
|
print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc');
|
||||||
|
|||||||
Binary file not shown.
@@ -13,11 +13,12 @@ bit 0=1: record/plot current step
|
|||||||
bit 1=2: custom chirp record/plot for IdCmd->ActPos transfer function
|
bit 1=2: custom chirp record/plot for IdCmd->ActPos transfer function
|
||||||
bit 2=4: custom chirp record/plot for DesPos->ActPos transfer function
|
bit 2=4: custom chirp record/plot for DesPos->ActPos transfer function
|
||||||
bit 3=8: custom chirp record/plot for various transfer function
|
bit 3=8: custom chirp record/plot for various transfer function
|
||||||
bit 4=16: plot the full bode recording
|
bit 4=16: slow linear move record/plot friction
|
||||||
bit 5=32: plot the full bode recording with an approximation model
|
bit 5=32: plot the full bode recording
|
||||||
bit 6=64: plot all raw acquired data files
|
bit 6=64: plot the full bode recording with an approximation model
|
||||||
bit 7=128: custom plots
|
bit 7=128: plot all raw acquired data files
|
||||||
bit 8=256: generate observer code (after files generated with matlab)
|
bit 8=256: custom plots
|
||||||
|
bit 9=512: generate observer code (after files generated with matlab)
|
||||||
|
|
||||||
-> check https://github.com/klauer/ppmac for fast data gathering server which supports
|
-> 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
|
phase gathering -> not yet compiling: /home/zamofing_t/Documents/prj/SwissFEL/PowerBrickInspector/ppmac/fast_gather
|
||||||
@@ -281,10 +282,10 @@ class MXTuning(Tuning):
|
|||||||
#[ssc]=StateSpaceControlDesign(mot1);
|
#[ssc]=StateSpaceControlDesign(mot1);
|
||||||
mat=scipy.io.loadmat(fn)
|
mat=scipy.io.loadmat(fn)
|
||||||
motid=int(re.search('(\d)\.mat',fn).group(1))
|
motid=int(re.search('(\d)\.mat',fn).group(1))
|
||||||
A=mat['Aoz']
|
A=mat['ozA']
|
||||||
B=mat['Boz']
|
B=mat['ozB']
|
||||||
C=mat['Coz']
|
C=mat['ozC']
|
||||||
D=mat['Doz']
|
D=mat['ozD']
|
||||||
V=mat['V']
|
V=mat['V']
|
||||||
u=('DesPos','IqMeas','IqVolts','ActPos')
|
u=('DesPos','IqMeas','IqVolts','ActPos')
|
||||||
y=('obsvOut',)
|
y=('obsvOut',)
|
||||||
@@ -348,6 +349,136 @@ class MXTuning(Tuning):
|
|||||||
EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||||
return (hdr,prog)
|
return (hdr,prog)
|
||||||
|
|
||||||
|
def friction(self, motor=1, pMin=-10000, pMax=10000, speed=(5,10,20,30,60), cnt=2, period=10, phase=False,
|
||||||
|
file='/tmp/gather.npz'):
|
||||||
|
if os.path.isfile(file):
|
||||||
|
f=np.load(file)
|
||||||
|
data=f['data']
|
||||||
|
meta=f['meta'].item()
|
||||||
|
meta['file']=file
|
||||||
|
else:
|
||||||
|
gpascii=self.comm.gpascii
|
||||||
|
gt=self.gather
|
||||||
|
gt.set_phasemode(phase)
|
||||||
|
m='Motor[%d].'%motor
|
||||||
|
address=('ActPos','IqMeas')
|
||||||
|
address=tuple(map(lambda x: m+x, address))
|
||||||
|
gt.set_address(*address)
|
||||||
|
# gt.set_property(MaxSamples=300000, Period=1)
|
||||||
|
gt.set_property(Period=period)
|
||||||
|
|
||||||
|
tSrv=gpascii.servo_period
|
||||||
|
tSrv=2E-4 # seconds
|
||||||
|
phOsv=gpascii.get_variable('sys.PhaseOverServoPeriod', float)
|
||||||
|
homePos=gpascii.get_variable('Motor[%d].HomePos'%motor, float)
|
||||||
|
JogTa=gpascii.get_variable('Motor[%d].JogTa'%motor, float)
|
||||||
|
gpascii.set_variable('Motor[%d].JogTa'%motor, -5)
|
||||||
|
|
||||||
|
subs={'prgId':999,'mot':motor,'num':100,'phase':'Phase' if phase else '','pMin':pMin,'pMax':pMax}
|
||||||
|
prog = ['''
|
||||||
|
&1
|
||||||
|
open prog {prgId}
|
||||||
|
linear abs
|
||||||
|
jog{mot}={pMin}
|
||||||
|
dwell 100
|
||||||
|
Gather.{phase}Enable=2
|
||||||
|
'''.format(**subs),]
|
||||||
|
|
||||||
|
for spd in sorted(speed*cnt):
|
||||||
|
prog.append('''
|
||||||
|
Motor[{mot}].JogSpeed=%d
|
||||||
|
jog{mot}={pMax}
|
||||||
|
dwell 0
|
||||||
|
jog{mot}={pMin}
|
||||||
|
dwell 0'''.format(**subs)%spd)
|
||||||
|
prog.append('''
|
||||||
|
dwell 10
|
||||||
|
Gather.{phase}Enable=0
|
||||||
|
close
|
||||||
|
&1
|
||||||
|
b{prgId}r
|
||||||
|
'''.format(**subs))
|
||||||
|
gpascii.send_block('\n'.join(prog))
|
||||||
|
res=gpascii.sync()
|
||||||
|
res=res.replace(GpasciiChannel.ACK+'\r\n','')
|
||||||
|
print(res)
|
||||||
|
t=time.time()
|
||||||
|
gt.wait_stopped()
|
||||||
|
print('time %f'%(time.time()-t))
|
||||||
|
self.data=data=gt.upload()
|
||||||
|
gpascii.set_variable('Motor[%d].JogTa'%motor, JogTa)
|
||||||
|
data[:,0]-=homePos
|
||||||
|
|
||||||
|
meta={'motor':motor,'date':time.asctime(),'ts':tSrv*period if not phase else tSrv*phOsv*period,'address':address}
|
||||||
|
np.savez_compressed(file, data=data, meta=meta)
|
||||||
|
meta['file'] = file
|
||||||
|
ts=meta['ts']
|
||||||
|
t=ts*np.arange(data.shape[0])
|
||||||
|
fig, ax1 = plt.subplots()
|
||||||
|
ax2=ax1.twinx()
|
||||||
|
ax3=ax1.twinx()
|
||||||
|
ax3.spines["right"].set_position(("axes", 1.12))
|
||||||
|
fig.subplots_adjust(right=0.80)
|
||||||
|
|
||||||
|
actPos=data[:,0]
|
||||||
|
iqMeas=data[:,1]
|
||||||
|
actVel=np.diff(actPos)/ts/1000.
|
||||||
|
actVel=np.hstack((actVel, actVel[-1]))
|
||||||
|
h1,=ax1.plot(t,actPos,'b',label="actPos (um)")
|
||||||
|
h2,=ax2.plot(t,iqMeas,'g',label="IqMeas (curr_bit)")
|
||||||
|
h3,=ax3.plot(t,actVel,'r',label="actVel (mm/s)")
|
||||||
|
ax1.yaxis.label.set_color(h1.get_color())
|
||||||
|
ax2.yaxis.label.set_color(h2.get_color())
|
||||||
|
ax3.yaxis.label.set_color(h3.get_color())
|
||||||
|
ax1.tick_params(axis='y', colors=h1.get_color())
|
||||||
|
ax2.tick_params(axis='y', colors=h2.get_color())
|
||||||
|
ax3.tick_params(axis='y', colors=h3.get_color())
|
||||||
|
|
||||||
|
hl=[h1,h2,h3]
|
||||||
|
ax1.legend(hl, [h.get_label() for h in hl],loc='best')
|
||||||
|
|
||||||
|
sz=100; weights = np.repeat(1.0, sz) / sz
|
||||||
|
|
||||||
|
idx=actVel[:-1]>0
|
||||||
|
arg=np.argsort(actPos[idx])
|
||||||
|
p1=actPos[idx][arg];c1=iqMeas[idx][arg]
|
||||||
|
c1 = np.convolve(c1, weights, 'same')
|
||||||
|
|
||||||
|
idx=actVel[:-1]<0
|
||||||
|
arg=np.argsort(actPos[idx])
|
||||||
|
p2=actPos[idx][arg];c2=iqMeas[idx][arg]
|
||||||
|
c2 = np.convolve(c2, weights, 'same')
|
||||||
|
|
||||||
|
fig, ax1 = plt.subplots()
|
||||||
|
hl=[]
|
||||||
|
hl+=ax1 .plot(actPos,iqMeas,'y-',label='raw')
|
||||||
|
hl+=ax1 .plot(p1,c1,'g-',label='vel>0')
|
||||||
|
hl+=ax1 .plot(p2,c2,'r-',label='vel<0')
|
||||||
|
|
||||||
|
#cAll=c1;pAll=p1
|
||||||
|
#cAll=cAll-cAll.mean()
|
||||||
|
#p=np.arange(500,28000,100) #np.arange(500,28000,128)
|
||||||
|
#c=np.interp(p,pAll,cAll)
|
||||||
|
#hl+=ax1.plot(p,c,'b.',label='vel<0')
|
||||||
|
#FfricLut=np.array([p,c]).T
|
||||||
|
Ffric=np.array([c1.mean(),c2.mean()])
|
||||||
|
print 'Avg current forward:',Ffric[0],'Avg current backward:',Ffric[1]
|
||||||
|
#print 'FfricLut',FfricLut
|
||||||
|
#print '//positions '+'%g,%g,%g'%tuple(p[0:3]),'...%g,%g,%g'%tuple(p[-3:])
|
||||||
|
#print 'float lutCur[%d]={'%len(p)
|
||||||
|
#for i in range(len(p)):
|
||||||
|
# print '%g,'%(c[i]),
|
||||||
|
#print '};'
|
||||||
|
import scipy.io
|
||||||
|
#fn='/home/zamofing_t/afs/ESB-MX/data/friction.mat'
|
||||||
|
#scipy.io.savemat(fn,mdict={'Ffric':Ffric,'FfricLut':FfricLut})
|
||||||
|
#print '\n\nuncomment line above to saved to matlab file',fn
|
||||||
|
|
||||||
|
#ax.xaxis.set_label_text(channels[1])
|
||||||
|
#ax1.yaxis.set_label_text('current in bits: '+channels[4])
|
||||||
|
ax1.legend(loc='best')
|
||||||
|
plt.show(block=False)
|
||||||
|
|
||||||
def check_fast_stage(self,file='/tmp/gather.npz'):
|
def check_fast_stage(self,file='/tmp/gather.npz'):
|
||||||
if os.path.isfile(file):
|
if os.path.isfile(file):
|
||||||
f=np.load(file)
|
f=np.load(file)
|
||||||
@@ -433,6 +564,31 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
|||||||
scipy.io.savemat(fn, mdict=f)
|
scipy.io.savemat(fn, mdict=f)
|
||||||
print('save to matlab file:'+fn)
|
print('save to matlab file:'+fn)
|
||||||
|
|
||||||
|
fn=os.path.join(self.baseDir, 'curr_step_ol_%d.npz' % mot)
|
||||||
|
doSet=not os.path.isfile(fn)
|
||||||
|
if doSet:
|
||||||
|
gpascii = self.comm.gpascii
|
||||||
|
IpfGain=gpascii.get_variable('Motor[%d].IpfGain'%mot,float)
|
||||||
|
IpbGain=gpascii.get_variable('Motor[%d].IpbGain'%mot,float)
|
||||||
|
IiGain =gpascii.get_variable('Motor[%d].IiGain' %mot,float)
|
||||||
|
gpascii.set_variable('Motor[%d].IpfGain'%mot,1)
|
||||||
|
gpascii.set_variable('Motor[%d].IpbGain'%mot,-1)
|
||||||
|
gpascii.set_variable('Motor[%d].IiGain' %mot,0)
|
||||||
|
plt.close('all')
|
||||||
|
mag=3000
|
||||||
|
self.bode_current(motor=mot, magMove=mag, magPhase=500, dwell=10, file=fn)
|
||||||
|
if doSet:
|
||||||
|
gpascii = self.comm.gpascii
|
||||||
|
gpascii.set_variable('Motor[%d].IpfGain'%mot,IpfGain)
|
||||||
|
gpascii.set_variable('Motor[%d].IpbGain'%mot,IpbGain)
|
||||||
|
gpascii.set_variable('Motor[%d].IiGain' %mot,IiGain)
|
||||||
|
self.homed&=~mot
|
||||||
|
ax,=plt.figure(1).axes
|
||||||
|
ax.set_ylim(0,200)
|
||||||
|
ax.set_title('step of 0->3000 with current in open loop')
|
||||||
|
plt.show(block=False)
|
||||||
|
save_figs(fn)
|
||||||
|
|
||||||
if mode&2:
|
if mode&2:
|
||||||
motLst = (1, 2) # (2,)#
|
motLst = (1, 2) # (2,)#
|
||||||
#recType:
|
#recType:
|
||||||
@@ -508,21 +664,30 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
|||||||
plt.show(block=False)
|
plt.show(block=False)
|
||||||
save_figs(fn)
|
save_figs(fn)
|
||||||
|
|
||||||
if mode&16: #plot the full bode recording
|
if mode&16: #record/plot friction
|
||||||
|
for mot in (1, 2):
|
||||||
|
plt.close('all')
|
||||||
|
fn=os.path.join(self.baseDir, 'friction%d.npz' % mot)
|
||||||
|
self.init_stage(mot,fn)
|
||||||
|
self.friction(motor=mot, file=fn);
|
||||||
|
save_figs(fn)
|
||||||
|
|
||||||
|
|
||||||
|
if mode&32: #plot the full bode recording
|
||||||
plt.close('all')
|
plt.close('all')
|
||||||
self.bode_full_plot(mot=1,base=self.baseDir)
|
self.bode_full_plot(mot=1,base=self.baseDir)
|
||||||
self.bode_full_plot(mot=2,base=self.baseDir)
|
self.bode_full_plot(mot=2,base=self.baseDir)
|
||||||
plt.show(block=False)
|
plt.show(block=False)
|
||||||
save_figs(os.path.join(self.baseDir,'bode_full_plot'))
|
save_figs(os.path.join(self.baseDir,'bode_full_plot'))
|
||||||
|
|
||||||
if mode&32: #plot the full bode recording with an approximation model
|
if mode&64: #plot the full bode recording with an approximation model
|
||||||
plt.close('all')
|
plt.close('all')
|
||||||
self.bode_model_plot(mot=1)
|
self.bode_model_plot(mot=1)
|
||||||
self.bode_model_plot(mot=2)
|
self.bode_model_plot(mot=2)
|
||||||
plt.show(block=False)
|
plt.show(block=False)
|
||||||
save_figs(os.path.join(self.baseDir,'bode_model_plot'))
|
save_figs(os.path.join(self.baseDir,'bode_model_plot'))
|
||||||
|
|
||||||
if mode&64: # plot all raw acquired data files
|
if mode&128: # plot all raw acquired data files
|
||||||
plt.close('all')
|
plt.close('all')
|
||||||
import glob
|
import glob
|
||||||
for fn in glob.glob(os.path.join(self.baseDir,'*.npz')):
|
for fn in glob.glob(os.path.join(self.baseDir,'*.npz')):
|
||||||
@@ -541,7 +706,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
|||||||
raw_input('press return')
|
raw_input('press return')
|
||||||
|
|
||||||
|
|
||||||
if mode&128: #custom plots
|
if mode&256: #custom plots
|
||||||
plt.close('all')
|
plt.close('all')
|
||||||
G1=signal.lti([1/8.8], [2.4E-3/8.8,1]) # num denum
|
G1=signal.lti([1/8.8], [2.4E-3/8.8,1]) # num denum
|
||||||
print('rise time %e s'%(2.4E-3/8.8))
|
print('rise time %e s'%(2.4E-3/8.8))
|
||||||
@@ -588,7 +753,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
|||||||
save_figs(os.path.join(self.baseDir,'iqCmd_TF'))
|
save_figs(os.path.join(self.baseDir,'iqCmd_TF'))
|
||||||
|
|
||||||
|
|
||||||
if mode&256: #generater code
|
if mode&512: #generater code
|
||||||
#before this can be done, the observer controller has to be designed with matlab:
|
#before this can be done, the observer controller has to be designed with matlab:
|
||||||
#s.a.ESB_MX/matlab/Readme.md
|
#s.a.ESB_MX/matlab/Readme.md
|
||||||
#clear;
|
#clear;
|
||||||
@@ -622,6 +787,8 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
|||||||
print(fnc+' generated.')
|
print(fnc+' generated.')
|
||||||
print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample')
|
print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample')
|
||||||
print('done')
|
print('done')
|
||||||
|
|
||||||
|
if mode&~512: # show and block only if not code generation
|
||||||
plt.show(block=False)
|
plt.show(block=False)
|
||||||
raw_input('press return')
|
raw_input('press return')
|
||||||
|
|
||||||
@@ -701,7 +868,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
|
|||||||
|
|
||||||
#plt.ion()
|
#plt.ion()
|
||||||
#args.host='MOTTEST-CPPM-CRM0573'
|
#args.host='MOTTEST-CPPM-CRM0573'
|
||||||
args.host=None
|
#args.host=None
|
||||||
if args.host is None:
|
if args.host is None:
|
||||||
comm=gt=None
|
comm=gt=None
|
||||||
else:
|
else:
|
||||||
|
|||||||
@@ -323,7 +323,7 @@ class DebugPlot:
|
|||||||
if mode&8:
|
if mode&8:
|
||||||
self.plot_trigger_jitter()
|
self.plot_trigger_jitter()
|
||||||
|
|
||||||
plt.show()
|
plt.show(block=False)
|
||||||
|
|
||||||
def plot_bode(self,xy=(0,1),mode=25,db=True):
|
def plot_bode(self,xy=(0,1),mode=25,db=True):
|
||||||
'''displays a bode plot of the data
|
'''displays a bode plot of the data
|
||||||
@@ -872,6 +872,10 @@ if __name__=='__main__':
|
|||||||
sp.gather_upload(fnRec=fn+'.npz')
|
sp.gather_upload(fnRec=fn+'.npz')
|
||||||
dp=DebugPlot(sp);dp.plot_gather(mode=11)
|
dp=DebugPlot(sp);dp.plot_gather(mode=11)
|
||||||
|
|
||||||
|
print('done')
|
||||||
|
plt.show(block=False)
|
||||||
|
raw_input('press return')
|
||||||
|
|
||||||
|
|
||||||
#sp.plot_points(sp.points);plt.show()
|
#sp.plot_points(sp.points);plt.show()
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user