From 3fafe53c3983eb770a0e443e35fb3806bcd14324 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Wed, 1 Feb 2017 15:21:56 +0100 Subject: [PATCH] friction compensating user servo --- cfg/mx-stage.cfg | 1 + cfg/torqueCtrl.cfg | 2 + logbook.md | 27 ++++- python/move_record.py | 137 ++++++++++++---------- src/Readme.md | 4 +- src/usrServo/pp_proj.ini | 5 +- src/usrServo/usrcode.c | 73 +++++++++++- src/usrServo/usrcode.h | 6 + src/usrServoSample/Makefile | 99 ++++++++++++++++ src/usrServoSample/pp_proj.h | 189 +++++++++++++++++++++++++++++++ src/usrServoSample/pp_proj.ini | 5 + src/usrServoSample/usralgomain.c | 114 +++++++++++++++++++ src/usrServoSample/usrcode.c | 109 ++++++++++++++++++ src/usrServoSample/usrcode.h | 132 +++++++++++++++++++++ 14 files changed, 841 insertions(+), 62 deletions(-) create mode 100644 src/usrServoSample/Makefile create mode 100755 src/usrServoSample/pp_proj.h create mode 100755 src/usrServoSample/pp_proj.ini create mode 100644 src/usrServoSample/usralgomain.c create mode 100644 src/usrServoSample/usrcode.c create mode 100644 src/usrServoSample/usrcode.h diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index a92299d..ea43e0a 100644 --- a/cfg/mx-stage.cfg +++ b/cfg/mx-stage.cfg @@ -39,6 +39,7 @@ $$$*** //!common() !common(PhaseFreq=20000,PhasePerServo=4) +//!common(PhaseFreq=20000,PhasePerServo=1) //!common(PhaseFreq=40000) !torqueCtrl() diff --git a/cfg/torqueCtrl.cfg b/cfg/torqueCtrl.cfg index 8006d05..836b346 100644 --- a/cfg/torqueCtrl.cfg +++ b/cfg/torqueCtrl.cfg @@ -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) diff --git a/logbook.md b/logbook.md index 279d889..793e7f1 100644 --- a/logbook.md +++ b/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... \ No newline at end of file + +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())) + diff --git a/python/move_record.py b/python/move_record.py index d297561..ff23e60 100755 --- a/python/move_record.py +++ b/python/move_record.py @@ -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() diff --git a/src/Readme.md b/src/Readme.md index 8f9514e..c8177d5 100644 --- a/src/Readme.md +++ b/src/Readme.md @@ -104,7 +104,7 @@ Motor[4].Ctrl =UserAlgo.ServoCtrlAddr[4].a 286 dmesg - 287 rmmod hello.ko + 287 rmmod hello.ko 288 dmesg 289 cat /proc/kallsyms | grep user_pid_ctrl 290 rmmod usralgo @@ -114,7 +114,7 @@ Motor[4].Ctrl =UserAlgo.ServoCtrlAddr[4].a 294 rmmod usralgo 295 insmod /tmp/usralgo.ko 296 cat /proc/kallsyms | grep user_pid_ctrl - 297 history + 297 history THIS ALSO WORKED !!! diff --git a/src/usrServo/pp_proj.ini b/src/usrServo/pp_proj.ini index a1fe6ba..ec74360 100755 --- a/src/usrServo/pp_proj.ini +++ b/src/usrServo/pp_proj.ini @@ -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 diff --git a/src/usrServo/usrcode.c b/src/usrServo/usrcode.c index 198575b..e5a743d 100644 --- a/src/usrServo/usrcode.c +++ b/src/usrServo/usrcode.c @@ -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; @@ -70,7 +141,7 @@ double user_pid_ctrl( struct MotorData *Mptr) else { Mptr->Servo.Integrator=0.0; return 0.0; - } + } } void CaptCompISR(void) diff --git a/src/usrServo/usrcode.h b/src/usrServo/usrcode.h index 09f0153..cb90646 100644 --- a/src/usrServo/usrcode.h +++ b/src/usrServo/usrcode.h @@ -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); diff --git a/src/usrServoSample/Makefile b/src/usrServoSample/Makefile new file mode 100644 index 0000000..2b95b4b --- /dev/null +++ b/src/usrServoSample/Makefile @@ -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) diff --git a/src/usrServoSample/pp_proj.h b/src/usrServoSample/pp_proj.h new file mode 100755 index 0000000..2dec7b8 --- /dev/null +++ b/src/usrServoSample/pp_proj.h @@ -0,0 +1,189 @@ + +#ifndef _PP_PROJ_H_ +#define _PP_PROJ_H_ +//*********************************************************************************** +// C header for accessing PMAC Global, CSGlobal, Ptr vars +// _PPScriptMode_ for Pmac Script like access global & csglobal +// global Mypvar - access with "Mypvar" +// global Myparray(32) - access with "Myparray(i)" +// csglobal Myqvar - access with "Myqvar(i)" where "i" is Coord # +// csglobal Myqarray(16) - access with "Myqvar(i,j)" where "j" is index +// _EnumMode_ for Pmac enum data type checking on Set & Get global functions +// Example +// global Mypvar +// csglobal Myqvar +// "SetGlobalVar(Myqvar, data)" will give a compile error because its a csglobal var. +// "SetCSGlobalVar(Mypvar, data)" will give a compile error because its a global var. +//************************************************************************************ + +#ifdef _PPScriptMode_ +enum globalP {_globalP_=-1}; +enum globalParray {_globalParray_=-1}; +enum csglobalQ {_csglobalQ_=-1}; +enum csglobalQarray {_csglobalQarray_=-1}; + +enum ptrM {_ptrM_=-1}; +enum ptrMarray {_ptrMarray_=-1}; +void SetEnumGlobalVar(enum globalP var, double data) +{ + pshm->P[var] = data; +} + +double GetEnumGlobalVar(enum globalP var) +{ + return pshm->P[var]; +} + +void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data) +{ + pshm->P[(var + index)%MAX_P] = data; +} + +double GetEnumGlobalArrayVar(enum globalParray var, unsigned index) +{ + return pshm->P[(var + index)%MAX_P]; +} + +void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[var] = data; +} + +double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[var]; +} + +void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data; +} + +double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q]; +} + +void SetEnumPtrVar(enum ptrM var, double data) +{ + im_write(pshm->Mdef + var, data, &pshm->Ldata); +} + +double GetEnumPtrVar(enum ptrM var) +{ + return im_read(pshm->Mdef + var, &pshm->Ldata); +} + +void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data) +{ + im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata); +} + +double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index) +{ + return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata); +} + +#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x) +#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x) +#define GetGlobalVar(i) GetEnumGlobalVar(i) +#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j) + +#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x) +#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x) +#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j) +#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k) + +#define SetPtrVar(i, x) SetEnumPtrVar(i, x) +#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x) +#define GetPtrVar(i) GetEnumPtrVar(i) +#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j) +// end of #ifdef _PPScriptMode_ +#else +#ifdef _EnumMode_ +enum globalP {_globalP_=-1}; +enum globalParray {_globalParray_=-1}; +enum csglobalQ {_csglobalQ_=-1}; +enum csglobalQarray {_csglobalQarray_=-1}; +enum ptrM {_ptrM_=-1}; +enum ptrMarray {_ptrMarray_=-1}; +void SetEnumGlobalVar(enum globalP var, double data) +{ + pshm->P[var] = data; +} + +double GetEnumGlobalVar(enum globalP var) +{ + return pshm->P[var]; +} + +void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data) +{ + pshm->P[(var + index)%MAX_P] = data; +} + +double GetEnumGlobalArrayVar(enum globalParray var, unsigned index) +{ + return pshm->P[(var + index)%MAX_P]; +} + +void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[var] = data; +} + +double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[var]; +} + +void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data) +{ + pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data; +} + +double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs) +{ + return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q]; +} + +void SetEnumPtrVar(enum ptrM var, double data) +{ + im_write(pshm->Mdef + var, data, &pshm->Ldata); +} + +double GetEnumPtrVar(enum ptrM var) +{ + return im_read(pshm->Mdef + var, &pshm->Ldata); +} + +void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data) +{ + im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata); +} + +double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index) +{ + return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata); +} + +#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x) +#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x) +#define GetGlobalVar(i) GetEnumGlobalVar(i) +#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j) + +#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x) +#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x) +#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j) +#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k) + +#define SetPtrVar(i, x) SetEnumPtrVar(i, x) +#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x) +#define GetPtrVar(i) GetEnumPtrVar(i) +#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j) +// end of #ifdef _EnumMode_ +#else +// ***** Standard default mode ***** +#endif +#endif +#endif //_PP_PROJ_H_ diff --git a/src/usrServoSample/pp_proj.ini b/src/usrServoSample/pp_proj.ini new file mode 100755 index 0000000..a1fe6ba --- /dev/null +++ b/src/usrServoSample/pp_proj.ini @@ -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 + diff --git a/src/usrServoSample/usralgomain.c b/src/usrServoSample/usralgomain.c new file mode 100644 index 0000000..3232aa8 --- /dev/null +++ b/src/usrServoSample/usralgomain.c @@ -0,0 +1,114 @@ +//--------------------------------------------------------------------------- +// Project PowerPMAC Firmware +// Delta Tau Data Systems, Inc. +// Copyright 2007. All Rights Reserved. +// +// SUBSYSTEM: User Algo Driver +// FILE: usralgo.c +// AUTH OR: Henry Bausley +// +// OVERVIEW +// ~~~~~~~~ +// This file is a device driver that exports functions that can be used by other +// kernel mode modules. ie. RtPmac.rtl can call functions that are part of this +// driver for use as user written servo, phase, plc rti, plc rt thread +// No one should ever have to touch this module! Custom routines are added +// as additional modules as whatever name the user wants. +// +// NOTES +// ~~~~~ +// - The driver must be loaded +// bash-3.0# insmod /opt/ppmac/usralgo/usralgo.ko +// +// - A node can be created on the file system the thing you fopen +// but it is not required. Could be used by gpos if ioctl's were added +// bash-3.0# mknod -m 666 /dev/usralgo c 240 0 +// +//-------------------------------------------------------------------------------- +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +//#define KDBEUG 1 + +#ifdef KDBEUG + #define PRINTK(arg) printk(arg) +#else + #define PRINTK(arg) +#endif + +#define USRALGO_MAJOR 240 +#define USRALGO_NAME "usralgo" + +struct SHM *pshm; // Pointer to shared memory +volatile unsigned *piom; // Pointer to I/O memory +void *pushm; // Pointer to user memory + + +// define which file operations are supported +struct file_operations usralgo_fops = +{ + .owner = THIS_MODULE, + .llseek = NULL, + .read = NULL, + .write = NULL, +// .readdir = NULL, + .poll = NULL, +#if LINUX_VERSION_CODE < KERNEL_VERSION(3,0,0) + .ioctl = NULL, +#else + .unlocked_ioctl = NULL, +#endif + .mmap = NULL, + .open = NULL, + .flush = NULL, + .release = NULL, + .fsync = NULL, + .fasync = NULL, + .lock = NULL, +}; + +//----------------------------------------------------------------------- +// initialize module -- this what is called when you do insmod ./usralgo.ko +//----------------------------------------------------------------------- +static int __init usralgo_init_module (void) +{ + int i; + + PRINTK(KERN_INFO "initializing module usralgo\n"); + pshm = GetSharedMemPtr(); + pushm = GetUserBufferPtr(); + piom = GetIOMemPtr(); + + i = register_chrdev (USRALGO_MAJOR, USRALGO_NAME, &usralgo_fops); + if (i != 0) return - EIO; + + return 0; +} + +//----------------------------------------------------------------------- +// close and cleanup module +//------------------------- +static void __exit usralgo_cleanup_module (void) +{ + PRINTK("cleaning up module usralgo\n"); + unregister_chrdev (USRALGO_MAJOR, USRALGO_NAME); +} + +module_init(usralgo_init_module); +module_exit(usralgo_cleanup_module); +MODULE_AUTHOR("www.deltatau.com"); +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("Linux Device Driver for usralgo"); diff --git a/src/usrServoSample/usrcode.c b/src/usrServoSample/usrcode.c new file mode 100644 index 0000000..198575b --- /dev/null +++ b/src/usrServoSample/usrcode.c @@ -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; +} + diff --git a/src/usrServoSample/usrcode.h b/src/usrServoSample/usrcode.h new file mode 100644 index 0000000..09f0153 --- /dev/null +++ b/src/usrServoSample/usrcode.h @@ -0,0 +1,132 @@ +#ifdef __KERNEL__ +#include +#else +#define EXPORT_SYMBOL(x) // x +#define KERN_ALERT +#define printk printf +#include +#endif +#include // Global Rt/Gp Externals and structures +#include + +int rtsprintf(char * buf, const char *fmt, ...); + +double 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] +//---------------------------------------- + + +