diff --git a/Readme.md b/Readme.md
index 42378a6..9eb2d91 100644
--- a/Readme.md
+++ b/Readme.md
@@ -58,3 +58,22 @@ gpasciiCommander --host SAROP11-CPPM-MOT6871 -i
 cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
 ./shapepath.py -v255
 
+
+
+Testing ServoFrq.
+-----------------
+
+!common(PhaseFreq=20000,PhasePerServo=4) -> 25000points, 20um error  leads the trajectory in x
+!common(PhaseFreq=20000,PhasePerServo=2) -> 25000points, 6.6um error
+!common(PhaseFreq=20000,PhasePerServo=1) -> 25000points, 6.1um error ossilates in y, x is quite good
+
+-> the servo loop has to be optimized!
+
+Testing linear,pvt,spline moves
+-------------------------------
+
+Tested with shapepath.py:
+
+
+
+
diff --git a/cfg/mx-stage.cfg b/cfg/mx-stage.cfg
index d76e1ed..f9bc282 100644
--- a/cfg/mx-stage.cfg
+++ b/cfg/mx-stage.cfg
@@ -37,7 +37,10 @@
 //Enc 7: Test Stepper: ssi_enc multiturn                       1 rev = 4096 enc_step
 
 $$$***
-!common()
+//!common()
+!common(PhaseFreq=20000,PhasePerServo=4)
+//!common(PhaseFreq=40000)
+
 !torqueCtrl()
 
 
diff --git a/cfg/torqueCtrl.cfg b/cfg/torqueCtrl.cfg
index 6735878..e5f1df5 100644
--- a/cfg/torqueCtrl.cfg
+++ b/cfg/torqueCtrl.cfg
@@ -51,7 +51,7 @@ Motor[1].pPhaseEnc=Acc84B[0].Chan[0].SerialEncDataA.a
 //Motor[1].pAbsPhasePos=Acc84B[0].Chan[0].SerialEncDataA.a
 
 !motor_servo(mot=1,ctrl='ServoCtrl',Kp=0.8,Kvfb=20,Ki=0.001,Kvff=40,Kaff=0,MaxInt=1000)
