diff --git a/PBinspect.pbi b/PBInspect.pbi similarity index 100% rename from PBinspect.pbi rename to PBInspect.pbi diff --git a/PBInspect1.pbi b/PBInspect1.pbi new file mode 100644 index 0000000..0764276 --- /dev/null +++ b/PBInspect1.pbi @@ -0,0 +1,113 @@ +[ + [ + "MotorListCtrl", + [ + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16 + ], + "name=p;caption=motor pos;state=67377148;dir=4;layer=0;row=0;pos=1;prop=100000;bestw=200;besth=350;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1;notebookid=-1;transparent=255" + ], + [ + "WatchListCtrl", + [ + [ + "Motor[1].idCmd", + "lambda v: '%.8g'%float(v)" +], + [ "Motor[1].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[1].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[2].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[2].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[3].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[3].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[4].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[4].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[5].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[5].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[6].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[6].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[7].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[7].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[8].iqCmd", "lambda v: '%.8g'%float(v)" ], + [ "Motor[8].idCmd", "lambda v: '%.8g'%float(v)" ], + [ "P2000", null ], + [ "P3000", null ] + ], + "name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 3 + ], + "name=m3;caption=motor3;state=67377148;dir=4;layer=0;row=5;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=694;floaty=941;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusGblListCtrl", + [], + "name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2058;floaty=457;floatw=208;floath=377;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 2 + ], + "name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=4;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2588;floaty=946;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 4 + ], + "name=m4;caption=motor4;state=67377148;dir=4;layer=0;row=6;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=905;floaty=792;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 5 + ], + "name=m5;caption=motor5;state=67377148;dir=4;layer=0;row=6;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1139;floaty=836;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 7 + ], + "name=m7;caption=motor7;state=67377148;dir=4;layer=0;row=7;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1353;floaty=657;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 6 + ], + "name=m6;caption=motor6;state=67377148;dir=4;layer=0;row=7;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1276;floaty=771;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusCoordListCtrl", + [ + 0 + ], + "name=c0;caption=coord0;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=457;floaty=774;floatw=208;floath=677;notebookid=-1;transparent=255" + ], + [ + "StatusMotorListCtrl", + [ + 1 + ], + "name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=4;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=669;floaty=892;floatw=208;floath=677;notebookid=-1;transparent=255" + ] +] \ No newline at end of file diff --git a/PBinspect2.pbi b/PBInspect2.pbi similarity index 94% rename from PBinspect2.pbi rename to PBInspect2.pbi index da30f66..421d2c1 100644 --- a/PBinspect2.pbi +++ b/PBInspect2.pbi @@ -52,6 +52,18 @@ [ "Motor[1].ServoOut", null + ], + [ + "P1000", + null + ], + [ + "Gather.Samples", + null + ], + [ + "Gather.MaxLines", + null ] ], "name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255" diff --git a/Readme.md b/Readme.md index 92672ce..980bf09 100644 --- a/Readme.md +++ b/Readme.md @@ -153,58 +153,71 @@ HelicalScan ----------- ``` PPMAC=MOTTEST-CPPM-CRM0485 -gpasciiCommander --host $PPMAC mx-stage_sim.cfg -i -Coord[1].SegMoveTime=.05 -will calculate all 0.05 sec the inverse kinematic. -default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints +cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX +PBInspect --host $PPMAC --cfg PBInspect2.pbi +cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg +gpasciiCommander --host $PPMAC sim_8_motors.cfg -i ssh root@$PPMAC sendgetsends -1 - send 1"SampleMessage\n" +cpx send 1"SampleMessage\n" -execute helicalscan.py +cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python +./helicalscan.py +``` -SCRATCH -------- +fwd/inv kinematic stuff +----------------------- +``` +Coord[1].SegMoveTime=.05, will calculate all 0.05 sec the inverse kinematic. +default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints &1p ->this will trigger:forward kinematic cpx pmatch ->this will trigger:forward kinematic +cpx ;linear rel; X0Y0Z0B0 ->this will trigger: inverse -cpx ;linear abs; X0Y0Z0B0 ->this will trigger: inverse +test_coord_trf() (w is in mdeg) -> Output: +input : cx:0.2 cz:0.3 w:5729.58 fy:0.4 +fwd_trf: dx:-1.14768 dz:-19.0991 w:5729.58 fy:0.4 +inv_trf: cx:0.2 cz:0.3 w:5729.58 fy:0.4 -#7j=0.1 //cx -#8j=0.2 //cy -#1j=0.3 //w -#2j=0.4 //fx +input : dx:0.2 dz:0.3 w:5729.58 fy:0.4 +inv_trf: cx:1.54768 cz:19.6991 w:5729.58 fy:0.4 +fwd_trf: dx:0.2 dz:0.3 w:5729.58 fy:0.4 -(cx,cz,w,fy) (2.6164986454377614, 17.758728111340563, 0.1, 4.3) -(dx,dz,w,y) (0.2, 0.3, 0.1, 4.3) - -#7j=2.6164986454377614 -#8j=17.758728111340563 -#1j=5729.578 // 0.1*1000.*360./(2*pi) -#2j=4.3 +#4j=0.2 //cx +#5j=0.3 //cz +#3j=5729.58 //w +#1j=0.4 //fy &1p -B0.1 X0.2 Y4.3 Z0.3 -cpx ;linear abs; X0.2 Z0.3, B0.1 Y4.3 +forward kinematic 0.199997 0.300003 5729.58 0.399994 +forward result -1.14769 -19.0991 5729.58 0.399994 -Motor[1,27,8].JogSpeed=8 +cpx ;linear abs; X.2Y.4Z.3B5729.58 +inverse kinematic 0.2 0.3 5729.58 0.4 +inverse result 1.54579 19.7026 5729.58 0.4 -#1,2,7,8j=10000 - -cpx ;linear abs; X0.2 Z0.3 B0.1 Y00000 -cpx ;linear abs; X0.2 Z0.3 B0.1 Y10000 - -input : dx:0.2 dz:0.3 w:5.72958 fy:3.3 -inv_trf: cx:2.34244 cz:18.2563 w:5.72958 fy:3.3 -fwd_trf: dx:0.2 dz:0.3 w:5.72958 fy:3.3 - -#7j=2.34244;#8j=18.2563;#1j=5729.58;#2j=3.3 +input : cx:10 cz:20 w:171887 fy:40 +fwd_trf: dx:28.9419 dz:-5.52304 w:171887 fy:40 +inv_trf: cx:10 cz:20 w:171887 fy:40 +#4j=10 //cx +#5j=20 //cz +#3j=171887 //w +#1j=40 //fy &1p +forward kinematic 10 20 171887 40 +forward result 28.942 -5.52296 171887 40 +input : dx:10 dz:20 w:171887 fy:40 +inv_trf: cx:-8.94193 cz:45.523 w:171887 fy:40 +fwd_trf: dx:10 dz:20 w:171887 fy:40 + +cpx ;linear abs; X10 Z20 B171887 Y40 +inverse kinematic 10 20 171887 40 +inverse result -8.94198 45.523 171887 40 ``` Motors final setup 20.12.17 @@ -242,6 +255,7 @@ SAR-CPPM-EXPMX3: biss:n timing:y PPMAC=MOTTEST-CPPM-CRM0485 ssh root@$PPMAC cd /ioc/SAR-CPPM-EXPMX1;iocsh startup.script +sendgetsends -1 ssh x06mx-cons-1 @@ -252,25 +266,3 @@ export CAQTDM_DISPLAY_PATH=/net/slsfs-crtl/export/sf/common/config/qt/:/net/slsf caqtdm -macro "P=SAR-EXPMX" ESB_MX_exp.ui ``` - - -SCRATCH --------- - -PPMAC=SAR-CPPM-EXPMX1 -gpasciiCommander --host $PPMAC -i -$$$*** -!common() -!SAR-EXPMX1() - -PPMAC=SAR-CPPM-EXPMX1 -PBInspect --host $PPMAC -i - - -Coord[1].SegMoveTime=.05 -will calculate all 0.05 sec the inverse kinematic. -default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints - - -ssh root@$PPMAC sendgetsends -1 - send 1"SampleMessage\n" diff --git a/cfg/MX1_home.cfg b/cfg/MX1_home.cfg index 2fa88b6..fc38312 100644 --- a/cfg/MX1_home.cfg +++ b/cfg/MX1_home.cfg @@ -5,12 +5,11 @@ statusHoming=0 //statusHoming: // 1: M1 phased // 2: M2 phased -// 4: M1 homed -// 8: M2 homed -// 16: M1 neg limit -// 32: M2 neg limit -// 64: homing done -// 128: homing failed +// 4: M1 pos limit found +// 8: M2 pos limit found +// 16: M1 homed +// 32: M2 homed +// 256: homing failed open plc PLC_Homing @@ -33,46 +32,95 @@ open plc PLC_Homing Motor[1].PhaseFindingStep=1 Motor[2].PhaseFindingStep=1 - timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec - - Motor[1].HomeVel=2 - Motor[2].HomeVel=2 - - home1 - home2 - timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec - statusHoming|=1 - while(1) + while(Motor[1].PhaseFindingStep>0 || Motor[2].PhaseFindingStep>0){} + if (Motor[1].PhaseFound==1) { - if(Motor[1].FeFatal && (statusHoming&2)==0) - { - statusHoming=statusHoming|2 - Motor[1].HomeVel=-Motor[1].HomeVel - home1 - } - if(Motor[2].HomeInProgress==0 && (statusHoming&4)==0) - { - statusHoming=statusHoming|4 - Motor[2].HomeVel=-Motor[2].HomeVel - home2 - } - if(Motor[1].HomeInProgress==0 && Motor[1].HomeInProgress==0) - { - statusHoming=statusHoming|8 - break - } + statusHoming|=1 + cmd "#1j/" + send 1"phasing Y_Achse ok\n" + } + else + { + send 1"phasing Y Achsde error!\n" + goto 0 //ERROR_END + } + if (Motor[2].PhaseFound==1) + { + statusHoming|=2 + cmd "#2j/" + send 1"phasing X_Achse ok\n" + } + else + { + send 1"phasing X Achse error!\n" + statusHoming|=256 + goto 0 //ERROR_END } - Motor[1].PhasePos=560 // 555 581 593 558 - Motor[2].PhasePos=1540 //1549 1531 1543 1537 + //move until FeWarn + Motor[1].JogSpeed=6 + Motor[2].JogSpeed=6 + timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec + jog+1 + jog+2 + while(1) + { + if(Motor[1].FeWarn && (statusHoming&4)==0) + { + cmd "#1j/" + send 1"Y_Achse +limit\n" + statusHoming|=4 + } + if(Motor[2].FeWarn && (statusHoming&8)==0) + { + cmd "#2j/" + send 1"X_Achse +limit\n" + statusHoming|=8 + } + if((statusHoming&12)==12) + break + } + timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec + Motor[1].HomeVel=-3 + Motor[2].HomeVel=-3 + home1 + home2 + timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec + while(Motor[1].HomeInProgress || Motor[2].HomeInProgress){} + + if(Motor[1].HomeComplete) + statusHoming=statusHoming|16 + else + { + statusHoming|=256 + send 1"Y_Achse home failed\n" + } + + if(Motor[2].HomeComplete) + statusHoming=statusHoming|32 + else + { + statusHoming|=256 + send 1"X_Achse home failed\n" + } + + //Motor[1].PhasePos=560 // 555 581 593 558 + //Motor[2].PhasePos=1540 //1549 1531 1543 1537 + goto 1 + +N0: //ERROR_END + statusHoming|=256 + +N1: //ENDPLC Motor[1].MaxDac=L10 Motor[1].FatalFeLimit=L11 Motor[1].JogSpeed=L12 Motor[2].MaxDac=L20 Motor[2].FatalFeLimit=L21 Motor[2].JogSpeed=L22 + send 1"homeing done\n" disable plc PLC_Homing close diff --git a/cfg/sim_8_motors.cfg b/cfg/sim_8_motors.cfg index 267666b..d8dfa7c 100644 --- a/cfg/sim_8_motors.cfg +++ b/cfg/sim_8_motors.cfg @@ -1,32 +1,6 @@ -//simulated stage without real motors needed +//simulated 8 motors without needing real ones $$$*** !common() -//!common(PhaseFreq=20000,PhasePerServo=4) -//!common(PhaseFreq=20000,PhasePerServo=1) -//!common(PhaseFreq=40000) - -//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 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 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/Enc 4: camera base plate X -// OBSOLETE: Enc 4: Interferometer 1 - -//Mot/Enc 5: camera base plate Y -// OBSOLETE Enc 5: Interferometer 2 - -//Mot 6: Backlight 2.3A - -//Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) -//Enc 7: Renishaw absolute BiSS - -//Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor) -//Enc 8: Renishaw absolute BiSS //use some step and direction lines to setup simulate motors // /afs/psi.ch/user/h/humar_t/public/Modules/XMI/cfg/xmi.cfg @@ -63,5 +37,6 @@ Motor[7].pLimits=0;Motor[7].AmpFaultLevel=0;Motor[7].pAmpEnable=0;Motor[7].pAmpF !motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True) Motor[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0 - -!coordTrf() \ No newline at end of file +Motor[3].JogSpeed=1000 # used for joging +Motor[3].MaxSpeed=1000 # used for motion program +#1..8j/ diff --git a/python/helicalscan.py b/python/helicalscan.py index 219f79d..4a4d3fe 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -149,12 +149,23 @@ class HelicalScan: def test_coord_trf(self): param = self.param - dx, dz, w, y, = (0.2,0.3,0.1,3.3) - print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) - (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) - print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r,fy) + cx, cz, w, fy, = (0.2,0.3,0.1,0.4) + #cx, cz, w, fy, = (10.,20,3.,40) + print 'input : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy) (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy) - print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) + print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y) + (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) + print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy) + + dx, dz, w, y, = (0.2,0.3,0.1,0.4) + #dx, dz, w, y, = (10.,20,3.,40) + print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y) + (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) + print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy) + (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy) + print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y) + + # plt.ion() # fig = plt.figure() @@ -164,6 +175,32 @@ class HelicalScan: # def my_anim_func3(self,idx): # self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist) + def mpl_test(self): + plt.ion() + fig = plt.figure() + ax = fig.add_axes([.2, .05, .75, .90]) + x=np.arange(0,2*np.pi,.1) + y=np.sin(x) + h=ax.plot(x,y) + # ax.set_position([.2,.05,.75,.90]) + #ax2 = ax.twinx() + ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax) + ax2.set_position([.1,.05,.75,.90]) + + ax2.spines['top'].set_visible(False) + ax2.spines['bottom'].set_visible(False) + + ax2.xaxis.set_visible(False) + ax2.patch.set_visible(False) + + y2=y+.1*np.random.random(y.shape) + h+=ax2.plot(x,y2,'g') + ax2.set_position(ax.get_position()) + ax2.set_ylabel('mylbl', color='r') + ax2.tick_params(axis='y', colors='b') + ax2.spines['left'].set_position(('axes',-.1)) + plt.legend(handels=h) + pass def show_vel(self): rec=self.rec @@ -172,10 +209,62 @@ class HelicalScan: y = np.diff(rec,axis=0) mx=y.max(0) mn=y.min(0) - y=y/(mx-mn) + scl=np.maximum(mx,-mn) + scl=np.where(scl==0, 1, scl) # replace 0 by 1 + y/=scl x = np.arange(0, y.shape[0]); x= x.reshape(-1,1).dot(np.ones((1,y.shape[1]))) - plt.plot(x, y) + lbl=('cx','cz','w','fy') + #plt.plot(x, y,label=lbl) + for i in range(y.shape[1]): + plt.plot(x[:,i], y[:,i],label=lbl[i]) + + plt.legend() + + def show_pos(self): + rec=self.rec + y = rec + #plt.ion() + fig = plt.figure(figsize=(20,6)) + c='bgrc' + lbl=('cx','cz','w','fy') + dx=.25/len(lbl) + for i in range(rec.shape[1]): + if i==0: + ax = fig.add_axes([.2, .05, .75, .90]) + axl =[ax,] + else: + ax = fig.add_axes(axl[0].get_position(), sharex=ax) + ax.spines['top'].set_visible(False) + ax.spines['bottom'].set_visible(False) + ax.xaxis.set_visible(False) + ax.patch.set_visible(False) + ax.spines['left'].set_position(('axes', -dx*i)) + axl.append(ax) + ax.set_ylabel(lbl[i], color=c[i]) + ax.tick_params(axis='y', colors=c[i]) + + x = np.arange(0, y.shape[0]); + h=[] + for i in range(rec.shape[1]): + h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i]) + plt.legend(handles=h) + cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove) + fig.obj=self + fig.data=y + + + @staticmethod + def onmove(event): + #print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%( + # event.button, event.x, event.y, event.xdata, event.ydata) + obj = event.canvas.figure.obj + data = event.canvas.figure.data + if(event.xdata): + idx=round(event.xdata) + msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:])) + #print msg + event.canvas.toolbar.set_message(msg) def interactive_cx_cz_w_fy(self): fig = plt.figure() @@ -578,21 +667,28 @@ class HelicalScan: prg.append(''' // Set the motors as inverse kinematic axes in CS 1 //motors CX CZ RY FY -// 7 8 1 2 +// 4 5 3 1 &1 -#7->I -#8->I +a +#4->0 +#5->0 +#3->0 +#1->0 + +#4->I +#5->I +#3->I #1->I -#2->I open forward - define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') + define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(scale='L16') + //send 1"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''') for i in range(2): #https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list @@ -626,7 +722,7 @@ open inverse define(DX='C6', DZ='C8', W='C1', Y='C7') //coord X Z B Y //D0 is set to $000001c2 - define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') + define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') define(p0_x='L10', p0_y='L11', p0_z='L12') define(p1_x='L13', p1_y='L14', p1_z='L15') define(scale='L16') @@ -638,6 +734,7 @@ open inverse prg.append(" qW=W") prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad prg.append(''' + p0_x=x_0+r_0*sin(phi_0+W) p1_x=x_1+r_1*sin(phi_1+W) p0_y=y_0 @@ -645,7 +742,7 @@ open inverse p0_z=z_0+r_0*cos(phi_0+W) p1_z=z_1+r_1*cos(phi_1+W) - scale=(qFY-y_0)/(y_1-y_0) + scale=(Y-y_0)/(y_1-y_0) p0_x=p0_x+scale*(p1_x-p0_x) p0_y=p0_y+scale*(p1_y-p0_y) p0_z=p0_z+scale*(p1_z-p0_z) @@ -654,10 +751,90 @@ open inverse qFY=Y //send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY //P1002+=1 - close ''') + # ****** TESTING CODE ****** + #this is testcode to check linear coordinate transformation and + #simple (same as before) fwd and inv-kinematics + #When using pvt mode they do not behave the same: + #(Power PMAC Users Manual P648) + #Enabling Move Segmentation: + #If segmentation mode is disabled, a circle-mode move will execute as a linear-mode move + #directly from the start point to the end point. Segmentation mode must also be enabled to use the + #special lookahead buffer, tool-radius compensation, and kinematic-subroutine calculations. + + #set ct to: + # 1 for simple linear transformation + # 2 simple linear fwd/inv kinematics + ct=0; + if ct==1: + #simple linear transformation + prg = [] + prg.append(''' +//motors CX CZ RY FY +// 4 5 3 1 +&1 +a +#4->0 +#5->0 +#3->0 +#1->0 + +#4->X +#5->Z +#3->B +#1->Y +''') + elif ct==2: + #simple linear fwd/inv kinematics + prg = [] + prg.append(''' +// Set the motors as inverse kinematic axes in CS 1 +//motors CX CZ RY FY +// 4 5 3 1 +&1 +a +#4->0 +#5->0 +#3->0 +#1->0 + +#4->I +#5->I +#3->I +#1->I + +open forward + define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') + define(DX='C6', DZ='C8', W='C1', Y='C7') + //coord X Z B Y + send 1"forward D0: %f \\n",D0 + if(D0>0) callsub 100 + D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 +N100: + DX=qCX + DZ=qCZ + W=qW + Y=qFY + send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y + P1001+=1 +close + +open inverse + define(DX='C6', DZ='C8', W='C1', Y='C7') + //coord X Z B Y + //D0 is set to $000001c2 + define(qCX='L4', qCZ='L5', qW='L3', qFY='L1') + qCX=DX + qCZ=DZ + qW=W + qFY=Y + send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY + P1002+=1 +close + ''') + if self.args.verbose & 4: for ln in prg: print(ln) @@ -695,7 +872,7 @@ close yRng : starting and ending height ''' prg=[] - acq_per=kwargs.get('acq_per',1) + acq_per=kwargs.get('acq_per',10) gather={"MaxSamples":1000000, "Period":acq_per} #Sys.ServoPeriod is dependent of !common() macro ServoPeriod= .2 #0.2ms @@ -703,7 +880,7 @@ close self.meta = {'timebase': ServoPeriod*gather['Period']} #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] # CX CZ W FY - channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"] + channels=["Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos"] prg.append('Gather.Enable=0') prg.append('Gather.Items=%d'%len(channels)) for k,v in gather.iteritems(): @@ -757,11 +934,16 @@ close pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y self.points=pt - prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics + #prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics + prg.append(' nofrax') #needed to set all axis to use AltFeedRate + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :])) prg.append(' dwell 100') prg.append(' Gather.Enable=2') + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics for i in range(pt.shape[0]): prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) prg.append(' dwell 100') @@ -776,7 +958,7 @@ close hRng = kwargs.get('hRng', (-.2,.2)) wRng = kwargs.get('wRng', (0,360000)) yRng = kwargs.get('yRng', (2.3,6.2)) - pt2pt_time = kwargs.get('pt2pt_time ', 100) + pt2pt_time = kwargs.get('pt2pt_time', 100) numPt=cntVert*cntHor pt=np.zeros((numPt,4)) if cntHor>1: @@ -798,6 +980,9 @@ close dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)] pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time) + prg.append(' nofrax') #needed to set all axis to use AltFeedRate + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)])) prg.append(' dwell 10') @@ -805,10 +990,14 @@ close if cnt>1: prg.append(' P100=%d'%cnt) prg.append(' N100:') + prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed + prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position for idx in range(1,pv.shape[0]): - prg.append(' P2000=%d'%idx) + #prg.append(' P2000=%d'%idx) prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)])) + #prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)])) + #prg.append('B%g:%g' %(idx*1000,0)) if cnt>1: prg.append(' dwell 10') prg.append(' P100=P100-1') @@ -847,6 +1036,7 @@ close ack=GpasciiCommunicator.gpascii_ack sys.stdout.write('wait execution...');sys.stdout.flush() while(True): + #Gather.MaxLines calculates maximum numbewr of gathering into memory com.write('P1000\n') val=com.read_until(ack) #print val @@ -938,20 +1128,27 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) hs.calcParam() - #hs.test_coord_trf() + hs.test_coord_trf() #hs.interactive_cx_cz_w_fy() #hs.interactive_dx_dz_w_y() + #SAR-CPPM-EXPMX1 MOTTEST-CPPM-CRM0485 hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485') #hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1) - #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0) + #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=3,wRng=(120000,120000)) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1) + #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0) + #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=5,wRng=(120000,120000)) + #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100) + #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000)) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1) - #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1, - # pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) + hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1, + pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) hs.load_rec() + hs.show_pos();hs.show_vel() hs.interactive_anim() - hs.show_vel(); plt.show() + #hs.show_vel(); plt.show() + #hs.show_pos(); plt.show() return diff --git a/python/shapepath.py b/python/shapepath.py index 83aa6fb..0c3d1b8 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -85,7 +85,7 @@ class ShapePath: #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",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="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} - cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} + cfg = {"sequencer": ['gen_grid_points(w=10,h=10,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=20,acq_per=10)','plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_swissfel_points(scale=300)','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=100,acq_per=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['opt_pts("'+fn+'.npz")','gen_prog(file="'+fn+'_corr.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'_corr.npz")']} @@ -182,7 +182,7 @@ class ShapePath: #ServoPeriod = .05 self.meta = {'timebase': ServoPeriod*gather['Period']} #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] - channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"] + channels=["Motor[3].ActPos","Motor[2].ActPos","Motor[1].ActPos","Motor[3].DesPos","Motor[2].DesPos","Motor[1].DesPos"] prg.append('Gather.Enable=0') prg.append('Gather.Items=%d'%len(channels)) for k,v in gather.iteritems():