sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2019-02-12 16:52:55 +01:00
parent 3412e33595
commit 5a05b7355e
8 changed files with 306 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
MXfastStageDoc/MXfastStage.tex
View File

@@ -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}

5
MXfastStageDoc/Scratch.tex
View File

@@ -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;

7
cfg/MX1_setup.cfg
View File

@@ -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
//--------- //---------

14
matlab/StateSpaceControlDesign.m
View File

@@ -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)

20
matlab/documentFunctions.m
View File

@@ -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 k =1:2 for sscType=0%0:1
[ssc]=StateSpaceControlDesign(mot{k});sim('observer'); for k=1:2
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
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');

BIN
matlab/observer.slx
View File

Binary file not shown.

201
python/MXTuning.py
View File

@@ -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,8 +787,10 @@ 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')
plt.show(block=False)
raw_input('press return') if mode&~512: # show and block only if not code generation
plt.show(block=False)
raw_input('press return')
def save_figs(fn): def save_figs(fn):
@@ -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:

6
python/shapepath.py
View File

@@ -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()