add trajectory.py and cleanup old setUsrServo.c

python/trajectory.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python
+# *-----------------------------------------------------------------------*
+# |                                                                       |
+# |  Copyright (c) 2019 by Paul Scherrer Institute (http://www.psi.ch)    |
+# |                                                                       |
+# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
+# *-----------------------------------------------------------------------*
+'''
+Trajectory comparison:
+pvt: position velocity time
+p0t: position velocity=0 time
+ift: inverse fourier transformation
+
+-> look at trajectory and frequency components
+'''
+import numpy as np
+import matplotlib as mpl
+import matplotlib.pyplot as plt
+
+w=40.  # ms step between samples
+ts=.2 # sampling time
+x = np.arange(0, 400, w)
+y=np.cos(x)
+
+xx = np.arange(0, 400, ts)
+
+ax=plt.gca()
+ax.xaxis.set_ticks(x)
+markerline, stemlines, baseline = ax.stem(x, y, '-')
+
+yf=np.fft.fft(y)
+
+
+#best trajectory with lowest frequency
+y_iftf=np.hstack((yf,np.zeros(len(xx)-len(x))))
+y_ift=np.fft.ifft(y_iftf)*w/ts
+ax.plot(xx,y_ift,'-b',label='ift')
+
+
+#plt.figure()
+#ax=plt.gca()
+#ax.xaxis.set_ticks(x)
+#markerline, stemlines, baseline = ax.stem(x, y, '-')
+
+#PVT move
+t=np.hstack((y[-1:],y,y[:1]))
+
+n=int(w/ts)
+v=(t[2:]-t[:-2])/(w*2)
+
+y_pvt=np.ndarray(len(xx))*0
+xx1=xx[:n]
+for i in range(len(x)-1):
+  d=y[i]
+  c=v[i]
+  a=( -2*(y[i+1]-y[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
+  b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
+  y_pvt[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
+
+ax.plot(xx,y_pvt,'-g',label='pvt')
+
+#PVT move with stop 
+v*=0
+y_p0t=np.ndarray(len(xx))*0
+for i in range(len(x)-1):
+  d=y[i]
+  c=v[i]
+  a=( -2*(y[i+1]-y[i]-v[i]*w)+   w*(v[i+1]-v[i]))/w**3
+  b=(3*w*(y[i+1]-y[i]-v[i]*w)-w**2*(v[i+1]-v[i]))/w**3
+  y_p0t[i*n:(i+1)*n]=a*xx1**3+b*xx1**2+c*xx1+d
+
+ax.plot(xx,y_p0t,'-r',label='p0t')
+
+ax.legend(loc='best')
+plt.show(block=False)
+
+
+fig=plt.figure()
+ax=fig.add_subplot(1,1,1)#ax=plt.gca()
+
+y_iftf=np.fft.fft(y_ift)
+y_pvtf=np.fft.fft(y_pvt)
+y_p0tf=np.fft.fft(y_p0t)
+
+
+#f=np.arange(0,1E3/(2*ts),1E3/(2*ts*(len(xx)-1)))
+f=np.linspace(0,1E3/(2*ts),len(xx))
+
+db_mag=20*np.log10(abs(y_iftf))
+ax.semilogx(f,db_mag,'-b',label='ift')  # Bode magnitude plot
+db_mag=20*np.log10(abs(y_pvtf))
+ax.semilogx(f,db_mag,'-g',label='pvt')  # Bode magnitude plot
+db_mag=20*np.log10(abs(y_p0tf))
+ax.semilogx(f,db_mag,'-r',label='p0t')  # Bode magnitude plot
+ax.yaxis.set_label_text('dB ampl')
+ax.xaxis.set_label_text('frequency [Hz]')
+plt.grid(True)
+
+ax.legend(loc='best')
+plt.show(block=False)
+
+
+

python/usr_code/Makefile

@@ -75,10 +75,6 @@ KBUILD_EXTRA_SYMBOLS := /usr/local/dtlibs/libppmac/Module.symvers
 #	cp -f /usr/local/usralgo/usralgomain.c $(PWD)
 all:
 	$(MAKE) -C $(KSRC) SUBDIRS=$(PWD) modules
-	#scp usralgo.ko root@$(PPMAC):/tmp
-	#scp pp_proj.ini root@$(PPMAC):/var/ftp/usrflash/Project/Configuration/pp_proj.ini
-	#scp pp_proj.ini root@$(PPMAC):/tmp/pp_proj.ini
-	#ssh root@$(PPMAC) projpp
 
 #	$(MAKE) -C $(KSRC) SUBDIRS=$(PWD) modules V=1
 #	mv -f usralgo.ko ../../bin/Debug/
@@ -101,3 +97,42 @@ clean::
 
 dbg:
 	@echo PATH $(PATH)
+
+.PHONY: tags
+tags:
+	find . -type f -iname "*.[chS]" | xargs ctags -a -f .tags
+	find $(INCLUDE) -type f -iname "*.[chS]" | xargs ctags -a -f .tags
+	find $(XENOMAI_INC_DIR) -type f -iname "*.[chS]" | xargs ctags -a -f .tags
+	find $(RTPMACINCLUDEDIR) -type f -iname "*.[chS]" | xargs ctags -a -f .tags
+
+
+Debug: all
+
+install: all
+	scp usralgo.ko root@$(PPMAC):/tmp
+	scp /home/zamofing_t/Documents/prj/SwissFEL/PBTools/pbtools/usr_servo_phase/userservo_util root@$(PPMAC):/tmp
+	-ssh root@$(PPMAC) LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -d 9
+	-ssh root@$(PPMAC) rmmod usralgo
+	-ssh root@$(PPMAC) insmod /tmp/usralgo.ko
+	-ssh root@$(PPMAC) LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -l 0 usr_servo_evr_tigger
+	-ssh root@$(PPMAC) LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -e 9
+
+
+#!common()
+#!encoder_sim(enc=1,tbl=1,mot=1)
+#!motor(mot=1,dirCur=0,contCur=100,peakCur=100,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
+#Motor[1].pLimits=0;Motor[1].AmpFaultLevel=0;Motor[1].pAmpEnable=0;Motor[1].pAmpFault=0
+
+#!common()
+#!encoder_sim(enc=9,tbl=9,mot=9)
+#Motor[9].pLimits=0;Motor[9].AmpFaultLevel=0;Motor[9].pAmpEnable=0;Motor[9].pAmpFault=0
+#Motor[9].Ctrl=UserAlgo.ServoCtrlAddr[0]
+
+
+
+	#scp pp_proj.ini root@$(PPMAC):/var/ftp/usrflash/Project/Configuration/pp_proj.ini
+	#scp pp_proj.ini root@$(PPMAC):/tmp/pp_proj.ini
+	#ssh root@$(PPMAC) projpp
+
+
+#MotorData