friction compensating user servo
This commit is contained in:
@@ -39,6 +39,7 @@
|
||||
$$$***
|
||||
//!common()
|
||||
!common(PhaseFreq=20000,PhasePerServo=4)
|
||||
//!common(PhaseFreq=20000,PhasePerServo=1)
|
||||
//!common(PhaseFreq=40000)
|
||||
|
||||
!torqueCtrl()
|
||||
|
||||
@@ -77,6 +77,8 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a
|
||||
!encoder_sim(enc=3,tbl=11,mot=11,posSf=13000./2048)
|
||||
!encoder_inc(enc=3,tbl=3,mot=3,posSf=13000./650000)
|
||||
!motor_servo(mot=3,ctrl='ServoCtrl',Kp=10,Kvfb=220,Ki=0.001,Kvff=240,Kaff=0,MaxInt=1000)
|
||||
//PhaseFreq=20000,PhasePerServo=1
|
||||
//!motor_servo(mot=3,ctrl='ServoCtrl',Kp=10,Kvfb=220*4,Ki=0.001/4,Kvff=240*4,Kaff=0*4*4,MaxInt=1000)
|
||||
!motor(mot=3,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=1,IpfGain=0,IpbGain=2,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=2000,WarnFeLimit=100,InPosBand=2)
|
||||
|
||||
|
||||
|
||||
27
logbook.md
27
logbook.md
@@ -461,4 +461,29 @@ ic=-sin(phi-240deg)*iq
|
||||
|
||||
...cant yet figure out the scaling...
|
||||
|
||||
measure the mass of the stage... c ant figure it out...
|
||||
|
||||
Measuring position depenmdent friction(=current)
|
||||
------------------------------------------------
|
||||
./move_record.py cfg = {"sequencer": ['prog_1(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_1()']}
|
||||
|
||||
Avg current forward: 37.2596025196 Avg current backward: -68.9828819531
|
||||
lut [[ 1.00000000e+03 -2.70746096e+01]
|
||||
[ 1.31399384e+03 -3.76975094e+01]
|
||||
[ 1.71398707e+03 -5.78253228e+01]
|
||||
...
|
||||
[ 2.57135808e+04 -1.09363816e+01]
|
||||
[ 2.61135740e+04 -1.42231517e+01]
|
||||
[ 2.65135673e+04 -1.19655511e+01]
|
||||
[ 2.69135605e+04 -6.38521106e+01]]
|
||||
|
||||
|
||||
|
||||
|
||||
Python to Matlab
|
||||
----------------
|
||||
|
||||
import numpy as np, scipy.io
|
||||
fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/prog_1.npz'
|
||||
fh=np.load(fn)
|
||||
scipy.io.savemat(fn[:-3]+'mat',mdict=dict(fh.iteritems()))
|
||||
|
||||
|
||||
@@ -14,37 +14,9 @@ verbose bits:
|
||||
4 upload progress
|
||||
8 plot gather path
|
||||
|
||||
#config file example:
|
||||
{
|
||||
"points": [
|
||||
[100,523],[635,632],[756,213],
|
||||
"sequencer":[
|
||||
'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)',
|
||||
'sort_points()',
|
||||
'gen_prog(file="'+fn+'.prg")',
|
||||
'plot_gather("'+fn+'.npz")']
|
||||
}
|
||||
|
||||
Sequencer functions are:
|
||||
- generate points (if not in the 'points' configuration)
|
||||
gen_rand_points(self,n=107,scale=1000)
|
||||
gen_grid_points(w=10,h=10,pitch=100,rnd=.2)
|
||||
- sorting points:
|
||||
sort_points(self)
|
||||
- generate/download/execute motion progran, upload trace of motors (gather data)
|
||||
gen_prog(self,prgId=2,file=None,host=None)
|
||||
if host=None nothing will be downloaded/executed and trace of motors will not be uploaded
|
||||
if file=None the program will not be saved and nothing will be executed
|
||||
gen_prog modes:
|
||||
-1 jog a 10mm square
|
||||
0 linear motion
|
||||
1 pvt motion
|
||||
2 spline motion
|
||||
|
||||
|
||||
- plot gathered data
|
||||
plot_gather("'+fn+'.npz")
|
||||
this makes only sence, if motion has been executed and data can be gathered from the powerbrick
|
||||
prog_1(),plot_1(): position dependent friction(=current) Move multiple times with different speeds
|
||||
generate friction lut and save to friction.mat
|
||||
prog_2(),plot_2(): plot position speed acceleration. Move multiple times with different accelerations
|
||||
|
||||
Acquired time is:MaxSamples*Period*.2
|
||||
'''
|
||||
@@ -65,8 +37,8 @@ class MoveRecord:
|
||||
self.fnPrg=fn+'.prg'
|
||||
self.fnNpz=fn+'.npz'
|
||||
#cfg = {"sequencer": ['prog_1(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_1()']}
|
||||
cfg = {"sequencer": ['plot_1()']}
|
||||
#cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
|
||||
#cfg = {"sequencer": ['plot_1()']}
|
||||
cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
|
||||
#cfg = {"sequencer": ['plot_2()']}
|
||||
self.cfg=dotdict(cfg)
|
||||
self.args=args
|
||||
@@ -158,19 +130,22 @@ class MoveRecord:
|
||||
prg=self.prg
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('jog3=1000')
|
||||
prg.append('jog3=100')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=2')
|
||||
#for spd in (5,10,20,30,40):
|
||||
for spd in (20,20,20,20,20,20,20,20,20,20):
|
||||
for spd in (10,10,10,10,20,20,20,20,20,20):
|
||||
prg.append('Motor[3].JogSpeed=%d'%spd)
|
||||
prg.append('jog3=27000')
|
||||
#prg.append('jog3=27000')
|
||||
prg.append('jog3=28500')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog3=1000')
|
||||
prg.append('jog3=100')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=0')
|
||||
|
||||
meta['HomePos']=-4885.72 #set the value of Motor[3].HomePos
|
||||
self.prg_epilog()
|
||||
#!mx-stage()
|
||||
# &1 #1->0
|
||||
# &1 #2->0
|
||||
# #3$
|
||||
@@ -181,11 +156,6 @@ class MoveRecord:
|
||||
# &1p
|
||||
|
||||
|
||||
#folgende parameter setzen: Motor[3].HomePos
|
||||
meta['HomePos']=-223.8 #Motor[3].HomePos
|
||||
meta['PhasePos']=1200 #Motor[3].PhasePos
|
||||
|
||||
|
||||
@staticmethod
|
||||
def onclick(event):
|
||||
print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
|
||||
@@ -206,6 +176,11 @@ class MoveRecord:
|
||||
channels=list(fh['channels'])
|
||||
meta=fh['meta'].item()
|
||||
|
||||
actPos=rec[:,0]-meta['HomePos']
|
||||
desPos=rec[:,1]-meta['HomePos']
|
||||
desVel=np.diff(desPos)
|
||||
|
||||
|
||||
time=np.arange(0,rec.shape[0])*meta['timebase']
|
||||
|
||||
fig=plt.figure();
|
||||
@@ -213,8 +188,8 @@ class MoveRecord:
|
||||
fig.canvas.set_window_title('position')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
hl+=ax.plot(time,rec[:,0],'r-',label=channels[0])
|
||||
hl+=ax.plot(time,rec[:,1],'g-',label=channels[1])
|
||||
hl+=ax.plot(time,actPos,'r-',label=channels[0])
|
||||
hl+=ax.plot(time,desPos,'g-',label=channels[1])
|
||||
hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
|
||||
ax.xaxis.set_label_text('ms')
|
||||
ax.yaxis.set_label_text('um')
|
||||
@@ -237,14 +212,55 @@ class MoveRecord:
|
||||
|
||||
fig=plt.figure();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
fig.canvas.set_window_title('overlay')
|
||||
fig.canvas.set_window_title('friction(=current) at position current')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
sz=10; weights = np.repeat(1.0, sz) / sz
|
||||
curr = np.convolve(rec[:,4], weights, 'same')
|
||||
|
||||
v = np.convolve(rec[:,4], weights, 'same')
|
||||
hl+=ax.plot(rec[:,1],v,'r-',label=channels[4])
|
||||
ax.xaxis.set_label_text(channels[1])
|
||||
hl+=ax.plot(desPos,curr,'y-',label=channels[4])
|
||||
|
||||
sz=100; weights = np.repeat(1.0, sz) / sz
|
||||
|
||||
idx=desVel[:-1]>0
|
||||
arg=np.argsort(desPos[idx])
|
||||
p1=desPos[idx][arg];c1=curr[idx][arg]
|
||||
c1 = np.convolve(c1, weights, 'same')
|
||||
|
||||
idx=desVel[:-1]<0
|
||||
arg=np.argsort(desPos[idx])
|
||||
p2=desPos[idx][arg];c2=curr[idx][arg]
|
||||
c2 = np.convolve(c2, weights, 'same')
|
||||
|
||||
hl+=ax.plot(p1,c1,'g-',label=channels[4]+' vel>0')
|
||||
hl+=ax.plot(p2,c2,'r-',label=channels[4]+' lut')
|
||||
#hl+=ax.plot(p1,c1-c1.mean(),'g-',label=channels[4]+' vel>0')
|
||||
#hl+=ax.plot(p2,c2-c2.mean(),'r-',label=channels[4]+' vel<0')
|
||||
|
||||
cAll=c1;pAll=p1
|
||||
cAll=cAll-cAll.mean()
|
||||
#p=p[::100];c=c[::100] # poor selection of points
|
||||
p=np.arange(500,28000,100) #np.arange(500,28000,128)
|
||||
c=np.interp(p,pAll,cAll)
|
||||
|
||||
hl+=ax.plot(p,c,'b.',label=channels[4]+' vel<0')
|
||||
FfricLut=np.array([p,c]).T
|
||||
|
||||
Ffric=np.array([c1.mean(),c2.mean()])
|
||||
print 'Avg current forward:',Ffric[0],'Avg current backward:',Ffric[1]
|
||||
#print 'FfricLut',FfricLut
|
||||
print '//positions '+'%g,%g,%g'%tuple(p[0:3]),'...%g,%g,%g'%tuple(p[-3:])
|
||||
print 'float lutCur[%d]={'%len(p)
|
||||
for i in range(len(p)):
|
||||
print '%g,'%(c[i]),
|
||||
print '};'
|
||||
import scipy.io
|
||||
fn='/home/zamofing_t/afs/ESB-MX/data/friction.mat'
|
||||
#scipy.io.savemat(fn,mdict={'Ffric':Ffric,'FfricLut':FfricLut})
|
||||
print '\n\nuncomment line above to saved to matlab file',fn
|
||||
|
||||
|
||||
#ax.xaxis.set_label_text(channels[1])
|
||||
ax.yaxis.set_label_text('current in bits: '+channels[4])
|
||||
legend = ax.legend(loc='upper right', shadow=True)
|
||||
|
||||
@@ -276,17 +292,20 @@ class MoveRecord:
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('Motor[3].JogTs=0')
|
||||
prg.append('Motor[3].JogSpeed=40')
|
||||
prg.append('Motor[3].JogSpeed=10')
|
||||
prg.append('Motor[3].JogTa=-2.5')
|
||||
|
||||
prg.append('jog3=10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=2')
|
||||
for acc in (-2.5,-1.25,-.5,-.25):
|
||||
prg.append('Motor[3].JogTa=%g'%acc)
|
||||
for spd in (5,10,20,30,40):
|
||||
prg.append('Motor[3].JogSpeed=%d'%spd)
|
||||
#for acc in (-2.5,-1.25,-.5,-.25):
|
||||
# prg.append('Motor[3].JogTa=%g'%acc)
|
||||
prg.append('jog3=17000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog3=10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('dwell 1000')
|
||||
prg.append('Gather.Enable=0')
|
||||
|
||||
self.prg_epilog()
|
||||
@@ -295,7 +314,7 @@ class MoveRecord:
|
||||
# #3$
|
||||
# an anschlag (kabel) bewegen
|
||||
# #3hmz
|
||||
# Motor[3].PhasePos=1200
|
||||
# Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300
|
||||
# #3j/
|
||||
# &1p
|
||||
|
||||
@@ -335,10 +354,10 @@ class MoveRecord:
|
||||
|
||||
p=rec[:,1]-p_avg
|
||||
hl+=ax.plot(time,p,'g-',label=channels[1])
|
||||
d=np.diff(p)*100
|
||||
hl+=ax.plot(time[1:],d,'g-',label='vel of '+channels[0])
|
||||
a=np.diff(d)*100
|
||||
hl+=ax.plot(time[2:],a,'g-',label='acc of '+channels[0])
|
||||
d=np.diff(p)*50
|
||||
hl+=ax.plot(time[1:],d,'b-',label='vel of '+channels[0])
|
||||
a=np.diff(d)*50
|
||||
hl+=ax.plot(time[2:],a,'m-',label='acc of '+channels[0])
|
||||
|
||||
#hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
|
||||
ax.xaxis.set_label_text('ms')
|
||||
@@ -355,10 +374,14 @@ class MoveRecord:
|
||||
hl+=ax.plot(time,v,'r-',label=channels[3])
|
||||
v = np.convolve(rec[:, 4], weights, 'same')
|
||||
hl+=ax.plot(time,v,'g-',label=channels[4])
|
||||
hl+=ax.plot(time,p/100.,'y-',label=channels[1])
|
||||
hl+=ax.plot(time,(rec[:,0]-p_avg)/100.,'g-',label=channels[0])
|
||||
hl+=ax.plot(time,(rec[:,0]-rec[:,1]),'c-',label='PosError')
|
||||
ax.xaxis.set_label_text('ms')
|
||||
ax.yaxis.set_label_text('current in bits')
|
||||
legend = ax.legend(loc='upper right', shadow=True)
|
||||
print 'abs average ',channels[4], np.abs(rec[:, 4]).mean()
|
||||
print 'abs average pos Error',np.abs(rec[:,0]-rec[:,1]).mean()
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
@@ -1,5 +1,8 @@
|
||||
# This section loads all Power PMAC programs & is run through the pre-processor/CmdProcessor. It is a sample project default load.
|
||||
[RTUSRCCODE]
|
||||
usrrtmodule=/tmp/usralgo.ko
|
||||
ServoCtrlName4=user_pid_ctrl
|
||||
|
||||
#ServoCtrlName2=usr_servo_ctrl_2
|
||||
ServoCtrlName3=usr_servo_ctrl_3
|
||||
#ServoCtrlName4=user_pid_ctrl
|
||||
|
||||
|
||||
@@ -47,10 +47,81 @@ extern struct SHM *pshm; // Pointer to shared memory
|
||||
extern volatile unsigned *piom; // Pointer to I/O memory
|
||||
extern void *pushm; // Pointer to user memory
|
||||
|
||||
double compensate_friction_servo_ctrl(MotorData *Mptr, int motID);
|
||||
|
||||
void user_phase( struct MotorData *Mptr)
|
||||
{
|
||||
}
|
||||
|
||||
double usr_servo_ctrl_2(MotorData *Mptr)
|
||||
{
|
||||
return pshm->ServoCtrl(Mptr);
|
||||
}
|
||||
|
||||
//friction MX-stage motor 3
|
||||
//Avg current forward: 28.2405057332 Avg current backward: -61.2462872333
|
||||
//positions 500,600,700 ...27700,27800,27900
|
||||
float lutFric[275]={
|
||||
-7.1551, -12.6559, -21.4081, -30.2418, -35.1982, -36.4386, -37.2831, -38.4421, -39.7832, -39.7141, -37.7057, -33.7864, -32.3466, -34.7608, -37.4658, -36.992, -35.7382, -37.1257, -39.5308, -41.5, -43.0684, -44.4203, -45.678, -47.696, -49.1327, -48.3342, -47.32, -47.6388, -47.6858, -43.7855, -37.843, -32.5668, -28.0956, -24.7008, -21.593, -17.8392, -13.6763, -9.10309, -5.04866, -1.3417, 1.9566, 4.49383, 6.35785, 9.39698, 15.521, 23.7353, 29.7179, 32.9107, 37.6373, 43.2183, 46.9492, 48.7774, 52.098, 57.1212, 60.8518, 61.575, 60.2766, 56.885, 51.2957, 43.5279, 33.8489, 22.2905, 7.2842, -9.01961, -23.3673, -33.5445, -40.3342, -46.3352, -51.7987, -55.6054, -58.5937, -62.3993, -66.5449, -67.4299, -65.8841, -64.9919, -66.0021, -65.5962, -62.5602, -58.2791, -54.7651, -53.3157, -52.0205, -48.1133, -42.9703, -37.9826, -33.8855, -29.9032, -24.1226, -16.7909, -9.89502, -2.63174, 5.23116, 11.8609, 15.8984, 20.6177, 27.263, 32.7512, 36.4521, 40.1571, 44.8302, 48.2186, 49.4304, 47.9396, 45.5347, 43.824, 43.1741, 41.7656, 39.9741, 38.9437, 38.9356, 39.0903, 39.6, 41.1204, 40.9819, 39.7462, 39.0561, 39.0774, 37.3204, 33.627, 31.4442, 30.3062, 26.8336, 19.1632, 9.99558, 2.36922, -4.35166, -12.2266, -21.8815, -30.2799, -35.5315, -38.337, -41.0337, -42.5536, -41.8356, -38.9385, -36.9616, -36.4229, -35.8889, -34.1475, -31.3523, -28.1945, -23.8853, -20.2734, -20.0726, -22.4272, -22.0745, -18.0298, -14.7222, -15.2457, -17.8645, -19.2889, -21.4733, -23.6443, -24.7379, -23.4422, -21.5436, -20.6753, -19.3729, -16.2246, -11.419, -7.87291, -5.41278, -1.40435, 3.3671, 4.66581, 2.70959, 2.15455, 3.87838, 4.97318, 4.11034, 4.4132, 6.59851, 10.736, 15.2254, 18.5329, 19.8407, 20.1992, 21.777, 24.5145, 26.9981, 28.7539, 30.8099, 32.6786, 34.7331, 36.3433, 35.6326, 31.7121, 26.2285, 21.5035, 17.4354, 9.14015, -2.01504, -12.9646, -21.3099, -28.7052, -35.4772, -41.5508, -46.6105, -48.9102, -50.1711, -51.4111, -52.8876, -51.1254, -47.2276, -42.8323, -37.7037, -32.1169, -27.9526, -24.7178, -20.2562, -15.264, -11.7584, -9.64182, -6.72713, -2.17278, 1.20068, 4.02159, 8.10266, 14.9257, 21.8962, 27.2483, 30.9286, 37.2023, 46.0579, 55.1473, 61.4506, 64.6756, 67.6285, 71.5098, 75.4504, 77.3359, 76.9234, 75.733, 73.8723, 71.3198, 70.6438, 71.6276, 71.2124, 68.2844, 65.7515, 65.9122, 65.9395, 66.131, 67.2553, 69.1219, 69.3801, 66.8238, 63.5434, 60.4531, 58.2197, 55.5132, 51.7531, 47.1773, 43.2781, 37.2329, 29.4104, 22.4017, 18.0676, 13.1263, 7.18284, 2.79397, 0.246639, -3.82759, -9.64515, -13.2226, -13.5821, -11.6253, -9.90048, -9.94788, -12.5784, -14.697, -13.6629, -10.2785, -8.82145, };
|
||||
|
||||
double usr_servo_ctrl_3(MotorData *Mptr)
|
||||
{
|
||||
//compensate_friction_servo_ctrl for motor 3
|
||||
static int cnt=0;
|
||||
float p,c;
|
||||
int idx;
|
||||
double ctrl_out;
|
||||
|
||||
p=(Mptr->ActPos-Mptr->HomePos-500.f)/100.f;
|
||||
//idx=(int)floor(p);
|
||||
idx=(int)p;
|
||||
if (idx<0)
|
||||
c=lutFric[0];
|
||||
else if (idx>273)
|
||||
c=lutFric[274];
|
||||
else
|
||||
c=lutFric[idx]+(lutFric[idx+1]-lutFric[idx])*(p-idx);
|
||||
|
||||
c=0.f;
|
||||
if(Mptr->DesVel>0)
|
||||
c+=28.2405057332f;
|
||||
//c+=45.f;
|
||||
else if (Mptr->DesVel<0)
|
||||
c-=61.2462872333f;
|
||||
//c+=45.f;
|
||||
else
|
||||
c+=(28.2f-61.2f)/2.f;
|
||||
|
||||
// The default servo algo.
|
||||
ctrl_out=pshm->ServoCtrl(Mptr);
|
||||
|
||||
// Mptr->IdCmd = 34;
|
||||
//compensate friction
|
||||
ctrl_out+=c;
|
||||
|
||||
//if(ctrl_out>200)ctrl_out=200;
|
||||
//if(ctrl_out<-200)ctrl_out=-200;
|
||||
//if (cnt%10000 == 0 )
|
||||
//{
|
||||
// printk("usr_servo_ctrl_3 (%d) (%d)\n",cnt,idx);
|
||||
//}
|
||||
//cnt++;
|
||||
return ctrl_out;
|
||||
|
||||
}
|
||||
|
||||
// Your custom super duper filter probable more than incrementing a P variable
|
||||
//if(Mptr->DesVelZero == 1 && Mptr->InPos == 1) // not cmd to move and no FE when using external position encoder
|
||||
//{
|
||||
// pshm->P[1] = 1.0;
|
||||
// Mptr->IdCmd = 34;
|
||||
//}
|
||||
//else
|
||||
//{
|
||||
// pshm->P[1] = 0.0;
|
||||
// Mptr->IdCmd = 3400;
|
||||
//}
|
||||
|
||||
double user_pid_ctrl( struct MotorData *Mptr)
|
||||
{
|
||||
//double *p;
|
||||
|
||||
@@ -11,6 +11,12 @@
|
||||
|
||||
int rtsprintf(char * buf, const char *fmt, ...);
|
||||
|
||||
double usr_servo_ctrl_2(MotorData *Mptr);
|
||||
EXPORT_SYMBOL(usr_servo_ctrl_2);
|
||||
|
||||
double usr_servo_ctrl_3(MotorData *Mptr);
|
||||
EXPORT_SYMBOL(usr_servo_ctrl_3);
|
||||
|
||||
double user_pid_ctrl( struct MotorData *Mptr);
|
||||
EXPORT_SYMBOL(user_pid_ctrl);
|
||||
|
||||
|
||||
99
src/usrServoSample/Makefile
Normal file
99
src/usrServoSample/Makefile
Normal file
@@ -0,0 +1,99 @@
|
||||
#------------------------------------------------------------------------------
|
||||
# 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
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
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@SAROP11-CPPM-MOT6871:/tmp
|
||||
scp pp_proj.ini root@SAROP11-CPPM-MOT6871:/var/ftp/usrflash/Project/Configuration/pp_proj.ini
|
||||
scp pp_proj.ini root@SAROP11-CPPM-MOT6871:/tmp/pp_proj.ini
|
||||
ssh root@SAROP11-CPPM-MOT6871 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
src/usrServoSample/pp_proj.h
Executable file
189
src/usrServoSample/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_
|
||||
5
src/usrServoSample/pp_proj.ini
Executable file
5
src/usrServoSample/pp_proj.ini
Executable file
@@ -0,0 +1,5 @@
|
||||
# This section loads all Power PMAC programs & is run through the pre-processor/CmdProcessor. It is a sample project default load.
|
||||
[RTUSRCCODE]
|
||||
usrrtmodule=/tmp/usralgo.ko
|
||||
ServoCtrlName4=user_pid_ctrl
|
||||
|
||||
114
src/usrServoSample/usralgomain.c
Normal file
114
src/usrServoSample/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");
|
||||
109
src/usrServoSample/usrcode.c
Normal file
109
src/usrServoSample/usrcode.c
Normal file
@@ -0,0 +1,109 @@
|
||||
//---------------------------------------------------------------------------
|
||||
// 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( struct MotorData *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;
|
||||
}
|
||||
|
||||
132
src/usrServoSample/usrcode.h
Normal file
132
src/usrServoSample/usrcode.h
Normal file
@@ -0,0 +1,132 @@
|
||||
#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, ...);
|
||||
|
||||
double user_pid_ctrl( struct MotorData *Mptr);
|
||||
EXPORT_SYMBOL(user_pid_ctrl);
|
||||
|
||||
void user_phase( struct MotorData *Mptr);
|
||||
EXPORT_SYMBOL(user_phase);
|
||||
|
||||
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