From 89518ec5a94d54e1fb29d3e3774ba70a2cd81f2b Mon Sep 17 00:00:00 2001 From: SPD3 Date: Fri, 22 Sep 2017 23:22:09 -0400 Subject: [PATCH] All of the test cases work for one dimensional motion profiling without jerk even the random cases. --- ...sTester$InvalidAccelerationException.class | Bin 648 -> 648 bytes ...inematicsTester$InvalidFinalPosition.class | Bin 624 -> 624 bytes ...alidNextVelocityFromLastAcceleration.class | Bin 681 -> 681 bytes ...ematicsTester$InvalidTrajectoryLogic.class | Bin 630 -> 630 bytes ...aticsTester$InvalidVelocityException.class | Bin 636 -> 636 bytes .../bin/KinematicsTester.class | Bin 30596 -> 31189 bytes .../bin/kinematics/KinematicsSimpler.class | Bin 16299 -> 16121 bytes .../MotionProfiling/src/KinematicsTester.java | 61 ++++++++++++++---- .../src/kinematics/KinematicsSimpler.java | 13 ++-- 9 files changed, 53 insertions(+), 21 deletions(-) diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidAccelerationException.class index e2381e764994dade6373e4174816f4b3dc0680b4..6be7dfd5b879a34be7373d4eaf768cff58b97b31 100644 GIT binary patch delta 22 dcmeBR?O@&D!NkQX$-v4g#lXcXJvoTU3;-}u1E>H1 delta 22 dcmeBR?O@&D!NkS#g@KjjD+3qHx5+_FW&lS_1~vcy diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class index fcc841eb7b706083f93c0c1328e7415dd6266c31..72ec180ab54131de3b559e81da118ba9247880a4 100644 GIT binary patch delta 21 ccmeys@_}W8Arq$-11qaG0~f2#WGf~!06q%@bN~PV delta 21 ccmeys@_}W8ArmJn11l>V0~ag%WGf~!06HWD1poj5 diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class index 698ccc588c73ff83e5b3b6540ca4d52eb578e0b4..01ee451dd5e3484b4b1857497fa33d59b1d9b9e9 100644 GIT binary patch delta 22 dcmZ3 delta 22 ecmZ3(0~f2nWCtcQ06cjFH~;_u diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class index 6349e1cb45be848f0cba037881bafb6b1ad2fe03..ba93379e7ff1a80add52318a3dc1895bf0c783fa 100644 GIT binary patch delta 22 dcmeyv@`q)E4HFlu00S$lAOjbx&}26zGXOzO1X}*!bIqBSqGD4)mbs^Owz<%pJv!HtXYQVrWrnmMI89Ybb?eW&?UB~N3a(&(%~fh4`pc~dzIFxKL#>vA;>rrOl}fGM zIt>lv-|e)mO6^3aQ~a5oFUeLGVF#6NbL-4oWxmX&vYmBOsk7)TfQ@=H;3dw`BSX4M zT`Jt)-|&fUD%~#n3t*!m;H#ix7**A=&4d>g(d6NY3Q2ltx_M+pY9Xh zQmjg!=%-SD(b;(ap~jm5r%!|qYM@GkL_e0w^wYf{+uc$_?{$buL)}#8E2eqp{SDO( zS80UkPj?FMwtYnyul_3N6nCgJs=^@uMx+^|(%1@v^y;sIAjhjTp~4{Dji!SAO;TyH z=ufxT&@}1ri%Q$8Nu{ZxSKS*c)@uc3d)X?PMLz+CUGKgf=tfphlsPKpiq6dbTz|Bs zIJbMxlA>a3{vgxbQRX~rmZjKZf?UBKr>iu>tw(PtRo-L1N(G|Fss1olRCvBQbSwjA zsx(VV!_V66=A2c5g|mLW~(%(!XonGDfmzm zB0KX`nqOfNRd!?uk*9?!Evm4Hym+cI5#6cM62Z=$6|tg<`c;UcS*p@9!OQ$g@ls_X zxm%@s1RM8NhK&l3LLtiLK9%klOf9PjQ?cf*Vq({5+`ALlqX1m#XW)@V6nW_`Y7L~RNKJKp!9~F*4p-Sfom9`14^eKn;6wlX6%get)wC4~5<5Ma< zEqKyrWtHS9xaLA+XS+(J6&8_~cU7&3cBr&d@Dw3lrHkofk$9{9yh<;K+(>6`JCXgO zN-ue4w-?zjtMrOzb_bFDs!C;^*|&*on@X>FW_J|X`&4?vGrN<>ep96bp4pv6_P`ZJX6ptu2WzE2zK^C??K4#Q0(AG!8eg(5ld6w); zhF>|hZMR~KdMM&V`MG0?vB4`FJH=&y_I`$p25!f8sz@uckei>^*n zjNuuhkIkrQQZjyd^TL7u6#(gvlisPl^J;-&%*?kQe z%Qz+D#-l|I|I8h)ly`f5Tx6X&M-^l1iDT!VesMAEvUAB-3pzAb$~#ocp3!P>KgHOg ze*P2ZH$1D9ci&O7Z|ZZ`l=6-To?o-%;Sa$^@yL|>dxK|VWE*Sz!rvg4$A2rhJUm9p z_-nu$B-hlT;g^x*s2&C)tY``TK{cm z`PZ|vziK~B$+(tUzMr=_0_}YKi*ll-(5dCYLkBNI;O(s=&(*(wE4ql@b9>g+O~^Ta zXpqa-mzX*G7c}Epa1jgZ53nn@jvAK z=iqFmyzkkkudJYJ2upr6e%gTjv_fxH2&hU$_*SRHNpxo&*)cx6C`reeyRNgDO}|d zJQA}+LCUn2Xy-R@IwUiat2`!*$p;zUxJPs#FOj9xP%1tvf7@qQ;sPaOo&7;{k*x9>ye1}eqKh6@Yo6JJV;<;Z9L7&xTjg~G0}6)o{K;bw z1$jM{r--bY$Z`oo8{NRXxezn52!1PX7{(hoP;19yM$E%7kgxJayfNChmH?+d6|q=b z54BJIa6lx#OBip;sJaW5{g6qI5=&mTxiFkJ!{ow#o+Ujck~iUq$}t>KIjO1JEW>p$ z1tnUmybW&)5T=~&wQUdvU71lfGBHC=P@(%nqbX07$x5OyXV-};gAIUrMv@qV; zF|YMRqVjaXSeQ1i%_|7wT`>`H;p&%{ha)F>-3xO{#ProU-NNu~!JmF$xKNu$ixW9o zH8<ZRepz{Mr%~Z6kbq9t9*>0Bxn?m`L-aBQ~7v7PP}Or zrO&aY0p@v_!SYEepUkH~`5vw?N;~c3W-*%D?76V6sXRN3W1?<9Ip#)FxtULcYoIY< z%^oIdKU)UZz+ALNz?sEOE*Zgd_;i)e;Lv=0{^YEZ!a~@ZOB5$LT(Us;a}4@*+lvb4 zGjV7Q8*C{(?R8Qdl~PQ_DaBZErKgC-3sqhu8js4KEHa_Q{F%aqN`%wR7QUA0-`B$V zTt@X$EJZ1%sW=9fQjG3ko|gQQ{FK@HrdWN^+I-R40<5xpIk}s$c*F`=$WgvXgonjg zK{fTB*LqbHC)uE&B`Uv*FGc(;vdqVd3Bjzgt*EBh4y;tqK_>TkOkIhv3{i zytx-Me%Ge(^hk4I;UJuHGv$CAJr>wws{Q;OBUM+d?!m1Bd+<#*(naWTmC9EOJ;vlu zHaSn>Sqp@juT}Yj!a@@47UJ$@;rwC5GwO{f1te^qhr1HlEz{vc`Gqt0Mc5qu%zE${eJis{usohtV1Ny)j&Yxst5+J9E zg?WmjdWvXHjM{v}t0s3*iD4aio6*2lM zG^d#Gr@)HtL~Kv76rd$|is+(#7%#=b&=n-R7Z#f4^()9W&mB@yJY-rAYe_+Nk(IyO8m52T4m6$!@ zMO+Mj4Tc|hp4dfOyu%0ahAxJ`fdCVokmiv(2?jPN*P!za6O)Wh}x+q$-)Vu}}RM?Lr z0N|%kr0ZvG_&C6tv-=fvcN`AObj)2aCXB;aImeQhhqh77ccECc;dmP(TpE*H$HeO< zmY^}gjaTli;&ivgl&3q#O>`YdHRmwwni+z_srZu>&c(Y#SG^QX0_iE^!E}oh1mYI3 zg`yVP@IFy7PA%6QS251AzbOgl-w0!J!f;j(=XgQLTcm{ZQ|M-5I6ti$okM~c;0rNf z5cB09RsNHh78|r_vB)_ia`19bELTQ_^It)Wh)796YrkO*oFXPADZ-ud!}$dqGZ)5; zNkU4VCEtR$s5PvW3FjB}^4(GQ{}^GcjvQevDa|30SjU(j`dUnv-?HD1GYV7Qk!hB=1+< zkemI*oIz72Vu7n&Er;qu4iYqp^^CX^fTvz~3d0`+Ec|LtSc>AGsC^VUb}y-~Q0#8} zZTHf2T7i`Vkz2+DNl$Gip4J6F*PVLV5!Y?PH_7? zxP1ZKz6fr2`{C9hTU@QFT5cmk<+cpm?gh86f!o)??Hl0sO+VZ^WQ+Sc0l3|v+wH_k z*{z2`v#8)O>Y_WMw*cd9z&HdL?;;kwhlub2{QV<0D1)3v-OFjzxSI11L5U6($}vFs z1W-N$l;eQ%A3*sWQ2y%&ibE^7)l{WWn486WXoda=70O9K`4&)40m}D)avD&61eBls zKykPO?k@c^P*`lJP|g9$Z-DYUpj-fy{{hM$fO5$X6o*!DBdThlxOujRRzmlctAKJ1 zP;L-jb5eiCXf*s`0y6}Gg8Nog3&m{!@PHDsfw4%6V^NgMq5&laP+|ck9#EKP_h8U3@AB4px|mz)k3M|;@A^P9Ty=mlsrJm2NWxy z%mkEKfKmh~B|)IzZdTPo@oQjiP81l*d_Y+MD2o7PF`(QDD0czMvLH}!Yqv_FxUc9u zwNfust=tDF_XElUfU*)$RsqTyKzT3-6kG`YhoICC70M%k@+hEe1e8sH@))3O0hGss zK*8PZe+Wv0P@y~xD9->&DWE(HC_4bkKF9;Oe!>(GbxGhU638frR{sky+0m|Ee@(!T93n=dgfr2aERSJbQa@mn5lqN1h z;0SgEP(B8fe*?-fK=}kvJ_D5h1c8DJTCP)p|0)S)>ocDQbPsuC4hVdAm31H zc9QzCZ|M$pipH_iZa@Y&SN1)bW8sAx>H&aM;w6P&^Kx6p1lGwJK=}nweuYlXK_|aK zC%;1{|8s-l{*uB01vhR3Kv|{hq*tgqvA-vbEo04tz9?jWg5|%!@)c^wu2BZNL75!; z<<1v{w>c%>(kXdd*R4`^wHFZ~vC9>T<6)G{Bd8sZq6{7lc4LFEi@Uy6%Wf-}c4+J- zg54yrTOI5sgWZ~7w{{SAaaTA1yE}Dbm{d7q@buA8=NP;`aNG(U4S}N(4d9Jw3~xdc zd8!+h!A?u*?X(nJH2$Zsv!qP5OEM0-6 z8?a;m3-(NScVOuWEWLtY!R_WM#lqZOqLMb&A6Nze%OGGG3@k%{Wf-uG2!aK-qN^55 z2RHYwB$hG2G8S0I1Iq+pnFuVCfyERA3$9dGEtZacu?UR?*-FwDuzVV@V7Uia?gf_nfyK2)8ArZiGOmMHEf%*;Nlz$ZcyWLXIC0|GuH@LRwslFusi}R?pv0=u;42BKZHfBVMD`$%}kEXOpeV=z71HO1eT|PWqS}C!wvL*2#Z+6 zhKA)iV0j)`b^*(a!15BXybLURf?&bz^(w^@coiEOme+vgbzpe|SoQq#p1sCQCWX^7g*i{mczjE0kC`sEJuLlXb>#8Fkhuu02x15~MA0$@oG5sMTJEHS_m2Q2Zxk^n4; zz;a6vEI9d9wOG2k4I-X;33bCtss${yfu%06)B~0jV7V1o8V13Fj}ipHa$5KN2SfGz zM0UGhEJl}JJVA=x6o)5yQiNcFOspG=^IuXb;5P&O7J%Op@LQ2tYE6r!HuQkhj@C*Y z+{AE~Gi=Orh7Ej$A%GZ8>0($Dx)|JE*?B_p`?k{Go|5|*=Wpg`7eX3@kPabqrM6Nx z>Mz|+SyBcql#F!0)RR_A?km|lokE!76av0E;a>>YYquYa;P6MWJpX|yRiEs-@!WK+G5WKN?RP})Q&Z4#6=1(t4trB9_PQWmUz zDovBJ=}yT^>!oS50qJ8>4(*Y0>9Ay>kB}afrqfwz2IEp5GawC@@>yM}fYnFZP_nYN z(k#|dDsm~CHE}4L)pIHv-&+Zw>=$)q<0N&6hf!I!27a1P!Q^VR={{+B8m+)5ZKg&m z^hqn$XtRCN=4!P0K4}Xznr}WAYqTYLy+k%GkuCLB;B1_YC21}cI1dV3K&_;OP~rl7 zV1E%5xtIna9V*=kMJ^!=(md%dS}HB2yOG`}Eu*c{a@vOUY3XiyUAl+%BQ2NirQ_0l z^f}TmrTgiEw1WOX`lqxKN?y$(rL``F(iMk7@f`=pt^vN3<6ogT^Jx!#S)nU*xh9Nz zdJbQP0|c}Y0@?%tZG?a}V{Ct%rb^q~fQ@hh8{hyPk|Kf*eHgh%`lHuxiK@<-V0kFeE6NMuhWvM0SY zkdC9~ByEQVN}+)rFw>pTz;lQ*&tp`50WoG5?UG)kccqu;q_mslYBnnbqUr@1|=(RTWzJ+IMr`J}xRpv{*9(DrDwGM{bQ zG}`MvX>Vw>H+|B|1N3*$g9a%ivbVjJxtbb4nfsy4H=)dO#GQXZq2+L^gHY;QaI1rG ztG5w%-a*`X8*%3l;?BE>JBJW=-b37ZA93eB#GS*4J0BqK97f#v5OL=t#GMZjca9+L ze2lnr1aapmi;@1#;*lmw$5=Dz6V?)G8|gFFO*+APNME=Ngtd1V2y5;%5PWnifPqwE zfH|ZI={=vqJ*?3_^hrAsfOb@)9rIc5Q;l}qC+&nr`@$#fOO5umPufY1_MJ{kWFIE7 z@4XdmA7H+LqEAB6-$BthQH`{Ngj3bhX(;-8DEbE|`bQ}G2Ppa{DEen8`X?y*3>5tf z6nzGYJ_|+v3PqoVqR&Cmzd_OGpy=~Z^zTsgc_{iK2AE42VE%F`)aL+$&!q)W=sbOZ z`89Np{{h1A8Ha!JM>wO2@2pSJp3`XOebO#yG~cScsL_0@^8YlNLzRi_&qQ`vsLRH# z-gM7q60U=hbPX!IPRTM+TN&Tlk~vM0CCWlNO_ph)Y@j=lE|nG9AcxUoNVm%2v`3Dh zy+~h|anE0lqEC<>m!s*F97{jSadcKra8u1!4qwB^+8kp)zF6m9HPRDUUxGWfPU=$1 zR_x<6b}#1xZ_?8s{hHC>=5di_T#~1Bl$*yXcP_hzMq zCo73Oe#2&pNLFIA(v;-zL|zR)ktC&syN|6g5M+9!)G?}*jx~>KC#t8ZqN%i)WHmV^ zD=iK`s&AxvTJuQ;d;1Bc6rX;JRt%3yOj<&6Ld>HZ35lcM1oWF=uNx7YmWcNniL6#y zVscy!ps1AtmXpJ6yzUHe)gU=8=FzPbpB!h;LZ_t^#~DRBlEPfmbQ z`68r8E{|eKQAzT)8!biUWJmz!(E{f2GOxdnHyK-vr|#v=Ysyo~c&joF+#OrsI^ebC z#A?PjiQ9I)WH;_wJteN%@hmykZ+@d zawj?_ccv3^I(;K|q3`6bbXx94XXFgJAsd+@cW3c(50)tRWcB3Utdrb_r6KJq_h-Z9 zCTxN{nB~Yr*$jCYTP_c0_siAU!}3VYLd&Td{tr_R@^1Wg92tpb3wZ`Mq8Zg;d79O7 zYQ(ZNtCeuPXB_X2ax!&d!+8%p87PzW;yrmL?yL=Cop~=jh0_ui!F%&QxRJJuKIeT= zE()LLeT(-4ReXbXKktvH7+l*c;{)&%2Wd)tAf6I%yUFo%kk;yX-yv@hsEPQhUatOB zMhYK{E#_OaOM5dZ<|-L*_GKKt<`~8i*iMGw^GOVE3n}^lQ^x1P_6P5yqhp(Ew;EgSA1I&+C^JKzMKk3>%?59`Gwy^sONTm+IKKzmq8yh= z8cFl{s!gQ6IhVh)s)Pm#FQTSAkE;Jdm zPc@M>Lj`dqd7&n#7l&$ zUDE4F(U9I;NN*mbHy_elK&kRVYA-LM40$mk2+lYo9WO7T>GEBahq4Rg zU4qjg!AG$P%W1ru1m`*>I3}0`JG&%^j~2N}@VF+yA&@>ynAQu~>2@J!+pTU7--w*e z;t#CFo=ay$h=(*G%ub@b_~WGVoIrIRhp1auUxEDa&&+;7*(?iauJP3ZLX1dFqPj|wvpToYateGypU3!Ib zM~3n3x4=;E_7Jn(w>O}q_!#6i%}^f{LUZ}H{g*@P=?Vta^srLOt)al@#a}D4G>^~0 zZDWYNmUljX0ix|}4+Jj?D@G-IAlMBnc7z3cKse?S{VQJ9Y!M<)#C7o|H;H@;ta>Y~ zdK)#ApM+IE1@S+FadA6hK`F+(W)0i=jcIs3q2w4(nLN%*c)wg zihP?c@--0j9{wsszQoPKw>T|)k#6A)F@}^OS}3@}Zq}btkvAA>2-DQio|Ky-$PG#% z#TilH3Sy+#jv)e%t^cA9BCw42(3f^G-0&m@ylU{4#n2aKW2Jlsl(bpQkxb=uCS5@o zb)THX#O&pJ)5AT~8s+gz-N$pBx5#7g&tCp|dUTT9F~;7on5}3{L-FD1v8C*ip0D{L zx(qKRLwanDaJXL$Oa*H};SqAj2u#tWjuBc;jWB0+q?R2SlNKv=j4!2(czYjZ ze1BG2Y*Ks@V8YnyYPCHJ)OvK2ASEo1XG!sbqzxDQwAQTnJuyjZOZfY&@K|rFc|T9lhHJs|TC|*Ao<(6e9-7V>P0{K#ZN) zcakDDQg~A2CW=mqzzZ+&)K?U+*k{Pk=+EVtkZ!d5%0b~Pq6ux#eA^lEPq=*K5PU?d zDCV)%Yp7^WJVn=t)}FRdeLEGvGwoD7rQ4}^(nn{{j^6dx(Q*SzGC0J2Z4D+>_=Z9u zmB1P2(+XO}x3eB>IJ?8)oVc9X1MzSVN%E^Ohcc=z@1>gZYlw?)P#1YW^^@PENpd+& zmEWR5`E8mbzeDro_u!=O!$CiyE%FgK=*O6}9;NN_Cs-DJie=GfSQZ_p{qpBjE`LFX z;J-Y5kdvmd~&} z`7E=_=hz~YT`8Yu8|2^FX88hpT)xP5$bYcsiFS8T!6?R6x%FZLbC||>R z=sGtT7_VXAyn#XDoeVPXZcz9LLl_^6bfO`g=NTe+f&408WQf*+%vJH8pD%U<8GO4< zn{#mqLFRpa7=PzEp4df4xZx8WPY5Rq_y^hqS>bcE5M-e0d^9GxeGqsGLH`g>VKkpz z)Sp^nLj4i)!s!W6kMNHX=C-r<^ryYZJBqwW+Rt|DPhEI>{%-`bD4b7d#E;=grAa)F ze}boInu?EeehLm^Xb$R%;1)~eY#0BGXX5ECoC7!xYCNvsE&x?LjmB8?ACybbg5Xb7 z`vzEW1wBN!8~EQl#X6%P!=xDy&JZC8+W*6sSP&L(V36`GhEg{dq~R#Zs@*`m_Ei>N ztaLR>{)?eMXWzIf9WJptHF2^bN-P2{kr92=h{MkWH6r2)W!9;415w`t6C$_xvMPov z3@bQ@E`uRrxmW{M<0r7@le(7DlOmkda!h7sCGsyeJVq;S&ioSYj96FbTt0q{sNhk^ zqJ@W(@LG@TmAcF?6TNh8{HA(37?rGN}~5cY{wMznX@3~Vr}zNlWthrwq*st0LwXhI38dGM9z}W` a=@Co^gh`#GNYOKVBvOKJ@EoL^*#83sJ=J#r literal 30596 zcmcg#2YeL88vkZ?Z*xgD34ugH51l~h7$6`yAe7Kc1nC^fK@O4}CZU6XU>8tCK@=7A ziJ*cghEoLcP*D*Pu{<76eWIYKC>EXt5#E1x_Zn9d^AqKEZoVns|7$a|Gjkli{PKPx z>cYQ{BF?DBV0*Ex$XaI4E6uf)mf1?8NJj2V>s)L5LTmAi_QR*mwB?mCs#P>)mb>U^ zd(rGdTM46vLp+)7Jxv+(9*oRA^9t?7_Of1#q*iHT7|DGd`L-yEWmG>@)fkGQIF;hb z%t+~}wGd5UOeGU3j1q=`mtk{?rrAnztHSWE2UyOtS462+m%vI2Xl zDB$5whYI%OBCEZa(KW3m4Dmo7T~=Z*p3x(1ObmLgqf%YL#Gt$qo3+d~!cl53v(L5V zmRN;c93=||X!#|%1=ix+0$aAdw9HzZXY1>jYb&wkk1enl+Ir{Z*&rGj=CMq@cV%ERSsxsSL zr52*$ykJtub@+GYE$~VTrLfay8qDkhNw%s8yQp-nS7-hz3uHD`?X0Ux-9%?0Y}B6tKXFDL z88TJsQR)8vf=^thQZLb82pf$6UnL#GqEhclciOSE>@PV+UzM^eEm^-#BiCeql?I6Z z44?3qVs-jNj!J_>XA^>l8h-}dJ`p*nAu0_O{n)G0Pw(P&=K##I`mUw@SZIYFg~l?Lf;G?nac zvPx4#e}=tArb(AyRM}pwDoqo;8s1pBUMn%%%U8)J`Ux@Y`uFWZms?3u&QPg9bY=_Y z`eW^71-<*uDJ^pp4Ykf6V=Hvz*~@$;$d&AIrb@HCdh~}hL`zj#CfHe887r!+UqvXIn^jsNcv)B_ zUaCzbx2SZhVB_|xuurXuF@TXsTGxBstUnpv@OqJtZ73=pSx7LyV64P^YjRZ zm`Isgtq|$jzrOkpXW6I$_#S663^7F32%d~tSrvH-uek`>c~YgVl@^hocU7;5wyCsT@DwdxrHkofsd%gXj7ran+!%Lm zCy~8VrRRLJJB#e+ReHfUyNk$vQKfR<>}y4~Q>B-Dvol2YUX@<)&F(6)_p5ZkH@ln2 z{-;U@eY3mA&}(!kieASShi40*-~2q=>@vHfSfw{a`Gncn5-1y8X3d+0H~)Fqz<7&M zMyL&pW;w-kt%dgdY!IgsZ%3)#O$D9Y)=P6(44sU*@XPzr;~A z#9CVB*>7+ZN71{C+6JrbUDB(=_ZT$~R;9na*jlK+NfNEU&!}avqMkjHA&wdLJnXhu zOJ-mvrdEK>7Df!4ILThSFm8UZO%ta{eyDY}vDp*jCYDCge;75M<=&d|--T*6!dg}! z_JTf9=~MbI_M~Rm%K8>sO9d;f(!9FVGJC|(NjfFAo<2vztwL{b>3c+RQFMk;lh90v zSNe*j8TQ`rCx%Yb*P@SaP%lg$*`kveI;YZi^gW|^YreiefTbaTVP`6IeGPt(kT9BQ zuT`<(rC2oXh?K0)^pxoNf=d6RU$FV64;&t~GHMk`-`N@hM!%`_J6(j(^9pR59~nDn zI*e|SH08gCC-xMTq6K;zO~b&1ykJc=Jc`mc0u@V&j+Rm4)wGUEg`qR0!YHLw=%q=q zr0OZ9!ky^i%IV-rv~neqT#5Qd;=m8C9J7(IxT;%R)h({-7FTtPtGdOg1s$;#unyLe z(cp-r=m8nXU4*6ffXB9b4X?)~v>Libd*LCx6bd$CQfZt76_g@y(}-rEAWrS^E-C zDOrE~dZPV(Q&%e$U0LJwA9@^BES)Er|2V#Rmtu)s8-1eU`{RnG=?f2bt&x|jSfa-) z-*#(lfjo7rgdD#ZvR_ zxu1{hzFM*LnmBjupqytFOP@neWZb@C97vPzdUxN(Zr0#cRxm-SxbEfgF%9M&Q!MQ#jaztT z=S{H7ZgW0a)TM<|(WO@Ytaih46ib)JMUVZk;VGq}_qMtN)1LlQsmM6+%$jBQ9RV9< zxv6*b2hWz64vxgd=OLCy&lg`D6|ZFdK6tV{|8${ZX}rMx#`=PJO4j!azn|1+cqv-0 zcedld)4D>uqe}PeNPbSSw0Y7psC(aaO4fU=f6DlDcOklXKQ?~Z%*jecr#8!G{n_M4 zNOeWc);o@UrDUCIv}oTuL!y-f=U0F8(&fqDD_P&|KGb1K`X7pA;It86-*d4(%GB$c zxbdTDs6Kq}rqL-Q5vNw(I(S3R{)$CD`NV_0kDLG-qd&86E{{{PUcKdp+*{T{JJ((r zS$5;9hoHETKhczT9g3xSy_r8vAG8@Qe|K)~C!I$sS%0Qg?BgAdLObt%tDLASbZWo% z(7}rkcxOlM_l@s(1YN}K?v;0GBXWK?dgjLqNGKb(cfIN6yA@03q2-?(e0P9iNmwv% zUH)Z2zIZxzU7u&g(E)Z!WuLPzT$2ayu-PIKFBOq3Sr56Rti}0`BKM>!ihb!iqcNw< zUf5nI$FMULhY$9(%D!RWGKwn3 zupimaQS2v-FP>S@P{d1>U10yi;SK9F43~1N?6hnp>!k<9pN*oy-&J;z{lO?!G}y=C zD70B|o&qh7k7j?eD^cvSi&O11$>;?*V^XAY96doP#~koxjQMs)`w{va3+E|{dYHMy zFidh2kBZ_r@S)Gmd<&`E%%cHQo0}D)+vHYWgSq<@M-<0lliD6sI-$=Tdttt<#LRI* zq(+hFZ8hYTnHs}mcmjrJ9xYOdDzC|FVVJSbo^316$Kef#)2}C^!5*SUvdXXFwK3Bu zbLhP>O7fX$7|bbID;J&`up?=alC@4xn0W(K(@(Z!rbh8bm>+mp&nYa#d5*%~B{SxT zS*3dxgxQlFe{;ZNiDoL|xQkZJ%RVsjB^GN(W`<86Z7Z8C!aQ^u?+YHP^vXb^*)U+_ zcuy4>aeGG;EHzruY%JdO&1F_*R|fjcyLwXqdHMDdoS?zXxUZvF9GZbGFe{^GUXl%l z7jrbmJE^?0m`)^W(+O|>1oc{#X9#MXMs-b*1*MzHy9-K9jp8$(733Z&?W3)3tUfR~=ujj%t`tp8J90JwPS9#G?-k%SEN1!ob z#y;kypTO!7#i8ExkR!K;4{bsh%?I+qDj&izFGwt!k~gQM1V-Z##YsL)<-tFWnjy0D7onpK2bbumh@>&riv z;JjS5jUO{X*M4x>7+XonP@Dy`&Hy)tQ|qBrKc&V$iuSV&FXLVUd}hL@>)jv&|d@dAw#1O;0OmcV8{4>8Q!n7kYD z7?hbWVAM3QJ$Q4~IEIe$$uKTM!rrZUNx6McDF>{e$>AoIFBNi#DVj1{JLjj%V7ba~ z7OtKMkgmX~yEK+@e5J~75h74PEt+zJa=;iJ&HN5VR)KrESTd)&T1<6J*KEDaeS$D` zHpbc1e21+xwb)SxCn(D+(24}E=nAV^HAv$apKRuLW37t5N^GfC{4I8%I}zSf?Zs#b zJ|?teOdY6<~7vvP@+vX3SQ#O2hAIF^H{8BF&CK)33iIH%pprZ9D2;+^` zjaRc>b|a(KUS1cv^B&59&-Ar|nXhML32s06`L_JjGKVIx;Ni;5AH++S)NF?)V%v># zppi1@Moe1*M7J0^B5bUsA%O7yo*7go;pz%8;bnw=v`HA$qatcO244t0N$jC5tKq#{ zMES=Nn!=XX7viV+KUDstFxdo{Y`^0CY+GTOH5UO~Ibcjj%zQgWHX(D(jJ%!n8Ad}j zCpAbguz3Y`!(iP*x+o!RIS;2|XW-atYJp>(4JTqXnY*I9DAWN4d|VjtPITz9>~^{< zwMxrOHS=8&cNn%a!BL0CIPPnx)e{Si!96Xg7OyG_rPVXSu7_fTJ-CyY#e^KGwW4T5 z753+fDflW3!Sl5~Vg!U`%g-t9?K-@b?V2@Vwlp52^gMfEAy`1Mz=aafhU-0x2nm=9 z&n~gewL9jNUd1^SAh_|%Z6Z!z+pUF$b6rK(!Uw9lCjg$wB{-ahzj7#er^A*?)3u4kB*kKP`TZNh2Ho7dYnl05n1H6kH1HmkO)_)5AJuHtB{<9| zN-#qai-&hreoU+pOxhYjjs$PJSC>aH#=^Co7SLWg=`rYlM6erbl zQbQ*-c2YAZwRBQzC$)7_2PbuQQihYdJE^CWvYgb%N&TEO&`E=xG|WjOos{dOu}&KA zq)ASi>ZCj;O?Q&rNrg^wIH|-*bDT8aNsF9xlarP^X{D2HbJCqoTIHmBoOGX)9&plw zPI}l$k2-0Klm6kPr=0Y(lXf_1ms7B}$4M_cX`hoSu-h}FO*y@~OOxW$w1eIR0`WX- zJdYU9qsCLbU=#G?(0~xkD4IW47zK9 z&sNluTI0WTYD?{?1ODrbQ{5TVg}URjC(c1#FHrjCgZ&}?scH(zmXjeA-xIV9-*Uj3_=vHj>oh_$t1tE#fC()1P^mFSr zPWshjv~@L=u7+yXKmqqsN9^YJqIJ|4FkQ^ofWZHtOXxcVs`wMiLVyw1MXHzE%aL;X z5V(C9ygUMK9|gCY!R?m7+`>(DZgB^tTDfH`Qf{|`+o!uo0dw}vjpnL!*9|Fq10Oj9- zp}4h@R?SenE#5~fbUsojp90E%0p%2+d=4mI0LmFa`6@6Jw^nd{s9LoW6bg%u6v{b3 z`3_Ld1IiD8@*|-93@HB#48^S#TuQ25C|;iJqm{^gu z`sh^yS1eouKtDGWTzRTiDC9K&_&|x+z?hk8uxLtQvDA^_{t1f%lmtLY30IC&A za082t(7@O=fKnS!>HtbzK&b~P4FIK47$~?w@i#$Kv@bX%fmpy#o20w;=Q8t)k>X6 zwQ?(<+y*Fj0Lq!ZJA_uY8f*Irk*@8%CR)BgnnnubB!K(}Kt7@N z>{A-Z{!7=hQ#77^;RR%{d$rQny;{K~@en|&(Y(BuF`;#G7Erzhly9MvbI{3m(8+n| z8?)8QuC};}h+QsI4Q`?o9z~saG-dHv%I0xl*u{0*>SebTOgl1mYl7WcU^f}; zUITVhz^-^d?plQhk}j_6hG2KQVGNV2W(>YQ8tD+g8v#cua5MpqrZkv0qZ@d0n#5ar zVHxJOl>Tl@!Cl|K3k$Yx!o3>TCgeOFSlR+hdtm7REFFQRGq7A61`F;8|6N$pBgK*l zEIokbI$-GqELp(P8(8{=!Ga6M)ry6AyF?XjY#^}Y0Lx%t83HUrfn_+bj0}SX7n7?O zOFJ+3t|FE(z%mwC#sSNCV3`0clYnJP7%aHeT)kL21jQmW7G^6+_m_NNu>s2rU?~6= zJFv_GmZC6Na4Wicu>{o%{TL~h5@0C>mN~#O7g**2%K~6o6b1{fR#z()jbqmk;%j3O z>tz|REC-eqz_JopZUL6tfW@;%Sp$2duD86Nn0!ZdWAWOQ^o1gZ7Z=Eo6DN-CN{;PH zj_pdm7Fg~BmUY15y=55)3vP2)uU>*;5o_4UuwXNjV>6RuGm}3GERO-p7GQZIjE&(E z_}_#@EMg~uBvqL42eUA3B^w!*^T6^0u>1tA{0yyJfL4BiR(|us5-1{X zu4XJgYeV1RrBkG%{-3~dnNo0ctdoQbj1s3CB$*~jN*FA-qF=3g34x_cgjl3lsv+T= zl@t#w3BXbVSZV@GQWz{a@m0N8GQ0*6U%f=S%OlkRmb$=FA6OayOG98u1(qgZu;6zA zLSQ*#c>cYSdVUh?78FaSM=!o0#cqnrlYA*cFkvRvEyVdRsU_gI0{qs1-v;p0$tJa> zo1}Jhm(+pokve&aVYxeO;0}WkHt<6TA;j>7A%-=Pi^1!aoiC)IZ!3fCDMhluuM43o zgwPE_$fS-^4;m!(q&(?5S}gUVJEY#UTJm1WE_Dk57d{Lj;AbC#3jurV`VqSWkqRN` zC=dx!U!euvqzWqL2sYx{;RGv5SSm|cDocZ@y)=XdN<+yi4Wot9aJo+#LC;7d=_M(b z_Df^DENG`&cw61V!;fQx5Z>Fm@L1PKg_p#7dTifU7`=iKEFdJYKE}IB-%8oRD)rYY z4GdUmP_Rlvv`WJQRvMwvMg^qhYP2x{X*X!J@dho4HA-R={f}f$r#evD1SoAHlr|Zb zJ_VLOm8MEoSo>6(E={AQQXbte<f zkvEYY&q8S_-7GDmTkyPHT27BhH`8W3pO9A2 z%hF2Phi8R!3w_5W9c^fNxGf>hv#q7olx@KEJj-GQ7F{uTFC$GRw#a~Cb&Xz z-ct{Kd66shCQTU20t)eFjkeM)sAWm)R)0b1G5tOWY8?di00gui0@?rpt%ra%Vr<_; z)1*hefQ@zo8|(&#pW6uzjFlOdszwg%b}(-Y$-WWh&R~SQgArB-Bis{=ur?TBT`6Q5qDlg+&P4}^9JJ1A;g_G5qI7~+<6mm z=P=^V+lV`d5qFLt?!1Gza|ChcD2tcgWr=tuNyk`g={?pK&koZ2>^kWq)<-(wF%TS? zGvX0z<2Dfd?o|i_G2#(Rh}=LbG$9=fDBRaH+M$57H$%`4YqTQ)>mAi-#{$xhYqSpn z(mv8?Cj!z=YP63HS`s^y#6A^@cCyc|y120r_hd-=1j_yt%Ki+>#;IyNyGS@$EqwuH ze-34zhO*B<*{7lGFQM$OpzJT9?6Xkz*HHFZDEk{I`&%gc8z}o6l>Hr)eGbb09?Cus zWq%K4|Aay20*jXZ2c`e&Q7q01y9SzHU5aI=Ln#)MQE?lJ*c7S9R(Abfc4pij_SKN9 zv^>Xmvb43y*y@IEw1k2 z@yVugUZb2R>G^sAts@Y$o)WiGVrH_|Ad*Ua_r#RM`1KD`&6GqJ2Z`VyQST)>p?fVX zr;}K6_gdIsPxl^DQW8>X&4S8P5~1;xF)TSIS>AkQSc*A`rvM2ab-SBIr4%K_3_D1N z9k_X|4_@oKSxv{qv0qmbtD7jD-F$ibF5J<1miE$q-jMVwSWn`g8X@TtnWR4{PP$AD zq$|{1CTcG;>MBc=Ez2}gHqm%lp{cT&rpwXfkYi|$97~Jkcv>MR(A{zkdO}X5XXRS7 zS5Be|IhhX0wdpN6g^tK|>0`MbeJ(ekujPhxPHsf!>WVxMYTw-wHY$8wPjj{P1#|H2w*hps7fOXXjj#G2i zUNble$3U9%W+*39SJr?xN6Lgl2{m{NY&R=3l1aQ3Qf6HJa`H4#V{qwf3vZ3QSX}#h zn72VH4p+GD=jli#KxPtei&P>mjkr=rsGpH{4tecBtwk58fsv{pg}2A%aI$u#YX+$h zGEPN|#}D;Iu`^M0G73Mw!z8>=(g|Zp^(BBN5sTstV=fVwl2KoVavD+eRmF%dQqmP% zhQs-M@tbY<^)e%bihqX4_qy0PP{ehyWW){LQG9d}A6>;qrsw|GHK<5(7c$8i6eD+| z1UZwEWY2MCG=S}5ara9s42-*9;+IT}jukvb$9O}5<+H5aCc9C}khjvr*5$lcvRuyl zRPceBraGqGd=RpRuHQt7?l1FTAD^|km7<$OGRaM)ftJg$v))vfIw zK51zBft4~#mPOO$eCp;aj|e#V0*-Fvok#_o-lWKV0HZH8ma}2t{iwIxp9bJLSRMca zA4n5XW|lmdX3Ik~gX^U;?uL4!0d+iaJ_Gxh95)LSxt-5MKN1yahAHD7<3K)(7ea*M zE?<3;ub>%|u5fW>6mn}7#Rp!bbdH}6!jHokLX$x|s)?*MDu|n@i#3rI`iraxS+kwI zbQU5@9z<71h)BzvXXMWCh;Xit2wo z)L9-&S@I1C594Soo)hHpG*g~Hg($m7p6n&T#cl~MHY7L`5?sU=1Nj6m3C?#*5WB;n zCD_d)LHtaPmjsV#5*!Zc!-VO6Av?n^x~ zGZ1yBlNHYbxd5WIL$ot#iCjoaTpdBKP%0_zhyqs-nOxVin8TpzoqD%8uGG!*zF~j}pDENTFqvecfOrWHU zXf3Bslsg+!H!*>W@17uKV2+iQlAyJgsP|FMpTHz8IWgI^gYGQwI*PrG*g}JyRD$qK*$ES=?62%&Zwu z8e1vVyXj2T)w@1e-Q^y*9;2A#cri%+QYRJ+#Ous{ogDojnUiBSQe1NMMj9$o14V&B z`gHIuBfWL~BeYNVm2JXTL=)Pe`LsLYANTmkv+xnEVmzwVuA`zkk&3Gmr=>PgW1R}% z1v(X}$vPFOT>nY(4$VQKD|D*QlLImHTrg%QrIJ5q?LuUnhHLe6;IIqnPFlrR zu|8}RyWZu%tR^n>lf0TFc@0eF9;z+hOLgV@5IY~B9`bt1kvGs}c_U4eH&KcF2+fmm zEnnUOM}8bmyp=Y|Pr-?|(c|)VdQyItcFQ|xkGzv!mY<`2@(Y;7?#3+kMLH>$(#O+w5Wa2zyjM%C^bxvS;MutXzJdRmdN(6Y_`b8~G#l1D+S;e{o4Z!AQOviv0uqjm zX!A2JAs{`^U%)5E0X@YoxIz;S0l6nK{87GJn>i~8U}COqqS_d&)A@@CQY8pYBx&f%{hrP5^H zh3`Wuj;8Srd_Ve)r+KI=0$~DGu;csyX4nbzDmIcUKuyH`%|)P!)L4vL|3tZ(TEIL@ z^{;>hPk_X5dj2Iky&C5>hg;eE0zPlk_CO#se!LG)#&IyD7!&}EBK`fA54hy;-@xh6iNlK5-zUWC9B_k!^d{MM(?AT4UV2}kwG zE()BzByhGKdJgN}4qjF=CPKd+@`^PUkCgMHf<=41jZ_yoATQH6)h1)*{JpKz(#_=u zI+s2L^e}VOUFM^!%bakBBBOgf-mi7n__tnTsD|M`I^x;xN+_nU^F4FnF<9=9sWwHM zQYhY3hnkw|QVUak8f0ofR#QW|!_kml(bSyYGPR&1rk3=B zDUBtWnzAOQbe3ak%SM^>cK<8B_ls#erJeFk$JRceP!x)lN_j&|9fmT}PHQ_*tK!g7 zT@1-8djpHG7+G`)Sy6PF;v+l~pK+5#qb9rAWR3dQ>Jp8Luc&ywQjFS3zWY1(oZtDK zbI+aJzbC%illj(9FI@lt1NUhlA>@c&Dd(VnmGqxK>+#Cdy}QiqT~BF3Esc#}wR z&gj?2ekQ=HIxso32F&_HEJONH`uHj4c_Aal5g&8E6$l z4i7FB4?7ytAEj+5n6#@iAC=DrOTBF>E?cIq<=6RjgOhB z9pak9hpU9oS&Dav4rc{+i7}@yWi7(I8A*}GHP?$*oaMMtTyf?*H?zvnaAbFMaM#F< z^LL9vmm9Yx>s?7HWtSB)aW_Jt7^gg8_fE)r5&V2vxuU|BD&F;GCExX4aTINaRG0^# zFRh${b+=Vcp|vBuUQVx9GwOA!vz*eN5FahriY3V<)r&D3EJuz*(Bt-|z5q9Qvv}!y z0sheEz#Q>-&Dj+A+G(lto`NQECeViGlV1j0XykcqQ&{N0l0P4%N6jiU4a1kf&&duf zW9NO(!-cF@r{Q(Rmm#phjy|z`p+8YE4J&@_tvbi0v%H=u_>)HsdtBioe*sNC1u{+J z@43|`SDuE(-)RKcKhw|L&_XkmdR^R%R{gema5j91YtKy%i9#b6>qU#>XsRT7!NqH zM;xqcb-(9P&8Zy;*Q=8jbXxq=aO}jlTf`@Ix1{BHEgVCax#WrFPi!I)JWe@lWe7C*+DI`n-i=x5@ut{3mtD@6he?{}}&I ztCWO=I+Y^2LusPb%6jTjhG>oQM_Q{Kqjk#DjL*_~0o)pgYw^D6Brgc#^iM7bv3so3^W8QJLCcI?`TUqj5c*CUgg* zoA&8>G^v-+gL)NX4Pz~%!Tu0Eq&L%keF+_a3;-;FT=UfdJ2=eupREEYgakj+dJAlT zjUa(VZ@_f62-5Tr_JYcPmQ3BiJK0jft_Lv8sDn#84JV-jEKs670#8CQKWR1EKDf%3 z6}q)GxQ%&fFsjwUk8EYY!~l)U)ILIBP!kHo-hl`JYeGt#w7z7USAR++LDkw{~HCCg&)c!rB zW>osJ^Oe#f_choFHqqPY5|^8-;!M+(;=6H}lb>Zd2}dA>@l~Fn${2bXZ5t}iVO*SV zc4MJPVf delta 2265 zcmZWrdr*|u6+h=Y-{Z0i!m=-R5#;THh&*;d5mAi9M39h(MAV3zWhHB17hy$dOcoz8 zAv95Y9baiph$c?$2#)<$<%3U$M&xzqtnSat+nm>7U?A6oB6)q z@1Do+oO|xho&D$2zer?U{m%!N0Kkvq1}F$~Wl))Wq`cku9UJ+1cYm&}e);<+&F7T< z>lw15Cf4yy{@7`Og$VAUaC{&ZiN-s_p?GYtqBk7(1xxGm1QyGf!z)tIhVxU=C*QTX ztIJYx0q6+U{_c3BKRgr<_7CVQ`lib<`7~KpzwBT4m&n9an!p-)%l45qkP3}h%l-K_ zN>sfhaIyR%bCaVUAuY6Xa4;N=cd(pUv_Ur83$#Xr{2`g^EWj1=ki96e@;iN2grhyH z!hP}JOv)+*EmBowX5Y_(GyAC+j+mwgzGvEIn${rLdm@9p-$*QaUo6Voh{jDvD?8f4 zyW@ESgW=&wZ0FFu5(>6SzoWjP9l_QY9Ez{IH?oT1q~0S4mMEvqG}M7G$6R-Y`(mL; ze8iOQl%F_!xIx}@6yrwea+cv{+2r)$7P;M-hg;>ivlJhfzjDr7*u&XBsE^waGQU^2 zzk9c-`otu(3wGW@4+~O&7rhEZb$OaH^$RibQMJV!o#a28x zXdfzy5S5b@Hp~>{UOsE-|}NzuV1p^isAbE6;W< zlPxtlxLihRa-_vy=t!R((kwr!@#?Eh4Xcd#y$&%urbSx)Ik;Aq`h8k*Ch{A9mW=vc zo`=nzGG}Kr$)EYFaGm_z?@n2NuRoUz23(riX^Gqu@XDfq2RF&afVb{3Ga~B(oR}@t zoh*>0_s(_&<)MHVL-Kr}3d8am^Imy7u+lx4rF*O_7Fwbou%OdYJ_RpN+`c2*Ygeb0 zcr5Hgm-#7?XKP!$YLju@xB(L)U%V>Lzyzo;2Y@=^50fc%-I&mnQ=}`ek)d26p%?X3C`&nn#~k?|$Qqx7J9n%1Zjynd7Y?{M5CwWw3ns$QnG z>Q!n}KV$ro*VowoXL?AzP3>AHJ*+wD5iPNhNhPh*YN$hNVQi!IS|4?42k24l722SE zKwaAJX`^N9pwzcEHp;|#?T#w8jszN8(-bs94MPP>e+X~g(e88z-I z385;Z!eF%0Gs3BC5c#xMRMS4u#Mn&xMF$-aA$nFs=%5&&=fp7M2t6-GX-piX7sMgP zpD-R}JjV8uG%jAH7scyz7}5c-2ogEw1p{_)n3q4ZJje&WlEnmc!A4NPB7TZ*)n{v6KYAluxEA3RAFj}p|5;*hDV^k_#gZYnFi+Do{TnM)bc9^98KlBJBpD>%&k zdqfAcVn4TK5g}NHyO>$wO&*Smr!hU3w`S&oyLm2Q0Hp(blgF1XLiZh5Uu^#uSko8y zyz{i;0{8s?!b4z_qYc?IvCNQn8opjSfrbryzy#3J+ChtosmcU1b+>PrXXD z+BwKw;MB}J7H##aN9BuH7B-fwJ^>bb88YZJWYcR 0.0) { + setpoint += maxAcceleration*KinematicsSimpler.getTrajectoryPointInterval(); + }else { + setpoint -= maxAcceleration*KinematicsSimpler.getTrajectoryPointInterval(); + } + } m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(setpoint * directionConstant), customMaxVelocity); + + + previousPoint = m_kinematicsSimpler.new Point(setpoint*directionConstant); + } - if (maxAcceleration < 0.01) { - maxAcceleration = 0.02; - } + m_kinematicsSimpler.createTrajectory(myPath, maxVelocity, maxAcceleration); printTrajectory(myPath); - System.out.println("Number: " + i); + System.out.println("Number: " + (i + 1)); checkTrajectoryPath(myPath, kinematicsTester1); } } @@ -1244,8 +1282,7 @@ private static void checkTrajectoryLogic(Path Key, KinematicsTester kinematicsTe && nextTrajectoryPoint.m_currentVelocity > 0.0) || (trajectoryPoint.m_currentVelocity < 0.0 && previousTrajectoryPoint.m_currentVelocity < 0.0 && nextTrajectoryPoint.m_currentVelocity < 0.0)) { - System.out.println(""); - System.out.println("setpoint.getm_X(): " + setpoint.getm_X()); + errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point in which the direction is changing however the velocity is not 0!"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); @@ -1302,7 +1339,7 @@ public InvalidTrajectoryLogic(String errMessage) { private static void printTrajectory(Path Key) { System.out.println("Trajectory Point: [vel, acc, pos, time]"); for (int i = 0; i < Key.getTrajectoryVector().size(); i++) { - if(Key.getTrajectoryVector().get(i).m_timestamp < 200) { + if(Key.getTrajectoryVector().get(i).m_timestamp < 150 || Key.getTrajectoryVector().get(i).m_timestamp > 275) { continue; } System.out.println("Trajectory Point: [" + Key.getTrajectoryVector().get(i).m_currentVelocity + ", " diff --git a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java index 839fb45..aef50d7 100644 --- a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java +++ b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java @@ -12,7 +12,7 @@ public class KinematicsSimpler { // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS - private static double m_trajectoryPointInterval = 0.25; + private static double m_trajectoryPointInterval = 0.1; public static double getTrajectoryPointInterval() { return m_trajectoryPointInterval; @@ -770,12 +770,7 @@ private void setTrajectoryVector(Path Key) { for (int a = i1; a < Key.setpointVector.size(); a++) { // If the end time of the setpoint being looked at is greater than the nextTime // set it to lastSetpoint and break out of the for loop - if (setpoint.m_x == 19.43488968199528) { - System.out.println(""); - System.out.println("setpointVector.get(a): " + setpointVector.get(a).m_x); - System.out.println("nextSetpointsEndTimes: " + nextSetpointsEndTimes); - System.out.println(""); - } + if (setpointVector.get(a).endDeltaTime + previousTime + nextSetpointsEndTimes > nextTime) { lastSetpoint = setpointVector.get(a); break; @@ -830,8 +825,8 @@ private void setTrajectoryVector(Path Key) { // velocity trajectoryPoint.m_acceleration = (nextVelocity * directionConstant) - trajectoryPoint.m_currentVelocity; try { - if ((trajectoryPoint.m_timestamp == 230.5 || trajectoryPoint.m_timestamp == 230.25) - && Key.setpointVector.get(0).m_x == -20.32764301488901) { + if ((trajectoryPoint.m_timestamp == 238.25) + && Key.setpointVector.get(0).m_x == -43.77381014821977) { System.out.println(""); System.out.println( "(lastSetpoint.startCruisingDeltaTime + previousTime + nextSetpointsEndTimes <= nextTime\n"