diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex index cbf8653..3c5ab66 100644 --- a/MXfastStageDoc/MXfastStage.tex +++ b/MXfastStageDoc/MXfastStage.tex @@ -787,12 +787,22 @@ Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1] The state space controller assumes that the system is observable and controlable. The bode plot shows a flat amplitude at low frequencies, which makes the feeling, that the system is observable and controlable. But in fact the reason of that flat amplitude is friction (section \ref{sec:friction}). The viscode damping is negligable.\\ This results to the fact that the stage consists of really 2 integrators and behaves without friction roughly like $F=m \cdot \ddot{x}$. But an integrator $\frac{1}{s}$ is neighter observable nor controlable. Therefore we have to check, how to implement an optimal controller for such a system.\\ -A controller consists in a feed forward and a feed back transfer function. -\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO] -Assume a plant consists of a integrator $\frac{1}{s}$ the overall transfer functions y/u and y/e are: ... Setting H(s)=s results in a overall transfer function of y/u=... and y/e=... .\\ -\end{tcolorbox} -Simulating a single integrator plant in MATLAB works. But with 2 integrators the system is unstable because of the two derivate elements. With a discrete differentiator it becomes stable again. In fact a derivate element is a very critical element.\\ + +A controller consists of a feed forward H(s) and a feed back G(s) path. + + +\begin{figure}[h!] +\center +\includegraphics[scale=.9]{model5.eps} +\caption{$1/s^2$ plant} +\end{figure} + + +Assume a plant consists of a integrator $\frac{1}{s}$ the overall transfer functions are $\frac{y}{u}=\frac{\frac{H}{s}+\frac{G}{s}}{1+\frac{G}{s}}=\frac{H+G}{s+G}$ and $\frac{y}{e}=\frac{\frac{G}{s}}{1+\frac{G}{s}}=\frac{G}{s+G}$. Setting H(s)=s and G(s)=k results in a overall transfer function of $\frac{y}{u}=\frac{s+G}{s+G}=1$ and $\frac{y}{e}=\frac{k}{s+k}$. So the output follows the input and k defines how fast errors will be compensated.\\ + + +Simulating a single integrator plant in MATLAB works. But with 2 integrators the system is unstable because of the two derivate elements. With a discrete differentiator it becomes stable again. In fact a derivate element is a very critical element, because it can create instability and adds a lot of noise. But in this case it is acceptable, because is acts on the trajectory and not on the feedback loop. The only thing that has to be granted is that the trajectory is twice differentiable. This is the case with limited jirk or with the pvt motion. A step response should be avoided and not used to tweak the parameters.\\ \includegraphics[scale=.45]{FF_FB_ctrl.png} The optimal parameters for a pure feed forward systems are calculated @@ -815,7 +825,7 @@ k=(11.84 \ccdot 2 \ccdot \pi)^2\\ To correct errors, posErrFB is increased. But in a discrete system, the regulation becomes instable, because the actPos always lags the desPos for one sample. To avoid instability, a fiter can attenuate this problem.\\ The Standard Delta Tau controller (figure \ref{fig:deltatau_std_ctrl}), shows avery similar feed forward and feedback loop structure with additional filters. So after all the measurements, we can calculate the optimal Kfff and Kaff values. -Kp and Ki values will attenuate the errors. Kvff=Kvfb makes the system stable at hither Kp values, but setting the filter B seems to be more appropriate.\\ +Kp and Ki values will attenuate the errors. Kvff=Kvfb makes the system stable at higher Kp values, but setting the filter B seems to be more appropriate.\\ diff --git a/MXfastStageDoc/model.svg b/MXfastStageDoc/model.svg index 8c181e7..9be858c 100644 --- a/MXfastStageDoc/model.svg +++ b/MXfastStageDoc/model.svg @@ -476,6 +476,279 @@ d="M 0.25977,0 0,0.96484 6.08594,2.5957 0,4.22656 0.25977,5.19141 8.14453,3.07813 a 0.50005,0.50005 0 0 0 0,-0.96485 L 0.25977,0 Z" style="color:#000000;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:medium;line-height:normal;font-family:sans-serif;text-indent:0;text-align:start;text-decoration:none;text-decoration-line:none;text-decoration-style:solid;text-decoration-color:#000000;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;baseline-shift:baseline;text-anchor:start;white-space:normal;clip-rule:nonzero;display:inline;overflow:visible;visibility:visible;opacity:1;isolation:auto;mix-blend-mode:normal;color-interpolation:sRGB;color-interpolation-filters:linearRGB;solid-color:#000000;solid-opacity:1;fill:#000000;fill-opacity:1;fill-rule:evenodd;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;color-rendering:auto;image-rendering:auto;shape-rendering:auto;text-rendering:auto;enable-background:accumulate" /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + x or y + + + + G1(s) + H1(s) + + + + 1/s + 1/s + + + + + + + x + x + x + e2 + u2 + .. + . + + + - + + + + + H2(s) + + G2(s) + + + + + + + - + + + + + + e1 + u1 + diff --git a/MXfastStageDoc/model5.eps b/MXfastStageDoc/model5.eps new file mode 100644 index 0000000..3fbcf50 --- /dev/null +++ b/MXfastStageDoc/model5.eps @@ -0,0 +1,500 @@ +%!PS-Adobe-3.0 EPSF-3.0 +%%Creator: cairo 1.14.6 (http://cairographics.org) +%%CreationDate: Tue Feb 26 08:58:51 2019 +%%Pages: 1 +%%DocumentData: Clean7Bit +%%LanguageLevel: 2 +%%BoundingBox: 260 287 596 398 +%%EndComments +%%BeginProlog +save +50 dict begin +/q { gsave } bind def +/Q { grestore } bind def +/cm { 6 array astore concat } bind def +/w { setlinewidth } bind def +/J { setlinecap } bind def +/j { setlinejoin } bind def +/M { setmiterlimit } bind def +/d { setdash } bind def +/m { moveto } bind def +/l { lineto } bind def +/c { curveto } bind def +/h { closepath } bind def +/re { exch dup neg 3 1 roll 5 3 roll moveto 0 rlineto + 0 exch rlineto 0 rlineto closepath } bind def +/S { stroke } bind def +/f { fill } bind def +/f* { eofill } bind def +/n { newpath } bind def +/W { clip } bind def +/W* { eoclip } bind def +/BT { } bind def +/ET { } bind def +/pdfmark where { pop globaldict /?pdfmark /exec load put } + { globaldict begin /?pdfmark /pop load def /pdfmark + /cleartomark load def end } ifelse +/BDC { mark 3 1 roll /BDC pdfmark } bind def +/EMC { mark /EMC pdfmark } bind def +/cairo_store_point { /cairo_point_y exch def /cairo_point_x exch def } def +/Tj { show currentpoint cairo_store_point } bind def +/TJ { + { + dup + type /stringtype eq + { show } { -0.001 mul 0 cairo_font_matrix dtransform rmoveto } ifelse + } forall + currentpoint cairo_store_point +} bind def +/cairo_selectfont { cairo_font_matrix aload pop pop pop 0 0 6 array astore + cairo_font exch selectfont cairo_point_x cairo_point_y moveto } bind def +/Tf { pop /cairo_font exch def /cairo_font_matrix where + { pop cairo_selectfont } if } bind def +/Td { matrix translate cairo_font_matrix matrix concatmatrix dup + /cairo_font_matrix exch def dup 4 get exch 5 get cairo_store_point + /cairo_font where { pop cairo_selectfont } if } bind def +/Tm { 2 copy 8 2 roll 6 array astore /cairo_font_matrix exch def + cairo_store_point /cairo_font where { pop cairo_selectfont } if } bind def +/g { setgray } bind def +/rg { setrgbcolor } bind def +/d1 { setcachedevice } bind def +%%EndProlog +%%BeginSetup +%%BeginResource: font DejaVuSans +11 dict begin +/FontType 42 def +/FontName /DejaVuSans def +/PaintType 0 def +/FontMatrix [ 1 0 0 1 0 0 ] def +/FontBBox [ 0 0 0 0 ] def +/Encoding 256 array def +0 1 255 { Encoding exch /.notdef put } for +Encoding 40 /parenleft put +Encoding 41 /parenright put +Encoding 43 /plus put +Encoding 45 /hyphen put +Encoding 47 /slash put +Encoding 49 /one put +Encoding 50 /two put +Encoding 71 /G put +Encoding 72 /H put +Encoding 101 /e put +Encoding 115 /s put +Encoding 117 /u put +Encoding 120 /x put +/CharStrings 14 dict dup begin +/.notdef 0 def +/G 1 def +/one 2 def +/parenleft 3 def +/s 4 def +/parenright 5 def +/H 6 def +/slash 7 def +/x 8 def +/e 9 def +/two 10 def +/u 11 def +/plus 12 def +/hyphen 13 def +end readonly def +/sfnts [ +<0001000000090080000300106376742000691d3900000924000001fe6670676d7134766a0000 +0b24000000ab676c7966f083c2a40000009c0000088868656164086b92ef00000bd000000036 +686865610d9f077b00000c0800000024686d7478409e077300000c2c000000386c6f63610000 +3fb800000c640000003c6d617870047b067100000ca000000020707265703b07f10000000cc0 +0000056800020066fe96046605a400030007001a400c04fb0006fb0108057f0204002fc4d4ec +310010d4ecd4ec301311211125211121660400fc73031bfce5fe96070ef8f272062900010073 +ffe3058b05f0001d0039402000051b0195031b950812a111ae15950e91088c1e02001c113404 +3318190b101e10fcecfce4fcc4310010e4f4ecf4ec10fed4ee11393930251121352111060423 +200011100021320417152e0123200011100021323604c3feb6021275fee6a0fea2fe75018b01 +5e9201076f70fc8bfeeefeed011301126ba8d50191a6fd7f53550199016d016e01994846d75f +60fecefed1fed2fece250000000100e10000045a05d5000a004040154203a00402a005810700 +a009081f061c03001f010b10d44bb00f5458b9000100403859ecc4fcec31002fec32f4ecd4ec +304b5358592201b40f030f04025d3721110535253311211521fe014afe990165ca014afca4aa +047348b848fad5aa0000000100b0fef2027b0612000d0037400f069800970e0d070003120600 +130a0e10dc4bb0135458b9000affc038594bb00f5458b9000a00403859e432ec113939310010 +fcec300106021514121723260235341237027b86828385a0969594970612e6fe3ee7e7fe3be5 +eb01c6e0df01c4ec0001006fffe303c7047b002700e7403c0d0c020e0b531f1e080902070a53 +1f1f1e420a0b1e1f041500860189041486158918b91104b925b8118c281e0a0b1f1b0700521b +080e07081422452810fcc4ecd4ece4111239393939310010e4f4ec10fef5ee10f5ee12173930 +4b535807100eed111739070eed1117395922b2002701015d406d1c0a1c0b1c0c2e092c0a2c0b +2c0c3b093b0a3b0b3b0c0b200020012402280a280b2a132f142f152a16281e281f2920292124 +27860a860b860c860d12000000010202060a060b030c030d030e030f03100319031a031b031c +041d09272f293f295f297f2980299029a029f029185d005d7101152e012322061514161f011e +0115140623222627351e013332363534262f012e01353436333216038b4ea85a898962943fc4 +a5f7d85ac36c66c661828c65ab40ab98e0ce66b4043fae282854544049210e2a99899cb62323 +be353559514b50250f2495829eac1e000000000100a4fef2026f0612000d001f400f07980097 +0e0701000b12041308000e10dc3cf4ec113939310010fcec3013331612151402072336123534 +02a4a096959596a08583830612ecfe3cdfe0fe3aebe501c5e7e701c20000000100c90000053b +05d5000b002c4014089502ad0400810a0607031c053809011c00040c10fcec32fcec3231002f +3ce432fcec30b2500d01015d133311211133112311211123c9ca02decacafd22ca05d5fd9c02 +64fa2b02c7fd390000010000ff4202b205d50003002d4014001a010201021a03000342029f00 +8104020001032fc43939310010f4ec304b5358071005ed071005ed5922013301230208aafdf8 +aa05d5f96d0000000001003b000004790460000b014340460511060706041103040707060411 +050401020103110202010b110001000a11090a0101000a110b0a0708070911080807420a0704 +01040800bf05020a0704010408000208060c10d44bb00a544bb00f545b4bb010545b4bb01154 +5b58b90006004038594bb0145458b90006ffc03859c4d4c411173931002f3cec321739304b53 +58071005ed071008ed071008ed071005ed071005ed071008ed071008ed071005ed5922014098 +0a04040a1a04150a260a3d04310a55045707580a660a76017a047607740a8d04820a99049f04 +9707920a900aa601a904af04a507a30aa00a1c0a03040505090a0b1a03150515091a0b290326 +0525092a0b200d3a013903370534073609390b300d4903460545094a0b400d59005601590259 +0357055606590756085609590b500d6f0d78017f0d9b019407ab01a407b00dcf0ddf0dff0d2f +5d005d09022309012309013309010464fe6b01aad9febafebad901b3fe72d9012901290460fd +dffdc101b8fe48024a0216fe71018f0000020071ffe3047f047b0014001b0070402400150109 +8608880515a90105b90c01bb18b912b80c8c1c1b1502081508004b02120f451c10fcecf4ecc4 +111239310010e4f4ece410ee10ee10f4ee1112393040293f1d701da01dd01df01d053f003f01 +3f023f153f1b052c072f082f092c0a6f006f016f026f156f1b095d71015d0115211e01333236 +37150e01232000111000333200072e0123220607047ffcb20ccdb76ac76263d06bfef4fec701 +29fce20107b802a5889ab90e025e5abec73434ae2a2c0138010a01130143feddc497b4ae9e00 +000100960000044a05f0001c009e4027191a1b03181c11050400110505044210a111940da014 +910400a00200100a02010a1c171003061d10fc4bb015544bb016545b4bb014545b58b90003ff +c03859c4d4ecc0c011123931002fec32f4ecf4ec304b5358071005ed0705ed01b01c10111739 +59220140325504560556077a047a05761b87190704000419041a041b051c74007606751a731b +741c82008619821a821b821ca800a81b115d005d25211521353600373e013534262322060735 +3e01333204151406070600018902c1fc4c73018d33614da7865fd3787ad458e80114455b19fe +f4aaaaaa7701913a6d974977964243cc3132e8c25ca5701dfeeb0000000200aeffe30458047b +00130014003b401c030900030e0106870e118c0a01bc14b80c0d0908140b4e020800461510fc +ecf439ec3231002fe4e432f4c4ec1112173930b46f15c01502015d1311331114163332363511 +331123350e0123222601aeb87c7c95adb8b843b175c1c801cf01ba02a6fd619f9fbea4027bfb +a0ac6663f003a800000100d9000005db0504000b002340110009019c07030502150400170a06 +15080c10dcfc3cfc3cec31002fd43cfc3cc43001112115211123112135211103ae022dfdd3a8 +fdd3022d0504fdd3aafdd3022daa022d0001006401df027f028300030011b6009c0204010004 +10dccc310010d4ec301321152164021bfde50283a400013500b800cb00cb00c100aa009c01a6 +00b800660000007100cb00a002b20085007500b800c301cb0189022d00cb00a600f000d300aa +008700cb03aa0400014a003300cb000000d9050200f4015400b4009c01390114013907060400 +044e04b4045204b804e704cd0037047304cd04600473013303a2055605a60556053903c50212 +00c9001f00b801df007300ba03e9033303bc0444040e00df03cd03aa00e503aa0404000000cb +008f00a4007b00b80014016f007f027b0252008f00c705cd009a009a006f00cb00cd019e01d3 +00f000ba018300d5009803040248009e01d500c100cb00f600830354027f00000333026600d3 +00c700a400cd008f009a0073040005d5010a00fe022b00a400b4009c00000062009c0000001d +032d05d505d505d505f0007f007b005400a406b80614072301d300b800cb00a601c301ec0693 +00a000d3035c037103db0185042304a80448008f0139011401390360008f05d5019a06140723 +06660179046004600460047b009c00000277046001aa00e904600762007b00c5007f027b0000 +00b4025205cd006600bc00660077061000cd013b01850389008f007b0000001d00cd074a042f +009c009c0000077d006f0000006f0335006a006f007b00ae00b2002d0396008f027b00f60083 +0354063705f6008f009c04e10266008f018d02f600cd03440029006604ee0073000014000096 +0000b707060504030201002c2010b002254964b040515820c859212d2cb002254964b0405158 +20c859212d2c20100720b00050b00d7920b8ffff5058041b0559b0051cb0032508b0042523e1 +20b00050b00d7920b8ffff5058041b0559b0051cb0032508e12d2c4b505820b0fd454459212d +2cb002254560442d2c4b5358b00225b0022545445921212d2c45442d2cb00225b0022549b005 +25b005254960b0206368208a108a233a8a10653a2d0000010000000259990c370eec5f0f3cf5 +001f080000000000d184f71800000000d184f718f7d6fc4c0e5909dc00000008000000010000 +000000010000076dfe1d00000efef7d6fa510e5900010000000000000000000000000000000e +04cd006606330073051700e1031f00b0042b006f031f00a4060400c902b2000004bc003b04ec +007105170096051200ae06b400d902e300640000000000000044000000ec0000015c000001cc +0000032c00000384000003e00000042c000005b00000068400000784000008080000085c0000 +088800010000000e0354002b0068000c000200100099000800000415021600080004b8028040 +fffbfe03fa1403f92503f83203f79603f60e03f5fe03f4fe03f32503f20e03f19603f02503ef +8a4105effe03ee9603ed9603ecfa03ebfa03eafe03e93a03e84203e7fe03e63203e5e45305e5 +9603e48a4105e45303e3e22f05e3fa03e22f03e1fe03e0fe03df3203de1403dd9603dcfe03db +1203da7d03d9bb03d8fe03d68a4105d67d03d5d44705d57d03d44703d3d21b05d3fe03d21b03 +d1fe03d0fe03cffe03cefe03cd9603cccb1e05ccfe03cb1e03ca3203c9fe03c6851105c61c03 +c51603c4fe03c3fe03c2fe03c1fe03c0fe03bffe03befe03bdfe03bcfe03bbfe03ba1103b986 +2505b9fe03b8b7bb05b8fe03b7b65d05b7bb03b78004b6b52505b65d40ff03b64004b52503b4 +fe03b39603b2fe03b1fe03b0fe03affe03ae6403ad0e03acab2505ac6403abaa1205ab2503aa +1203a98a4105a9fa03a8fe03a7fe03a6fe03a51203a4fe03a3a20e05a33203a20e03a16403a0 +8a4105a096039ffe039e9d0c059efe039d0c039c9b19059c64039b9a10059b19039a1003990a +0398fe0397960d0597fe03960d03958a410595960394930e05942803930e0392fa039190bb05 +91fe03908f5d0590bb039080048f8e25058f5d038f40048e25038dfe038c8b2e058cfe038b2e +038a8625058a410389880b05891403880b03878625058764038685110586250385110384fe03 +8382110583fe0382110381fe0380fe037ffe0340ff7e7d7d057efe037d7d037c64037b541505 +7b25037afe0379fe03780e03770c03760a0375fe0374fa0373fa0372fa0371fa0370fe036ffe +036efe036c21036bfe036a1142056a530369fe03687d036711420566fe0365fe0364fe0363fe +0362fe03613a0360fa035e0c035dfe035bfe035afe0359580a0559fa03580a03571619055732 +0356fe035554150555420354150353011005531803521403514a130551fe03500b034ffe034e +4d10054efe034d10034cfe034b4a13054bfe034a4910054a1303491d0d05491003480d0347fe +0346960345960344fe0343022d0543fa0342bb03414b0340fe033ffe033e3d12053e14033d3c +0f053d12033c3b0d053c40ff0f033b0d033afe0339fe033837140538fa033736100537140336 +350b05361003350b03341e03330d0332310b0532fe03310b03302f0b05300d032f0b032e2d09 +052e10032d09032c32032b2a25052b64032a2912052a25032912032827250528410327250326 +250b05260f03250b0324fe0323fe03220f03210110052112032064031ffa031e1d0d051e6403 +1d0d031c1142051cfe031bfa031a42031911420519fe031864031716190517fe031601100516 +190315fe0314fe0313fe031211420512fe0311022d05114203107d030f64030efe030d0c1605 +0dfe030c0110050c16030bfe030a100309fe0308022d0508fe030714030664030401100504fe +03401503022d0503fe0302011005022d0301100300fe0301b80164858d012b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b002b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b2b +2b2b2b2b2b2b2b2b2b2b2b1d00> +] def +/f-0-0 currentdict end definefont pop +%%EndResource +%%BeginResource: font AbyssinicaSIL +11 dict begin +/FontType 42 def +/FontName /AbyssinicaSIL def +/PaintType 0 def +/FontMatrix [ 1 0 0 1 0 0 ] def +/FontBBox [ 0 0 0 0 ] def +/Encoding 256 array def +0 1 255 { Encoding exch /.notdef put } for +Encoding 46 /period put +/CharStrings 2 dict dup begin +/.notdef 0 def +/period 1 def +end readonly def +/sfnts [ +<00010000000900800003001063767420024f10a4000001ac0000001a6670676d06599c370000 +01c800000173676c7966576c884d0000009c000001106865616400ce5d4b0000033c00000036 +686865610cd808450000037400000024686d74780a8d00e700000398000000086c6f63610000 +0164000003a00000000c6d61787002230559000003ac0000002070726570181aca2d000003cc +000000720002006eff030792053d00030007002900b800062fb800004558b800042f1bb90004 +00093e59b800004558b800002f1bb9000000073e59303113112111252111218706f2f8f50724 +f8dc0524f9f8060819f9c60000010079000001b4013b00130079bb00000004000a00042b4107 +0009000a0019000a0029000a00035d00b800004558b800052f1bb9000500053e59b8000fdc41 +1b0007000f0017000f0027000f0037000f0047000f0057000f0067000f0077000f0087000f00 +97000f00a7000f00b7000f00c7000f000d5d410500d6000f00e6000f00025d303125140e0223 +222e0235343e0233321e0201b4182b3921213a2b18182b3a2121392b189e213a2a19192a3a21 +21392b18182b39000000002a003d007b003d01120004000c0510001b053b002b05db00460000 +b800002c4bb800095058b101018e59b801ff85b800441db9000900035f5e2db800012c202045 +6944b001602db800022cb800012a212db800032c2046b003254652582359208a208a49648a20 +4620686164b004254620686164525823658a592f20b00053586920b000545821b040591b6920 +b000545821b0406559593a2db800042c2046b00425465258238a592046206a6164b004254620 +6a61645258238a592ffd2db800052c4b20b0032650585158b080441bb04044591b21212045b0 +c05058b0c0441b2159592db800062c2020456944b001602020457d691844b001602db800072c +b800062a2db800082c4b20b003265358b0401bb000598a8a20b0032653582321b0808a8a1b8a +235920b0032653582321b800c08a8a1b8a235920b0032653582321b801008a8a1b8a235920b0 +032653582321b801408a8a1b8a235920b80003265358b0032545b8018050582321b801802321 +1bb003254523212321591b2159442db800092c4b535845441b2121592d000001000000017fff +d7c4e79a5f0f3cf50009080000000000cecbfe5e00000000ced094a7f8bcfd560b5807f20000 +000900020001000000000001000006c1fec102000b8ff8bcfdf20b5800010000000000000000 +00000000000000020800006e028d0079000000000000005400000110000100000002014a0010 +00bd0005000100000000000a00000200035000010001b800002b00ba0001000200022b01ba00 +03000200022b01bf000300b80096007500540032000000082bbf0004002a0022001b0013000c +000000082b00bf000100b80096007500540032000000082bbf0002005c004b003b002a001900 +0000082b00ba0005000400072bb8000020457d691844000000> +] def +/f-1-0 currentdict end definefont pop +%%EndResource +%%EndSetup +%%Page: 1 1 +%%BeginPageSetup +%%PageBoundingBox: 260 287 596 398 +%%EndPageSetup +q 260 287 336 111 rectclip q +0 g +0.58701 w +0 J +0 j +[] 0.0 d +4 M q 1 0 0 -1 0 841.889771 cm +453.543 488.145 30.594 27.762 re S Q +q 1 0 0 -1 0 841.889771 cm +365.668 445.625 30.594 27.762 re S Q +BT +12 0 0 12 454.07006 336.511261 Tm +/f-0-0 1 Tf +(G)Tj +7.8 0 0 7.8 463.374747 334.111261 Tm +(1)Tj +12 0 0 12 468.341153 336.511261 Tm +(\(s\))Tj +-8.52137 3.517765 Td +(H)Tj +7.8 0 0 7.8 375.108146 376.324444 Tm +(1)Tj +12 0 0 12 380.074552 378.724444 Tm +(\(s\))Tj +ET +0.79937 w +q 1 0 0 -1 0 841.889771 cm +430.172 459.719 m 430.172 461.184 428.988 462.371 427.523 462.371 c 426.059 + 462.371 424.875 461.184 424.875 459.719 c 424.875 458.258 426.059 457.07 + 427.523 457.07 c 428.988 457.07 430.172 458.258 430.172 459.719 c h +430.172 459.719 m S Q +0.559705 w +q 1 0 0 -1 0 841.889771 cm +456.363 445.609 27.789 27.785 re S Q +q 1 0 0 -1 0 841.889771 cm +532.898 445.613 27.789 27.785 re S Q +BT +12 0 0 12 460.917813 378.286456 Tm +/f-0-0 1 Tf +[(1/s)-4860(1/s)]TJ +ET +0.79937 w +q 1 0 0 -1 0 841.889771 cm +430.574 459.508 m 456.086 459.508 l S Q +449.656 384.46 m 449.449 383.687 l 454.316 382.382 l 449.449 381.081 l +449.656 380.308 l 455.961 381.999 l 456.352 382.105 456.352 382.663 455.961 + 382.769 c h +449.656 384.46 m f* +q 1 0 0 -1 0 841.889771 cm +484.43 459.508 m 532.898 459.508 l S Q +526.473 384.46 m 526.266 383.687 l 531.129 382.382 l 526.266 381.081 l +526.473 380.308 l 532.773 381.999 l 533.168 382.105 533.168 382.663 532.773 + 382.769 c h +526.473 384.46 m f* +q 1 0 0 -1 0 841.889771 cm +560.688 459.508 m 607.965 459.508 l S Q +q 1 0 0 -1 0 841.889771 cm +322.562 459.508 m 340.051 459.508 l S Q +333.621 384.46 m 333.414 383.687 l 338.281 382.382 l 333.414 381.081 l +333.621 380.308 l 339.926 381.999 l 340.316 382.105 340.316 382.663 339.926 + 382.769 c h +333.621 384.46 m f* +q 1 0 0 -1 0 841.889771 cm +396.266 459.785 m 424.875 459.719 l S Q +418.441 384.23 m 418.238 383.456 l 423.105 382.167 l 418.242 380.851 l +418.453 380.081 l 424.75 381.784 l 425.145 381.89 425.141 382.448 424.75 + 382.554 c h +418.441 384.23 m f* +q 1 0 0 -1 0 841.889771 cm +405.062 502.027 m 424.902 502.027 l S Q +418.477 341.941 m 418.27 341.167 l 423.133 339.862 l 418.27 338.562 l 418.477 + 337.788 l 424.777 339.48 l 425.172 339.585 425.172 340.144 424.777 340.249 + c h +418.477 341.941 m f* +BT +12 0 0 12 438.036856 387.159796 Tm +/f-0-0 1 Tf +(x)Tj +5.047274 -0.161788 Td +(x)Tj +6.084001 0.199007 Td +(x)Tj +-21.021069 -6.664575 Td +(e)Tj +7.8 0 0 7.8 326.74213 305.231524 Tm +(2)Tj +12 0 0 12 270.812931 387.001593 Tm +(u)Tj +7.8 0 0 7.8 278.430118 384.601593 Tm +(2)Tj +12 0 0 12 438.022208 395.794073 Tm +/f-1-0 1 Tf +(..)Tj +5.206453 -0.161788 Td +(.)Tj +/f-0-0 1 Tf +-6.954862 -0.560555 Td +(+)Tj +0.0529093 -1.534005 Td +(-)Tj +ET +0.8 w +q 1 0 0 -1 0 841.889771 cm +453.531 502.305 m 430.574 502.027 l S Q +436.98 337.71 m 437.195 338.48 l 432.344 339.843 l 437.227 341.089 l 437.031 + 341.862 l 430.703 340.249 l 430.309 340.148 430.301 339.589 430.691 339.476 + c h +436.98 337.71 m f* +q 1 0 0 -1 0 841.889771 cm +427.738 499.191 m 427.523 462.371 l S Q +425.484 373.077 m 426.258 372.874 l 427.535 377.749 l 428.867 372.886 l + 429.637 373.101 l 427.91 379.398 l 427.801 379.792 427.242 379.788 427.137 + 379.394 c h +425.484 373.077 m f* +q 1 0 0 -1 0 841.889771 cm +509.934 459.785 m 509.941 502.027 l 484.43 502.027 l S Q +490.863 337.788 m 491.07 338.558 l 486.203 339.862 l 491.07 341.167 l 490.863 + 341.941 l 484.555 340.249 l 484.164 340.144 484.164 339.585 484.555 339.48 + c h +490.863 337.788 m f* +0.58701 w +q 1 0 0 -1 0 841.889771 cm +291.969 445.625 30.594 27.762 re S Q +BT +12 0 0 12 292.383829 379.030987 Tm +/f-0-0 1 Tf +(H)Tj +7.8 0 0 7.8 301.407267 376.630987 Tm +(2)Tj +12 0 0 12 306.373673 379.030987 Tm +(\(s\))Tj +ET +0.58701 w +q 1 0 0 -1 0 841.889771 cm +453.543 524.996 30.594 27.762 re S Q +BT +12 0 0 12 454.070011 299.66087 Tm +/f-0-0 1 Tf +(G)Tj +7.8 0 0 7.8 463.374698 297.26087 Tm +(2)Tj +12 0 0 12 468.341104 299.66087 Tm +(\(s\))Tj +ET +0.79937 w +q 1 0 0 -1 0 841.889771 cm +345.348 459.508 m 345.348 460.969 344.16 462.156 342.699 462.156 c 341.234 + 462.156 340.051 460.969 340.051 459.508 c 340.051 458.043 341.234 456.855 + 342.699 456.855 c 344.16 456.855 345.348 458.043 345.348 459.508 c h +345.348 459.508 m S Q +q 1 0 0 -1 0 841.889771 cm +345.535 459.508 m 365.375 459.508 l S Q +358.949 384.46 m 358.742 383.687 l 363.605 382.382 l 358.742 381.081 l +358.949 380.308 l 365.25 381.999 l 365.645 382.105 365.645 382.663 365.25 + 382.769 c h +358.949 384.46 m f* +0.8 w +q 1 0 0 -1 0 841.889771 cm +453.25 538.875 m 345.535 538.875 l S Q +351.965 300.937 m 352.172 301.71 l 347.305 303.015 l 352.172 304.319 l +351.965 305.089 l 345.656 303.401 l 345.266 303.292 345.266 302.734 345.656 + 302.628 c h +351.965 300.937 m f* +q 1 0 0 -1 0 841.889771 cm +342.699 536.043 m 342.699 462.34 l S Q +340.621 373.116 m 341.395 372.909 l 342.699 377.776 l 344.004 372.909 l + 344.773 373.116 l 343.086 379.425 l 342.977 379.819 342.418 379.819 342.312 + 379.425 c h +340.621 373.116 m f* +q 1 0 0 -1 0 841.889771 cm +580.809 459.508 m 580.809 538.875 l 484.43 538.875 l S Q +490.863 300.937 m 491.07 301.71 l 486.203 303.015 l 491.07 304.319 l 490.863 + 305.089 l 484.555 303.401 l 484.164 303.292 484.164 302.734 484.555 302.628 + c h +490.863 300.937 m f* +0.79937 w +q 1 0 0 -1 0 841.889771 cm +260.176 459.508 m 291.676 459.508 l S Q +285.246 384.46 m 285.039 383.687 l 289.906 382.382 l 285.039 381.081 l +285.246 380.308 l 291.551 381.999 l 291.945 382.105 291.945 382.663 291.551 + 382.769 c h +285.246 384.46 m f* +BT +12 0 0 12 344.623722 371.073126 Tm +/f-0-0 1 Tf +(-)Tj +-1.051815 1.337736 Td +(+)Tj +ET +q 1 0 0 -1 0 841.889771 cm +430.387 501.84 m 430.387 503.305 429.199 504.488 427.738 504.488 c 426.273 + 504.488 425.09 503.305 425.09 501.84 c 425.09 500.379 426.273 499.191 427.738 + 499.191 c 429.199 499.191 430.387 500.379 430.387 501.84 c h +430.387 501.84 m S Q +q 1 0 0 -1 0 841.889771 cm +345.348 538.691 m 345.348 540.152 344.16 541.34 342.699 541.34 c 341.234 + 541.34 340.051 540.152 340.051 538.691 c 340.051 537.227 341.234 536.043 + 342.699 536.043 c 344.16 536.043 345.348 537.227 345.348 538.691 c h +345.348 538.691 m S Q +q 1 0 0 -1 0 841.889771 cm +320.02 538.875 m 339.863 538.875 l S Q +333.438 305.089 m 333.23 304.316 l 338.094 303.015 l 333.23 301.71 l 333.438 + 300.941 l 339.738 302.628 l 340.133 302.734 340.133 303.292 339.738 303.398 + c h +333.438 305.089 m f* +BT +12 0 0 12 401.782071 343.372979 Tm +/f-0-0 1 Tf +(e)Tj +7.8 0 0 7.8 409.164884 340.972979 Tm +(1)Tj +12 0 0 12 399.391495 388.053009 Tm +(u)Tj +7.8 0 0 7.8 407.008683 385.653009 Tm +(1)Tj +ET +Q Q +showpage +%%Trailer +end restore +%%EOF diff --git a/matlab/DeltaTauParam.m b/matlab/DeltaTauParam.m index 205c307..719c87a 100644 --- a/matlab/DeltaTauParam.m +++ b/matlab/DeltaTauParam.m @@ -10,11 +10,30 @@ function [pb]=DeltaTauParam(mot) 'Ts', 2E-4, ... % 0.2ms=5kHz 'MaxDac' ,2011.968, ... 'MaxPosErr', 10000); + %pb.ss_plt=mot.ss_plt; pb.sel={3,[3]}; + %pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]}; + %pb.ss_plt=mot.ss_c1; pb.sel={3,[3]}; + %pb.ss_plt=mot.ss_d1; pb.sel={3,[3]}; + %pb.ss_plt=mot.ss_1; pb.sel={2,[2]}; + %pb.ss_plt=mot.ss_p; pb.sel={2,[2]}; + %pb.ss_plt=mot.ss_q; pb.sel={2,[2]}; + %pb.ss_plt=mot.ss_cq; pb.sel={3,[3]}; + %pb.ss_plt=mot.ss_cqr; pb.sel={3,[3]}; + pb.A=[1];pb.B=[1];pb.C=[1];pb.D=[1];pb.E=[1];pb.F=[1]; + if mot.id==1 %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,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,homing='enc-index') %pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000; pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000; + %pb.Kp=0.1;pb.Kvfb=0;pb.Ki=0.00;pb.Kvff=0;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000; + %filter [z^0 z^-1 ... z^-n]; + pb.A=[1]; + pb.B=[1]; + pb.C=[1]; + pb.D=[1]; + pb.E=[1]; + pb.F=[1]; %19.8Hz 0dB K=(19.8*2*np.pi)**2=15477.1 Ts=5kHz=.2ms %Kaff = 1/(Ts*Ts*K) = 1/((19.8*2*np.pi)**2/5000**2) = 1615.2877200403302 %Kfff=100 @@ -29,13 +48,6 @@ function [pb]=DeltaTauParam(mot) %pb.MaxInt=200 %200mA should be enought to fix static errors end - pb.ss_plt=mot.ss_plt; pb.sel={3,[3]}; - %pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]}; - %pb.ss_plt=mot.ss_c1; pb.sel={3,[3]}; - %pb.ss_plt=mot.ss_d1; pb.sel={3,[3]}; - %pb.ss_plt=mot.ss_1; pb.sel={2,[2]}; - %pb.ss_plt=mot.ss_p; pb.sel={2,[2]}; - %pb.ss_plt=mot.ss_q; pb.sel={2,[2]}; %mdlName='stage_closed_loop'; %open(mdlName) diff --git a/matlab/DeltaTauSim.slx b/matlab/DeltaTauSim.slx new file mode 100644 index 0000000..d8c6e00 Binary files /dev/null and b/matlab/DeltaTauSim.slx differ diff --git a/matlab/sample.slx b/matlab/sample.slx index 9a75239..fc76441 100644 Binary files a/matlab/sample.slx and b/matlab/sample.slx differ diff --git a/python/ShapePathAnalyser/ShapePathAnalyser.py b/python/ShapePathAnalyser/ShapePathAnalyser.py index 4b783f2..e1205ed 100755 --- a/python/ShapePathAnalyser/ShapePathAnalyser.py +++ b/python/ShapePathAnalyser/ShapePathAnalyser.py @@ -6,6 +6,7 @@ # | Author Thierry Zamofing (thierry.zamofing@psi.ch) | # *-----------------------------------------------------------------------* from __future__ import print_function +#from __future__ import absolute_import,division,generators,nested_scopes,print_function,unicode_literals,with_statement import os,sys import wx import wx.py diff --git a/python/helicalscan.py b/python/helicalscan.py index d0dfe24..d7f4988 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -62,7 +62,8 @@ verbose bits: #points: #dx,dz,w,fy -> pb coord. be aware of inverted dx,dz signs - +from __future__ import print_function +#from __future__ import absolute_import,division,generators,nested_scopes,print_function,unicode_literals,with_statement import os, sys, json,re import numpy as np import matplotlib as mpl @@ -644,8 +645,9 @@ class HelicalScanTests(): class HelicalScan(MotionBase): - def __init__(self,comm, gather, verbose): - MotionBase.__init__(self,comm, gather, verbose) + def __init__(self, comm, gather, verbose, **kwargs): + if comm==None: return + MotionBase.__init__(self, comm, gather, verbose, **kwargs) def load_rec(self,fn_npz='/tmp/helicalscan.npz'): try: @@ -849,12 +851,17 @@ class HelicalScan(MotionBase): if comm is None:return gt=self.gather gt.set_phasemode(False) - #gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos") - gt.set_address("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos","Gate3[1].Chan[1].UserFlag") + #up to now the trigger is not saved + #if self.meta['sync_flag']&2: + # address=("Motor[4].ActPos", "Motor[5].ActPos", "Motor[3].ActPos", "Motor[1].ActPos","Coord[1].Q[11]") + #else: + # address=("Motor[4].ActPos", "Motor[5].ActPos", "Motor[3].ActPos", "Motor[1].ActPos","Gate3[1].Chan[1].UserFlag") + address=("Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos") + gt.set_address(*address) gt.set_property(MaxSamples=1000000, Period=acq_per) ServoPeriod= .2 #0.2ms #Sys.ServoPeriod is dependent of !common() macro #ServoPeriod=comm.gpascii.servo_period - self.meta = {'timebase': ServoPeriod*acq_per} + self.meta.update({'acq_per':acq_per,'address':address}) def setup_coord_trf(self,fnCrdTrf='/tmp/coordTrf.cfg'): param=self.param @@ -1107,6 +1114,7 @@ close yRng = kwargs.get('yRng', self.param[:,1]) pt2pt_time = kwargs.get('pt2pt_time', 100) smt = kwargs.get('smt', 1) # SegMoveTime, default = 1ms -> velocity calc not yet 100% correct (smt=0 not 100% working) + dwell = kwargs.get('dwell', 100) # wait time at end of motion numPt=cntVert*cntHor pt=np.zeros((numPt,4)) @@ -1147,8 +1155,6 @@ close prg.append(' Coord[1].SegMoveTime=%d'%smt) #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]): - if sync_frq is not None and idx%sync_frq==0: - prg.append('Coord[1].Q[0]=%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)) @@ -1159,14 +1165,11 @@ close prg.append(' {') prg.append(' linear abs') prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0, 1, 2, 3)])) - prg.append(' dwell 100') + prg.append(' dwell %d' % dwell) prg.append(' goto 100') prg.append(' }') else: - prg.append(' dwell 1000') - if sync_frq is not None: - prg.append(' Coord[1].Q[0]=-1') - prg.append(' Coord[1].Q[1]=0') + prg.append('dwell %d' % dwell) prg.append(' Gather.Enable=0') prg.append('close') #prg.append('&1\nb%dr\n'%prgId) @@ -1188,6 +1191,10 @@ close gt=self.gather gt.wait_stopped(verbose=True) self.rec=rec=gt.upload() + + channels=self.meta['address'] + + channels = ["Motor[4].HomePos", "Motor[5].HomePos", "Motor[3].HomePos", "Motor[1].HomePos"] ofs = np.ndarray(len(channels)) for i, v in enumerate(channels): @@ -1208,7 +1215,22 @@ if __name__=='__main__': comm = PPComm(host=args.host) gather = Gather(comm) gpascii = comm.gpascii - hs=HelicalScan(comm, gather, args.verbose) + + # direct start + #hs=HelicalScan(comm, gather, args.verbose,sync_mode=0) + + #simulated start and frame trigger no sync + #hs=HelicalScan(comm, gather, args.verbose,sync_mode=1,sync_flag=3) + + #simulated start and frame trigger with sync + #hs=HelicalScan(comm, gather, args.verbose,sync_mode=2,sync_flag=3) + + #simulated start real frame trigger no sync + #hs=HelicalScan(comm, gather, args.verbose,sync_mode=1,sync_flag=1) + + #simulated start real frame trigger with sync + hs=HelicalScan(comm, gather, args.verbose,sync_mode=2,sync_flag=1) + if mode==0: # gpasci: #1,4,5p // y,-x ,-z # 0deg 256.7 -762.5 -396.4 @@ -1222,6 +1244,13 @@ if __name__=='__main__': y=(1405.7, 1019.2), z=((-1309.6, -1010.9, -2410.3), (-1219.4, -918.8, -2510.4))) + hs.calcParam(x=((-70.25682272433501, 287.48437780001825, -477.99870973215764), + (-41.466601738723284, 274.71565975031103, -504.9508517714285)), + y=(1207.6326052816642, 1704.2138281362475), z=( + (-1196.2847548574225, -1686.284754857423, -1686.284754857423), + (-1196.2847548574225, -1686.284754857423, -1686.284754857423))) + + ### use simulation motors ### # os.chdir(os.path.join(os.path.dirname(__file__),'../cfg')) # 'sim_8_motors.cfg' @@ -1232,22 +1261,21 @@ if __name__=='__main__': # fh=open("/sf/bernina/config/swissmx/exchange/helical.cmd") # fh=open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/helical.cmd") #fh = open("/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/lyso001_0063_helical_debug.cmd") - fh = open("/sf/bernina/data/p17592/res/20181130/helicaltest/lyso005/lyso005_0072_helical_debug.cmd") + #fh = open("/sf/bernina/data/p17592/res/20181130/helicaltest/lyso005/lyso005_0072_helical_debug.cmd") # find /sf/bernina/data/p17592/res/ -name '*.cmd' - s = fh.read(); - s = s.replace('calcParam', 'hs.calcParam') - eval(s) + #s = fh.read(); + #s = s.replace('calcParam', 'hs.calcParam') + #eval(s) # &1p # cpx X0 Z0 B0 Y258 # cpx X0 Z0 B120000 Y258 - - hs.setup_coord_trf() - hs.setup_sync(mode=0) # None: no sync at all mode=1: sync on timing UserFlag - hs.setup_gather() + hs.setup_gather(acq_per=1) + hs.setup_sync(verbose=args.verbose & 32) + hs.setup_coord_trf() # reset to shape path system # hs.gen_prog(mode=-1) # hs.gen_prog(mode=0,cntHor=1,cntVert=3,wRng=(120000,120000)) @@ -1274,6 +1302,8 @@ if __name__=='__main__': # hs.gen_prog(mode=1,cntHor=7,cntVert=2,hRng=(-100,50),wRng=(000,10000),smt=0) hs.run() + hs.wait_armed() # wait until motors are at first position + hs.trigger(0.5) # send a start trigger (if needed) ater given time print('wait until gather finished:') fn = '/tmp/helicalscan' hs.gather_upload(fn + '.npz') diff --git a/python/shapepath.py b/python/shapepath.py index f5f8e2b..1f66e4d 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -41,6 +41,7 @@ Enc 7: Interferometer X ''' from __future__ import print_function +#from __future__ import absolute_import,division,generators,nested_scopes,print_function,unicode_literals,with_statement import os, sys, time import numpy as np import matplotlib as mpl @@ -674,7 +675,7 @@ class ShapePath(MotionBase): ts=self.meta['srv_per'] scale=kwargs.get('scale', 1.) cnt=kwargs.get('cnt', 1) # move path multiple times - dwell=kwargs.get('dwell', 100) # synchronization mark all n points + dwell=kwargs.get('dwell', 100) # wait time at end of motion CoordFeedTime=1000. #Defaut deltatau value try: pt=self.ptsCorr