bugfixing

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

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
+#2->   Y
-#3->   Y
+#3->   X
 
 Coord[1].AltFeedRate=0
 Coord[1].Tm=1  //1ms time

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
+//Mot 2: Stage Y 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)
+//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
+//Mot 3: Stage X 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)
+//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

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

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])
+      errx = (rec[:, 2] - rec[:, 5])
-      erry = abs(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()
 

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
+      velxAct = np.diff(rec[:, 2])/tb
-      velxDes = np.diff(rec[:, 4])/tb
+      velxDes = np.diff(rec[:, 5])/tb
-      velyAct = np.diff(rec[:, 2])/tb
+      velyAct = np.diff(rec[:, 1])/tb
-      velyDes = np.diff(rec[:, 5])/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()
 

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[:, 5], rec[:, 4], 'b-',label='recDesPos')
-    hl+=ax.plot(rec[:,1],rec[:,2],'g-',label='recActPos')
+    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[2].set_data(rec[:idx+1, 5], rec[:idx+1, 4])
-      hl[3].set_data(rec[:len, 1], rec[:len, 2])
+      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_xlim(rec[idx, 2]-x,rec[idx, 2]+x)
-      ax.set_ylim(rec[len, 2]-y,rec[len, 2]+y)
+      ax.set_ylim(rec[idx, 1]-y,rec[idx, 1]+y)
       canvas.draw()
 
   @staticmethod

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

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()
+          idx[a:a+grp_sz]=a+pts[a:a+grp_sz,idxB].argsort()
-        #print(idx)
+      #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[:, 5], rec[:, 4], 'b-') # desired path
-      hl=ax.plot(rec[:,1],rec[:,2],'g-')
+      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)
+      errx=rec[:,2]-rec[:,5]
-      hl = ax.plot(err, 'r-')
+      erry=rec[:,1]-rec[:,4]
-      ax.xaxis.set_label_text('datapoint (timebase: %g ms per data point)' % meta['timebase'])
+      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()

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>2000)ctrl_out=2000;
-    if (ctrl_out<-150)ctrl_out=-150;
+    if (ctrl_out<-2000)ctrl_out=-2000;
     return ctrl_out;
   }
   else {