usr_servo code generation

2018-11-02 16:56:25 +01:00
parent 8807ccbd85
commit d7e0cf7764
12 changed files with 863 additions and 67 deletions
--- 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);


 ```
--- 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


--- 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;
--- a/matlab/observer.slx
+++ b/matlab/observer.slx
--- 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;
--- a/matlab/stage_closed_loop.slx
+++ b/matlab/stage_closed_loop.slx
--- 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('<usr_header>',hdr1+'\n\n'+hdr2)
+      fh=open(fnh,'w')
+      fh.write(s)
+      fh.close()
+      print(fnh+' generated.')
+      s=open(fn_ct).read()
+      s=s.replace('<usr_code>',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)

--- a/python/usr_code/Makefile
+++ 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)
--- a/python/usr_code/pp_proj.h
+++ 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_
--- a/python/usr_code/usralgomain.c
+++ 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 <linux/module.h>
+#include <linux/init.h>
+#include <linux/version.h>
+#include <linux/errno.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/interrupt.h>
+#include <linux/sched.h>
+#include <asm/uaccess.h>
+#include <asm/io.h>
+#include <linux/vmalloc.h>
+#include <linux/mman.h>
+#include <linux/slab.h>
+#include <RtGpShm.h>
+
+
+//#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");
--- a/python/usr_code/usrcode_template.c
+++ 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
+
+<usr_code>
+
+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;
+}
+
--- a/python/usr_code/usrcode_template.h
+++ b/python/usr_code/usrcode_template.h
@@ -0,0 +1,134 @@
+#ifdef __KERNEL__
+#include <linux/module.h>
+#else
+#define EXPORT_SYMBOL(x) // x
+#define KERN_ALERT
+#define printk printf
+#include <gplib.h>
+#endif
+#include <RtGpShm.h>	// Global Rt/Gp Externals and structures
+#include <rtpmacapi.h>
+
+int rtsprintf(char * buf, const char *fmt, ...);
+
+<usr_header>
+
+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]
+//----------------------------------------
+
+
+