From 9c6d9df97f06286935d16b74d18e37d63587e6a3 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 6 Oct 2022 13:11:35 +0200 Subject: [PATCH] enhance sync part 2 --- Readme.md | 7 +++---- python/shapepath.py | 4 ++-- src/triggerSync/triggerSync | Bin 67398 -> 67570 bytes src/triggerSync/triggerSync.c | 24 ++++++++++++++++++++---- 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/Readme.md b/Readme.md index 0d12a4f..5e4f771 100644 --- a/Readme.md +++ b/Readme.md @@ -1309,9 +1309,6 @@ start debug tools PPMAC=SAR-CPPM-EXPMX1 PBInspect --host=$PPMAC& gpasciiCommander --host $PPMAC -i -ssh root@$PPMAC -ssh root@$PPMAC rm /tmp/gather_server /tmp/triggerSync -ssh root@$PPMAC ls -l /tmp ``` restart IOC @@ -1323,7 +1320,6 @@ telnet $PPMAC 50001 Ctrl-X dbgf SAR-CPPM-EXPMX1:MOD_VER -caget SAR-CPPM-EXPMX1:MOD_VER ``` @@ -1331,6 +1327,7 @@ checking versions ----------------- ``` git: 7a968aac967 +caget SAR-CPPM-EXPMX1:MOD_VER asyn 427.0.2 motorBase alpha_220518 asynMotor alpha_220518 @@ -1461,6 +1458,8 @@ EncTable[20].index5 = 0 EncTable[20].index5 = 0 EncTable[20].ScaleFactor=1E-3 +Sys.Udata[0];EncTable[20].PrevEnc + 2.1 trigger zu spät 1.9 trigger zu früh diff --git a/python/shapepath.py b/python/shapepath.py index 9fd0563..90eb0ce 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -771,8 +771,8 @@ EncTable[20].ScaleFactor=1E-3 prg.append('N100:') prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position for idx in range(1,pv.shape[0]): - prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)])) - #if idx%78==10: #sync data all 256 points + prg.append(f'N{idx} ' + 'X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)])) + #if idx%256==4: #sync data all 256 points # prg.append('Coord[1].Q[12]=Sys.ServoCount') # #prg.append(f'Coord[1].Q[13]={idx}') # prg.append(f'Coord[1].Q[13]=Motor[1].DesPos') diff --git a/src/triggerSync/triggerSync b/src/triggerSync/triggerSync index 0eb681fc829564dd00958f97b2cb570c2f693be6..7aa55cb25cbf6a9594550381e2edb750cae4b20c 100755 GIT binary patch delta 10208 zcmb_ieN-G(mcP|q-E@bKGzozQI?xc3G|;q!#E2S%C_CFmG$_n2Q8bW10!k7jB#TZb z)r>gH%)znQ!n3%>gc&m#ToWg=(VcisR6IvrTt6nW8(rd{OD0j132{h_d*Tu7{oSfp zP|1n<&-6J^?|1Kg_ua2|tGZsl`E&Wg<8mb1awgK7v+kyR2NC~~jb!{Mk{G*W{SUC3 z$m#|?M({1*=YX&M^}c?c^R$a{wZB8M6l+mNF-M-GsiIe0askip*RkaWU_baT=vZoY z$^JTs%85iNxE8u7+s9MEkAqwa_>+J=LVhQ3E3lBatOFhg9uR!lB`f|~lYkuyb^$l( z0+vqTBq&}X@EuTajK~re_(7K(z{1)Bskr_R1lXV&A+Q>_9JpKHBXB$nxd#e!fDI4IfH`7y`U3O}@G@Y-0Ys!g2CfznP%k&o zk_0{?APIfR(s~GR$czxKgg}ovV9S)c4-MJgv?~4}#FHRFdbVfJ)yG^33G>8w6lH6S zPeI-oj}y%@#uteajqxOrLyXy6pD{-4yv7)7#?K=~TbRpcj={(wT4;>@*oww@8PQ^6 zyaMJL<8q>T#<+sWV~nfewK1+GN;1X`L~di;0`F4|&`y+Rj6+0=jBywCpE2%1c^l(C zqI6@tOMPcnT1Hm0GHlrw&WK*RCQP()H#vfG);X+kJV?net8#2nQFaid(Y~zcM95*; zr@lsx(jk`|ktnGgNS@A8@`Rm4Y<5u2sAqhAF3Bd^ z?W~YpHz@8t@ByUOGErqQ5t!Hz)TL|lA zQeUq^k>N~=Obs;om^bXqjZDT<&h@2KSnMRXFlE1PieZB1-YfVc%&)jnD-h4hVj@=V z4$7WJHV8`Oh|^!o*UnsAn`xd{tbZgn8|n4DsGf2aC3_y63OgdO)nm3*^(SUz`^(4U zsYpc3Ftcbfj`AkG6oH0#wIwlaR{BG{h*o2f#N_)gTWbJoIk+jMD-!9XO(8hGH(5YKkcqJKj@VM=&T&D=5=a+fa0FWK80~ z8_TLk=cHLs*5h*)NM}^n+!Wh5rZ%enxz{KI+#mjULaI?uNYxQZY6z9ac0?4qB#an9 z50vWYCj(HpD=4RGn+4;&D6K(6aj#xlsV^g4yUgj*OG}b_6^Vv3CAvy!4Hi2WeOql| zCqQ9{zG>}TOS{y`x#_uI!e@^i5R24J3+GPLYOu&-MzQslQlxh+vC-(bz6S7`d5%`go_Xo1Q!^SaE2e(6 za$dgj67pZh4X2uHKD}0=r-O3J`A`pv`GNEf@FR$5-;Pyvt{u5+Tr0ZZ zZXxccXS5{$>Pr4f@MknzUO_cm`kfhC54vLi4vmOZg`NI}kki_sRX#!(l$GSCzdvrt z&EFEY$V}Yk9SF+FcbanCjU4yD=x*eq$CPuUUfv5keW8ewNv&c0VZXJ*8?L&36j9~0 zXEPOt#vux>S?{o~J?q!*?R-~3&&-<|&{}-m)B!5u`9rw#aEmX=r{oc5wiX%V&SW$i z1wb--VF~6gNlK6&h?yl3asGw}cB1$Mp+eMt>NTyVV$oCgJqtWkb zSd-M`L(sG?%E8iZJYoIz+{w4ET+4f%xxNr5C`7?ZldfT`p*KE7bUxf%>k#dtsr{4( zdpVOwITOcUm5K!(c?)CSA*(<7;>QTQhcS%E7|E0z;Y=wGN|&MLwVOBQYx2C~&2_h0C#nMoBTL$1c?$DQ2E^SJ{~KXnW!P?E>Q5vShpO}*53 zIvg=6AE1dTVAiFVg8Xzt5tRc_E+CYq8Qb}r#N5I>%b^V8pODHWsX z*XYeY;pv$u%+57O0fy(WB5pB!pXJ9CT~DstbOUKJ-#|ike4)_3?4GROyH+YHE zJ-(MJ%N{SoHza(4AJZ;W{xF^=F_hA+H{ybOCfTFW6NR`|Mx%eK#5IKLL0or5qo=&M z9*IUjO~Q3F8a>mh4&-KMj*S53;EzAdBI)5ul0qwOgm*H#)DyY?wB!htCyZGt_gG)G zB(7A-<1<+-ijVoqW8;;ix;JlwCEKUQ_4eK?_Z^~wQg7lR zpTF<9vjeM6Vsb%!+s5roB^ump>1_!#wzUDGc$i%mXx`l1-dx|({BUCf$UoA_wSi`U zkPd1z^LD6Tzn>wURMS4lRZb!qJD_{%`fY9P^;;hx=@X)7i>#fph9frXs39m4jH@S1^{nOym&ZTYbRqgd{n0tZfSO0D8 zozO@v&FdS%$N9s!#qytQMy4x3CtIj|`QnN(4)Y=w1#G{~e|wjr{DFu;WJ<_#OBDg8T_ zZe1^Yx&ylVti63qs;Fl@f)ZhhrbiHZn4s$slx|S8_Mni&1vF~y4I8AQY0{+~OE`eV1;}sU1r<*0(n_vyr=OoVzfZJ24nEt?w6#32wL!fcJUR-b&BX~twhJIJ)@$$~hzgB)lH z*I&4HYkPzG{>c1vf9-N@!zbWX{&W43&5Bpcbu9DTZZ-aR%02#u<;Lz$z;S_`po=su zH~K(A8dr@@ZCNflLPGisv30q=o4Ge6WN=aF!tJVZJUQLpA=p}bNbt_^L+ZifX;PQ^ z_VF|&EQIuqkg#Azs(b0CEe(w%Wn;ld+4?{d@>JPgREg9%Y3lo;m8GtRsi+|8T%6?K>3v@#_}pJ@x3= zhV-X)Zs@qCwB&Y2snxl=;Pan0Ro<%#)aB~71fbR^x>lvVI(R>9e8 zEM>Fr%jW-emcZOKR#(j<6<;{!I#So{X>w$*>22Ec%L9(PZ?nFr1g!7U?h;FkI(Xt* zY4p&?C+178BUju;4#l2qlIIkY^}#V%8-p9zoKdnqK6dEQ&kC%IQe5na_~^F*qJIR2 zPh{6mp>U5G`Ezz48&6&Q7c(Hl`Qi}cCvfE{7Cr+b{HeaP+4ej{F)v50#85w8QwPqb zsE?nGwc{{^^6_4qIc^Ny>M#!x)_jjT!9z^V&r#ju*>?X5ym()smhlkl`-5r|4{I^3 zRU_jIZ2V%rRy_IvIp?W{=lHxb#8(5vO0Kf zvF!kAb%T2BT(;$8l6nc^hgI+Ai!7h5RfD*kzCmrpD6kfI+@rNC=Hui~ySF16n!I+(1HM*i;Ol2I zRpO=&0=H#63~+R`3F0_{L!{wkz8Yslkmpl4#)y{2*B0V~AGKxT^P4#fS_*mHjr@!u zFx$7yuoEd}J0F3XGQsC%e+D%!b=UdKJC86Gbry@uZx9{iV;cV_w3t`<09fNgE3a}A zSaV9OGU1|pS&h4pQ50aZ1TuPyl!(R4fqC0uaVZsxvs%W?$AeG6qt;#sDE>;2NwN?^ zA+3RS%_o?L)uj8cz&n7oTtOboNk73}_G5Z60s?O&oFD3;_Ug;-0X_!b!djw8ItY38 zhL7u5{uP%(S%n}h6NGnM}RFG7l4a_`A8Lfw%WT4n9uhZ z$KiSH5^$lwZt&SbufRDCAhdws5Cj}5d&f-zD}YB$3W8WL49q8_yujb_Lo{%lz#AYx z2`mh@v_ZaA9seRziGXUDp+f4QNoo(I27!}=mIH`e9k6jK`ETHJSPZv@fZ1vh2FrWU zR|D)3^FM@Dr+`HmEuXrmD9dXiT!aFy4-p5e1q;GW<1F4f8+@O7@uC-mm? zWGm9|Wq};sPwNDKodM=z#n!cc4L-Xi;w;Bt!7gACN12a*kuBpyUtR+FVemzqVglE%fgc2J0v6>U?*Qid6N#073LFP4A}Bur16iM8 zAPT|T18l7KTsa6MCc^LFfd`mFjF0>uNI?w`&~ngSt%*LhE2ka9Hegwb#$jl46U$)1sR8`t1sL7FK8otbjre8HW%b`>o)YVIWd({~_>UV25t7 zy&m%Az}-TA8x{mKQey9fd^>Q3kVgc)$ACq|6#J7#juI`LBRhVKVg9j){Y6ZGyb%HW zYshg9@L{3w58!i7MM&)*VSY6>UB58s4EXGjSkV3@@Zhw(LNO91)Phi>Pl!o`z{P0+ zikT0OI0Yg?G59r!pK~rk7;~MA=AzbPkLU))+y;G|LM~E#V%9?6DGWpkV;+FM2r%!J zSoPRfDat62h)>Kv!@Y6f6++uHm|X`f5+3s#Xm0@)`H4X|yj>>wcVQiSV_5fJ;12;C z`ac$YYzh8tOw6BMq-6C#K$LXMB^bc&i-bF5m|FoX;_H|L?4D8RK=89TzQR2R_GA_t zB;xJBttzV>IH)`1SOo*v0b!uyF8tic{Noo#69mF00aTRN3+xaIP*GVyV9~4`_%)CD z$2X4MkbeREI3bV6d+$+T5mCp#L!J$60v{j8@39BHK8!?&9N2>aFSGOzImW;*1~e3m zgC7UX9^k3P@eTMguqY7cEG&SCTEqg*H1Lza7b$cu0-gXC4mgz}5cZk~Hv#tn`*jbT ztANK$7wr1uB1O*4h)@GCS2Wt3GlZ0m0S5){24(|9iWDavkjy{0a{dwu z!w?XC)cO47MdN%C^1Bd#gHVW%6OV)D-%B~U$XdY=Id*;meFMN=VZeD9m;&MeuNnaNoIgS0x=Q|0*c)+qCrD<$!@Ge5|SZ8f`oX` z4tayc9%``;eRzs(ny}k!j2mp@l5W#;q9NyST-*~aTVsrhJrJWCD~V|1DQ*X6Ki~K6 z8(wI%|Lo*U-u-;P-}}AaU-x?-@6G4Wh_C)g4CfjyU+T--dV78kWAtSvCgOj_1j2Sv z{Q}l8W^{la$N2{E4}-6K`$liE2JFnEe1}CL-XLwk9C3-FioIeN3(sNxfQk*b0uOkii*`{en9vEB z?V^|53aA2xE==f$;`JQg2Mh-dA&wt`d@rA$j_cdtpTf10^S=e$j`<@hX4ilZfM2L$ z;W@h`7DTuL^5_x-$e?;Iun~9^xQF9+uwWJ(I?nM+;FH0chaJF|fbA;ZPzO8>%ncHr zL`1I5%L`Ts3X?(L3$lJpI13NC$HJqSKnnN*0@7FrsA0oT!6$>6okc%`rv>2m0=9Ak zp8t4}!o$Ft2bI7SF~7P1`vvd`V9f!FNH=gD zkAQr=fre!85dnegOBJ?3fI_B)unHF1v79;-%`CDj_k2 zkGG&~wefbyYvX3d7HQ*l#*(ygkTHukhPiHS4E-)`+|O7-3X_zDsce>LjKhqr(8fd9 zirV-DW2?0BDVVE`M;S}e#%CGJ(#GfDwKkq$ELj_0V$7k9r+tj2Yv39-vo?+}wvvx& z&)BrF4dtzklNrm@##!>&MH$(ykEJogkx+JY_NEYHRR@{HC+4(Zjdy%3wMrJ_OGRz<3=k*<HS-C`t1aFM^Y}Qd;)Dj&z`_f@OuQjFH(s zaoH5%bsi?_Xs?q(a$Q?)1j^?InX=G`)jKd3ij5C&LuZ4cu_NLNgd>&yaKz8THLsT3 z;S;l_d`5BEU2^~8{6$&M-4l(x1KM-)*y62H*0bahqVGl&i;*WcsQ~-uAt!T{`|m6! z)vvwau{SfU=VbkT5(|$xS$J+R;3nRf)#HdHu)K8@thmg|9I7d+)~KdvCS<<9fbS=M z&2377WS%KwjFdZkV&;cr5R`~vYrzCvyF9oyu@t_T>q2}k(wlX&e92ytn)%sW$P$LF zS$bRL!ldlng3;*&R>C8unOPFCvHXaOg|X7pa&uC~qKq$T5v9h$NvXT8Tg#2LEL6NG zW*cVQH8-dvnzR+eX-(6z;8V*xf=!tnoy+g(=&as^0A?s9cLUpLgQ&~dm!x^_Cls=J zd(}$Fc#Nx@$Z5epn+Yc$1FFm)`hjPLlYGCzYs=}JM( zFO~iUiq3(I2^4s3S$TX(h5=XIN zC3cw`(6b&**w4N{2xZ-vv5uEjFUl&62==OFm0pEZb?Z~b%jz1+Y6fLBbAz%9t7Vl- zePv-?Il4$;kbTwK^$_d+adM{g7~B#JZLeGjo9>4eeKJ~3^sJWi{w z;*`wv`5L^wnp|>y>T=1Y8ku%43ZaZTPaQM4)uM@_<)u9t>_t^SoY?_>2f`cd+~Bu& zdN$kVt32&~-HuS&fNG3eq&=o~pVVh52wEpYVLq;|@c;|WM=JF-64DjE5>iCUrA107 z`}41pgR>LKIQ@m2iM9N2%|f_QZ`sqOX53k9R(DIFIG}A5)oY@wGLB< z4c>|tiB^%aOj_vSnbWNL%pq2JMq)lFMX9xqA6&#?p zgaR=u`%SkfuO2~HJjqS5RC(AsYMb9sJ;~)d;p=2kJ-L#hQr#6xm1BX@9Civ>h<>Qd z8jXHa=Zdg^du~u|r6V}2uRN^=QAvQu@J5%%9i$8enXgu-rxt$3x?e=%N+=6-d|er0 z`4qGftEZmu1*bp(p&sIY?X;lmj*ajC=?UD~Wf6uE*=R_!SSSL+9jJTd1lQzoXt~i# zAS1AlQxHj}t3q(JctWfXS9v7UCc#_k2f7XP6*iE-o7$3hD1baN$&mg3j!czy&vAWd z{&r;CT4dzGj0`Cj1p9&@xNi^yJAz>6L9in*gplG-lRVb>wn4DXe$qji3CA3x@72fY@aE47UJt--iP7X?b3G+)%Z%*}Te z?*ZOzs;5~Xf85Ecq2y~qsoolVO(d1hg}p&KD#;n@Xd_OERcZ@hC%SK7;dKVm0XuDD zc~C-KN{Pq_jXm?NuZvn{0o|9y4sxoce4<^js%v zL8JQB(H=Em58v1JZUV(eDvEzB(e*oi>}0>#vK z+as=8{7d@2Wsiq2T&ZRCJ^vB~&PNpreJT4SzlY3vbT8Jj)lG^@)jna>$mMFjJPp?DvJ3 zi-|uRlk=Q^Cwx5I?A&TFJ*S#m@Ek5^Z+>ckO4%^@MFN~%t9k`Tim)6uB@N@MNOpqy8*!;mv1_ZHkD z*yPcI{ptDrceqG`F@!9K9ucaD$$fU9uRSCJw!frw#bnnbQx; zCTYw+pAnxvLodrk-VIVs>)t>M;IW5VoAxM!&enaL)Yn?uwsVupe!jJCYcn6e+*;$~ z!{2Y*v14yz``sJwB^`fM(Iy|EA%lv(&0(erNsY>13qmd?(;_P6epLk86$~Y8=(lou zQP6;#|D@>f5urj}|JJI+3OtlaHuMFG_A)`Jk{@|1T}Y9C_*SWqCZB#Qt<1sL^9c&O zy=~KDySG6IPabK;mZH5NHMM|}p;GEuvCz`cv>TLwoOnL#{s0z@V6wNNZRevzW$fr~ zLtk@IV_O^GE|u*sYTDh@-qg_C^myZTkRMjbTZ)vk^QB+VqkWEX{6QAo{A+ZvfcxF}b@eVdd$fT_Pk zzSfq&NhV+$2%W~hqtd)x<*Hq0_p5f@uG%%epWB55kX>WMhFz5^90i*RH7Xdp1B+k?!1w$a2C_B2+i+*b$=)?l0ZDjr(*TboU$k28dMCKzcZ(pD2nR zP6!i0)x#;hptL<&+t?;CVIyN3K0?^{m$tN{g)pI3C3F)34J1YgfzZ&Q>Ult2JknXZ zv!T7Q^r6*E*hE1z_RV&dwsfeihzSp(uqF_9qN%+oM!QTj-@SWU8quL*)Jaeu(`J81 zoqd65+H6i6Cz?;2%!y<2o_9QWLcdnozKM81eymL}8S%h&(t;*%tYa}*y;j}RiP#xJ zr^>#xR@>c)v^f%0>gBcCK2O93(PXFB@*SU;IZvFS{j2W%l`Juvvaag5#u>DU6RGWS z`m9|3ZrZYf2xs#~keC~jC=}&K-pvr0-1~0&!4`3a3hT5NDLc}W><6&x#c$8i_Y=3Xi zC=3q2^oLP{@QVEC)Q*f}`*-ZRsiOQ|ONG&Tu;!je%kwt>V|k9ZduKrY^VBl)O@X&} zT5gv!E@q`$mf~OX=Dxt8LzZ$&_dUiti;NWq%ME6^?&8gYfB313%LIGnf|FqQHy;GV zC2N@RkrCJvhh4P9&y3HE46nZIH5yX;W&bDHW_oXf)1dp4+(kJ!mFOEcARqt4X+lr( z1mts{ilxvX5c!m6ZluPFz_f_~i+J!a-TQy3*`@ zma!L~kc+P_M<1@cy2=!Xy7J4RtGR}Y0r~LNRXEhnUR`PUXjs06%m4gH&iZtv;p#cL z`qNcHp4|NDyOR2<%BZ}mqT`EvMyBwPpRV*)jN?R(;fKPfDp$@?j_n*H5DFh}!xTPs zKGLVW`M`Oq6a}#QzG4$UU_nS$Cs1^h3H%W~g-F4vbk*LQK%U-A0T&`#3LiU-u(wxs ze0KE?$^zgoc!?}var6Np35$;c(+FwSmE65|X!uRdWbsW>nkO+7q;GLQp zYL~j~A%wCHzJ}CSRKjt{hapd|ut@$DyTn{4Kv=^GXCVN{syV)Z%}vRrw|q3=68Pjt zEyte&Q*P-sBlu*s^CU36^CD~m4gwc*>;R^4x;W0;4#Erw7EZv6R#&S|U_G!OSX1Cb zzC>VpYe)AFx+c@3<`4BKS*w6-iO&+-Bln}3&78%qvI;lg@S_quXwY-QpP6BIh z>i!*kz?fSjm`_&oFc{u|7Zbo)eEz$@w8wZD4IkNAiHj1a5w1Z2?ExMKqXCK9g81_H z)5gW%56OWqDx_ve4MHtG#uk{7hXG&4_+8*3;(%`hzYEKT0C_fyPeW@vw2tukgTR!H zUXD)ycLVb`n8q{E=K|KO`W@a{>)){&Dg7jn2L*_}afbe+0H$Ka);0bG29R4kTVfm* z5P^9dMS3OL4}!*1lXtQ7w<-W=D;(pKE;2)1QO6DG-ARD9exCO z8km=Z*a>;^m?u{J0r+8H9zpS0BzhKDGZ2N~f-kZ4j#Y!OMMwAzJU9SMA;w4iPlRle z251K8uHt#fHv{tsi5G#3<-nh_CH=c)lMU-$tA5CQO4swMG`b2a7KO z^T?Z~z|R8K47vh*4wz@o^cNUNnWol-kJ$?TAO_q3a~kj{Fb}+W1@uRNxqh>R&f+A6 zn!tJ}JOxZ4#K(L$_@rQsYM}YsP`Do0!f^xSiQmKJ_krIH%u`~f%}1qF!{sT0^}sx0 zlKD9;M@a_Ckr_Ww(Ek9!{A(y6g**ag?8fY)z$dxF{|29O%0p^?AM@dm_8S)S$Ka2E z&lfa*0RswS@^~mCV8R3l^{NMPNf79W39z_j$XOOJk5C-_!omW~Ll}39oh^k&@x18b z?g9T2_*A6$#61Lk0SveUaSuaZGBE9xcscaHX_6m^$0zP5Ft-($dI1U|?iawMjVCI(B9@H9XE>w$$g%1 ziwFhdz&yT|CBO&f6nt#DRu=riMgM~o%R%tVz_)Sve*>-q<`J^|2W~P%-~jme zSbm2+=p4d`hsc6G=p7`M8Y0Ui_@@9h1=Ha70+R>0Yq5L@)!Ldnww)e diff --git a/src/triggerSync/triggerSync.c b/src/triggerSync/triggerSync.c index 9944c36..d7562f2 100644 --- a/src/triggerSync/triggerSync.c +++ b/src/triggerSync/triggerSync.c @@ -76,8 +76,8 @@ void trigsync_func(void *arg) int i; RTIME rtStart,rtLast,rtCur,rtDiff; //rt* is real time of the rt-process unsigned int scStart,scLast,scCur,scDiff; //sc* are servo counts of the motion - float srvPer; - float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed + double srvPer,srvPer2; + double mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed unsigned int maxDiff; struct GateArray3* gate3_1=GetGate3MemPtr(1); @@ -90,6 +90,7 @@ void trigsync_func(void *arg) } //unsigned int lastQ12=0; //Q12 value for further sync + double mvTimer; double mxActPos,mxDesPos,mxHomePos=pshm->Motor[2].HomePos; double myActPos,myDesPos,myHomePos=pshm->Motor[1].HomePos; @@ -98,6 +99,7 @@ void trigsync_func(void *arg) rt_task_wait_period(NULL); pshm->Coord[1].Q[0]=-2; srvPer=pshm->ServoPeriod; + srvPer2=pshm->ServoPeriod; rtStart=rt_timer_read(); printf("Wait for 'arm' event...\n"); @@ -193,13 +195,27 @@ void trigsync_func(void *arg) mtDes=i*mtPt2Pt+timeOfs; srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff; //srvPer=pshm->ServoPeriod; //default speed - pshm->Coord[1].DesTimeBase=srvPer; + + // mvTimer > 0 motion leading FEL -> decrease srvPer + // mvTimer < 0 motion lagging FEL -> increase srvPer + // rtDiff > mtPt2Pt -> increase srvPer + // rtDiff < mtPt2Pt -> decrease srvPer + mvTimer=pshm->Motor[1].MoveTimer; //time in current section + if(mvTimer>mtPt2Pt/2) //lagging + mvTimer-=mtPt2Pt; + //srvPer2*=(rtDiff/1E6-mvTimer)/mtPt2Pt; + srvPer2=pshm->ServoPeriod*(rtDiff/1E6-mvTimer)/mtPt2Pt; + + pshm->Coord[1].DesTimeBase=srvPer2; pshm->Coord[1].Q[0]++; if(mode&8) { mxActPos=pshm->Motor[2].ActPos;mxDesPos=pshm->Motor[2].DesPos; myActPos=pshm->Motor[1].ActPos;myDesPos=pshm->Motor[1].DesPos; - printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos); + //printf(" X,Y ActPos:(%.5g %.5g) DesPos:(%.5g %.5g) ", mxActPos-mxHomePos, myActPos-myHomePos, mxDesPos-mxHomePos, myDesPos-myHomePos); + //printf(" X,Y Nsync:(%d) ", pshm->Coord[1].Nsync); + //printf(" X,Y MoveTimer:(%.5g %.5g) ", pshm->Motor[1].MoveTimer,pshm->Motor[2].MoveTimer); + printf(" Nsync:%d, MoveTimer:(%.5g) srvPer2:%.6f ", pshm->Coord[1].Nsync,mvTimer,srvPer2); printf("Trigger count:%d, rtDiff:%.3fms scDiff:%d mtAct:%.3f mtDes:%.3f srvPer:%.6f ENCVAL:%u \n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer,ENCVAL); } rtLast=rtCur;