diff --git a/Readme.md b/Readme.md index ed02393..54740de 100644 --- a/Readme.md +++ b/Readme.md @@ -13,6 +13,9 @@ Inductance mH 2.4 Max.BuxVoltage V 80 Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2 +Weight top stage: 250g=2.5N + + Mecapion rot stage ------------------ diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg index 442b062..a92299d 100644 --- a/cfg/mx-stage.cfg +++ b/cfg/mx-stage.cfg @@ -36,7 +36,7 @@ //Mot 7: Test Stepper: Vextra PK244M 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) //Enc 7: Test Stepper: ssi_enc multiturn 1 rev = 4096 enc_step -//$$$*** +$$$*** //!common() !common(PhaseFreq=20000,PhasePerServo=4) //!common(PhaseFreq=40000) @@ -50,8 +50,8 @@ //#3-> + .5X +1. Y + 0.01A #1-> A -#2-> X -#3-> Y +#2-> Y +#3-> X Coord[1].AltFeedRate=0 Coord[1].Tm=1 //1ms time diff --git a/cfg/torqueCtrl.cfg b/cfg/torqueCtrl.cfg index e5f1df5..8006d05 100644 --- a/cfg/torqueCtrl.cfg +++ b/cfg/torqueCtrl.cfg @@ -20,11 +20,11 @@ //Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step //Enc 1: Rotation stage LS Mecapion 1 rev = 1048576 enc_steps -//Mot 2: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step -//Enc 2: Stage X Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) +//Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step +//Enc 2: Stage Y Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) -//Mot 3: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step -//Enc 3: Stage Y Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) +//Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step +//Enc 3: Stage X Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step) //Mot 4: Test Servo: Trinamic QBL 4208 motor 8 poles 1 rev = 4*2048=8192 phase_step //Enc 4: Test Servo: Incremental encoder mounted with motor 1 1 rev = 2000 enc count (500 inc_ quadrature encoder) @@ -54,7 +54,7 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a !motor(mot=1,dirCur=0,contCur=1000,peakCur=2000,timeAtPeak=1,IiGain=1.5,IpfGain=0,IpbGain=3,JogSpeed=180.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./8192,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=3000,WarnFeLimit=1000,InPosBand=10) -//Stage X Parker MX80L +//Stage Y Parker MX80L //-------------------- //Motor[2].pPhaseEnc -> PowerBrick[0].Chan[1].PhaseCapt.a // 1 el_step = 13mm = 2048 phase_step = 166400000 PhaseCapt =256*650000 (256=scaling of encTable) @@ -64,10 +64,10 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a !encoder_sim(enc=2,tbl=10,mot=10,posSf=13000./2048) !encoder_inc(enc=2,tbl=2,mot=2,posSf=13000./650000) !motor_servo(mot=2,ctrl='ServoCtrl',Kp=16,Kvfb=800,Ki=0.001,Kvff=1000,Kaff=0,MaxInt=1000) -!motor(mot=2,dirCur=0,contCur=500,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=200,WarnFeLimit=100,InPosBand=2) +!motor(mot=2,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=200,WarnFeLimit=100,InPosBand=2) -//Stage Y Parker MX80L +//Stage X Parker MX80L (top stage, mounted on Y stage) //-------------------- //Motor[3].pPhaseEnc -> PowerBrick[0].Chan[1].PhaseCapt.a // 1 el_step = 13mm = 2048 phase_step = 166400000 PhaseCapt =256*650000 (256=scaling of encTable) @@ -77,7 +77,7 @@ 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) -!motor(mot=3,dirCur=0,contCur=500,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) +!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) //Test Servo: Trinamic QBL 4208 motor diff --git a/logbook.md b/logbook.md index 351d932..c5e2617 100644 --- a/logbook.md +++ b/logbook.md @@ -346,10 +346,58 @@ Motor[3].LimitBits = 9 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144 Motor[3].DacShift=0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].AdcMask = $FFFC0000 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].ctrl=Sys.servoctrl // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 -Motor[3].DtOverRotorTc = 0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 +Motor[3].DtOverRotorTc = 0 // Hardware Interface //12/9/2016 0:8:53 AM - 129.129.144.138 Motor[3].IxCoupleGain = 0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].SlipGain = 0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].AdvGain = 0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].Stime = 0 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 Motor[3].PhaseOffset =-683 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 BrickLV.Reset = 1 // Hardware Interface //12/9/2016 :8:53 AM - 129.129.144.138 + + + +*** 16.1.2017 *** +================== + +Parker stage current step response +---------------------------------- + +Motor[2].IiGain=0 +Motor[2].IpfGain=1 +Motor[2].IpbGain=-1 + +Current step: Magnitude 4000 bits +Phasing 0 +Dwell 10 ms + +Motor 2 Phase pos at hard limit (towards kable) + +> Motor[2].PhasePos;#2$ +Motor[2].PhasePos=1317.26495427904138 +Motor[2].PhasePos=1286.11002741653601 +Motor[2].PhasePos=1310.62328616849777 +Motor[2].PhasePos=1310.62328616849686 +Motor[2].PhasePos=1307.26431089312473 +Motor[2].PhasePos=1323.77420922660667 +Motor[2].PhasePos=1323.77735999583911 +Motor[2].PhasePos=1331.42989161901664 +Motor[2].PhasePos=1332.17854273394619 + +Use Motor[2].PhasePos=1330 +Use Motor[3].PhasePos=150 (or 1200???) + +*** 19.1.2017 *** +================== + +Friction force of Parker stage + +X(top stage): 1N max 0.1N min.. depends on the magnetic poles +#3out20 -> Force 2N +#3out30 -> Force 3N (changes in torque at poles) +Weight: ca 250g + + +Y(lower stage): 1N max 0.1N min.. depends on the magnetic poles +#2out20 -> Force 2.5N iqCmd=402 +#2out30 -> Force 3N iqCmd=603 (changes in torque at poles) +Weight: ca 750g diff --git a/python/MAError.py b/python/MAError.py index 62522d9..fbf4213 100644 --- a/python/MAError.py +++ b/python/MAError.py @@ -133,8 +133,8 @@ class MAErrorFrame(wx.Frame): err=doc.err except AttributeError: rec=doc.fh['rec'] - errx = abs(rec[:, 1] - rec[:, 4]) - erry = abs(rec[:, 2] - rec[:, 5]) + errx = (rec[:, 2] - rec[:, 5]) + erry = (rec[:, 1] - rec[:, 4]) errxy = np.sqrt(errx ** 2 + erry ** 2) doc.err = err = (errx, erry, errxy) canvas.InitChild(meta,err) @@ -175,7 +175,7 @@ class MAErrorFrame(wx.Frame): if msg == 0: canvas = self.canvas ax = canvas.ax - len = usrData + idx = usrData err = self.doc.err hl = canvas.hl #hl[0].set_data(rec[:len, 4], rec[:len, 5]) @@ -185,7 +185,7 @@ class MAErrorFrame(wx.Frame): x = (x[1] - x[0]) / 2; #y = ax.get_ylim(); #y = (y[1] - y[0]) / 2; - ax.set_xlim(len-x, len + x,emit=True) + ax.set_xlim(idx-x, idx + x,emit=True) #ax.set_ylim(rec[len, 2] - y, rec[len, 2] + y) canvas.draw() diff --git a/python/MAVelocity.py b/python/MAVelocity.py index 6e7fb53..b2ee517 100644 --- a/python/MAVelocity.py +++ b/python/MAVelocity.py @@ -148,10 +148,10 @@ class MAVelocityFrame(wx.Frame): #datapoint timebase: 2 ms () per data point #velocity: um/ms (deltatau desVel= motor units per serco cycle) rec=doc.fh['rec'] - velxAct = np.diff(rec[:, 1])/tb - velxDes = np.diff(rec[:, 4])/tb - velyAct = np.diff(rec[:, 2])/tb - velyDes = np.diff(rec[:, 5])/tb + velxAct = np.diff(rec[:, 2])/tb + velxDes = np.diff(rec[:, 5])/tb + velyAct = np.diff(rec[:, 1])/tb + velyDes = np.diff(rec[:, 4])/tb velAct = np.sqrt(velxAct**2+velyAct**2) velDes = np.sqrt(velxDes**2+velyDes**2) doc.vel = vel = (velxAct,velxDes,velyAct,velyDes,velAct,velDes) @@ -194,7 +194,7 @@ class MAVelocityFrame(wx.Frame): if msg == 0: canvas = self.canvas ax = canvas.ax - len = usrData + idx = usrData hl = canvas.hl #hl[0].set_data(rec[:len, 4], rec[:len, 5]) #hl[3].set_data(rec[:len, 1], rec[:len, 2]) @@ -203,7 +203,7 @@ class MAVelocityFrame(wx.Frame): x = (x[1] - x[0]) / 2; #y = ax.get_ylim(); #y = (y[1] - y[0]) / 2; - ax.set_xlim(len-x, len + x,emit=True) + ax.set_xlim(idx-x, idx + x,emit=True) #ax.set_ylim(rec[len, 2] - y, rec[len, 2] + y) canvas.draw() diff --git a/python/MAxyPlot.py b/python/MAxyPlot.py index 1f58cbd..74a46df 100644 --- a/python/MAxyPlot.py +++ b/python/MAxyPlot.py @@ -59,8 +59,8 @@ class MPLCanvasImg(FigureCanvas): ax.add_collection(ec) - hl+=ax.plot(rec[:, 4], rec[:, 5], 'b-',label='recDesPos') - hl+=ax.plot(rec[:,1],rec[:,2],'g-',label='recActPos') + hl+=ax.plot(rec[:, 5], rec[:, 4], 'b-',label='recDesPos') + hl+=ax.plot(rec[:,2],rec[:,1],'g-',label='recActPos') ax.xaxis.set_label_text('x-pos um') ax.yaxis.set_label_text('y-pos um') @@ -264,16 +264,16 @@ class MAxyPlotFrame(wx.Frame): if msg==0: canvas=self.canvas ax=canvas.ax - len=usrData + idx=usrData rec=self.doc.fh['rec'] hl=canvas.hl - hl[2].set_data(rec[:len, 4], rec[:len, 5]) - hl[3].set_data(rec[:len, 1], rec[:len, 2]) + hl[2].set_data(rec[:idx+1, 5], rec[:idx+1, 4]) + hl[3].set_data(rec[:idx+1, 2], rec[:idx+1, 1]) #ax.draw_artist(hl[2]) x=ax.get_xlim();x=(x[1]-x[0])/2; y=ax.get_ylim();y=(y[1]-y[0])/2; - ax.set_xlim(rec[len, 1]-x,rec[len, 1]+x) - ax.set_ylim(rec[len, 2]-y,rec[len, 2]+y) + ax.set_xlim(rec[idx, 2]-x,rec[idx, 2]+x) + ax.set_ylim(rec[idx, 1]-y,rec[idx, 1]+y) canvas.draw() @staticmethod diff --git a/python/PBMotionAnalyzer.py b/python/PBMotionAnalyzer.py index e1bb52d..44b3557 100755 --- a/python/PBMotionAnalyzer.py +++ b/python/PBMotionAnalyzer.py @@ -21,6 +21,14 @@ from MAVelocity import * import wxutils as ut class MADoc(): ''' + lenRec=fh['rec'] + lenPts=fh['pts'] + pts= X,Y array + #rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos + #res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos + #idx 0 1 2 3 4 5 + + Implemented messages 0: The time slider has changed. usrData=index diff --git a/python/shapepath.py b/python/shapepath.py index 3d4714c..aa6f98d 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -83,10 +83,10 @@ class ShapePath: #cfg = {"points": [[0, 0],[100, 0],[200, 0],[300, 0],[400, 0],[400, 100],[300, 100],[200, 100],[100, 100],[0, 100],[10, 200],[100, 200],[200, 200],[300, 200],[400, 200],[410, 300],[300, 300],[200, 300],[100, 300],[0, 300],[0, 400],[100, 400],[200, 400],[300, 400],[400, 400]],"sequencer": ['gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']} - #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} + cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=2,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} - cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=100)','plot_gather("'+fn+'.npz")']} + #cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)','plot_gather("'+fn+'.npz")']} self.cfg=dotdict(cfg) self.args=args @@ -124,10 +124,16 @@ class ShapePath: self.points=pts def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs): + ''' + kwargs: + acq_per : acquire period: acquire data all acq_per servo loops (default=1) + pt2pt_time : time to move from one point to the next point + ''' prg=[] - gather={"MaxSamples":1000000, "Period":1} + acq_frq=kwargs.get('acq_frq',1) + gather={"MaxSamples":1000000, "Period":acq_frq} #Sys.ServoPeriod is dependent of !common() macro - ServoPeriod= .2 + ServoPeriod= .2 #0.2ms #ServoPeriod = .05 self.meta = {'timebase': ServoPeriod*gather['Period']} #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] @@ -250,24 +256,26 @@ class ShapePath: #if verb&2: # self.plot_points(pts) - #sort points along y - pts=pts[pts[:, 1].argsort()] - #if verb&2: - # self.plot_points(pts) - - #group sorting cnt=pts.shape[0] idx=np.ndarray(cnt,dtype=np.int32) grp_cnt=int(np.sqrt(cnt)) grp_sz=int(np.ceil(float(cnt)/grp_cnt)) + if self.args.yx==True: + idxA=0;idxB=1 + else: + idxA=1;idxB=0 + + #sort points along idxA + pts=pts[pts[:,idxA].argsort()] + #group sorting along idxB for i in range(grp_cnt): a=i*grp_sz #print a,a+grp_sz if i%2: - idx[a:a+grp_sz]=a+pts[a:a+grp_sz,0].argsort()[::-1] + idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()[::-1] else: - idx[a:a+grp_sz]=a+pts[a:a+grp_sz,0].argsort() - #print(idx) + idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort() + #print(idx) pts=pts[idx] if verb&2: @@ -294,8 +302,11 @@ class ShapePath: def plot_gather(self,fnOut='/tmp/shapepath.npz',fnLoc='/tmp/gather.txt'): meta=self.meta - pts=self.points + pts=self.points # X,Y array rec = np.genfromtxt(fnLoc, delimiter=' ') + #rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos + #res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos + #idx 0 1 2 3 4 5 if fnOut: # time base: # Gather.Period=10 @@ -308,8 +319,8 @@ class ShapePath: #hl=ax[0].plot(x, y, color=col) hl=ax.plot(pts[:,0],pts[:,1],'r.') hl=ax.plot(pts[:,0],pts[:,1],'y--') - hl = ax.plot(rec[:, 4], rec[:, 5], 'b-') - hl=ax.plot(rec[:,1],rec[:,2],'g-') + hl = ax.plot(rec[:, 5], rec[:, 4], 'b-') # desired path + hl=ax.plot(rec[:,2],rec[:,1],'g-') # actual path ax.xaxis.set_label_text('x-pos um') ax.yaxis.set_label_text('y-pos um') cid = fig.canvas.mpl_connect('button_press_event', self.onclick) @@ -319,11 +330,18 @@ class ShapePath: fig = plt.figure() ax = fig.add_subplot(1, 1, 1) - err=np.sqrt((rec[:,1]-rec[:,4])**2+(rec[:,1]-rec[:,4])**2) - hl = ax.plot(err, 'r-') - ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)' % meta['timebase']) + errx=rec[:,2]-rec[:,5] + erry=rec[:,1]-rec[:,4] + err=np.sqrt(errx**2+erry**2) + + hl = [] + hl += ax.plot(errx, 'b-',label='x-error') + hl += ax.plot(erry, 'g-',label='y-error') + hl += ax.plot(err, 'r-',label='error') + ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)'%meta['timebase']) ax.yaxis.set_label_text('pos-error um') - print('average error %g um'%err.mean()) + legend = ax.legend(loc='upper right', shadow=True) + print('average error x %g um, y %g um, %g um'%(np.abs(errx).mean(),np.abs(erry).mean(),err.mean())) plt.show() if __name__=='__main__': @@ -358,6 +376,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0) parser.add_option('-n', '--dryrun', action='store_true', help='dryrun to stdout') + parser.add_option('--yx', action='store_true', help='sort y,x instead x,y') parser.add_option('--cfg', help='config file containing json configuration structure') (args, other)=parser.parse_args() diff --git a/src/usrServo/usrcode.c b/src/usrServo/usrcode.c index 5fa87a4..198575b 100644 --- a/src/usrServo/usrcode.c +++ b/src/usrServo/usrcode.c @@ -63,8 +63,8 @@ double user_pid_ctrl( struct MotorData *Mptr) Mptr->Servo.Integrator+=Mptr->PosError*Mptr->Servo.Ki; // I term ctrl_out+=Mptr->Servo.Integrator; // Combine - if (ctrl_out>150)ctrl_out=150; - if (ctrl_out<-150)ctrl_out=-150; + if (ctrl_out>2000)ctrl_out=2000; + if (ctrl_out<-2000)ctrl_out=-2000; return ctrl_out; } else {