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

src/Readme.md

@@ -166,3 +166,11 @@ usrflash/Project/Bin/Debug/usralgo.ko
 
 
 
+
+
+Usr_C PLC
+---------
+scp -r usralgo.ko root@MOTTEST-CPPM-CRM0573:/opt/ppmac
+
+rmmod usralgo
+insmod /mypath/usralgo.ko  (e.g. insmod /opt/ppmac/usrflash/Project/Bin/Debug/usralgo.ko)

src/setUsrServo/Makefile

@@ -1,18 +0,0 @@
-INC=-I/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac -I/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac
-
-LIB=-L/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac -lppmac -L/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/usr/local/xenomai-2.6.2.1/lib -lxenomai -lpthread_rt -lpthread -lrt -ldl
-
-CC=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc
-LD=$(CC)
-
-all: setUsrServo
-
-setUsrServo.o: setUsrServo.c
-	$(CC) -g $(INC) -c setUsrServo.c -o setUsrServo.o
-
-setUsrServo: setUsrServo.o
-	$(LD) -g $(LIB) setUsrServo.o -o setUsrServo
-	scp setUsrServo root@SAROP11-CPPM-MOT6871:/tmp
-
-clean:
-	rm setUsrServo setUsrServo.o

src/setUsrServo/setUsrServo.c

@@ -1,61 +0,0 @@
-#include <stdio.h>
-//http://forums.deltatau.com/showthread.php?tid=24 ->examples to write directly to shared memory
-#include "RtGpShm.h" // Global Rt/Gp Shared memory pointers
-#include "pRtGpShm.h" // Global Rt/Gp Shared memory pointers
-//extern "C"
-//{
-  void mydbg(char* s, int a, int b);
-//}
-main()
-{
-  int i;
-  int status;
-  char buf[256];
-  printf("hello\n");
-  printf("there\n");
-  InitLibrary();
-  for (i=0;i<256;i++)
-  {
-    printf("hello %d\n",i);
-    snprintf(buf,sizeof(buf),"Gate3[0].GpioData[0].16.8=%d\n",i);
-    status=Command(buf); //ppmac gplib function
-    printf("%s ->%d\n",buf,status);
-    //mydbg("gugus", i, 2*i);
-    usleep(10000);
-  }
-  {
-    int i;
-    struct SHM *p;  // Pointer to shared memory
-    p=pshm;
-    printf("Sys.ServoCtrl 0x%p\n",p->ServoCtrl);
-
-    //p->Motor[4].Servo.Kp=123.f;
-    printf("Motor[4].Servo.Kp %f\n",p->Motor[4].Servo.Kp);
-
-    printf("Motor[4].Ctrl 0x%p\n",p->Motor[4].Ctrl);
-
-    printf("UserAlgo.ServoCtrlName[4] %s\n",p->UserAlgo.ServoCtrlName[4]);
-    printf("UserAlgo.ServoCtrlAddr[4] %x\n",p->UserAlgo.ServoCtrlAddr[4]);
-
-    printf("Motor[4].UserPhase 0x%p\n",p->Motor[4].UserPhase);
-    printf("Motor[4].UserServo 0x%p\n",p->Motor[4].UserServo);
-
-
-    //for(i=0;i<8;i++)
-    //{
-    //  printf("UserAlgo.ServoName %s\n",p->UserAlgo.ServoName[i]);
-    //  printf("UserAlgo.ServoAddr %x\n",p->UserAlgo.ServoAddr[i]);
-    //  printf("UserAlgo.ServoCtrlName %s\n",p->UserAlgo.ServoCtrlName[i]);
-    //  printf("UserAlgo.ServoCtrlAddr %x\n",p->UserAlgo.ServoCtrlAddr[i]);
-    //}
-
-    strcpy(p->UserAlgo.ServoCtrlName[4],"user_pid_ctrl");
-    p->UserAlgo.ServoCtrlAddr[4]= 0xa35e012c;
-
-
-    //p->Motor[4].Ctrl=0x123456;
-    //UserAlgo
-  }
-
-  CloseLibrary();
-}

src/usrServo/Makefile

@@ -20,6 +20,7 @@
 #------------------------------------------------------------------------------
 
 PPMAC=SAR-CPPM-EXPMX1
+PPMAC=MOTTEST-CPPM-CRM0573
 #PPMAC=SAROP11-CPPM-MOT6871
 
 PMAC_ARCH=ppc465-2