-!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=1000,PhaseFindingTime=1000,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=3000,WarnFeLimit=1000,InPosBand=10)
+!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
diff --git a/python/data/mode0.npz b/python/data/mode0.npz
new file mode 100644
index 0000000..b00a5fd
Binary files /dev/null and b/python/data/mode0.npz differ
diff --git a/python/data/mode0.prg b/python/data/mode0.prg
new file mode 100644
index 0000000..220172f
--- /dev/null
+++ b/python/data/mode0.prg
@@ -0,0 +1,421 @@
+Gather.Enable=0
+Gather.Items=6
+Gather.MaxSamples=1000000
+Gather.Period=1
+Gather.Addr[0]=Motor[1].ActPos.a
+Gather.Addr[1]=Motor[2].ActPos.a
+Gather.Addr[2]=Motor[3].ActPos.a
+Gather.Addr[3]=Motor[1].DesPos.a
+Gather.Addr[4]=Motor[2].DesPos.a
+Gather.Addr[5]=Motor[3].DesPos.a
+open prog 2
+  linear abs
+X(131.055) Y(12.0362)
+dwell 10
+Gather.Enable=2
+  linear abs
+X131.055 Y12.0362
+X132.488 Y53.4272
+X190.567 Y19.1229
+X199.997 Y18.5218
+X371.919 Y1.38335
+X459.856 Y44.6123
+X518.201 Y25.6627
+X586.513 Y20.1075
+X614.465 Y33.0746
+X617.877 Y13.2369
+X645.57 Y35.3624
+X704.414 Y31.8389
+X725.594 Y11.4275
+X739.264 Y39.1878
+X828.94 Y4.69548
+X862.319 Y48.6903
+X867.167 Y29.1902
+X919.783 Y36.0338
+X924.967 Y46.0073
+X962.77 Y16.8717
+X989.41 Y65.3042
+X903.72 Y83.4224
+X859.581 Y58.5349
+X799.796 Y76.9564
+X697.631 Y60.2255
+X689.768 Y58.3564
+X680.349 Y63.2076
+X614.528 Y70.0422
+X612.867 Y81.3696
+X521.037 Y54.338
+X440.264 Y76.4868
+X407.183 Y69.167
+X333.145 Y81.1014
+X304.199 Y75.3591
+X265.04 Y66.1495
+X80.532 Y85.3109
+X79.5221 Y89.603
+X71.0361 Y87.1293
+X39.6155 Y59.9443
+X24.6787 Y67.2496
+X117.32 Y107.004
+X158.97 Y110.375
+X209.75 Y115.703
+X221.396 Y100.014
+X240.829 Y100.294
+X249.42 Y105.906
+X282.807 Y120.197
+X296.14 Y118.728
+X424.663 Y107.618
+X470.621 Y115.834
+X523.248 Y93.9405
+X596.433 Y117.526
+X693.956 Y90.8857
+X790.84 Y97.2429
+X820.993 Y97.1013
+X837.945 Y96.0984
+X862.063 Y97.258
+X863.856 Y117.532
+X959.142 Y103.298
+X988.374 Y102.045
+X970.494 Y133.439
+X815.524 Y159.414
+X770.581 Y146.947
+X656.33 Y138.183
+X618.826 Y133.461
+X517.379 Y132.068
+X409.054 Y162.954
+X380.335 Y147.809
+X368.585 Y136.9
+X347.234 Y148.141
+X335.498 Y147.686
+X277.596 Y128.861
+X270.328 Y131.483
+X241.557 Y169.025
+X240.02 Y160.539
+X216.457 Y166.048
+X210.383 Y128.926
+X208.877 Y161.31
+X117.38 Y159.845
+X102.414 Y156.383
+X3.86035 Y178.58
+X16.3285 Y185.232
+X153.369 Y199.596
+X237.807 Y171.853
+X253.942 Y213.312
+X289.406 Y183.191
+X326.701 Y232.744
+X370.853 Y197.054
+X373.216 Y222.864
+X377.752 Y179.604
+X398.221 Y209.844
+X407.241 Y232.234
+X511.319 Y224.317
+X565.421 Y183.28
+X687.488 Y215.508
+X729.991 Y171.63
+X793.698 Y223.925
+X822.118 Y189.848
+X950.953 Y233.42
+X990.339 Y216.897
+X839.189 Y237.742
+X696.463 Y247.399
+X677.817 Y270.008
+X675.689 Y244.889
+X672.048 Y245.367
+X663.078 Y263.322
+X653.108 Y253.292
+X577.229 Y237.893
+X568.218 Y246.557
+X566.601 Y265.389
+X552.078 Y271.653
+X466.311 Y244.426
+X391.797 Y242.179
+X364.912 Y260.904
+X284 Y238.413
+X227.415 Y254.356
+X221.161 Y253.191
+X117.316 Y271.252
+X68.0741 Y251.941
+X62.636 Y241.902
+X19.1932 Y301.575
+X55.3743 Y301.599
+X70.8698 Y292.794
+X135.474 Y298.282
+X247.685 Y318.234
+X405.374 Y321.043
+X449.292 Y304.468
+X518.835 Y306.81
+X543.678 Y282.7
+X554.688 Y287.052
+X587.6 Y272.822
+X622.231 Y279.068
+X660.174 Y290.078
+X699.479 Y297.437
+X699.622 Y327.72
+X730.122 Y311.945
+X783.234 Y288.398
+X922.757 Y294.077
+X952.143 Y300.029
+X961.936 Y292.148
+X896.547 Y367.562
+X759.282 Y364.545
+X671.383 Y344.718
+X524.184 Y365.1
+X514.513 Y367.928
+X509.807 Y333.965
+X456.52 Y337.477
+X444.587 Y336.102
+X355.369 Y356.707
+X315.428 Y363.711
+X310.381 Y373.035
+X232.627 Y348.519
+X204.924 Y341.698
+X196.582 Y368.725
+X174.658 Y327.988
+X104.628 Y348.476
+X96.804 Y343.392
+X24.3132 Y342.611
+X11.7141 Y359.978
+X7.8841 Y372.679
+X89.4945 Y405.942
+X110.477 Y405.036
+X174.772 Y389.135
+X227.76 Y410.302
+X259.423 Y373.813
+X272.437 Y379.057
+X283.519 Y379.927
+X320.017 Y383.464
+X403.793 Y399.812
+X455.444 Y401.714
+X501.063 Y376.389
+X534.917 Y404.244
+X628.898 Y398.434
+X716.86 Y396.06
+X724.168 Y399.025
+X813.798 Y396.506
+X896.761 Y406.733
+X963.663 Y383.442
+X983.426 Y399.802
+X991.89 Y376.741
+X994.401 Y451.822
+X869.489 Y454.162
+X706.575 Y414.857
+X697.429 Y453.543
+X679.393 Y453.697
+X652.103 Y431.418
+X618.015 Y428.769
+X570.197 Y438.602
+X521.848 Y414.662
+X517.309 Y424.089
+X399.161 Y431.76
+X359.508 Y437.032
+X317.983 Y414.263
+X253.643 Y446.136
+X207.47 Y424.685
+X152.355 Y417.486
+X139.073 Y426.904
+X62.713 Y424.032
+X58.0292 Y434.417
+X15.6061 Y428.796
+X63.9553 Y485.628
+X144.848 Y488.056
+X248.413 Y505.866
+X250.398 Y483.394
+X262.118 Y456.141
+X362.189 Y470.649
+X374.17 Y463.575
+X453.094 Y493.958
+X543.806 Y456.911
+X607.249 Y477.647
+X638.762 Y490.305
+X725.254 Y501.324
+X739.551 Y490.459
+X773.456 Y456.41
+X857.723 Y457.223
+X858.777 Y458.503
+X882.041 Y458.604
+X966.39 Y507.636
+X976.459 Y468.651
+X981.829 Y478.37
+X967.795 Y546.885
+X951.874 Y575.751
+X930.819 Y520.761
+X807.319 Y569.101
+X791.725 Y528.895
+X726.342 Y536.923
+X672.582 Y528.94
+X602.763 Y544.883
+X592.042 Y572.252
+X497.777 Y581.082
+X456.15 Y568.434
+X442.036 Y519.952
+X432.984 Y528.234
+X432.281 Y521.996
+X430.402 Y510.017
+X420.539 Y557.369
+X300.404 Y549.501
+X277.34 Y524.38
+X248.753 Y576.157
+X162.12 Y559.682
+X18.7898 Y617.635
+X109.784 Y606.308
+X115.484 Y618.48
+X131.289 Y604.118
+X162.493 Y615.56
+X164.694 Y621.478
+X274.824 Y592.23
+X277.629 Y586.784
+X281.73 Y586.41
+X423.855 Y606.393
+X458.139 Y590.984
+X535.633 Y589.91
+X552.192 Y584.476
+X569.965 Y590.873
+X600.392 Y588.74
+X612.096 Y616.934
+X703.217 Y634.786
+X881.103 Y581.273
+X934.214 Y613.966
+X976.761 Y604.846
+X956.084 Y643.99
+X943.748 Y681.82
+X822.407 Y653.421
+X756.779 Y636.061
+X740.098 Y680.514
+X713.767 Y639.187
+X684.934 Y656.762
+X666.767 Y670.638
+X591.027 Y659.176
+X574.325 Y653.201
+X536.177 Y681.393
+X497.391 Y639.473
+X495.97 Y681.74
+X423.655 Y645.894
+X318.569 Y667.41
+X246.063 Y665.859
+X241.419 Y662.505
+X160.23 Y661.118
+X118.274 Y639.921
+X2.70321 Y647.197
+X29.9503 Y737.254
+X30.2353 Y710.337
+X56.8481 Y696.997
+X64.1475 Y692.472
+X131.798 Y716.327
+X311.796 Y696.343
+X351.893 Y721.407
+X548.814 Y715.189
+X577.14 Y695.27
+X589.695 Y745.398
+X622.384 Y710.528
+X683.281 Y695.625
+X764.562 Y698.248
+X765.325 Y748.664
+X806.194 Y703.889
+X860.551 Y727.044
+X881.735 Y692.532
+X881.897 Y719.62
+X904.044 Y690.025
+X947.371 Y730.856
+X978.618 Y799.159
+X966.147 Y791.88
+X941.378 Y799.203
+X940.21 Y750.765
+X906.555 Y774.047
+X842.855 Y818.033
+X778.695 Y777.408
+X696.482 Y813.679
+X669.917 Y785.153
+X637.583 Y813.054
+X588.317 Y831.048
+X524.97 Y750.595
+X461.479 Y780.529
+X374.296 Y748.788
+X360.546 Y828.657
+X349.44 Y781.48
+X273.542 Y798.047
+X264.556 Y774.234
+X181.151 Y788.546
+X20.651 Y758.379
+X20.2184 Y832.62
+X65.6049 Y856.276
+X97.8445 Y862.192
+X123.82 Y848.008
+X149.448 Y868.126
+X258.684 Y849.038
+X267.207 Y877.399
+X293.02 Y848.944
+X423.706 Y857.125
+X447.125 Y846.409
+X518.15 Y842.777
+X628.982 Y872.651
+X630.448 Y874.288
+X633.998 Y867.289
+X722.056 Y866.382
+X778.157 Y870.012
+X778.172 Y844.235
+X975.522 Y855.803
+X977.495 Y876.505
+X980.58 Y871.786
+X976.226 Y889.794
+X968.972 Y904.948
+X820.767 Y908.844
+X739.884 Y898.062
+X730.709 Y881.72
+X676.242 Y879.235
+X623.295 Y886.961
+X536.579 Y896.671
+X530.537 Y922.111
+X502.721 Y922.348
+X446.394 Y907.876
+X437.587 Y891.773
+X435.865 Y891.923
+X392.569 Y878.372
+X382.808 Y895.386
+X187.131 Y903.984
+X160.468 Y886.305
+X100.227 Y919.483
+X61.0785 Y907.733
+X28.9525 Y895.691
+X16.4296 Y929.529
+X99.6149 Y945.302
+X143.353 Y944.669
+X185.636 Y952.792
+X186.193 Y944.372
+X223.082 Y952.749
+X231.702 Y949.319
+X333.507 Y924.159
+X345.352 Y928.081
+X355.613 Y940.432
+X401.26 Y929.291
+X502.389 Y942.584
+X541.381 Y923.306
+X568.045 Y925.597
+X575.946 Y929.296
+X671.957 Y948.861
+X751.022 Y927.212
+X848.15 Y935.832
+X880.678 Y945.027
+X975.884 Y932.561
+X974.256 Y990.345
+X972.919 Y960.835
+X829.897 Y968.286
+X814.966 Y985.491
+X796.391 Y959.167
+X773.084 Y958.741
+X735.194 Y962.189
+X714.241 Y998.847
+X699.575 Y967.966
+X690.785 Y987.349
+X577.543 Y959.433
+X457.059 Y979.962
+X397.257 Y999.278
+X392.676 Y956.406
+X378.245 Y979.527
+X338.159 Y961.57
+X304.446 Y997.962
+X296.556 Y992.011
+X46.8064 Y970.731
+X33.3046 Y958.983
+dwell 100
+Gather.Enable=0
+close
+&1
+b2r
diff --git a/python/data/mode1.npz b/python/data/mode1.npz
new file mode 100644
index 0000000..39792d3
Binary files /dev/null and b/python/data/mode1.npz differ
diff --git a/python/data/mode1.prg b/python/data/mode1.prg
new file mode 100644
index 0000000..d31d7f9
--- /dev/null
+++ b/python/data/mode1.prg
@@ -0,0 +1,423 @@
+Gather.Enable=0
+Gather.Items=6
+Gather.MaxSamples=1000000
+Gather.Period=1
+Gather.Addr[0]=Motor[1].ActPos.a
+Gather.Addr[1]=Motor[2].ActPos.a
+Gather.Addr[2]=Motor[3].ActPos.a
+Gather.Addr[3]=Motor[1].DesPos.a
+Gather.Addr[4]=Motor[2].DesPos.a
+Gather.Addr[5]=Motor[3].DesPos.a
+open prog 2
+  linear abs
+X131.055 Y12.0362
+dwell 10
+Gather.Enable=2
+  pvt10 abs
+X131.055:71.6202 Y12.0362:2069.55
+X132.488:2975.58 Y53.4272:354.334
+X190.567:3375.44 Y19.1229:-1745.27
+X199.997:9067.59 Y18.5218:-886.977
+X371.919:12993 Y1.38335:1304.53
+X459.856:7314.1 Y44.6123:1213.97
+X518.201:6332.85 Y25.6627:-1225.24
+X586.513:4813.2 Y20.1075:370.594
+X614.465:1568.19 Y33.0746:-343.534
+X617.877:1555.28 Y13.2369:114.392
+X645.57:4326.89 Y35.3624:930.104
+X704.414:4001.21 Y31.8389:-1196.75
+X725.594:1742.46 Y11.4275:367.443
+X739.264:5167.28 Y39.1878:-336.599
+X828.94:6152.75 Y4.69548:475.125
+X862.319:1911.36 Y48.6903:1224.74
+X867.167:2873.21 Y29.1902:-632.824
+X919.783:2889.99 Y36.0338:840.854
+X924.967:2149.38 Y46.0073:-958.107
+X962.77:3222.14 Y16.8717:964.845
+X989.41:-2952.53 Y65.3042:3327.54
+X903.72:-6491.45 Y83.4224:-338.464
+X859.581:-5196.19 Y58.5349:-323.299
+X799.796:-8097.48 Y76.9564:84.5275
+X697.631:-5501.38 Y60.2255:-930.004
+X689.768:-864.126 Y58.3564:149.107
+X680.349:-3762.03 Y63.2076:584.292
+X614.528:-3374.1 Y70.0422:908.099
+X612.867:-4674.55 Y81.3696:-785.21
+X521.037:-8630.15 Y54.338:-244.141
+X440.264:-5692.67 Y76.4868:741.45
+X407.183:-5355.93 Y69.167:230.731
+X333.145:-5149.23 Y81.1014:309.604
+X304.199:-3405.27 Y75.3591:-747.596
+X265.04:-11183.3 Y66.1495:497.593
+X80.532:-9275.88 Y85.3109:1172.68
+X79.5221:-474.797 Y89.603:90.9188
+X71.0361:-1995.33 Y87.1293:-1482.94
+X39.6155:-2317.87 Y59.9443:-993.983
+X24.6787:3885.25 Y67.2496:2352.99
+X117.32:6714.54 Y107.004:2156.28
+X158.97:4621.47 Y110.375:434.955
+X209.75:3121.34 Y115.703:-518.054
+X221.396:1553.94 Y100.014:-770.465
+X240.829:1401.18 Y100.294:294.605
+X249.42:2098.91 Y105.906:995.131
+X282.807:2336.01 Y120.197:641.078
+X296.14:7092.8 Y118.728:-628.943
+X424.663:8724.04 Y107.618:-144.686
+X470.621:4929.25 Y115.834:-683.86
+X523.248:6290.61 Y93.9405:84.5821
+X596.433:8535.42 Y117.526:-152.739
+X693.956:9720.35 Y90.8857:-1014.14
+X790.84:6351.84 Y97.2429:310.777
+X820.993:2355.24 Y97.1013:-57.2259
+X837.945:2053.47 Y96.0984:7.83595
+X862.063:1295.53 Y97.258:1071.67
+X863.856:4853.98 Y117.532:302.008
+X959.142:6225.91 Y103.298:-774.352
+X988.374:567.572 Y102.045:1507.06
+X970.494:-8642.5 Y133.439:2868.48
+X815.524:-9995.65 Y159.414:675.361
+X770.581:-7959.71 Y146.947:-1061.58
+X656.33:-7587.73 Y138.183:-674.259
+X618.826:-6947.52 Y133.461:-305.742
+X517.379:-10488.6 Y132.068:1474.65
+X409.054:-6852.2 Y162.954:787.029
+X380.335:-2023.47 Y147.809:-1302.71
+X368.585:-1655.08 Y136.9:16.6092
+X347.234:-1654.32 Y148.141:539.265
+X335.498:-3481.87 Y147.686:-964.015
+X277.596:-3258.51 Y128.861:-810.139
+X270.328:-1801.95 Y131.483:2008.24
+X241.557:-1515.38 Y169.025:1452.8
+X240.02:-1254.99 Y160.539:-148.879
+X216.457:-1481.89 Y166.048:-1580.63
+X210.383:-379.025 Y128.926:-236.915
+X208.877:-4650.11 Y161.31:1545.95
+X117.38:-5323.15 Y159.845:-246.308
+X102.414:-5676 Y156.383:936.734
+X3.86035:-4304.26 Y178.58:1442.45
+X16.3285:7475.43 Y185.232:1050.81
+X153.369:11073.9 Y199.596:-668.961
+X237.807:5028.64 Y171.853:685.792
+X253.942:2579.94 Y213.312:566.913
+X289.406:3637.96 Y183.191:971.608
+X326.701:4072.34 Y232.744:693.146
+X370.853:2325.75 Y197.054:-494.016
+X373.216:344.952 Y222.864:-872.53
+X377.752:1250.26 Y179.604:-651.003
+X398.221:1474.47 Y209.844:2631.52
+X407.241:5654.9 Y232.234:723.664
+X511.319:7909.01 Y224.317:-2447.72
+X565.421:8808.46 Y183.28:-440.468
+X687.488:8228.46 Y215.508:-582.508
+X729.991:5310.47 Y171.63:420.851
+X793.698:4606.36 Y223.925:910.912
+X822.118:7862.75 Y189.848:474.778
+X950.953:8411.06 Y233.42:1352.45
+X990.339:-5588.17 Y216.897:216.079
+X839.189:-14693.8 Y237.742:1525.09
+X696.463:-8068.63 Y247.399:1613.31
+X677.817:-1038.7 Y270.008:-125.464
+X675.689:-288.436 Y244.889:-1232.04
+X672.048:-630.547 Y245.367:921.645
+X663.078:-946.974 Y263.322:396.22
+X653.108:-4292.48 Y253.292:-1271.48
+X577.229:-4244.54 Y237.893:-336.733
+X568.218:-531.357 Y246.557:1374.83
+X566.601:-806.966 Y265.389:1254.79
+X552.078:-5014.53 Y271.653:-1048.19
+X466.311:-8014.07 Y244.426:-1473.71
+X391.797:-5069.95 Y242.179:823.945
+X364.912:-5389.85 Y260.904:-188.266
+X284:-6874.86 Y238.413:-327.401
+X227.415:-3141.95 Y254.356:738.896
+X221.161:-5504.95 Y253.191:844.78
+X117.316:-7654.34 Y271.252:-62.5103
+X68.0741:-2733.98 Y251.941:-1467.52
+X62.636:-2444.04 Y241.902:2481.69
+X19.1932:-363.084 Y301.575:2984.85
+X55.3743:2583.83 Y301.599:-439.039
+X70.8698:4004.99 Y292.794:-165.815
+X135.474:8840.76 Y298.282:1271.97
+X247.685:13495 Y318.234:1138.03
+X405.374:10080.3 Y321.043:-688.255
+X449.292:5673.08 Y304.468:-711.645
+X518.835:4719.32 Y306.81:-1088.44
+X543.678:1792.63 Y282.7:-987.929
+X554.688:2196.08 Y287.052:-493.887
+X587.6:3377.16 Y272.822:-399.179
+X622.231:3628.7 Y279.068:862.785
+X660.174:3862.41 Y290.078:918.45
+X699.479:1972.43 Y297.437:1882.14
+X699.622:1532.14 Y327.72:725.402
+X730.122:4180.62 Y311.945:-1966.1
+X783.234:9631.73 Y288.398:-893.417
+X922.757:8445.41 Y294.077:581.521
+X952.143:1958.99 Y300.029:-96.4568
+X961.936:-2779.81 Y292.148:3376.65
+X896.547:-10132.7 Y367.562:3619.85
+X759.282:-11258.2 Y364.545:-1142.19
+X671.383:-11754.9 Y344.718:27.7626
+X524.184:-7843.54 Y365.1:1160.47
+X514.513:-718.833 Y367.928:-1556.75
+X509.807:-2899.64 Y333.965:-1522.51
+X456.52:-3261 Y337.477:106.87
+X444.587:-5057.55 Y336.102:961.475
+X355.369:-6457.95 Y356.707:1380.43
+X315.428:-2249.4 Y363.711:816.399
+X310.381:-4140.07 Y373.035:-759.57
+X232.627:-5272.86 Y348.519:-1566.84
+X204.924:-1802.23 Y341.698:1010.29
+X196.582:-1513.27 Y368.725:-685.506
+X174.658:-4597.72 Y327.988:-1012.46
+X104.628:-3892.72 Y348.476:770.186
+X96.804:-4015.73 Y343.392:-293.25
+X24.3132:-4254.49 Y342.611:829.317
+X11.7141:-821.455 Y359.978:1503.4
+X7.8841:3889.02 Y372.679:2298.21
+X89.4945:5129.65 Y405.942:1617.83
+X110.477:4263.87 Y405.036:-840.382
+X174.772:5864.12 Y389.135:263.297
+X227.76:4232.53 Y410.302:-766.077
+X259.423:2233.87 Y373.813:-1562.23
+X272.437:1204.81 Y379.057:305.691
+X283.519:2379.01 Y379.927:220.35
+X320.017:6013.69 Y383.464:994.259
+X403.793:6771.35 Y399.812:912.482
+X455.444:4863.52 Y401.714:-1171.15
+X501.063:3973.64 Y376.389:126.504
+X534.917:6391.76 Y404.244:1102.26
+X628.898:9097.14 Y398.434:-409.196
+X716.86:4763.46 Y396.06:29.5532
+X724.168:4846.91 Y399.025:22.3019
+X813.798:8629.68 Y396.506:385.401
+X896.761:7493.25 Y406.733:-653.211
+X963.663:4333.25 Y383.442:-346.583
+X983.426:1411.38 Y399.802:-335.013
+X991.89:548.727 Y376.741:2601
+X994.401:-6120.09 Y451.822:3871.06
+X869.489:-14391.3 Y454.162:-1848.24
+X706.575:-8602.99 Y414.857:-30.9857
+X697.429:-1359.1 Y453.543:1942
+X679.393:-2266.28 Y453.697:-1106.21
+X652.103:-3068.87 Y431.418:-1246.41
+X618.015:-4095.32 Y428.769:359.154
+X570.197:-4808.36 Y438.602:-705.338
+X521.848:-2644.41 Y414.662:-725.626
+X517.309:-6134.36 Y424.089:854.909
+X399.161:-7890.03 Y431.76:647.148
+X359.508:-4058.9 Y437.032:-874.857
+X317.983:-5293.27 Y414.263:455.178
+X253.643:-5525.66 Y446.136:521.124
+X207.47:-5064.39 Y424.685:-1432.46
+X152.355:-3419.87 Y417.486:110.945
+X139.073:-4482.09 Y426.904:327.294
+X62.713:-4052.18 Y424.032:375.613
+X58.0292:-2355.34 Y434.417:238.174
+X15.6061:296.305 Y428.796:2560.55
+X63.9553:6462.08 Y485.628:2963.03
+X144.848:9222.91 Y488.056:1011.94
+X248.413:5277.52 Y505.866:-233.137
+X250.398:685.234 Y483.394:-2486.29
+X262.118:5589.54 Y456.141:-637.229
+X362.189:5602.59 Y470.649:371.743
+X374.17:4545.26 Y463.575:1165.44
+X453.094:8481.8 Y493.958:-333.2
+X543.806:7707.76 Y456.911:-815.567
+X607.249:4747.79 Y477.647:1669.7
+X638.762:5900.25 Y490.305:1183.89
+X725.254:5039.45 Y501.324:7.6731
+X739.551:2410.06 Y490.459:-2245.74
+X773.456:5908.59 Y456.41:-1661.77
+X857.723:4266.1 Y457.223:104.68
+X858.777:1215.94 Y458.503:69.0254
+X882.041:5380.63 Y458.604:2456.62
+X966.39:4720.9 Y507.636:502.362
+X976.459:771.971 Y468.651:-1463.26
+X981.829:-433.24 Y478.37:3911.69
+X967.795:-1497.75 Y546.885:4869.04
+X951.874:-1848.8 Y575.751:-1306.17
+X930.819:-7227.78 Y520.761:-332.521
+X807.319:-6954.68 Y569.101:406.674
+X791.725:-4048.82 Y528.895:-1608.89
+X726.342:-5957.14 Y536.923:2.25046
+X672.582:-6178.95 Y528.94:398.009
+X602.763:-4027.02 Y544.883:2165.6
+X592.042:-5249.32 Y572.252:1809.94
+X497.777:-6794.58 Y581.082:-190.898
+X456.15:-2787.06 Y568.434:-3056.48
+X442.036:-1158.31 Y519.952:-2009.99
+X432.984:-487.708 Y528.234:102.195
+X432.281:-129.088 Y521.996:-910.862
+X430.402:-587.101 Y510.017:1768.63
+X420.539:-6499.94 Y557.369:1974.19
+X300.404:-7159.96 Y549.501:-1649.45
+X277.34:-2582.53 Y524.38:1332.84
+X248.753:-5761 Y576.157:1765.13
+X162.12:-11498.2 Y559.682:2073.91
+X18.7898:-2616.79 Y617.635:2331.29
+X109.784:4834.72 Y606.308:42.2381
+X115.484:1075.24 Y618.48:-109.516
+X131.289:2350.43 Y604.118:-146.035
+X162.493:1670.24 Y615.56:868.03
+X164.694:5616.53 Y621.478:-1166.46
+X274.824:5646.73 Y592.23:-1734.7
+X277.629:345.327 Y586.784:-291.013
+X281.73:7311.32 Y586.41:980.443
+X423.855:8820.44 Y606.393:228.7
+X458.139:5588.89 Y590.984:-824.162
+X535.633:4702.68 Y589.91:-325.405
+X552.192:1716.61 Y584.476:48.1392
+X569.965:2409.99 Y590.873:213.177
+X600.392:2106.54 Y588.74:1303.06
+X612.096:5141.26 Y616.934:2302.34
+X703.217:13450.4 Y634.786:-1783.06
+X881.103:11549.8 Y581.273:-1041.02
+X934.214:4782.89 Y613.966:1178.63
+X976.761:1093.48 Y604.846:1501.21
+X956.084:-1650.65 Y643.99:3848.74
+X943.748:-6683.84 Y681.82:471.548
+X822.407:-9348.47 Y653.421:-2287.96
+X756.779:-4115.46 Y636.061:1354.67
+X740.098:-2150.59 Y680.514:156.292
+X713.767:-2758.15 Y639.187:-1187.63
+X684.934:-2350.01 Y656.762:1572.55
+X666.767:-4695.38 Y670.638:120.726
+X591.027:-4622.07 Y659.176:-871.852
+X574.325:-2742.47 Y653.201:1110.8
+X536.177:-3846.69 Y681.393:-686.415
+X497.391:-2010.36 Y639.473:17.3717
+X495.97:-3686.83 Y681.74:321.08
+X423.655:-8870.07 Y645.894:-716.478
+X318.569:-8879.58 Y667.41:998.25
+X246.063:-3857.52 Y665.859:-245.29
+X241.419:-4291.64 Y662.505:-237.08
+X160.23:-6157.21 Y661.118:-1129.18
+X118.274:-7876.36 Y639.921:-696.043
+X2.70321:-4416.21 Y647.197:4866.66
+X29.9503:1376.6 Y737.254:3157.01
+X30.2353:1344.89 Y710.337:-2012.85
+X56.8481:1695.61 Y696.997:-893.235
+X64.1475:3747.49 Y692.472:966.498
+X131.798:12382.4 Y716.327:193.568
+X311.796:11004.8 Y696.343:253.973
+X351.893:11850.9 Y721.407:942.294
+X548.814:11262.4 Y715.189:-1306.83
+X577.14:2044.06 Y695.27:1510.44
+X589.695:2262.21 Y745.398:762.92
+X622.384:4679.33 Y710.528:-2488.63
+X683.281:7108.88 Y695.625:-613.996
+X764.562:4102.2 Y698.248:2651.91
+X765.325:2081.6 Y748.664:282.005
+X806.194:4761.3 Y703.889:-1080.97
+X860.551:3777.07 Y727.044:-567.85
+X881.735:1067.27 Y692.532:-371.205
+X881.897:1115.45 Y719.62:-125.328
+X904.044:3273.7 Y690.025:561.782
+X947.371:3728.7 Y730.856:5456.68
+X978.618:938.843 Y799.159:3051.19
+X966.147:-1862.03 Y791.88:2.20116
+X941.378:-1296.89 Y799.203:-2055.74
+X940.21:-1741.11 Y750.765:-1257.76
+X906.555:-4867.74 Y774.047:3363.42
+X842.855:-6393.01 Y818.033:168.011
+X778.695:-7318.62 Y777.408:-217.733
+X696.482:-5438.94 Y813.679:387.268
+X669.917:-2944.99 Y785.153:-31.2393
+X637.583:-4079.97 Y813.054:2294.78
+X588.317:-5630.61 Y831.048:-3122.94
+X524.97:-6341.89 Y750.595:-2525.96
+X461.479:-7533.71 Y780.529:-90.3383
+X374.296:-5046.69 Y748.788:2406.39
+X360.546:-1242.79 Y828.657:1634.57
+X349.44:-4350.18 Y781.48:-1530.5
+X273.542:-4244.23 Y798.047:-362.296
+X264.556:-4619.55 Y774.234:-475.066
+X181.151:-12195.2 Y788.546:-792.752
+X20.651:-8046.63 Y758.379:2203.72
+X20.2184:2247.69 Y832.62:4894.88
+X65.6049:3881.3 Y856.276:1478.58
+X97.8445:2910.76 Y862.192:-413.398
+X123.82:2580.19 Y848.008:296.727
+X149.448:6743.2 Y868.126:51.504
+X258.684:5887.94 Y849.038:463.637
+X267.207:1716.81 Y877.399:-4.73766
+X293.02:7824.97 Y848.944:-1013.69
+X423.706:7705.25 Y857.125:-126.744
+X447.125:4722.17 Y846.409:-717.403
+X518.15:9092.82 Y842.777:1312.1
+X628.982:5614.91 Y872.651:1575.56
+X630.448:250.793 Y874.288:-268.062
+X633.998:4580.38 Y867.289:-395.282
+X722.056:7207.95 Y866.382:136.137
+X778.157:2805.8 Y870.012:-1107.37
+X778.172:9868.24 Y844.235:-710.44
+X975.522:9966.18 Y855.803:1613.51
+X977.495:252.931 Y876.505:799.12
+X980.58:-63.4738 Y871.786:664.421
+X976.226:-580.421 Y889.794:1658.13
+X968.972:-7772.93 Y904.948:952.503
+X820.767:-11454.4 Y908.844:-344.299
+X739.884:-4502.9 Y898.062:-1356.18
+X730.709:-3182.07 Y881.72:-941.38
+X676.242:-5370.72 Y879.235:262.028
+X623.295:-6983.16 Y886.961:871.827
+X536.579:-4637.87 Y896.671:1757.53
+X530.537:-1692.92 Y922.111:1283.83
+X502.721:-4207.14 Y922.348:-711.793
+X446.394:-3256.68 Y907.876:-1528.75
+X437.587:-526.475 Y891.773:-797.612
+X435.865:-2250.92 Y891.923:-670.025
+X392.569:-2652.84 Y878.372:173.126
+X382.808:-10271.9 Y895.386:1280.57
+X187.131:-11117 Y903.984:-454.061
+X160.468:-4345.2 Y886.305:774.933
+X100.227:-4969.46 Y919.483:1071.41
+X61.0785:-3563.72 Y907.733:-1189.57
+X28.9525:-2232.45 Y895.691:1089.82
+X16.4296:3533.12 Y929.529:2480.51
+X99.6149:6346.18 Y945.302:756.98
+X143.353:4301.05 Y944.669:374.506
+X185.636:2141.99 Y952.792:-14.8264
+X186.193:1872.28 Y944.372:-2.13227
+X223.082:2275.43 Y952.749:247.322
+X231.702:5521.29 Y949.319:-1429.51
+X333.507:5682.5 Y924.159:-1061.88
+X345.352:1105.26 Y928.081:813.659
+X355.613:2795.39 Y940.432:60.5062
+X401.26:7338.84 Y929.291:107.583
+X502.389:7006.06 Y942.584:-299.267
+X541.381:3282.76 Y923.306:-849.348
+X568.045:1728.29 Y925.597:299.506
+X575.946:5195.63 Y929.296:1163.22
+X671.957:8753.76 Y948.861:-104.22
+X751.022:8809.66 Y927.212:-651.447
+X848.15:6482.82 Y935.832:890.748
+X880.678:6386.67 Y945.027:-163.544
+X975.884:4678.9 Y932.561:2265.91
+X974.256:-148.219 Y990.345:1413.67
+X972.919:-7217.94 Y960.835:-1102.93
+X829.897:-7897.65 Y968.286:1232.84
+X814.966:-1675.29 Y985.491:-455.99
+X796.391:-2094.15 Y959.167:-1337.53
+X773.084:-3059.87 Y958.741:151.097
+X735.194:-2942.11 Y962.189:2005.3
+X714.241:-1780.95 Y998.847:288.851
+X699.575:-1172.83 Y967.966:-574.912
+X690.785:-6101.61 Y987.349:-426.611
+X577.543:-11686.3 Y959.433:-369.322
+X457.059:-9014.31 Y979.962:1992.23
+X397.257:-3219.15 Y999.278:-1177.83
+X392.676:-950.579 Y956.406:-987.553
+X378.245:-2725.84 Y979.527:258.222
+X338.159:-3689.94 Y961.57:921.766
+X304.446:-2080.13 Y997.962:1522.05
+X296.556:-12882 Y992.011:-1361.54
+X46.8064:-13162.6 Y970.731:-1651.43
+X33.3046:-675.086 Y958.983:-587.436
+X33.3046:0 Y958.983:0
+X33.3046 Y958.983
+dwell 1000
+Gather.Enable=0
+close
+&1
+b2r
diff --git a/python/data/mode2.npz b/python/data/mode2.npz
new file mode 100644
index 0000000..3a24a93
Binary files /dev/null and b/python/data/mode2.npz differ
diff --git a/python/data/mode2.prg b/python/data/mode2.prg
new file mode 100644
index 0000000..aff42b4
--- /dev/null
+++ b/python/data/mode2.prg
@@ -0,0 +1,421 @@
+Gather.Enable=0
+Gather.Items=6
+Gather.MaxSamples=1000000
+Gather.Period=1
+Gather.Addr[0]=Motor[1].ActPos.a
+Gather.Addr[1]=Motor[2].ActPos.a
+Gather.Addr[2]=Motor[3].ActPos.a
+Gather.Addr[3]=Motor[1].DesPos.a
+Gather.Addr[4]=Motor[2].DesPos.a
+Gather.Addr[5]=Motor[3].DesPos.a
+open prog 2
+  linear abs
+X(131.055) Y(12.0362)
+dwell 10
+Gather.Enable=2
+  spline10 abs
+X131.055 Y12.0362
+X123.046 Y66.0431
+X198.675 Y13.5057
+X172.914 Y21.278
+X385.916 Y-8.67788
+X464.788 Y54.9754
+X516.539 Y23.4303
+X593.24 Y17.0205
+X618.555 Y38.5421
+X613.83 Y6.24297
+X640.378 Y39.6373
+X710.692 Y34.6536
+X726.846 Y3.39882
+X726.596 Y49.5632
+X838.323 Y-8.38571
+X867.074 Y59.2728
+X859.206 Y24.7996
+X927.688 Y35.5122
+X919.53 Y52.5255
+X964.631 Y3.94364
+X1008.13 Y70.3566
+X896.795 Y90.5901
+X862.189 Y51.3167
+X806.859 Y82.8152
+X681.914 Y57.7485
+X690.028 Y57.2363
+X689.749 Y62.8771
+X603.834 Y69.2934
+X627.895 Y87.7628
+X519.194 Y46.1413
+X432.315 Y81.3982
+X414.01 Y65.958
+X325.63 Y84.0475
+X305.901 Y75.937
+X289.264 Y61.421
+X49.949 Y87.7891
+X80.7681 Y90.7307
+X74.8585 Y91.2478
+X36.8682 Y54.1959
+X6.74897 Y61.8414
+X125.819 Y113.068
+X157.448 Y110.049
+X216.272 Y119.206
+X220.099 Y97.3526
+X242.636 Y99.4052
+X245.287 Y104.46
+X286.149 Y122.823
+X276.942 Y120.335
+X438.424 Y104.397
+X469.509 Y120.852
+X519.822 Y86.3607
+X592.377 Y125.896
+X694.063 Y85.3862
+X801.962 Y98.3261
+X823.193 Y97.2448
+X836.751 Y95.738
+X865.783 Y94.0723
+X848.273 Y123.283
+X970.151 Y101.135
+X996.226 Y96.6035
+X993.342 Y134.343
+X797.186 Y165.822
+X782.132 Y146.329
+X643.538 Y137.509
+X629.483 Y132.907
+X518.525 Y126.688
+X395.786 Y170.626
+X377.507 Y147.102
+X370.185 Y133.209
+X345.631 Y150.09
+X343.193 Y150.747
+X269.157 Y125.286
+X273.912 Y125.663
+X237.018 Y176.697
+X243.691 Y158.206
+X213.543 Y173.153
+X209.621 Y117.342
+X223.875 Y166.951
+X104.625 Y160.178
+X116.345 Y152.107
+X-14.6432 Y181.171
+X-4.43353 Y183.947
+X162.136 Y206.614
+X249.191 Y160.319
+X250.72 Y225.242
+X289.101 Y169.912
+X325.558 Y246.951
+X377.818 Y186.804
+X372.854 Y234.375
+X375.096 Y167.354
+X400.129 Y211.152
+X391.398 Y237.285
+X519.648 Y229.837
+X554.094 Y171.069
+X700.749 Y228.192
+X726.456 Y155.601
+X799.579 Y238.32
+X805.382 Y176.906
+X965.861 Y243.436
+X1022.09 Y210.669
+X837.785 Y239.606
+X675.783 Y245.24
+X675.063 Y277.963
+X675.941 Y240.623
+X672.936 Y242.454
+X663.245 Y267.987
+X664.093 Y254.186
+X566.084 Y233.882
+X566.985 Y244.862
+X568.753 Y267.484
+X563.952 Y277.235
+X464.435 Y240.262
+X383.859 Y238.683
+X373.916 Y267.774
+X279.946 Y232.008
+X219.026 Y257.208
+X237.426 Y249.987
+X108.215 Y277.481
+X60.7735 Y250.396
+X68.9701 Y230.283
+X5.92254 Y311.516
+X58.8219 Y303.07
+X62.685 Y290.412
+X127.54 Y295.872
+X240.105 Y321.09
+X424.335 Y324.274
+X445.021 Y301.316
+X526.285 Y311.219
+X545.984 Y277.956
+X551.037 Y290.148
+X587.313 Y269.409
+X621.679 Y278.274
+X659.946 Y290.686
+X706.006 Y293.616
+X694.563 Y335.397
+X726.353 Y313.24
+X768.833 Y283.528
+X941.113 Y294.031
+X955.408 Y302.335
+X974.467 Y278.265
+X908.526 Y380.634
+X751.055 Y367.346
+X681.267 Y338.017
+X501.262 Y368.026
+X513.685 Y374.059
+X517.904 Y327.719
+X449.627 Y338.292
+X457.468 Y332.439
+X347.156 Y358.974
+X309.613 Y363.324
+X322.499 Y378.675
+X224.285 Y345.57
+X201.697 Y336.057
+X198.846 Y380.019
+X182.676 Y317.784
+X94.2601 Y352.738
+X107.582 Y342.674
+X14.3313 Y339.586
+X10.2526 Y360.756
+X-6.35597 Y369.252
+X99.5992 Y411.637
+X103.258 Y407.535
+X176.657 Y382.957
+X231.314 Y419.911
+X262.531 Y366.858
+X272.759 Y379.786
+X279.283 Y379.482
+X312.138 Y381.329
+X409.147 Y402.22
+X456.45 Y406.251
+X503.024 Y367.526
+X524.896 Y409.854
+X629.902 Y397.862
+X730.302 Y395.17
+X710.447 Y399.94
+X814.909 Y394.381
+X899.438 Y412.32
+X971.519 Y376.833
+X985.309 Y406.372
+X992.883 Y360.384
+X1015.64 Y463.945
+X875.822 Y461.103
+X680.947 Y401.858
+X698.91 Y459.965
+X680.935 Y457.436
+X653.236 Y428.147
+X620.304 Y426.688
+X570.285 Y444.23
+X514.547 Y409.101
+X536.243 Y424.382
+X386.079 Y432.16
+X359.82 Y441.705
+X321.786 Y405.156
+X250.614 Y455.023
+X208.961 Y422.31
+X145.382 Y414.717
+X149.586 Y428.953
+X50.767 Y421.823
+X64.319 Y437.084
+X0.477348 Y418.387
+X58.5314 Y494.695
+X141.069 Y485.493
+X265.344 Y512.58
+X248.776 Y484.19
+X247.393 Y449.18
+X376.871 Y474.246
+X363.013 Y457.333
+X451.13 Y505.196
+X548.351 Y447.281
+X612.571 Y478.993
+X629.598 Y490.579
+X737.287 Y504.972
+X736.283 Y494.323
+X765.062 Y450.599
+X871.591 Y457.146
+X855.076 Y458.7
+X871.861 Y450.449
+X978.77 Y522.305
+X977.243 Y460.534
+X985.063 Y468.571
+X968.109 Y553.493
+X952.73 Y589.727
+X947.893 Y503.54
+X789.335 Y583.858
+X800.023 Y520.856
+X724.405 Y539.592
+X675.259 Y524.952
+X592.914 Y542.979
+X605.966 Y575.342
+X489.004 Y584.662
+X451.565 Y574.406
+X441.192 Y510.492
+X431.593 Y530.654
+X432.478 Y522.953
+X431.733 Y500.128
+X438.918 Y566.572
+X284.225 Y552.376
+X278.261 Y511.563
+X258.427 Y587.533
+X171.57 Y547.278
+X-20.2644 Y629.182
+X124 Y602.392
+X113.8 Y622.903
+X128.723 Y599.817
+X167.327 Y616.48
+X146.706 Y627.34
+X292.711 Y588.263
+X277.413 Y585.939
+X258.726 Y583.017
+X441.829 Y612.292
+X450.937 Y588.595
+X545.789 Y590.637
+X551.99 Y582.504
+X567.856 Y592.294
+X603.513 Y583.685
+X598.859 Y618.658
+X688.757 Y646.681
+X901.899 Y566.905
+X935.975 Y620.935
+X987.299 Y596.801
+X954.693 Y644.209
+X961.916 Y692.859
+X813.121 Y651.581
+X748.621 Y625.759
+X741.706 Y694.811
+X714.184 Y629.37
+X683.157 Y657.378
+X676.362 Y674.861
+X581.187 Y658.262
+X577.9 Y647.506
+X536.284 Y693.078
+X491.164 Y625.441
+X507.786 Y694.759
+X429.117 Y636.334
+X313.139 Y671.255
+X234.753 Y666.16
+X254.176 Y662.177
+X153.692 Y664.419
+X130.544 Y635.176
+X-21.0998 Y633.4
+X34.444 Y756.75
+X25.8473 Y708.074
+X60.067 Y695.528
+X54.089 Y687.742
+X113.073 Y723.634
+X335.113 Y688.836
+X325.756 Y726.62
+X576.912 Y717.473
+X579.769 Y683.595
+X586.339 Y759.564
+X617.683 Y707.201
+X679.884 Y692.704
+X777.982 Y690.283
+X758.641 Y764.529
+X803.946 Y692.567
+X866.08 Y736.656
+X885.239 Y682.265
+X878.232 Y729.067
+X900.515 Y678.287
+X949.384 Y726.277
+X985.905 Y811.756
+X968.197 Y789.446
+X937.444 Y808.496
+X945.624 Y738.811
+X911.563 Y770.597
+X842.931 Y832.135
+X781.704 Y764.591
+X687.208 Y824.478
+X670.878 Y775.748
+X640.405 Y814.705
+X590.664 Y847.456
+X524.995 Y732.197
+X465.428 Y790.808
+X362.057 Y730.187
+X360.105 Y849.831
+X360.239 Y770.856
+X262.39 Y804.777
+X276.959 Y767.88
+X194 Y795.959
+X-6.02689 Y740.977
+X12.5819 Y841.051
+X67.796 Y859.233
+X98.8885 Y865.541
+X123.878 Y842.291
+X135.514 Y874.66
+X275.47 Y841.13
+X264.325 Y886.868
+X275.541 Y842.837
+X441.584 Y860.275
+X439.191 Y845.228
+X511.515 Y837.193
+X647.209 Y877.357
+X630.101 Y875.727
+X619.913 Y866.274
+X727.382 Y865.626
+X787.504 Y874.913
+X745.282 Y838.011
+X1008.08 Y854.281
+X977.31 Y880.742
+X981.82 Y867.998
+X976.709 Y890.269
+X992.463 Y906.825
+X809.547 Y911.29
+X727.933 Y898.989
+X738.258 Y879.411
+X675.989 Y877.533
+X628.923 Y886.63
+X523.134 Y894.05
+X534.166 Y926.312
+X507.472 Y924.799
+X438.475 Y908.147
+X436.406 Y889.064
+X442.794 Y894.207
+X386.98 Y873.278
+X413.794 Y896.788
+X158.962 Y908.364
+X166.064 Y877.828
+X96.7115 Y926.971
+X59.9081 Y907.782
+X25.6854 Y888.045
+X0.47826 Y932.54
+X106.189 Y948.036
+X143.596 Y943.21
+X192.59 Y955.549
+X180.138 Y941.573
+X227.793 Y954.717
+X216.171 Y952.94
+X348.501 Y919.312
+X345.616 Y926.677
+X349.715 Y944.347
+X392.012 Y925.219
+X512.746 Y948.012
+X543.435 Y919.711
+X571.172 Y925.362
+X561.262 Y926.652
+X674.781 Y955.73
+X748.011 Y922.167
+X858.917 Y935.736
+X870.232 Y948.637
+X992.023 Y920.853
+X974.208 Y1004.89
+X996.534 Y954.674
+X808.549 Y966.661
+X815.574 Y992.746
+X797.18 Y954.85
+X775.514 Y958.095
+X732.371 Y956.653
+X713.194 Y1010.1
+X698.596 Y959.588
+X708.193 Y995.232
+X578.75 Y951.359
+X446.945 Y980.165
+X388.053 Y1009.64
+X394.317 Y945.407
+X382.521 Y986.373
+X337.097 Y952.512
+X300.143 Y1005.02
+X336.866 Y994.566
+X7.43166 Y969.143
+X33.3046 Y958.983
+dwell 100
+Gather.Enable=0
+close
+&1
+b2r
diff --git a/python/data/path_phase20k_PhasePerServo1.npz b/python/data/path_phase20k_PhasePerServo1.npz
new file mode 100644
index 0000000..57bccba
Binary files /dev/null and b/python/data/path_phase20k_PhasePerServo1.npz differ
diff --git a/python/data/path_phase20k_PhasePerServo2.npz b/python/data/path_phase20k_PhasePerServo2.npz
new file mode 100644
index 0000000..7af2423
Binary files /dev/null and b/python/data/path_phase20k_PhasePerServo2.npz differ
diff --git a/python/data/path_phase20k_PhasePerServo4.npz b/python/data/path_phase20k_PhasePerServo4.npz
new file mode 100644
index 0000000..dfc37e3
Binary files /dev/null and b/python/data/path_phase20k_PhasePerServo4.npz differ
diff --git a/python/shapepath.py b/python/shapepath.py
index 3056cf9..19184e7 100755
--- a/python/shapepath.py
+++ b/python/shapepath.py
@@ -36,6 +36,13 @@ Sequencer functions are:
     gen_prog(self,prgId=2,file=None,host=None)
       if host=None nothing will be downloaded/executed and trace of motors will not be uploaded
       if file=None the program will not be saved and nothing will be executed
