usr_servo code generation
This commit is contained in:
@@ -26,12 +26,10 @@ clear;
|
|||||||
clear global;
|
clear global;
|
||||||
close all;
|
close all;
|
||||||
[mot1,mot2]=identifyFxFyStage();
|
[mot1,mot2]=identifyFxFyStage();
|
||||||
[pb]=simFxFyStage(mot1,1);
|
[pb]=simFxFyStage(mot1);
|
||||||
[ssc]=StateSpaceControlDesign(mot1,1);
|
[ssc]=StateSpaceControlDesign(mot1);
|
||||||
|
[pb]=simFxFyStage(mot2);
|
||||||
[mot1,mot2]=identifyFxFyStage();
|
[ssc]=StateSpaceControlDesign(mot2);
|
||||||
[pb]=simFxFyStage(mot2,2);
|
|
||||||
[ssc]=StateSpaceControlDesign(mot2,2);
|
|
||||||
|
|
||||||
|
|
||||||
```
|
```
|
||||||
|
|||||||
@@ -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 !!!
|
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
|
||||||
%
|
%
|
||||||
% builds a state space controller designed for the plant.
|
% 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
|
% https://www.youtube.com/watch?v=Lax3etc837U
|
||||||
|
|
||||||
%ss_ol=mot.ssPlt;
|
%ss_ol=mot.ssPlt;
|
||||||
ss_ol_plt=mot.ssPlt; %real plant (model of real plant)
|
ssPlt=mot.ssPlt; %real plant (model of real plant)
|
||||||
ss_ol_plt.Name='open loop plant';
|
ssPlt.Name='open loop plant';
|
||||||
ss_ol_mdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
|
ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
|
||||||
ss_ol_mdl.Name='open loop model';
|
ssMdl.Name='open loop model';
|
||||||
|
|
||||||
[Ap,Bp,Cp,Dp]=ssdata(ss_ol_plt);
|
[Ap,Bp,Cp,Dp]=ssdata(ssPlt);
|
||||||
[Am,Bm,Cm,Dm]=ssdata(ss_ol_mdl);
|
[Am,Bm,Cm,Dm]=ssdata(ssMdl);
|
||||||
|
|
||||||
|
|
||||||
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
|
%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')
|
setoptions(h,'IOGrouping','all')
|
||||||
|
|
||||||
% step answer on open loop:
|
% step answer on open loop:
|
||||||
@@ -37,8 +37,8 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
|
|||||||
xp0 = zeros(1,length(Ap));
|
xp0 = zeros(1,length(Ap));
|
||||||
xm0 = zeros(1,length(Am));
|
xm0 = zeros(1,length(Am));
|
||||||
|
|
||||||
[yp,t,x] = lsim(ss_ol_plt,u,t,xp0);
|
[yp,t,x] = lsim(ssPlt,u,t,xp0);
|
||||||
[ym,t,x] = lsim(ss_ol_mdl,u,t,xm0);
|
[ym,t,x] = lsim(ssMdl,u,t,xm0);
|
||||||
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
|
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')
|
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 ***
|
% *** space state controller ***
|
||||||
%
|
%
|
||||||
%place poles for the controller feedback
|
%place poles for the controller feedback
|
||||||
if motid==1
|
use_lqr=0;
|
||||||
%2500rad/s = 397Hz -> locate poles here
|
if use_lqr %use the lqr controller
|
||||||
%6300rad/s = 1027Hz -> locate poles here
|
Q=eye(length(ssMdl.A));
|
||||||
if length(poles)==4
|
R=1;
|
||||||
p1=-6300+2800i;
|
[K,P,E]=lqr(ssMdl,Q,R,0);
|
||||||
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
|
|
||||||
else
|
else
|
||||||
%2500rad/s = 397Hz -> locate poles here
|
if mot.id==1
|
||||||
%6300rad/s = 1027Hz -> locate poles here
|
%2500rad/s = 397Hz -> locate poles here
|
||||||
if length(poles)==4
|
%6300rad/s = 1027Hz -> locate poles here
|
||||||
p1=-6300+2800i;
|
if length(poles)==4
|
||||||
p2=-6200+1500i;
|
p1=-6300+280i;
|
||||||
P=[p1 p1' p2 p2'];
|
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
|
else
|
||||||
p1=-3300+2800i;
|
%2500rad/s = 397Hz -> locate poles here
|
||||||
p2=-1900+130i;
|
%6300rad/s = 1027Hz -> locate poles here
|
||||||
p3=-2900+80i;
|
if length(poles)==4
|
||||||
p4=-2300+450i;
|
p1=-6300+2800i;
|
||||||
p5=-2000+20i;
|
p2=-6200+1500i;
|
||||||
p6=-1500+10i;
|
P=[p1 p1' p2 p2'];
|
||||||
%p1=-3300+2800i;
|
else
|
||||||
%p2=-1500+500i;
|
p1=-3300+2800i;
|
||||||
%p3=-1200+10i;
|
p2=-1900+130i;
|
||||||
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
|
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
|
||||||
end
|
K = place(Am,Bm,P);
|
||||||
K = place(Am,Bm,P);
|
%K = acker(Am,Bm,Pm);
|
||||||
%K = acker(Am,Bm,Pm);
|
end %if lqr
|
||||||
|
|
||||||
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
|
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
|
||||||
%Nbar(2)=1; %the voltage stuff is crap for now
|
%Nbar(2)=1; %the voltage stuff is crap for now
|
||||||
if length(V)>1
|
if length(V)>1
|
||||||
@@ -110,7 +118,7 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
|
|||||||
% *** observer controller ***
|
% *** observer controller ***
|
||||||
%
|
%
|
||||||
%observer poles-> 5 times farther left than system poles
|
%observer poles-> 5 times farther left than system poles
|
||||||
if motid==1
|
if mot.id==1
|
||||||
op1=(p1*5);
|
op1=(p1*5);
|
||||||
op2=(p2*5);
|
op2=(p2*5);
|
||||||
if length(poles)>4
|
if length(poles)>4
|
||||||
@@ -143,27 +151,27 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
|
|||||||
Ct = [ Cm zeros(size(Cm)) ];
|
Ct = [ Cm zeros(size(Cm)) ];
|
||||||
|
|
||||||
% step answer on closed loop with observer controller:
|
% step answer on closed loop with observer controller:
|
||||||
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
|
ss_t = 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');
|
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
|
||||||
|
|
||||||
|
|
||||||
% *** disctrete observer controller ***
|
% *** disctrete observer controller ***
|
||||||
%
|
%
|
||||||
Ts=1/5000; % 5kHz
|
Ts=1/5000; % 5kHz
|
||||||
ss_od = c2d(ss_o,Ts);
|
ss_tz = c2d(ss_t,Ts);
|
||||||
ss_od .Name='discrete obsvr ctrl';
|
ss_tz .Name='discrete obsvr ctrl';
|
||||||
% step answer on closed loop with disctrete observer controller:
|
% step answer on closed loop with disctrete observer controller:
|
||||||
t = 0:Ts:.05;
|
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
|
%plot all bode diagrams of desPos->actPos
|
||||||
figure();
|
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');
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
||||||
|
|
||||||
figure();
|
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');
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
||||||
|
|
||||||
|
|
||||||
@@ -172,15 +180,25 @@ function [ssc]=StateSpaceControlDesign(mot,motid)
|
|||||||
Bo=[Bm L];
|
Bo=[Bm L];
|
||||||
Co=K;
|
Co=K;
|
||||||
Do=zeros(size(Co,1),size(Bo,2));
|
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';
|
mdlName='observer';
|
||||||
open(mdlName);
|
open(mdlName);
|
||||||
|
|
||||||
|
|
||||||
|
ssPltz = c2d(ssPlt,Ts);
|
||||||
|
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
|
||||||
|
|
||||||
|
|
||||||
%state space controller
|
%state space controller
|
||||||
ssc=struct();
|
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));
|
ssc=setfield(ssc,k,eval(k));
|
||||||
end
|
end
|
||||||
|
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -101,7 +101,9 @@ function [mot1,mot2]=identifyFxFyStage()
|
|||||||
end
|
end
|
||||||
|
|
||||||
function mot=fyStage()
|
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);
|
mot.tfc=currstep(mot);
|
||||||
|
|
||||||
opt=tfestOptions;
|
opt=tfestOptions;
|
||||||
@@ -171,7 +173,9 @@ function [mot1,mot2]=identifyFxFyStage()
|
|||||||
end
|
end
|
||||||
|
|
||||||
function mot=fxStage()
|
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);
|
currstep(mot);
|
||||||
|
|
||||||
opt=tfestOptions;
|
opt=tfestOptions;
|
||||||
|
|||||||
Binary file not shown.
@@ -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 !!!
|
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
|
||||||
%
|
%
|
||||||
%loads the current (11.10.2018) controller settings of the
|
%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
|
Ts=2E-4; % 0.2ms=5kHz
|
||||||
MaxDac=2011.968;
|
MaxDac=2011.968;
|
||||||
MaxPosErr=10000;
|
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_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')
|
%!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;
|
Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
|
||||||
|
|||||||
Binary file not shown.
@@ -15,6 +15,8 @@ Modes:
|
|||||||
4 plot the full bode recording
|
4 plot the full bode recording
|
||||||
5 plot the full bode recording with an approximation model
|
5 plot the full bode recording with an approximation model
|
||||||
6 plot all raw acquired data files
|
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
|
-> 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
|
||||||
@@ -201,6 +203,90 @@ class MXTuning(Tuning):
|
|||||||
ax.semilogx(f, phase,'-k',lw=1) # Bode phase plot
|
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'))
|
# 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):
|
def bode(mdl):
|
||||||
w,mag,phase = signal.bode(mdl,1000)
|
w,mag,phase = signal.bode(mdl,1000)
|
||||||
@@ -250,6 +336,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
|
||||||
if args.host is None:
|
if args.host is None:
|
||||||
comm=gt=None
|
comm=gt=None
|
||||||
else:
|
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=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=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')
|
#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()
|
plt.show()
|
||||||
#------------------ Main Code ----------------------------------
|
#------------------ Main Code ----------------------------------
|
||||||
#ssh_test()
|
#ssh_test()'/tmp/usrcode.c'
|
||||||
ret=parse_args()
|
ret=parse_args()
|
||||||
exit(ret)
|
exit(ret)
|
||||||
|
|
||||||
|
|||||||
103
python/usr_code/Makefile
Normal file
103
python/usr_code/Makefile
Normal file
@@ -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)
|
||||||
189
python/usr_code/pp_proj.h
Executable file
189
python/usr_code/pp_proj.h
Executable file
@@ -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_
|
||||||
114
python/usr_code/usralgomain.c
Normal file
114
python/usr_code/usralgomain.c
Normal file
@@ -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");
|
||||||
117
python/usr_code/usrcode_template.c
Normal file
117
python/usr_code/usrcode_template.c
Normal file
@@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
134
python/usr_code/usrcode_template.h
Normal file
134
python/usr_code/usrcode_template.h
Normal file
@@ -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]
|
||||||
|
//----------------------------------------
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Reference in New Issue
Block a user