diff --git a/matlab/Readme.md b/matlab/Readme.md index 5d2d6bd..c4f23e2 100644 --- a/matlab/Readme.md +++ b/matlab/Readme.md @@ -26,12 +26,10 @@ clear; clear global; close all; [mot1,mot2]=identifyFxFyStage(); -[pb]=simFxFyStage(mot1,1); -[ssc]=StateSpaceControlDesign(mot1,1); - -[mot1,mot2]=identifyFxFyStage(); -[pb]=simFxFyStage(mot2,2); -[ssc]=StateSpaceControlDesign(mot2,2); +[pb]=simFxFyStage(mot1); +[ssc]=StateSpaceControlDesign(mot1); +[pb]=simFxFyStage(mot2); +[ssc]=StateSpaceControlDesign(mot2); ``` diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index cd15116..2ae1427 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -1,4 +1,4 @@ -function [ssc]=StateSpaceControlDesign(mot,motid) +function [ssc]=StateSpaceControlDesign(mot) % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!! % % builds a state space controller designed for the plant. @@ -17,18 +17,18 @@ function [ssc]=StateSpaceControlDesign(mot,motid) % https://www.youtube.com/watch?v=Lax3etc837U %ss_ol=mot.ssPlt; - ss_ol_plt=mot.ssPlt; %real plant (model of real plant) - ss_ol_plt.Name='open loop plant'; - ss_ol_mdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer - ss_ol_mdl.Name='open loop model'; + ssPlt=mot.ssPlt; %real plant (model of real plant) + ssPlt.Name='open loop plant'; + ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer + ssMdl.Name='open loop model'; - [Ap,Bp,Cp,Dp]=ssdata(ss_ol_plt); - [Am,Bm,Cm,Dm]=ssdata(ss_ol_mdl); + [Ap,Bp,Cp,Dp]=ssdata(ssPlt); + [Am,Bm,Cm,Dm]=ssdata(ssMdl); %sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only - figure();h=bodeplot(ss_ol_plt,ss_ol_mdl); + figure();h=bodeplot(ssPlt,ssMdl); setoptions(h,'IOGrouping','all') % step answer on open loop: @@ -37,8 +37,8 @@ function [ssc]=StateSpaceControlDesign(mot,motid) xp0 = zeros(1,length(Ap)); xm0 = zeros(1,length(Am)); - [yp,t,x] = lsim(ss_ol_plt,u,t,xp0); - [ym,t,x] = lsim(ss_ol_mdl,u,t,xm0); + [yp,t,x] = lsim(ssPlt,u,t,xp0); + [ym,t,x] = lsim(ssMdl,u,t,xm0); figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)'); legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos') @@ -51,44 +51,52 @@ function [ssc]=StateSpaceControlDesign(mot,motid) % *** space state controller *** % %place poles for the controller feedback - if motid==1 - %2500rad/s = 397Hz -> locate poles here - %6300rad/s = 1027Hz -> locate poles here - if length(poles)==4 - p1=-6300+2800i; - p2=-6200+1500i; - P=[p1 p1' p2 p2']; - else - p1=-3300+2800i; - p2=-2700+500i; - p3=-2500+10i; - %p1=-3300+2800i; - %p2=-1500+500i; - %p3=-1200+10i; - P=[p1 p1' p2 p2' p3 p3']; - end + use_lqr=0; + if use_lqr %use the lqr controller + Q=eye(length(ssMdl.A)); + R=1; + [K,P,E]=lqr(ssMdl,Q,R,0); else - %2500rad/s = 397Hz -> locate poles here - %6300rad/s = 1027Hz -> locate poles here - if length(poles)==4 - p1=-6300+2800i; - p2=-6200+1500i; - P=[p1 p1' p2 p2']; + if mot.id==1 + %2500rad/s = 397Hz -> locate poles here + %6300rad/s = 1027Hz -> locate poles here + if length(poles)==4 + p1=-6300+280i; + p2=-6200+150i; + P=[p1 p1' p2 p2']; + else + p1=-3300+2800i; + p2=-2700+500i; + p3=-2500+10i; + %p1=-3300+2800i; + %p2=-1500+500i; + %p3=-1200+10i; + P=[p1 p1' p2 p2' p3 p3']; + end else - p1=-3300+2800i; - p2=-1900+130i; - p3=-2900+80i; - p4=-2300+450i; - p5=-2000+20i; - p6=-1500+10i; - %p1=-3300+2800i; - %p2=-1500+500i; - %p3=-1200+10i; - P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6']; + %2500rad/s = 397Hz -> locate poles here + %6300rad/s = 1027Hz -> locate poles here + if length(poles)==4 + p1=-6300+2800i; + p2=-6200+1500i; + P=[p1 p1' p2 p2']; + else + p1=-3300+2800i; + p2=-1900+130i; + p3=-2900+80i; + p4=-2300+450i; + p5=-2000+20i; + p6=-1500+10i; + %p1=-3300+2800i; + %p2=-1500+500i; + %p3=-1200+10i; + P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6']; + end end - end - K = place(Am,Bm,P); - %K = acker(Am,Bm,Pm); + K = place(Am,Bm,P); + %K = acker(Am,Bm,Pm); + end %if lqr + V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) %Nbar(2)=1; %the voltage stuff is crap for now if length(V)>1 @@ -110,7 +118,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid) % *** observer controller *** % %observer poles-> 5 times farther left than system poles - if motid==1 + if mot.id==1 op1=(p1*5); op2=(p2*5); if length(poles)>4 @@ -143,27 +151,27 @@ function [ssc]=StateSpaceControlDesign(mot,motid) Ct = [ Cm zeros(size(Cm)) ]; % step answer on closed loop with observer controller: - ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName); - figure();lsim(ss_o,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer'); + ss_t = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName); + figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer'); % *** disctrete observer controller *** % Ts=1/5000; % 5kHz - ss_od = c2d(ss_o,Ts); - ss_od .Name='discrete obsvr ctrl'; + ss_tz = c2d(ss_t,Ts); + ss_tz .Name='discrete obsvr ctrl'; % step answer on closed loop with disctrete observer controller: t = 0:Ts:.05; - figure();lsim(ss_od ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete'); + figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete'); %plot all bode diagrams of desPos->actPos figure(); - h=bodeplot(ss_cl(3),ss_o(3),ss_od(3)); + h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3)); setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); figure(); - h=pzplot(ss_cl(3),ss_o(3),ss_od(3)); + h=pzplot(ss_cl(3),ss_t(3),ss_tz(3)); setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); @@ -172,15 +180,25 @@ function [ssc]=StateSpaceControlDesign(mot,motid) Bo=[Bm L]; Co=K; Do=zeros(size(Co,1),size(Bo,2)); + + ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'}); + + ss_oz = c2d(ss_o,Ts); + [Aoz,Boz,Coz,Doz]=ssdata(ss_oz); mdlName='observer'; open(mdlName); + +ssPltz = c2d(ssPlt,Ts); +[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz); + + %state space controller ssc=struct(); - for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od","numV","denV"] + for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV"] ssc=setfield(ssc,k,eval(k)); end - + save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); end diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index 5bf2470..bba9884 100644 --- a/matlab/identifyFxFyStage.m +++ b/matlab/identifyFxFyStage.m @@ -101,7 +101,9 @@ function [mot1,mot2]=identifyFxFyStage() end function mot=fyStage() - mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1); + motid=1; + mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid); + mot.id=motid; mot.tfc=currstep(mot); opt=tfestOptions; @@ -171,7 +173,9 @@ function [mot1,mot2]=identifyFxFyStage() end function mot=fxStage() - mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2); + motid=2; + mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',motid); + mot.id=motid; currstep(mot); opt=tfestOptions; diff --git a/matlab/observer.slx b/matlab/observer.slx index 2ad9c10..9e2673c 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index 9449d75..72e0652 100644 --- a/matlab/simFxFyStage.m +++ b/matlab/simFxFyStage.m @@ -1,4 +1,4 @@ -function [pb]=simFxFyStage(mot,motid) +function [pb]=simFxFyStage(mot) % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!! % %loads the current (11.10.2018) controller settings of the @@ -9,7 +9,7 @@ function [pb]=simFxFyStage(mot,motid) Ts=2E-4; % 0.2ms=5kHz MaxDac=2011.968; MaxPosErr=10000; - if motid==1 + if mot.id==1 %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) %!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') Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index c0b1669..0a29f9a 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ diff --git a/python/MXTuning.py b/python/MXTuning.py index 53cc6cb..30d7c14 100755 --- a/python/MXTuning.py +++ b/python/MXTuning.py @@ -15,6 +15,8 @@ Modes: 4 plot the full bode recording 5 plot the full bode recording with an approximation model 6 plot all raw acquired data files +7 custom chirp test +8 generate observer code (after files generated with matlab) -> 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 @@ -201,6 +203,90 @@ class MXTuning(Tuning): ax.semilogx(f, phase,'-k',lw=1) # Bode phase plot # tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s')) + def usr_servo_gen_code(self,fn='/tmp/ssc1.mat'): + import scipy.io,re + #the file ssc[1|2].mat has been generated with matlab: + #[mot1, mot2]=identifyFxFyStage(); + #[pb]=simFxFyStage(mot1); + #[ssc]=StateSpaceControlDesign(mot1); + mat=scipy.io.loadmat(fn) + motid=int(re.search('(\d)\.mat',fn).group(1)) + A=mat['Aoz'] + B=mat['Boz'] + C=mat['Coz'] + D=mat['Doz'] + V=mat['V'] + u=('DesPos','IqMeas','IqVolts','ActPos') + y=('obsvOut',) + progSample='''double usr_servo_ctrl_{motid}(MotorData *Mptr) +{{ + pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001; //lowpass of Position error + return pshm->ServoCtrl(Mptr); +}}'''.format(motid=motid) + + prog='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr) +{{ + //x[n+1]=A*x[n]+B*u + //y=C*x[n]+D*x[n] + //u=[{u}].T + double x[{A.shape[0]}]; //new state + static double _x[{A.shape[0]}]={{0,0,0,0}}; //old state + //double {u}; // input values + double {y},iqCmd; // output values + double maxDac=Mptr->MaxDac; +'''.format(motid=motid,A=A,xInit=','.join('0'*A.shape[0]),u=', '.join(u),y=', '.join(y)) + s=' //input values\n' + for i in range(len(u)): + s+=' double {u}=Mptr->{u};\n'.format(u=u[i]) + prog+=s+'''\n + if (Mptr->ClosedLoop) + { + return iqCmd; + } + else + { + Mptr->Servo.Integrator=0.0; + return 0.0; + } +''' + + s=' //x[n+1]=A*x[n]+B*u;\n' + for i in range(A.shape[0]): + s+=' x[%d]='%i + for j in range(A.shape[1]): + s+='%+28.22g*_x[%d]'%(A[i,j],j) + for j in range(B.shape[0]): + s+='%+28.22g*%s'%(B[i,j],u[j]) + s+=';\n' + prog+=s+'\n' + + s=' //y=C*x[n]+D*x[n];\n' + for i in range(C.shape[0]): + s+=' %s='%y[i] + for j in range(C.shape[1]): + s+='%+28.22g*_x[%d]'%(C[i,j],j) + s+=';\n' + prog+=s+'\n' + + + prog+=''' iqCmd=DesPos*{V}-{y}; + if (iqCmd>maxDac) + {{ + iqCmd=maxDac; + }} + else + {{ + if (iqCmd<-maxDac) + {{ + iqCmd=-maxDac; + }} + }} + return iqCmd; +}}'''.format(V=V[0,0],y=y[0]) + + hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr); +EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid) + return (hdr,prog) def bode(mdl): w,mag,phase = signal.bode(mdl,1000) @@ -250,6 +336,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #plt.ion() args.host='MOTTEST-CPPM-CRM0573' + args.host=None if args.host is None: comm=gt=None else: @@ -342,10 +429,42 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' tune.custom_chirp(motor=1,minFrq=100,maxFrq=3000,amp=10,tSec=5,mode=0,file='/tmp/cst_chirp0.npz') #tune.custom_chirp(motor=2,minFrq=1,maxFrq=1000,tSec=5,mode=1,file='/tmp/cst_chirp1.npz') #tune.custom_chirp(motor=1,minFrq=1,maxFrq=3000,tSec=5,mode=2,file='/tmp/cst_chirp2.npz') - + elif mode==8: #generater code + #before this can be done, the observer controller has to be designed with matlab: + #s.a.ESB_MX/matlab/Readme.md + #clear; + #clear global; + #close all; + #[mot1,mot2]=identifyFxFyStage(); + #[pb]=simFxFyStage(mot1); + #[ssc]=StateSpaceControlDesign(mot1); + #[pb]=simFxFyStage(mot2); + #[ssc]=StateSpaceControlDesign(mot2); + #after this go to: python/usr_code and call make to build the controller + #to activate the controller checkout: PBTools/pbtools/usr_servo_phase + base=os.path.dirname(__file__) + (hdr1,prog1)=tune.usr_servo_gen_code('/tmp/ssc1.mat') + (hdr2,prog2)=tune.usr_servo_gen_code('/tmp/ssc2.mat') + fn_ct=os.path.join(base,'usr_code/usrcode_template.c') + fn_ht=os.path.join(base,'usr_code/usrcode_template.h') + fnc=os.path.join(base,'usr_code/usrcode.c') + fnh=os.path.join(base,'usr_code/usrcode.h') + s=open(fn_ht).read() + s=s.replace('',hdr1+'\n\n'+hdr2) + fh=open(fnh,'w') + fh.write(s) + fh.close() + print(fnh+' generated.') + s=open(fn_ct).read() + s=s.replace('',prog1+'\n\n'+prog2) + fh=open(fnc,'w') + fh.write(s) + fh.close() + print(fnc+' generated.') + print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample') plt.show() #------------------ Main Code ---------------------------------- - #ssh_test() + #ssh_test()'/tmp/usrcode.c' ret=parse_args() exit(ret) diff --git a/python/usr_code/Makefile b/python/usr_code/Makefile new file mode 100644 index 0000000..600d19b --- /dev/null +++ b/python/usr_code/Makefile @@ -0,0 +1,103 @@ +#------------------------------------------------------------------------------ +# Copyright (C) Delta Tau Data Systems Inc., 2007 +# All rights reserved. +# +# Generic makefile for any c realtime C plc 0, user servo or user phase +# For a new project change the following +# +# 1.) usralgo-objs should be assigned the 'C' source code files that need to be compiled +# 2.) issue the command 'make depend' the first time a project is created and +# (every time an additional 'C' file is added to the project the command +# 'make depend' must be issued) +# 3.) issue the command make clean +# 4.) issue the command make +# +# Notes +# -------- +# Change DTDEBUG above to -O2 for release w/ optimization +# Change DTDEBUG above to -g3 for debug +# arm,i386,i385hv,ppc460-2,ppc460-1,ppc405 +#------------------------------------------------------------------------------ + +PPMAC=SAR-CPPM-EXPMX1 +PPMAC=MOTTEST-CPPM-CRM0573 +#PPMAC=SAROP11-CPPM-MOT6871 + +PMAC_ARCH=ppc465-2 + +ifeq ($(PMAC_ARCH),ppc465-2) +ARCH=powerpc +#CROSS_COMPILE=powerpc-meau-linux-gnu- +CROSS_COMPILE=/opt/eldk-4.2/usr/bin/ppc_4xxFP- +KDIR=/opt/powerpc-465-rootfs/usr/src/linux-3.2.21-serengeti-smp +KSRC=/opt/powerpc-465-rootfs/usr/src/linux-3.2.21-serengeti-smp +#CC=powerpc-meau-linux-gnu-gcc +#AS=powerpc-meau-linux-gnu-as +#LD=powerpc-meau-linux-gnu-gcc +CC=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc +AS=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc +LD=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc +#STRIP=i686-meau-linux-gnu-strip +INCLUDE=/opt/powerpc-465-rootfs/usr/lib/gcc/powerpc-linux-gnu/4.6/include +XENOMAI_INC_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include +XENOMAI_LIB_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/lib +RPATH=-Wl,-rpath-link,/opt/powerpc-465-rootfs/lib/powerpc-linux-gnu +ROOTFS_DIR=/opt/powerpc-465-rootfs +endif + +#RTPMACINCLUDEDIR=/usr/local/dtlibs/rtpmac +#LIBPPMACINCLUDEDIR=/usr/local/dtlibs/libppmac +#THESE PATHS MUST BE ABSOLUTE ! +RTPMACINCLUDEDIR=/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac/ +LIBPPMACINCLUDEDIR=/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac/ + +export ARCH +export CROSS_COMPILE + +OBJS := ${patsubst %, %.o, $(MODULES)} +CLEANMOD := ${patsubst %, .%*, $(MODULES)} +PWD := $(shell if [ "$$PWD" != "" ]; then echo $$PWD; else pwd; fi) + + +obj-m += usralgo.o +usralgo-objs := usralgomain.o \ +usrcode.o + +LDFLAGS := -nostdlib +EXTRA_CFLAGS := -O2 -DCONFIG_460EX -D_GNU_SOURCE -D_REENTRANT -D__XENO__ -mhard-float -I$(RTPMACINCLUDEDIR) -I$(LIBPPMACINCLUDEDIR) -I$(XENOMAI_INC_DIR) -I$(XENOMAI_INC_DIR)/posix -I$(KSRC)/include/xenomai -I$(KSRC)/include/xenomai/posix -I$(INCLUDE) $(ADD_CFLAGS) --sysroot=$(ROOTFS_DIR) +KBUILD_EXTRA_SYMBOLS := /usr/local/dtlibs/libppmac/Module.symvers + +%.o: %.S + $(CC) -s -D__KERNEL__ -x c -E $< -o $*.i + $(AS) -mbooke -o $@ $*.i + +#all:: +# 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/ + + +#modules: +# @echo "$(CFLAGS)" + +bclean:: + $(RM) *.o .*.o.d .*.o.cmd *.ko *.log + $(RM) -R .tmp* + $(RM) .runinfo + rm -rf .runinfo .tmp* .*.o.d .*.o.cmd .*.cmd *.o *.ko *.mod.c *.i *.so Module.symvers modules.order + +clean:: + $(RM) *.o .*.o.d .*.o.cmd *.ko + $(RM) -R .tmp* + $(RM) .runinfo + rm -rf .runinfo .tmp* .*.o.d .*.o.cmd .*.cmd *.o *.ko *.mod.c *.i *.so Module.symvers modules.order + +dbg: + @echo PATH $(PATH) diff --git a/python/usr_code/pp_proj.h b/python/usr_code/pp_proj.h new file mode 100755 index 0000000..2dec7b8 --- /dev/null +++ b/python/usr_code/pp_proj.h @@ -0,0 +1,189 @@ + +#ifndef _PP_PROJ_H_ +#define _PP_PROJ_H_ +//*********************************************************************************** +// 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. +//************************************************************************************ + +#ifdef _PPScriptMode_ +enum globalP {_globalP_=-1}; +enum globalParray {_globalParray_=-1}; +enum csglobalQ {_csglobalQ_=-1}; +enum csglobalQarray {_csglobalQarray_=-1}; + +enum ptrM {_ptrM_=-1}; +enum ptrMarray {_ptrMarray_=-1}; +void SetEnumGlobalVar(enum globalP var, double data) +{ + pshm->P[var] = data; +} + +double GetEnumGlobalVar(enum globalP var) +{ + return pshm->P[var]; +} + +void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data) +{ + pshm->P[(var + index)%MAX_P] = data; +} + +double GetEnumGlobalArrayVar(enum globalParray var, unsigned index) +{ + return pshm->P[(var + index)%MAX_P]; +} + +void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[var] = data; +} + +double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[var]; +} + +void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data; +} + +double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q]; +} + +void SetEnumPtrVar(enum ptrM var, double data) +{ + im_write(pshm->Mdef + var, data, &pshm->Ldata); +} + +double GetEnumPtrVar(enum ptrM var) +{ + return im_read(pshm->Mdef + var, &pshm->Ldata); +} + +void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data) +{ + im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata); +} + +double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index) +{ + return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata); +} + +#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x) +#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x) +#define GetGlobalVar(i) GetEnumGlobalVar(i) +#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j) + +#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x) +#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x) +#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j) +#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k) + +#define SetPtrVar(i, x) SetEnumPtrVar(i, x) +#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x) +#define GetPtrVar(i) GetEnumPtrVar(i) +#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j) +// end of #ifdef _PPScriptMode_ +#else +#ifdef _EnumMode_ +enum globalP {_globalP_=-1}; +enum globalParray {_globalParray_=-1}; +enum csglobalQ {_csglobalQ_=-1}; +enum csglobalQarray {_csglobalQarray_=-1}; +enum ptrM {_ptrM_=-1}; +enum ptrMarray {_ptrMarray_=-1}; +void SetEnumGlobalVar(enum globalP var, double data) +{ + pshm->P[var] = data; +} + +double GetEnumGlobalVar(enum globalP var) +{ + return pshm->P[var]; +} + +void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data) +{ + pshm->P[(var + index)%MAX_P] = data; +} + +double GetEnumGlobalArrayVar(enum globalParray var, unsigned index) +{ + return pshm->P[(var + index)%MAX_P]; +} + +void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[var] = data; +} + +double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[var]; +} + +void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data; +} + +double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q]; +} + +void SetEnumPtrVar(enum ptrM var, double data) +{ + im_write(pshm->Mdef + var, data, &pshm->Ldata); +} + +double GetEnumPtrVar(enum ptrM var) +{ + return im_read(pshm->Mdef + var, &pshm->Ldata); +} + +void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data) +{ + im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata); +} + +double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index) +{ + return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata); +} + +#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x) +#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x) +#define GetGlobalVar(i) GetEnumGlobalVar(i) +#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j) + +#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x) +#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x) +#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j) +#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k) + +#define SetPtrVar(i, x) SetEnumPtrVar(i, x) +#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x) +#define GetPtrVar(i) GetEnumPtrVar(i) +#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j) +// end of #ifdef _EnumMode_ +#else +// ***** Standard default mode ***** +#endif +#endif +#endif //_PP_PROJ_H_ diff --git a/python/usr_code/usralgomain.c b/python/usr_code/usralgomain.c new file mode 100644 index 0000000..3232aa8 --- /dev/null +++ b/python/usr_code/usralgomain.c @@ -0,0 +1,114 @@ +//--------------------------------------------------------------------------- +// Project PowerPMAC Firmware +// Delta Tau Data Systems, Inc. +// Copyright 2007. All Rights Reserved. +// +// SUBSYSTEM: User Algo Driver +// FILE: usralgo.c +// AUTH OR: Henry Bausley +// +// OVERVIEW +// ~~~~~~~~ +// This file is a device driver that exports functions that can be used by other +// kernel mode modules. ie. RtPmac.rtl can call functions that are part of this +// driver for use as user written servo, phase, plc rti, plc rt thread +// No one should ever have to touch this module! Custom routines are added +// as additional modules as whatever name the user wants. +// +// NOTES +// ~~~~~ +// - The driver must be loaded +// bash-3.0# insmod /opt/ppmac/usralgo/usralgo.ko +// +// - A node can be created on the file system the thing you fopen +// but it is not required. Could be used by gpos if ioctl's were added +// bash-3.0# mknod -m 666 /dev/usralgo c 240 0 +// +//-------------------------------------------------------------------------------- +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +//#define KDBEUG 1 + +#ifdef KDBEUG + #define PRINTK(arg) printk(arg) +#else + #define PRINTK(arg) +#endif + +#define USRALGO_MAJOR 240 +#define USRALGO_NAME "usralgo" + +struct SHM *pshm; // Pointer to shared memory +volatile unsigned *piom; // Pointer to I/O memory +void *pushm; // Pointer to user memory + + +// define which file operations are supported +struct file_operations usralgo_fops = +{ + .owner = THIS_MODULE, + .llseek = NULL, + .read = NULL, + .write = NULL, +// .readdir = NULL, + .poll = NULL, +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,0,0) + .ioctl = NULL, +#else + .unlocked_ioctl = NULL, +#endif + .mmap = NULL, + .open = NULL, + .flush = NULL, + .release = NULL, + .fsync = NULL, + .fasync = NULL, + .lock = NULL, +}; + +//----------------------------------------------------------------------- +// initialize module -- this what is called when you do insmod ./usralgo.ko +//----------------------------------------------------------------------- +static int __init usralgo_init_module (void) +{ + int i; + + PRINTK(KERN_INFO "initializing module usralgo\n"); + pshm = GetSharedMemPtr(); + pushm = GetUserBufferPtr(); + piom = GetIOMemPtr(); + + i = register_chrdev (USRALGO_MAJOR, USRALGO_NAME, &usralgo_fops); + if (i != 0) return - EIO; + + return 0; +} + +//----------------------------------------------------------------------- +// close and cleanup module +//------------------------- +static void __exit usralgo_cleanup_module (void) +{ + PRINTK("cleaning up module usralgo\n"); + unregister_chrdev (USRALGO_MAJOR, USRALGO_NAME); +} + +module_init(usralgo_init_module); +module_exit(usralgo_cleanup_module); +MODULE_AUTHOR("www.deltatau.com"); +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("Linux Device Driver for usralgo"); diff --git a/python/usr_code/usrcode_template.c b/python/usr_code/usrcode_template.c new file mode 100644 index 0000000..f96d502 --- /dev/null +++ b/python/usr_code/usrcode_template.c @@ -0,0 +1,117 @@ +//--------------------------------------------------------------------------- +// 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 + + + +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/python/usr_code/usrcode_template.h b/python/usr_code/usrcode_template.h new file mode 100644 index 0000000..fbe2e73 --- /dev/null +++ b/python/usr_code/usrcode_template.h @@ -0,0 +1,134 @@ +#ifdef __KERNEL__ +#include +#else +#define EXPORT_SYMBOL(x) // x +#define KERN_ALERT +#define printk printf +#include +#endif +#include // Global Rt/Gp Externals and structures +#include + +int rtsprintf(char * buf, const char *fmt, ...); + + + +double usr_servo_sample(MotorData *Mptr); +EXPORT_SYMBOL(usr_servo_sample); + +void user_phase_sample( struct MotorData *Mptr); +EXPORT_SYMBOL(user_phase_sample); + +void CaptCompISR(void); +EXPORT_SYMBOL(CaptCompISR); + +double CfromScript(double arg1,double arg2,double arg3,double arg4,double arg5,double arg6,double arg7,struct LocalData *Ldata); +EXPORT_SYMBOL(CfromScript); + +//---------------------------------------- +// Required Kinematic define Names +//---------------------------------------- +#define KinPosMotor L +#define KinVelMotor R +#define KinEnaAxisA 0x1 +#define KinPosAxisA C[0] +#define KinVelAxisA C[32] +#define KinEnaAxisB 0x2 +#define KinPosAxisB C[1] +#define KinVelAxisB C[33] +#define KinEnaAxisC 0x4 +#define KinPosAxisC C[2] +#define KinVelAxisC C[34] +#define KinEnaAxisU 0x8 +#define KinPosAxisU C[3] +#define KinVelAxisU C[35] +#define KinEnaAxisV 0x10 +#define KinPosAxisV C[4] +#define KinVelAxisV C[36] +#define KinEnaAxisW 0x20 +#define KinPosAxisW C[5] +#define KinVelAxisW C[37] +#define KinEnaAxisX 0x40 +#define KinPosAxisX C[6] +#define KinVelAxisX C[38] +#define KinEnaAxisY 0x80 +#define KinPosAxisY C[7] +#define KinVelAxisY C[39] +#define KinEnaAxisZ 0x100 +#define KinPosAxisZ C[8] +#define KinVelAxisZ C[40] +#define KinEnaAxisAA 0x200 +#define KinPosAxisAA C[9] +#define KinVelAxisAA C[41] +#define KinEnaAxisBB 0x400 +#define KinPosAxisBB C[10] +#define KinVelAxisBB C[42] +#define KinEnaAxisCC 0x800 +#define KinPosAxisCC C[11] +#define KinVelAxisCC C[43] +#define KinEnaAxisDD 0x1000 +#define KinPosAxisDD C[12] +#define KinVelAxisDD C[44] +#define KinEnaAxisEE 0x2000 +#define KinPosAxisEE C[13] +#define KinVelAxisEE C[45] +#define KinEnaAxisFF 0x4000 +#define KinPosAxisFF C[14] +#define KinVelAxisFF C[46] +#define KinEnaAxisGG 0x8000 +#define KinPosAxisGG C[15] +#define KinVelAxisGG C[47] +#define KinEnaAxisHH 0x10000 +#define KinPosAxisHH C[16] +#define KinVelAxisHH C[48] +#define KinEnaAxisLL 0x20000 +#define KinPosAxisLL C[17] +#define KinVelAxisLL C[49] +#define KinEnaAxisMM 0x40000 +#define KinPosAxisMM C[18] +#define KinVelAxisMM C[50] +#define KinEnaAxisNN 0x80000 +#define KinPosAxisNN C[19] +#define KinVelAxisNN C[51] +#define KinEnaAxisOO 0x100000 +#define KinPosAxisOO C[20] +#define KinVelAxisOO C[52] +#define KinEnaAxisPP 0x200000 +#define KinPosAxisPP C[21] +#define KinVelAxisPP C[53] +#define KinEnaAxisQQ 0x400000 +#define KinPosAxisQQ C[22] +#define KinVelAxisQQ C[54] +#define KinEnaAxisRR 0x800000 +#define KinPosAxisRR C[23] +#define KinVelAxisRR C[55] +#define KinEnaAxisSS 0x1000000 +#define KinPosAxisSS C[24] +#define KinVelAxisSS C[56] +#define KinEnaAxisTT 0x2000000 +#define KinPosAxisTT C[25] +#define KinVelAxisTT C[57] +#define KinEnaAxisUU 0x4000000 +#define KinPosAxisUU C[26] +#define KinVelAxisUU C[58] +#define KinEnaAxisVV 0x8000000 +#define KinPosAxisVV C[27] +#define KinVelAxisVV C[59] +#define KinEnaAxisWW 0x10000000 +#define KinPosAxisWW C[28] +#define KinVelAxisWW C[60] +#define KinEnaAxisXX 0x20000000 +#define KinPosAxisXX C[29] +#define KinVelAxisXX C[61] +#define KinEnaAxisYY 0x40000000 +#define KinPosAxisYY C[30] +#define KinVelAxisYY C[62] +#define KinEnaAxisZZ 0x80000000 +#define KinPosAxisZZ C[31] +#define KinVelAxisZZ C[63] +#define KinAxisUsed D[0] +#define KinVelEna D[0] +//---------------------------------------- + + +