+      gen_prog modes:
+      -1 jog a 10mm square
+       0 linear motion
+       1 pvt motion
+       2 spline motion
+
+
  - plot gathered data
      plot_gather()
        this makes only sence, if motion has been executed and data can be gathered from the powerbrick
@@ -75,8 +82,11 @@ 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="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=0)', 'plot_gather()']}
         #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=-1)', 'plot_gather()']}
         #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=1000)', 'plot_gather()']}
-        cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
         #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=1,pt2pt_time=10)', 'plot_gather()']}
+        #cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.2)', 'sort_points()','gen_prog(file="/tmp/shapepath.prg",host="SAROP11-CPPM-MOT6871",mode=2,pt2pt_time=10)', 'plot_gather()']}
+        fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/mode2'
+        cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAROP11-CPPM-MOT6871",mode=2,pt2pt_time=10)','plot_gather("'+fn+'.npz")']}
 
       self.cfg=dotdict(cfg)
       self.args=args
@@ -115,9 +125,11 @@ class ShapePath:
 
     def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
       prg=[]
-      gather={"MaxSamples":100000, "Period":1 }
-      #Sys.ServoPeriod= 0.2
-      self.meta = {'timebase': 0.2*gather['Period']}
+      gather={"MaxSamples":1000000, "Period":1}
+      #Sys.ServoPeriod is dependent of !common() macro
+      ServoPeriod= .2
+      #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"]
       prg.append('Gather.Enable=0')
@@ -130,7 +142,7 @@ class ShapePath:
 
       prg.append('open prog %d'%(prgId))
       # this uses Coord[1].Tm and limits with MaxSpeed
-      if mode==-1:
+      if mode==-1: #### jog a 10mm square
         pos=self.points
         prg.append('  linear abs')
         prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
@@ -145,7 +157,7 @@ class ShapePath:
         prg.append('jog3:-10000')
         prg.append('dwell 100')
         prg.append('Gather.Enable=0')
-      elif mode==0:
+      elif mode==0: #### linear motion
         pos=self.points
         prg.append('  linear abs')
         prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
@@ -156,7 +168,7 @@ class ShapePath:
           prg.append('X%g Y%g'%tuple(pos[idx,:]))
         prg.append('dwell 100')
         prg.append('Gather.Enable=0')
-      elif mode==1:
+      elif mode==1: #### pvt motion
         try:
           pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
         except KeyError:
@@ -187,6 +199,26 @@ class ShapePath:
         prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
         prg.append('dwell 1000')
         prg.append('Gather.Enable=0')
+      elif mode==2: #### spline motion
+        try:
+          pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
+        except KeyError:
+          print 'missing pt2pt_time, use default=100ms'
+          pt2pt_time=100.
+        pos=self.points
+        pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
+        pcor[(0,-1),:]=pos[(0,-1),:]
+        pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
+        #pcor=pos
+        prg.append('  linear abs')
+        prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
+        prg.append('dwell 10')
+        prg.append('Gather.Enable=2')
+        prg.append('  spline%g abs'%pt2pt_time) #100ms to next position
+        for idx in range(pcor.shape[0]):
+          prg.append('X%g Y%g'%tuple(pcor[idx,:]))
+        prg.append('dwell 100')
+        prg.append('Gather.Enable=0')
 
       prg.append('close')
       prg.append('&1\nb%dr\n'%prgId)
@@ -291,6 +323,7 @@ class ShapePath:
       hl = ax.plot(err, 'r-')
       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())
       plt.show()
 
 if __name__=='__main__':