From 81eb2de9a8b7a91154d531034c1622239f8c2295 Mon Sep 17 00:00:00 2001 From: SPD3 Date: Mon, 1 Jan 2018 18:38:37 -0500 Subject: [PATCH] adding coe to repo to make sure that everything is public before build season --- eclipse-workspace/MotionProfiling/.DS_Store | Bin 0 -> 8196 bytes ...sTester$InvalidAccelerationException.class | Bin 648 -> 648 bytes ...inematicsTester$InvalidFinalPosition.class | Bin 624 -> 624 bytes ...inematicsTester$InvalidJerkException.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$NaNException.class | Bin 600 -> 600 bytes .../bin/KinematicsTester.class | Bin 22466 -> 22724 bytes .../KinematicsSimpler$BeforeWhatTime.class | Bin 1725 -> 1725 bytes .../kinematics/KinematicsSimpler$Point.class | Bin 2018 -> 2018 bytes .../bin/kinematics/KinematicsSimpler.class | Bin 25859 -> 28344 bytes .../MotionProfiling/build.properties.yaml | 4 +- .../MotionProfiling/src/KinematicsTester.java | 216 +++++----- .../src/kinematics/KinematicsSimpler.java | 377 ++++++++++++++---- eclipse-workspace/Vision.yaml | 135 +++++++ 16 files changed, 546 insertions(+), 186 deletions(-) create mode 100644 eclipse-workspace/MotionProfiling/.DS_Store create mode 100644 eclipse-workspace/Vision.yaml diff --git a/eclipse-workspace/MotionProfiling/.DS_Store b/eclipse-workspace/MotionProfiling/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..30c92a3324a90c49f2939d5b437b9756dee33e2e GIT binary patch literal 8196 zcmeHLO>Epm6n-y%wcVs~LYu^GH(4bF(vmj+4Mk16$)>1L0}b8$HZ6%aHi^Np!`hq8 z4=Itj04f}iTGXBpT&Q|L6v3$%E~P!dp+qDgap1rKE*udEB;L$;tLEPRs54Y|(y&EicGfqwoM0x85GfwONM^j5r0pg*`GylA10e$; z10e$;10e(d4+iL)&6>2$cVAe;F=QZQ;E`m2y&s~~8O8!!7RX;6H1HCDkd*+u(J|El zJSG}oEWl-fJd~~}ss{{0F(@%mgp)nW+zDd=E(;XlfFc|)SQ&!~1^Ma}7v=7NF@fP2 zG7vJbnF03rv;tg3| zTc4us&Bnzqik$bdDXwKxl$D*@sUGF@ur=X232VU|Q;R*yu!m0jQsyjWGc@7a>1oFs zQ%fdvdm?S?zS(Kp%-I|E%{iYPo%7t3ALou^s+#u9vnKgjvt~DWQ)nr>(wQ0Gavg8b zOnHPgu|~0R`9|TMNc9s>HMTu-Jib!2t5_{5Rg`nYjpJJ{S!QY=V;TO`i0)08j$t{I zL$j7=C2h0c(WlK4te54Lvhs=|ZLhXJYuXqhsWsAYI%&O{w)}a%MrD8l@@F)BQe`5g_<9@8fDvaU*Y{Mhijxp@SICf(Xp2Y+X;2@5ofhJDk z6grr}Ib6U+d;>4xJNPcXk00PC_$l7R&+r!B#;@=W-o<;kifdw*Ia#gtiu3GH?Z;yGZ{vo$_n?NjXoM=bTC!^3 zw{M*cnUuA?`;^E4iOQVYDCDfb)g4Uu(uzjvh(sgR5@|&ZPHQ7iN~C5rhhOW!!HA;f zl5qJ`q8)BfB5FQqRN~f_Rz*?sNW{YW^ii@FkX*&3lzqwxSyq8Bqtr&?vZUfh@og); z>u?jkgs26sQ>@~ diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidFinalPosition.class index ef5797ab7121eaa4af0e0781981660d92ac5a433..f5f72cccaa8c375ceb85f29a56e6ab6afed5a567 100644 GIT binary patch delta 21 ccmeys@_}W8Arog611ozq0~dSEWGf~!07jbyXaE2J delta 21 ccmeys@_}W8Arof<11ozY0~dSKWGf~!07pFqdH?_b diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidJerkException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidJerkException.class index 89c1f804f42d1c7a659e2cc26a578206c35ada00..e3194bd00f01f14c447d36d0fe71545d0978f3da 100644 GIT binary patch delta 21 ccmeys@_}W8ArogJ11ozH0~dSpWGf~!07Li%9RL6T delta 21 ccmeys@_}W8Arogh11oz50~dSdWGf~!07RMvF8}}l diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidNextVelocityFromLastAcceleration.class index 6b5904db468430e6c7e650f7d381d3f643e06f12..edf0db19fcb0704b5402bbb8f75f823ab777476d 100644 GIT binary patch delta 21 ccmZ306z=`HUIzs delta 21 ccmZ306(q;NB{r; diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidTrajectoryLogic.class index 088b3b85833f22f9ff32798cf672b170cd14e5df..ec9c809e09060bd4ef62bb2bc53b51bcecfa1d37 100644 GIT binary patch delta 21 ccmeyy@{MJKITL3y11ozA0~dSiWCtcQ07&o!n*aa+ delta 21 ccmeyy@{MJKITL3m11ozM0~dSuWCtcQ07;SstpET3 diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$InvalidVelocityException.class index a4402258f7a2eb745e674cced545ac6e3ad378a2..a91818f966757a4d87f10e70d2538238ffe7de5b 100644 GIT binary patch delta 21 ccmeyv@`q)EEfZ%111ozZ0~dSLWH%-=07c0KG5`Po delta 21 ccmeyv@`q)EEfZ%P11ozx0~dS3WH%-=07h#CL;wH) diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester$NaNException.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester$NaNException.class index 6afa9800a9d90f13b304860c149ef3f149ef2b36..6230099e8904ccd295387af4e766516815cfc6ff 100644 GIT binary patch delta 21 ccmcb?a)V`qBok*311ozk0~dSWWF;mu07Cf%J^%m! delta 21 dcmcb?a)V`qBopT(23GdT3|#C}CMz+S0RTgr1yBG0 diff --git a/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class b/eclipse-workspace/MotionProfiling/bin/KinematicsTester.class index 159c71c73a23bad47f305f597bf76dac211050ee..2c364d8f35c789890ab6f45dd415bdc5ff9108cf 100644 GIT binary patch literal 22724 zcmc&+34D~r*?(qtGqbySNkT|q0|6HWNx~(HK!W9r95Ebn6AxUHO|p{g(qzNo(RyI5 z_7;^xi`ABb^(r1AiVCgOdTX(sRYU}>iuG)@pyvC}yvM%TBgtC)ef)H1cHTLj|8vbV z&pVqZzj@$cA{xz(3UV;YIU^JfHU^@h>gEN(=4h}>v&qI08jZ9~keaeo6O7X6WH~`~Dhd`v5+}8bJWW=P*638* zdWmx{h;*)EX|6X`qj7S*`gHJ}9IjDT{cXisrqOuYqTK`ezb^VcjV8!N=cGoO$l3Ts z~X&}2Byn2R`lO?7Z(G!zMIG*vE|y%H`XIzJkyUIEWg4WD;9qcNTNyg@U= zs{#$7nsT_)u$;-7QqXip#qrL{?7t}35UCDD*V?+zWHc^S_u0WUQFDm&+LeLkXv`l) z!h+6VG(6R4dr#YfvltCdHDsE;CrB?TXP?7pNUE+eA6gly3snm`lTrVK&a(rvZA;JZ zWNGOktgi6wfu=h6{a#62JGLuq8TmG!Z;ivI&_l^_R$!&EwRo+E&4SKiG;oEreJ1Xd zgXRXJ^>XJqN27CT88*ebV03CjpjnbqRBT&PHIDaE6$Rwpw;U4}b-oGfJ5shF5Py8< zM958r`MyqKJLKxkkW7-1i=nfI(x0TZ zjh$NuG_E^yCB_aiyy1;&^*K)H0x&oSy~>G+Ob}dVG)3i|^5U z6W`<0YkZPbDbZVGZ1GEtenr=qPRY)<6T5>|A~T$5GS8jZ)(t1h_ZajsIy<$0PMmsF zbF{-wWCj4K!YjU~Naq8iv-EWm=QMU$d$5)EGLx)jI)Rh8-K4vY#jRv065GgVi;Ai^rEX9g{3N8vYkG?uw51=ZuWyl8H63EnV30 z%Sn&EL!-Op4%S2MU}iigpWLg_eey|9^~4^p$)`ItYLibr>Zy#6Wa|SO?UJpzsx@poiP;|v*Z89d+5M4Pe-z)F4Uqm&vmgDvG?(_tNb60+y+w&p zgDw2CWf?SjOQQqyHr4=$7{}Gtw>T87pBY9zXJw=TIf4j+vIX7Q#>Lrt*BO`jrsll)g2uR{h`!M1OZqS3wZO`i5FoNq5NbkaW3BOSI*L%-Ums8s^`nS{EqSjDGzA)ikVdmy z`fEAyH#k5gTRP~ zkhV(Dy+Ke<>mY+@)EjJSnuUZ=pbj>Xn?Njj6oOaEoT6sV4+o>B=$u+T=w%@sqj2`r z48bdHWi%o^wSx(>9F6sq0673QE?dGU-oHuy+st#w7OmyYVv`W`O0Afy;Mp2Ac&BDH1FUUVb6&yHO}xII)ZT? zoweYsbU)enU;{LE9P`8Z;5-$ls)jEsu7Yo6$7}2aHV6U%D@;oQ!}T0%x9mPW_7}rL zzQj({*br94NI;cZf^%vmg&izJca8A3FR>Dh4VA*mtWoZAksRS=!`Vo|M&P^{v*1}^ zsm4xXqa^C(h=!yq#d1~b6pf9RQ)B_;tiT!*r<;vICPNusz>j2ZbqgF`u*Sd2NO?ex zF#I4=FTt8-CDg{)U_2J5sDW-agV9)h>Tr?)opOkg&(LfWnB@-` z4Na-t37w^DFlmk34Vrb=V7L?8W{xp~OgT=q7%VeVdJr24n_)tx;ZCH_!LA-NZNmuq zq*z+UGuFf=&Dc>>beDw2diC!3_F(Nw{S5-6DY~NAkersDflUDN6O&95vlGL%CcwHF zTdc7q(*AqHH0;rw;RB@gRB7xiX+6?Kz-|3 zhO*e7*%W4EN`*-}0Bk6-lhL^n^p1(rKAYrFjfUJEI3%5ijA#wcxLF;elM?gGxd9fq^KkM*f%>^b3j&%7jjv-VMY-0u$55m&I7^m zG6TkU8}HBD432M>3{(R~7!?Zpbq^yW*_u!j0(HcfvWMI|tMNt)Z}{r`^^w&)!m-A%Q@x3C&A6FWi;1L>gswpQAI(F8!gpr#O$_S`?pth)3#=JG`Dvez&eL#k4b+ezMXBese_!)i< zGmi71BVw_*T;rGUHFBi#u5r(hL+oL&RBE2=el1e;i4$5;e3_q5>^tVb2Zd9IyV*uY zi;}Z92H795yC#$8da%~9s?OWUb*J>Rab4kt`k8yD6%Q-1s%3H3Fxi8>xbsa+jlz7I z)QtJY5f@+a8pHNgY8L!Kux4g>vYCo1H@yhX235FKusYPx06NhvMW+Cx65HXUb2fn_ zkEN`L@#0k;QK?N|@v=kRSeO#5jWh-47|~UXYeqmZP<5JIi$h(?J~_&91!soMXhj}- zq<=({3{<>yh_yTh1kt7M4H*n`p`J%li{ zJA*n`hB$_e#|rY?ievjM-0qNz?U6ytrEd0TMg=yG$2+6Hz}dxk-SH{T{wYSnKP4X| zLv=$0_PN{)$)pi6qWwzDW&X3EgTvr%IakHmne!;RDYmA#cgS}!H z9N%+L=yUj_k+UYhIrgsO-RuQMCnM=4-Lh#Q`i5OpEZd4sQiVMsC4IM>z1YRLQ7OhD zRV60Z)C6nfB(EajA_WlVum52rNlb#2i_;G;Z)j|vJfmc&Gm1RU>_-cGOFxw@ax)~K zGCUr?AG06AN!%~bWooNArK4!3>RBGcc5I^Q_@I|zhjz)xy+~ePD3>WZ-6%T_>KA*g z%(3HBmqZNbJU`OXR2`g#i#0gMongI-)nBQNkRBJtIWrs%HmNtPL7Y4bjf1+K8k_IU zGah$1$3wl0X1Nd8dJi$0XLs>x9g1u8j477k(|yJR)3|N3YxXn^?8)evcufG3(+^u2 zhAgsU%cvF_h+M4`sd@*wF4C6nq^x_WXCC#s zhw^uz(LsIi=~Z2_P-c^SELqH>Ir zWf#;g=if>x#XvOIv%ZqK=X-c zDZ*M02F{;2W;y)d-s80;h0|3>}xEF&E^Q zgX9@FXjLMFo{Lk6WW+DfE*UX>7ga62n|dy7rL!0Br1Q?$MGS(eu0&hOE~>Gf|K*C-NK@N?ji|Geg z-iJF?KLldfn!HeU?1{c44WeHG(XWD}e}L%MLG(Tl{bmZHFEfb7&AMYx^yO&~{SJtJ z7eu}XqTdJ6he7neQV?x&8aM2YJ<(UBLGK2*-aBswXKvMuT{eY&BD%b#=r2M$~BpoNgtvaArb1m00!*VUS z2YKuf*{rl9qMxLJs0fIPfoLcY4FjU#Kr|AFMsWETM0 zg+O+37sxh!7sza8)DhX{G?85nWb1(JM?iKZko_3Qt^%^Ff$XPUAiJ?kWOo~Ow!Hgx zmO^G(Y2vyDxYh&Lb-;B!&0rg73EN0#vrQCWH`s8^v#hMbva(-)7r0VLEL~hT1J^CU z^*i9&3S7Sju5G}z9k~9`1+Fb!;`$bTF@?PH=%zF!_D3Mw0c7_8*}c%j`=E)f&_o=H z*!?zS$^7EY-vzQ163e4o(nPi!$o2r)pMmTVAo~lDJql!x0omhSAp2dH$kMj4>=^kS zeTQu$=y1?XwUa#yT+ad5-)JP;3tfBxy7+hK;)~G5mumx|)V@T{1 zNbFNc>@!GAo(s)bKAFUB>k`+ol-TWQB0CCXUjf-SkXSoaAjMMUV05m-LDdeI4OudY z-Pt9wKc;)`N?~JZdV7bLavYiDbM&B*jx3ty$fiY(93bmS%N)IIsO)DiGgiE-OH^s= zVIHNhuyk=109Rk&Dx^`4{xriefR;Fpqq7};3OMX%%4B+o-+36(eGbm1i5a#Pcq%h& z$I0mXkMiRl`Ozvr?w21A%8%W#%$Yjkawlc*4Dxb8+1x{UJd+A|4;sL;EH-0GWCUgQ zOxDbt4z9@>1TIreUQr9}p z^0{norN7IjZl9Bef>I1N8+=E&zb=(kpFEcIk=<^dJgS~Vx#!@Mv< z7xJ7ZvP`C-JOjUW%0wGS%C(a9U|FckjO<_%?GONDStTG8a^nBqM`*}bl;Qk}h6*}# zgh~Vm!X0Uw@G6z?Dv?M>&V#N`M|es!E{F!QkRmn2>Vl}1z*%-(9?RYEYZ~6Io_QN@ zBwsJq8^WrzG=ce`2{GYJLCyu&T_K$15Kc9OvmC;yrE$Ct0;#1rC>Qa1TFyhX2IX3m zm+`U?BT6t6RO0x0OEA|Pf>{r76tKSF&pHU9pAv_Y)+Y!-GTcY5WDH-H z2Oi^AyN$=wB#)g>V9g#dl)vnv$CvJ8g=H?Yk^xnBj&VzqQMQrqwwVlH=Vtltn8{?7 zqT>L?QFPm1<}Gj)cw1Tj#Xh%EX}8bq;~TfM?^iPpE^ryM9V+vb6nNU$z*28PhR@r^ z2FpQe@&X7E*7&jzfu6=-63zvvG1zFE#hqv0Oth6v8n)Z#@nzOweyKK{PknmYQ)e1c`j6NR$)!1ys&2f>vHk zi&0<7FQjEC>-Z0;kzWGsyp%5Gm(fr0eG|WeZsI?pJNcFL5Wk9^6D?l45%+iXr*~p~+2)~#aOHg-YME?4q#*OK`uYp3^%r7&X`zcLvmLKa+@?zu z^f>-gj%%R{5ZvC;1$xq9lwNuYhW%j z?fU=6z!!CH;8Yc7;HE_{H7vR_>p0u8j*6H?p9>dlvyS##V%8A``-yLbb^IRIu?^O- z9oBIxtYbT@<2G2w9k7l&VI8-@I&Oz`+y?8|0qeLM)^QK4<33o&4p_%dSVtSI<9^!B zAD~zGF4~9k0DlnH@eqB+aY2RuNon{-`RWrdPmHhyH9c(k1lqieod>ZUh-of;X=*Nh zyb#bemk$3=n!8-rT-;_eG?z|ivdt6ISGB6rSGB6CuXD(A%=Gmr&6j$LpGn#DRK1RM zJYZ}@-b6h$TX!re~We`|mv zOMjg{;QQzZf0MrAuQCrmpp^C&8Q4JWOr`Z2N_zxK8$wdqfgc>1@u3Glu`%2&6lsN0 z+I~=^2Gls-=IiP!@J?TMydJ7B9Ipp%QFpwV)ND9j4^n`-MPs4N`3u^PxbY6^_A;O_0`v3i1nr8T_P2E5P3Q}Uc2v2^2l_*NggQ=Y-luo!)Mdz z+0yfhvk|=F)`N4C5xzQqq?Fb;d6WsCt3c@I4}0<_D{ChUmwBwRs8WxpiONmH2 z4&P1r)zYKjYtJ^es5Dn^Zeu?z&66^5S|<(M7y)lzi*Ni18XrZx*^6C<0MiOQ*U7*$ z+X_6b_8-Z!BP9Cc#79W^e&)+fY{#3h&m^tyB|iFveAF}PqhHC{@)C!vH)fM^)`McH zYQ25g5UL_fm& z-#qpkL}hh2!v2PR;MnVU5#QCZo1Kex;{U|S?mal)_o)y6fClmpaqRvWj`tHPLphOu zhG^k)TEPE}h~Y~_3jd`Bj>wFELl^UQx&%kq%Y~DEC0ul^$e;~^(`MnJ-w7{m6Fq2$ z$f8{$n_dt(bWrr9_fh^!Fi5SeT5ht?WiXrS)ly`_C_Mj+c4~t>!F)^GyDMqjtP`)BYvi;&D z_JKIrLE;ogz8LKoAWn6R5n~)@im{H680T0gN*yDa2G~Z&B{_iD}LuV!CsjnBkm&a*CMgJX2IS=Zi|`IbxPGEM_~SDA$NN&MU=S z=T-bY=SHzmZZg;&21+MsZZfFpkQ}2WvKs)ZgY39!7 zM3$!pgxAO`bBBlt_M3K`Q#wA!W2*dJ^ItZ}}K z;<{2{+9ytFK2HQGzD&s(+dX3ODEtG?L@0ZRzk*ATQ*V_0#1nL!coGWr6ipOQ(_--q zohP27TJbzZ#NTL@*sHiGlQjTiL0)S>Mw=3#17I82qv++tIYPc7miM}AVYPonO;7}4 zl~(JaRj^w9d?HU7NOONp=fQb-<==(UdFMj92ltT4QKdjWj;JpS8|60YGepLQ{bR@2 z8S9*^zid-Sz=K&8+vs!Y#K6A&Wf`gl#z@~*hGYcV&0tg@$fbh?Tv{Xi8k$$Ysi`i; zS2f6MZ&6WfW!SiX9Lr4;067^9yeVs9f3&^Tz}OE8R=DO2D^ zw|JEXq8uq+hj)F0CWw9b6N_)sbd;51Kg|_y(L$6};s7lZZ)4X#KtYrZC?nz!HH&xX zB9xbkcj*f8Px?8^_2PZnC_bQHquhdWE6VNSFx??Oq`OhxgYp5C4~dWH&*Ecx0_D@< zQ#vd@V};^V#Ivunq2dcRMjT;P;wa+Quh^)eh8#&%B?0vL(X+8ZEQB@Ygi|w>pxd9D1-sQ>-WZ`1} zd=+oy;EyN_Vu#VsiF+1n)w(VfgB6qeYszuTm?;|@A63C4;DYzZ?Q}1>24Zu$P_zBg}PGMOwCK7Km)&0Wv`+_T;D z?&PT-ckU*l5#n=2hM?@}k!ZLoR2wO)nH#RD4Oc5lL+x3iRiR;(p=iahnM=dD zQnh%6({)ayYGq}(T2QZ&MCGu=RCfDlK|QJ#mxn8BLvtfl;Yro8s+rZ1ibyn6IV%>4 zVpdTs$a_**Wh5G@9V5sypkSV$w286uu%b>F-P-JaGAYZXZ1M||C+Qh{Bq+zEbdqxH z`gQM4zB~79uIg%%BCnvV5+Eq8t6Cbao*Pg+JRj-|_>#OH34@KuL4;M#jYD3Yo@Wj}vaCNwR{_;p=czjt|7(B+-L@OqSYQjA2 zF(w_$(@wUgEe%(oImNW?owKh={dmrjRHT-KYicx|t0QIM4iG!QqypE($;2M&#Dh#4 z%oC4ji{kN>mD9r2E8?7}iL-I2NyA(dq^2y@1j9}G2~SX#ih{YZ zddYLojkPXf9bE53lSc7+%R7SaC(&}P)rY#RMJA1QE!sYi|LvkDn>2hmB4}i5Hm}dr z=&DdRNhE zD@(J5aJs^_hpH=J_q()U+TpI?GV)wD-x&v|&`Iy(jL=HkwfLRJ8bxOa>b=5opUIxG z&#X}Ga`rqwHR()R45wHTuANvJs$o(F6u6eum81Q%kV5SHmSW-otvg}MBV{Q9@yE1I z1UqFpZZP5l{PR;eMR<*~Fs_X?ig^)}Dov^)c*e|7xn&DrmjDcYu=Vj8{^(X=^wG8j z?p2P{)Dg+C2dpPK_66&KNmymlYFdL3#!~2nv_L!BLqu`0#v*8)NoUhJV1L>2u+~r8 zb6bE$wP&v6*nz_vS+&-h{eoeoL+afo+`n3ll@5caZTE~dCWVw^;+F?ZHUT5k>sqJ&})Wd6P zn|Pvl0FWxY5_E<&eezVcV|B+^9iP1w;**`rAy3~D@yT@^pZ=WGtpD$+Z$k=dL|=M>DcSgkup$j4;{@y1(>(zYVG zHYrl;)|!?g)Wg4g<$oiqZJ}@8F34UknX!i=3eB0_d^xt(TTHr>ol+<5l;R;8x7=mY zAGxKoZgGcT-1=ve?&emXZsm}NYwtB_E7#`e+9oj=H*YuTK5ov{%?a0_<1X${Z_*C# z;ME-va^VC&J-n8Klbt5*q6a_@;+Q$%+Latx%+p6-%oh)6>S`mE!>n3FG`nf9qCN5C zgR9M?hv;EUjj0fR4VdZlW0g4e#}H@DZI6pCE@(Z@XtvB~AMK~N6ul{Ea3Z{DA!Kbf&ZM_F$I`iK zaamn;HBKjS>E!tinDj2U`l}YN)VYvYl&k-4()(PUQMH(Pi6ltXN=5$=bX4NdWU*&L zU8J%+TYuiRzX8MCU#I^Ax)tepaj6G z#iz@$f4QFjZIXNJ2d_X@FvR=6j8BY^0_(^a*TPyS!4_PJ@Hto>3)cjrvDzR~oMp?k zqP9HHQC}X`+fOC*GBzS&owl1;M(A@gOYLKMla64V&tfe&%jPEwA0pQjd7>+f4~|I9 zMAf!s1qJJPt}sZlGKh()0*;uz6KiDRKjTFsgcGeuukBBgT^l~J$gkMmp^@{~LY?|)Iy zD+;i^IlrcH6eqE?XOwGyGml63#TYS8iLu~$!UWEMOqyc6n80GQ6fqHopag*y6NCu74mMauQg1f8^9wS^HTxF+$pa`&>VWgwNsuF`C{gdcrub9T3 z-x(X9nweH2=rnus^2oAfNLeFy9b6i&T^$ZbTU;*$ZyDeF1Qn~7N1&7z8NuU*4)5(1 z2q8|irjE8SpjEO31#Eqd)9P3`L4#82Y*J@7cyU^r$!U#c@N6UL3Q;hoPn@|E9iDOO zEUtF(;3va2k}NuiRT2ivwwgz!SQa65_)3m));7)5Ft}66=ir!1J!Vc z!|+DW<1*@K55tt_X@l(JT4jfS?JOtWF8Bs zv)!JU>s%01d*UV?TZa~x-MEGhWr~`jPkeKOjSt97@Pe1+KnBUfY;iVpru6`QG$$M7 z9{W|LSNvSi3_d`F?C>@c4O%vmPZ;HqY8;>t_j3=9X|&Gb?O>pn2}STaEZ2thXcqIT&*5IXgw(SSuoJ+JvFZ z>G03$LX}=|W3&CU=8iK9@`CO)t0M?;Ssdc?x@WZd;D}X&FraQF#|}4};&;qdzh$Nv zvsc_I==hdVuvRn=86s76RY7i0;x?F%ge4wdT^(9GHCi5CGqVoogbA^_XnBoGal6{4 zE8$;tQUN*olt!yrSLk&2xGQyne!Z=FSsbY1?15`;-HPi`n9pR%m}8&g@fELMTwhtT zpj_edsnPM4<6MRJ_O?pm0JIRdAy!8!D?ul^rRWqAG|+XzYMo6W$>#_w+;(%3N95>| zS3m6+_p#ZzAAs95JFq)oUXw`S6ym4{v2gEq0? zryKo5%MNO~o1kyn(8ZQNH+Z(eeBGd!^KaGxuIZreiDrH1YTD|b6RWE(3s1rg7@VD^ zJMT=bcV9!<#6UHtMx)_s{VFt!<4_O#`0XacWo&Zo<|e0eY@J=rhR@Zzzg=D0#yfDZ z#qOEdG~0*;?1s4FxMnxq$rs3~-81=pKDNv{Y^2+pkd;_Q*HCX{U_E#_Vvv__JmJa9 z*PKum;M0fWC=3L?oq!(vuk$W47wn+S?bJD!x@@Pst*A8UD17>LmrRsd#P0@q01S>Z z#V~xs0hiJT?x6g{FsUm|J;by+P&ew1jy~!^JyFB!{~aYyIIw|wHBj#c>eE2|8~E#x z20E^Rj&Gom4OG}bCv72b$)I`~yH%s;O?{{s-FUp}H<Pu-fkLCl@>G*~_bmfZ1rGZnp?}d&FP?-bri$U@fgy|&+z-A$aWk&oO^~{LLyJ+Ep zZPa-|JuRBQgO*I+MFJaBR)V^LyQti0%cW&IXn8%Y;D%g^<!yj38LQt(FalxeW6V>uD~6BqA%(I(I0~7k3i%nAo?JP{tQHak%H(rr*ReT@DqJ; z2Z;VRi2ffC`3;Ev4~YH_M1P-x=s2fw&F=6M9cw?)0!By}6cA}NT%_YRm7+<)+Xm6N z+IRSgzN7<0cLLFwAUYdFcLvcpAi7H%MB_SP8$|!fwjN6k$$B(3E0?wlO)r=?68S*W z4QP4*O;4I8dg160#1$nroCLFKK(1yvre%s_T5w_U@FQ|*X;Va(cL31OwZxk-id&Bg3W6HumZe zkfq$Kt5Qf`DrED4>~tVo2xMmf*&-nODUh8BWJ}sWcFmDMmU7QFbP(AxAgchf2#}ox zWGjHI63D86EY=3HYmWpnmtknCUmZnO2V|>&Yz>gD1+sNOb~ccm17zp5f$X{?fy|{& zO_6m}8ZQE}^+5IuAiD&}ehFlk0@-Cic10V=Hnxdun{7dsw%>xJ*t5(I%HmbPbv1Bp z0IuKC6mcz`F0P|RVk3pbCKs;Rj{TVC*pKUv1g;c2)=^xy0N1U+bsKQq4qSHt*YAOA z3vm6R4P2Yr#B~UEF~xr6(hVKlv3r2*ULe~BWZNN$_dycxha}cR5*u8|TC(^z9SLM9 zb}W}}?jW+=K(+_S9s;t5f$R|=dlbkX1G2~4Kz7TKK<0|Vn_3T-D`;}$Oy$d-1+wRW z>;)j(2V^e-*-Jq7GLXH}2C`d^1Tt5s-V~Y3eYXqQn?Uv!ki7$B2Y~EdAbStU{tjdx zw1I5%kwBJm&pIlN2Z8KUAp0E1z5uc>f$S?F`zMfn-3GGTjs&uld)844@-2{k2V_40 z*^fZhNDGm2In%&zwG2-i$Zl^F*&+C{6noY|n{W6i+wfDsFlo5aiKZBtbh?p6i;Qdv z896RoE%~xLjs&h0JJwNL0pQ99uI|9q1GstuS1;f?8n}A5feSy+uw(peoK%zZZ7a~q z`L?6^bn$!s_yd3Zi9hb)k8S*Me#Dy&UT8ggs1IOPY|oQ9kuQGI4%nrEckLfrys1MBH8d9t1!+)L&4^hjA@ z+IlJSBv;qdPafKi zT(lRQmDEoN>FY@aAY+q81*Z}O~LWNBs5B~4^E%pDN(mmhP zAVvGXrGW|r;U2VW^GfvQl_0H-30)*CL;D{_VRx|?W7 zyL$RJ-bjHi!o+5kIFcYTAqk1inScxo$;H^5CD@!0HfISor<_K~FgB!|W}=)Ym(fyL zL2FR1MR~qlPU~fau90WaZ)KI05V>!~v`TD%<|x+ZB8 zI3XUOw3ZmYI2Sy|&2<-#CozxHpT(Lvc;WEjF510dhsY^Pi$A18)IB4;tTI|R^1LpU zk?Xx8&zn%0%tCbRr8$ajuN3+7)AIfGqRae%SBtba;0?%)H#Z*O8O42UXpOvkP zI2zg(FrjMDvU<_msqe@2naTA7SUY)%wDlug8nXI1c2nH2eKQ)Nam~Ne?H{=@mqhqPcyY=e!9)nXX-^!p)bK>7^uRG zfUiNEJg{C&g!GqPcQegQ*d=E(uqqbqA|^QoYEnYxr_6BpI2D8jd=7ns_XK=_j0()p zn$`N$(>pwMhD|pnKCu;=XS6)=;5`yU$UvDXe+#s%yzZDx%-XO0Px66&<9(k^KK;9r8mN$xh@+NUW z{!V-)Z!!GxRwEl_uH0-KD{nJK%R7vdbrUniEHaGqoTpb| zu&laVyv5W0!-zRgPdJ{$XS^*=6|=D;ImX|_96Y5{o`GL@XipNby^9wwOZthqsPocH z@`}^oUHtGjv$gN>(poW6%tM`zE*E{oeBFAJ$PuUGDTD43>0$v!nzWt1N5aobnY5Gs zjkKQ^q3Ryou4WJB75ju&*ZFYN_>WkG)(oTstpvM8=a~+ji$NzF+uxAch<9S%xOY>u z8~+!OX9~x(XFZdCX z%g9e-BX9f$M&5+Xi@0t7e=zdYH(`K^e6a8hVo9MN7R;Y)!5UxHj%Hyq3l=w7<+jOc zP5ogl?U-s>!em9@rd{gaxY<#EieleL?t=P10QG+m>c1Q6zX$5S8|wcM)c+Bv|D#a< zhoJrsL;W9u`acf!e*)_NBwZk%rYrFMdigB%jap2e1Tq*`{-?y@5vYGEBO*s z9k)j0t6GIN^2>09Np|RpC+p?pS5TL;kf}B3y@YDuHz~Gi;0H66mTJ(a?NbdJ7FP}G zVyngl5a%igwV!Upg+49v_(j?&y3UsnbiJw;biHa^&}WkGFbR6^tnX+J%PD>i=aN(X zg5KB(Iqd`%`IF@|UL!LLm1m@nqY}m8M;_de6UeLeK9T2hMn zIx6wOZz(G(K}Qx?YqtGG{(ja^ab51zoZ@VD|$lylirp8 zq7P9XlwZ@g^568m{6hHTH(F>nb8svCj?nsTq5T#@TPy0YhX)fv3&DyD?KfhT7TWF* zq}6zmxZh!kucF?zW%t1cTbA7idumzs463ok*N0@VCBA;T4C1?nZ8F_qi?0tqeLO|i z`L*~45fc0K`j+_e`YgWI`irqXTkO+Wko%CYsi}1v?G~EicDscp=%XXi_^qZ(qD$HG zBKTLh<3gT1`v~JPw9wk8&mt8_%U9MJ)SW8J%-kW?T9I*pk1|E3H7ae1CG15Aw!KB2 zwisLOdi!uw1er`P>Xh%R7wMQ+q~q}7OB?CmLWt!3x+dT2RA=bwjC>O!nThxr0YWIx ze?58gGB#1CJpU#t;it*m;SJna+IXWe-g*}2Tz9i^LLwOB8Fb)yo+H$y386k8LaoO+ z0phS0hF*|r=)ioFT}LNonx0-{4a2PK%pTjJhS& zs3v)q@4wj!l^u(aZnoG=^Qi(M-DXi?lp3evyH0zEV{nIyVTzDpe#lsnKGaI!QdMP8J`iF$SrzMt3#NI9`o6O4UST zj+$gFSCfskYKrl5lozR~#?@+?u|bs>x2YM%eX7*hiE_7^X*{cD8PBWP#_MX1@rjyi ze2MaFb(+Vc=6TX(qbFCL!A2VQL*N`dd2u6+Co8f*GDqBwB$y#a#h;9;ou_BxPY5sV ze)tuwo$wI_@h9UV=V`a|^mY7+Jf0l9{nF_mPo|D6BpqdZF1Cm}VfmS9R5Dr{qTxdUSK;55FmqDA17539u}QEZSb;Au;vKlS8E>(?$ZfFMe~WUHvLcO;Ei>*1Jh<7!wz{0J z6z(UX#O6kX`%T-??DU}7=pwo_!s55o2|qx@G?cm>>!IEB`M#rxMUA2>Wj7(kT1s#) z5C8AcBq6gtuBZ_gN}J@<5KZyjYoyoUap(-NH4PmVeS^FZ3E*Dkl=huwfo#*=5e(%y4~Y zJ6$>i?qe|iuP985r*4qn49Hj^vO)=Jif}L^bqjgat(2xVgRQqx7QSbz+d$#%)B|Nu z-9dfS?`Z(aL23)^z@0Q&-KD9Y0+#cI(|BTdjE;Qy7VS`dC^Uetmp&AFXb=L69l(}G zBY=qyg%W@J%%kNAF*5GWH`pkbgPrM^w?{HNi_!+>r}bF&t#w49K|DAu{-q3G3e$UBPsPb@cMrT(hbg8Wp;hWp&9&JO0TAy%Tx&x_o!%$|;41Ob*o!r~<3-)0cuJ>b5TnQN zq$p;Kk&jkEjI2nN4G)suKhP4Wub=nAXRm99v}zv<9OP)0ch~5F8Tsiv5cx;fK$CN$Z|Ii9Y?%S%UlU)mop9+1j#hXU>|Gb}6k>q@Qo`{^ z4wkH1?!)m>;+qviK4V1>;7S1>(xw&k$Ey%iv+B8e@d7g{(sL7aLyenVBE4R`w1xUR zbi4vOnsu;+=>@0HYt8#G(N@y93G1)c%X11gk@bI$j>rB!fw<&J%1}?iPCbp-P4ERUZT@bE>thmV)Zxpl9wrrvJz!Xy-GFeH98071?qLW zSiM14qP#}ENgLH$bQ8*(QQm>_PW3kZS-nHsP;N)L6Xk>IU3yr(M^B=BM!ip;st@pY z3*JZE@|+l?J{BX?C!$at6ep`s#i{Bu#4VqTHR?-oBkJ!`{}S8P4_XbbWE+fIAC?Wx zfH&n-9DbKa$7s*)q2cr`@)oQHW9VJ|q9+}9>|YZ5;eRFm?#yH2O+2we+>3YjoU*%s zZWeE&&QI6S72+K{W#CQ3YOPErJ&3=c$I6t6tMhZT$IeD7tkzzQRj40%f1qp+``Rox z2U^I49z7)iQGQ0^{{TaDj=lf@ diff --git a/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$BeforeWhatTime.class b/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$BeforeWhatTime.class index 5a175cfddd26a246a03f4f9116066e163051b0e8..c5f9e02ddd721a6ec0db292e575b0ac0972e0d8c 100644 GIT binary patch delta 23 fcmdnXyO(#vWmeWV3|TC%C*NQ-VSGKAmu&+8b9D(# delta 23 fcmdnXyO(#vWmZ-*hAbA-$v0R{7)>YhvTXnWU26v` diff --git a/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$Point.class b/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler$Point.class index 894e896fc36749d53438877f863b55a71d29d866..a8f54d2d22eff972e5f2c790ed7ce870811efc50 100644 GIT binary patch delta 22 dcmaFF|A>D>A`2%YgD3+V0|Nu&<}8*9W&lEr1iAnK delta 22 ccmaFF|A>D>A`2%2gD3+V0}yP^VyR#T077^KxBvhE diff --git a/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class b/eclipse-workspace/MotionProfiling/bin/kinematics/KinematicsSimpler.class index b50e58718ffc495eedd60fa09a8c0e2b87715dbc..dcc0a8b7c39187c0ed7456118860cf4b9e5d2037 100644 GIT binary patch literal 28344 zcmdUYd3=;b@^@AD^UO0dnM^_w!V^ehfB+#Nmk3OV93db#tRRRdVKRi6V=xJqw=S#h zD&ESa3%Y95bzMa{6m`}0W@T4bWd#w}6Lobx@j`jO-OoIeNhU=6ecpf8&ywn=y1Kf% zx~jUK>V9Tl`03%th-j#&w@5JM)>PL=>MEM5!;J%{#EY}4>lW8W8Z0uH($B0|S~0M; zqP}Y2j0I;#!c9#6x-+Wl8=ETX!x5(ZsU5Kx$rP-pZHQD)ylSWxs7Fif=h|FD7 z(KNfd4kHq$Ok(nuhHIsDP^4(ZpcHu1)4dPM>1OT|o` z7dKQdt!RpjZ8PpE|G&mvt_CirL{>2M{F!+wQ81Z{)qI%>e}-C11&dr)v3yRXHX5#O zTH&L?bc{tqn7Vf;871@p4W(nDc8!rHy^t5moQt5GzyZ5u4S8clhC z#?u5es|bf9jg5tanEL$O5;=NPt~!_)ph+|tgREG*I8qO5&n+tNo77iqd?URy6;kN0 z8ctUYXD}Vz-pCHiK#llU0XiXZ#Dl#wo2hHxg6hVCiu!_TM>`8DUG*HGx|zCbw21WjZZDr_&kmFO7@glOhwUo2EyW=~aR6 z=-n|>$8P)1@nNDB0UAhtrF-E3RT4Z_5K5)Bc#ig@%7y6}ENQB)9q8aj`l*VlErN@1 zTzNuWfX<{E=n;HL8)&59B!!&3@MU{4uNfLrHkBK<0*>MNJG@@hUI;`s$ zY0+8ki1p^B&kfLdbUp+Q+nclSj5*b3jK%*JVtz4oO_1u(d0FLslwwrTg#o&V zF2(|Yt6G3bi3%870;S+_-ey!t)j&!CtoR;|MGWwlGzr}!C)@zz}MwinSYQ+~o z^$eJ1xo7lShzIbWXN(3!s`0e}x{j^~;o|5rc*-JhE>|jeV}MpG*@%j2^|&@b>-6K& zYR~AWPt^Z;#&{h@ieO`aZgJ=rs>eSB=r)Ibq37|0?(PiGU20rL<5>+&o-zHMD$kf= zr{JSM)BP6R*IFS~G&V)*0NufQssp0<|iC3($*dRr)McGea!W(3EHx;~JJ! zH)2yMkJL6*sNtwKe-F?&%nr_qs&{mlUFQ%wRGP+Sb2F}9a@Cd4T8(PKrn|91OiOlnt&ti^f^sBCK zOLv$25WGdj>_|f$*fHZ_f%_ow5OyC-?8z#Ua(9`SIkN?Gj?Wt#joZ26=1X=WZgq>R z;S|I;i5wCQP;p6R>Nd&oSS`-T<)aAF&<%P{Y?tiOxD$L8)}@VjXGfbVY88`ArjfBF zY3Zd68}txef;WTustM7C#O@S>&P+#vd4W5!0=IEN!d$yxqt(vuaK5+0E^^==W0FYh z-R&`}9-WWvHjbt`)urqpxhe(>TsOE(=$uis6pirEFOlm$W~&QoV+8ahDVzBH;l6 z9;n_ijK=D-BNh*aYi$W2#x^umte8|^8CgDKNz;slWmwe8#)Mc;RD<5@>Lk2nZ3VWI zm@H7^jx9EAH5RoSCtD~}ze=^)w6kav8lrV=bP;+U)4)~|i7hMCt8J~TVTY$^qZ%-j z7>^c7CNW7SN#fGe<)U`LHnVVvTO1SI=V593$2hea#oR^JwUH!W2&ImZC+b$4rf{uV z=1J<`uEe`)YiyB*TFbaN^V?J=_0*QYoqL=^p5_o=&LjqH>XX`ccSg{mn2*vl9SONu zQ?%*)a3XfXz77T4i;vk_^vL?)APcu*B- z2=KXl9-?ogqnAWxKp(POr&MBODka7j1o%R}2rn2&evPk(X{h{B~ETVFY|A+oeOx}?!Xsn)k3TnGyQw3C5KDh-ATd-=~$TpUsiNEoByc0b;u+7of2 zgP7DfZV}Gmu&Prck;MRWsv(>?3kN9%NP_9%jKy%ElQ><|)-WYD<<$+y z5^G13zpT2p)-kyD2?F(zelIGfZ!5Z1Aq_8xy=zvWKZ&IHSIHMT<7qSGBkp7+!J% zJyu7zpy`Rk9>?mKX1S6$tO_FE0Ii!Ho!Gv|YnS~1mut*t&2HCY!0={k(DbQb zqGpyy5_T{pW~^as_Kpb-R)I`nixvXyGbS~*Vlg|4Fh5tvRO2hcI;rcLRy-E8b+w8| z+VDb{7mMUnWqE5)jxf)iapG`VmMU6*SjU6-6=G%QrirXZ7u#D=R z%ynbf+0|OAbCf>q#4btuyAD-)B4)tCCEgvFTivus?d=VVqqQ-IJGFLqqUKWdKG)Tp(2ix6Ay7>l?Vfv-wg0;ypO5sh=%6KLmz#) zOn%{L5e{pcR#BtUKrQ+i3tgLbjt4y#d>cDs;dl9ci{Ham-=R!e-?j+w2fQE7d4axz zq2DZZH;LNgzS_l=0`wiry>%ZWeHhwD0sdI^)GwhYC|Za8sCJ(O_*2!c&}pX!7dM=w zgAFwJEWn?u20fex=%YjOF9ZCQA|DXrf28LS=;=U!zfn#5CpJBjIei!4?^VN$_z>fw zRZ~6~;2%|Wr;3Fzi9~Dk^C7{Oz(dNPz&m;vDt zNLH&WE;Dr?p053IA4JxnincLZzwn9_oS=#T)S%;m z;{;U&=KQ({21J@j$7}uq$GmkIjy=9e9f>2*YHXT@`$R}zwqvItaJK}+A=(C8L}w^R ziD2GYyOvu^xF6#LxrqygeYcZ~cUb@zKf=@T2cnM(Y@rDQ={Qk~7mLoV9YZ`to2NIW z#Dqtpfg#J`&a2f<(KeN-6HV?(qS|rd7UaAa!~WajH+3c-|mUHIHHCbRIx;;UB<@{B2nooVr>UV-~^gWbg$+tiVy z<32|*AAYC?BOTg)M zZ3z*lM`tYwFPcytsjVE}&=AGtRk(Zqtmu-4Fp?wcdS~{OwijFnsOIqTlj`du4dZZH z)u`U(f~|YP`R8G8DZv<>+n1((%d`Y;QyuZ%1f4~1X$hvDj8Xe2b;pJ+AxVRTx-^QC zP}ffZ{puCwrcw~}X?W@dI`yBK_5h{dkAk2K{Pm!kD2FmNhVX=2t+6Jh zpi#B{;;q!V*w{>6TG|EmAWRG+9o?F`eZZjxH#ICQ2z85bIsw#*Q zx0UiYQ?A0?hbpOu=4Q>Kz@du`1TCl^_1E+A($O>k z)QA+RIH`x(=)sKj(6^l)n$g3Zx`$$nOYwCi*j^6`hkJh*iBMN*+xEp{!NCHL>-s%N zgVs^7U;iyMY%>jNrbVix{%xSs6fibY|Ls&dFJshJ8qtN%Ck`3 zLMN7(&6Jl!7_Zz;z2+6?m|JOHi6_VN7sc7!sD!zdPVL`!GtI|9767|&t`e-X$|X=S z!d)$k9>5Z8p*-4##dwIO;NsPE+D?n;QEH;cXc;|07toWmlAfa9($jP;J)^}m(V1dg zOxML1;=0&EKmai@O+lZvREGsvgua|o6L9qug_3Qx7EDsf-TKr-D*Bm@6$J^Prg)VrUhL`xa)9`u4L8#=fOHvfxsEpO5G?Q5p$nm3n z(`LFP$B)I!Nx`40ZFFfUpwRfYJQ@mS(JED{u~E8e8(mWqO3(2>sG2-TH&lf(>&>uT(*Sc`BLt#^0;bF<R#4jJ4OJ z?CQ3Uf+Uoyp4QlQIFzqS^X=TKP=Q_G@U-(=ca=6sAObD2k2^DKT4vps4(rIi${X3k{0JpuK&>&4as46tX z8A$(dXsA8ZVQCMEv%Xz7!hD9=#rA-(JuDm=5e$_m?3&=}b-YWJZV0Yk%P;DZ(-41S zLn_r+^$-n>a?y4vO3|8a)EpXPk8vu?+~!exR86SdF1N=hBD<_69GcLw=!%D0;`o{Y zp+!`M7NCve%i{P64t|wArp5+VH-{F)*4(8Fhe|Mz1=7)AsA?A7jrm>Q7I_bl7ifjf zB2{kOME7PqxP`U=Y9F*6&dLtJJ;5_2pdVG}>?HJG6uLMG{e(i7B%%MR&}B*Jrxkik z68c$%E=@u|5409`VvU|cGu>V?v6(ii+BKR41_zPGOf+tAp;Eh4+l@BGOJKYSf)Bca z4~- z&e}{n07|iDO;J_3BfOb*=J-`{ryaz@OLo96tg=T`g+|$8?R`QBbYdtsm1P}!5 zu@Xo5@gfL%dwMIoNHn0sIiB`COgq!l%=A0xN*{Oj<#@O7(YJhl`Tck!E~UvdjR%QC z1{snEnWdh~JeTtzBovS+GVkUy@gDpt-kD!RerXqVqJL8tdXMtyLnKr_ra|;2ID8Er z-_Q*Do=&2Jc;o&NufadjCCL3;$4ob|pxao|7Qn4+&~t!0*~FJG9(*!l(r5VEA;4Y^ zvY&HN>W%Lf2H|N0K2I0}I2Ld+;52-QFoQGrSAZt~&IX*zSzL(^2=(Ha&!wd*X@6)=x`i5}cr6ax0) z{-T(VMm~Iin9T#ld`PK@2aEG~h`5}O5vzEpxDjwIA1nUI!^9(ikMeNwB;a3pgm?w; zRSt>WfP1(^d<6J0j}!+15AkubFOQPP@@RP+kCC%^tgPWOc`=WZYq?zB$K&O*JVCz6 z$IEYdBJx&~4U?xEUY=oO^Gu@;U@`y7=+7q@0{{mCjsz^_S;jQLnS7#gI?pkpfDJs? zXyTKMrGU!-e+{^j=NUHuuHjRRb$qIE6X4B!ny~?JBjBxoxAU3CeOzN~<67fkz(={> zc#0Pr&+%EtKLKCkM&lh`V(jOo#z%mk@G|27FE=HxFw^;L)8=!`u7G)bu35dUt})iUz-iQ(mW6FLjH|;1z%#W23*INnm6-h=KX*V@^4MtoHKU-?&QnO-F$`l z9^ignWq!q1numCmNAOjiT)x^<$k%vA^0l5(e4S?o;0b)ar#u;%@MqRP`3q|ge`)One3QSjJ_G!czqWqh1768rdp-P(H-*3T<^ksO zciw@3L->1d3IE`o0yv$2^e*S0yes*T_X6=}ZTige#KqLc3~k>Ptv zWcv1qEZ-NRv)?DO{oO?u{|I6GXNesDDWa>tQFQZPE^_^AM4tZ-fOm*|{{!ME|8~H~ zM1g;Y=r!44*Qe|eH>B(rH>P|cR;L^kYXX*79q@~_fjqG;P$bp|hKid5$BW+w zP8J&iVX-l=NZb-w2Dn_@8dwE*wYV+tu(&<&jJPAPQ#1#5i#r4F0qz%f1-=q@2M&q5 zQib?qswwVC%>m36e@Z<{+?zT~{5iEu+?P5HaHhCF^;EGbwF>Y|@j&WI@nGr|Vsq*r z#Fo^D#n#l9#kSOs#6v+L9u9UD+k<_?Bf%kn@GimQ#9xB5#pA&m@kH=q@nrCK;)UQ< z^kMKOp;MD$BaKr@M>jQzQY=}Gl1d;WtBDdmepV@Gl<<`^e0!JRCF)~xB$>Mh&*34+ zfBI-p@C-f%>1|wd4bI}BD5cP%U>`mfm;f~fb9op_LAoU9-_BuX|1Q!nCDl)7?mY9k+qQZDySt>#fE<@2D_Gk7#g1w1Xa zjK`qVgXgA>K#JQ>giz-2rWr84XWXY;R6D#u<>%O{{T!6>G4cos^f#!NbqXQMRA zXrN>GM3lxDYbcfHpi~C0`VXF~=|16&e3GU+fP2036dio$Vm?{XdG0W7<5Lu!=TT!7 zpQ`Ar0Y)yLrs%Ah@_RmA(OL84=X?fA<<=7UD$hr0g7u}G%M~b1^m=4DFFc+@WrcjK1Rl=2l+32+*N z>_LBI;a1;7srn(f=4J}ycoYur8F}j{BgYfmjAW$xdqXqz$?^2dbB`I2NYPb^m<=F9 zvz#0cQncN?~6`Q!0h4bd6iff#r%4>n338^KY zo=J`uGth?>DYnl}!QhH=v`}LWTzWf2ABiA0wV7^hR-)D&;K3=~ig!&s#Ha5F9hDIJ zhGuJdXJajSJDck$wX?C#G*L3CIH-B3@oeG75O|iPZKtYvTX|VYx?AX!H!P!>p6b^* zBevw0V$~@p4dc$Sl=yF^Z1ALl7Sx)Xu;`sSXBd^+c=;ASzZnNU={c4|c(QM(%9jbIq^^V(#qLxhw>XixcD8_u{eiC7DVS zvlGLfZuOFS1kYqN-4_U5LJ%#H=<9yI5ynL%o|R z#qI$vh3ddf4XU?dbgSJP{QWr|yEoKbt+`%;jiN^zF|`P(gK7US&RNS{>n{(`W#}&# zx~c`}yyagH{ngcpi7lt+;*4lYuA7%eYt8>tGN zsF=&)vC|Y1|Bw#sWTz=x31vElf`f=mZ77}X&JIBrWY83Ojory%qmDaOl}707SXC&; zPP4nTRG>|)q8lnQt90EeyK{|=8f7Cju%8$cmo^;AvvcfjVLLAzI?6uEPQ~t<7N1o! zU*NU~bq|Jmsu8&(j)r=v^ArcIqQ4~Mi>l|Dn%Eu6}>4V`JGg%wHbhVi*O4P&wjM^!qtpWRP6 zhYi?xlxygR1?``lc%%k}Y?FAT(~?Z*UT6^dZ%Wi5$8zpdx^+}Cev`uYOT;@mCs}XE zw;Xs$<-T^)`}k>;LHH>U$sQgg8{{f^wVV4gGLWkx@d}yZUz8?xQ*W_{28ws+81XKJ z#CtSaypP!b1DY-N(?anzohc4bllX>~iErs_@g1EfexMu0L0T(*q?^P~v_bmmE}262 z$N)VpQ|UPwq#ZJicFKI(Eeq&<*@Hfoz33a+hb6K@UfCb1+M~Hh4&q}FnU4Y-4R}1@ zWO)pqB!}?HawsoA7AY!+@i~Ail>8E>94n-@*d=Sl zK3OLY$a-;5Mx{^I%WQd;%!S+s0+z}qSuU5#$#R*TBUi|i$A_40P(KEO@#PUCUeY&!Z5E$=l0@=s=lywB_^?>F=11Lk14$s8gdG)n-Flbg-aa*H_@uv~66r^;>ST)@-h zL*{(>uvsa$n~UV5W>o&gJX=0)UL>D1SIfVe8{|{w!}4kKS^13lGT^K7S@UiAocR&p zC-Qmod-;M#$mcz#+~LWSfAbW}7d=A&kCi(;)8yYh5%~|#dGaOC@8rv#8vxhHe|qkd zuXwh}rJkqd>z?Q2ZqH7+$FoZ!PbT+zK9_HLz5_fc_gR*F%Sw~~wvLi-TSf9+Ymj`; z8UYxR?^}}rr^yej8FIh%E5H)~!+;Crht?wbk##0u4PYH$z5LiZ5Ab~XAL}Z>Yvrfb z_42>gjex5G*8<)oKeKL=pId*DUs(6aFRh2=SJtEQYwK_FfVEqGYweTYS?>Vump@ql zkq7bS`-64BDE6iq{k;8+{@$_1(cXwLzO#>`iob5!;Nw3`j;rBkb3W;*mXUWRPF@HZj@BEMUe0vtFtJQ zmXYKK@s4ZYx8vN>+IVS$cU7$R9^f{E&ZOt!rJdf>W4JV5p3cR1Xt(zS-hy0=Mc;Uj z<*g`rd9w93Z^PT5k59H<=7&&9;XTO6K8#W-?=#-#?VwBJ{l;E?1S3u7&y833QItCI zSM(x3hI~&3{8gS)k~!2fRytmu&wo+1u*U`bIJjiO8Yl1*;G6~9OXnw1%9h*3kNj7Z zY`H^x!cU>pRqhdc`Dv7L<@@4goh`_h2ap?o2DkzvU##P2QR;y_Nf|$fQlYU}OpKMP zA%o|E>uofN@>ppSGD0r^SA={@4(~vz*gVd~ndL6d^8xOZs~lCT=OFSBe?uwFvP3V& z&zn&vD-9X`ohW5mM~PJaJ4&6cB4Ow}P8Vc)_c4Cb43`2I@-sSr)Xln<*Xit1o^=yn z%`c($DC=Qv(7BlI)}tKNxtN~T-*~>x#q_dv<8(DkwQ<9BTFgJ;y&I>5vD6_onv z{D;0$s`Li=4|diCrP#Xgt2kv*IguYJbdXFWL<(XVfhy!gpk^u=Lf(v?nP#^`P&Ox9 zpzH9Fv8qc;!mAt#(IM(eUY%QTs1;UKIGGhxI(e5?mGSy)?~io4l)hd*-yx`ZdNLo7 zBKx6vf<+ZZnwJqw&rb36>>|bYVLDPm`~W(0m|^H{ovshzM6BD-(m4$j z*I$#-LJcni#lY_)#4e;ZAxY~)@+ZHqj!6HSA(ZfIj0lBPNu`BN+}@VjfG`?-thg&Sf=Q@Rx0F+g`xVJ; z{I4y%SKZ%ExN>dG)?GQ*rBmWvrMMYrc%`?{ftnb;fppk<{tHY#E(i|{hRiJfcgQLV z`M4w{;Y~ac45bD`X<}F>{YpO-C)x|#H~Nym%1)dUc#!rMtzNH^82Y-QdIv>DU~V&c z)eCuVJ1qwX|Kq};G&@xjheN6Q)F{WjHvY&%Q6gO5AOYd#J%sj2}RJ-D4!AzN3fgMQntsS0(~k50dKR|hIR zQ3QBp%0;ipG34rwJr0vs&~e5l{;$*Vd<=>HRK9*^wOjA1&V9ZTTD*QeK4crX%@}Fk@Ov6 zi+BhxE&7r@?}inVF^{BiGI@L#!}f6c-sg#Z|`N#kGK|jhDnaHVidxZ0WWZZX*5XGrN~0j86_VDWp|Oy`*Ez- zRd&|lWq|TzhUkLYR4R~Z!bT~Lddh&vK@ix9dgIr!b+AY$krZ7;H;ibd_@l@LT?V~{ z13h&Rm_;8^y2wK*8<#^Yk&lv%UrIbt9HnY`FkYGqR4tAJM~Lpab~5!9Jy7bZ0!7gi zrCbEJlSCm(SqS@v#ais6?wZc#1EA}rT8O@sEqW_DF(TGmwpd3uh&~#32Yvxrg~{3C zLz*Cpfa`)#cNiUwb~*Y(kXq_|2p`p?>fLv+wD@&w$`;P_RXBs|E0i}KCr?K9_mpyw zI?G-^P}vW(Jm&{$7&XAkJB0N}YrmGg1I^r6s&1)VI{m=I475qrqyLtAio` zHi0ifwuqw#+>KooxXbWI?a%nc%Xk7S9>!jq0@Uw-`w;)G!M{Q3UlDxLUii$QD(tQb z1)A~s4^Xiu+oN1)98nVr+PLUu2ax1irLs*P+zt!GoFlyR`ZU{cFwT9kaLBLux*hC_ zcd#2g)l0qh1wU zS$DtS=y+9*czlT>au)SCwb;q|ZZTTnJ&E*sP1S~-s}@$pYg(?5RZOli$+c+o4t%0y z=e8rdP(Eau@2oq7#c5ht>}^*pU$l#TqeWORX<>~$Vqqm`yis{^Tu?t&-cZwDsE@vK@rnMPG(hT(Xu1ZM?Z2zCJ$z37Oly%4qKV&>-?7%K? zz`V-5HXdw!LFd9fe~w`5E8O$=p2izL&@|&9%`|>Q5Os(u@crkRfOV#zCR5U-fR~vD ztp&WSy5IEEW;2Bz0(`_wr6)~fh5>h)Y4oz0POkv&GBfCPGn4iJz5)0# z;HPF5eQkE3pK#aCW9G0A_wBN9XJn|E&!y&3Jl-te)6DKX-|T^VbKP-gt|#AU_T~*{ zA8s~__-?Z=Z^pg6C(NVyd2;~2g!_5B%|ZNyIas8bLqr$zSW#dO6Gi55(a#(qhM6Np zsaYx}08TQG6VuF5;#YvP&Cz0xIYyiUc)D38BIY>J1h@q7GQca$@#21Sf_T|HUhFX^ ziZ9Jc(ljT_&gK+35al7rbB;Er$>RYho73e2bEZ7gJV8dyIdVDjpBI`Z$zPlET9?$U|n>@S2rIff+GIm&;7b5H>;u4(4h%2*b@qbyEu4wYgfTJ4Co;h1#=G!YeaM>ctu#N268+ zT-efqQ(>Erq*%%#X(Zw-ba~!vs7C1O? zjnUy(A7X9gg)UBB$)g=Q<8>EjyiJ=N9KM?!B-9P4Anl|+LfwE$$KBt2p>9Ay>kwwC z8&Fwd5^l<>8&KJ}Db-D=8&I}5ojVD21FEZBk9)G}22`%xgj#h2Dqja&k5bM-DwUZB z$uGaBu`xIz1}}=i3Jq1(CQFA`I=7^vGF%Hn+@t?Ppr+%iLy*0b*h-)xig757rHvBm z;v8H)9kB@_T_wMvKO+kK9vunw8-IYfgu_2UFe)nR-Pdtz%d*6YePSLi+83#rEP4yi z`V1`^wk;I<;0vfVe2=eG1@A2Sz>OSah21fI`9wU@a>mz|7Fy_WGqz}OIWd2 zQx5rTUrT=KVk&?UTuHsmtLSLJ0p>MSW?oAt0L}tD1@KhBGXNKu*U?JzYBX3Fs7a68hR6OAK(Xo`vE@&{M1~_>E?}air2GkuHqbAFfIV>X|7j> zOrw-pa&Om=arInXv4bI_q^{UOPf$`B#2hiTSX&BI*a&A1`nt07Fzp;{hko3_1aD7M(`( z0W0u-15^iCj}wct0i7@QosZs~pLu{!)7CqjI!(01f{>pyL9 xc=N$K4E|P%2;dO%@VodukcDHqQZK+N$Zjy;B0yxMMK%7NiGQ_d;RUQC{(lo^s|^4E literal 25859 zcmdtKd3=;b5;tDe-E$8~$i*{2FhYnSAeRVCKm-hdgj0#&u__@MLQKf8$#CiJda>ww zuDXh%E?#SN-SuF(T)Y=WaX}QtTRhf#Jyus;@!W`7l%cbXQkb zS65e8)zg#hKmGk-A{rs8JtUZVH#Rg!7S+TWYFmd)O%!K0ELz+YY4MO^$~v=VY0Z$P zn&$c;Gv}Wfsf{uD7oE}2+#0KCu8lDDnbsAFF-#dXO)Zg{x)rlp8k%E~Iwn4aDYWQ} zSWAs27j0QFD~gs%G`1|QX+nEBQ@CjM+{)_XrkqhdVdAt&MeRcrrEys_F|r`q5}CWO zCRW|B2pox1E1A4wYnvLH8)D;_WXXUzOzOC3UBpLB-Gk&KKU3y3%y-6;Me`#q)iv{* z&@p>jw6>;cPEAXLevVhV#~Lzzl9V5$T*}ZitRQ*F3!<@w4XsRp`Uv=UBvM#1plg|E z3I#!Ok=u__K2x{Zv6|Y(={1YvWA+5S>H~=f(}938YU=8Y+^VCqYGMnSs!F;Xv`Ax( zIS7hghcQ}M{z=PgBa34V(dJeU6*G-&Ux0KqDXMJ7x;505L;B6SPolG)UP+J!P$^U1 z;+BS`HL=Kq6uzhapW?e*GcKn_RxtHD$ULornWZH+FCS}>4<|Zk*zo7dVncGs-!9~S+jU?q#3)tcS-qxw6Ws+jq%bnNMVrf zI74@w$#i6AoE?yW=J@y^osi7&FfUaz^&C*x&{|m2T-ac1XJMVAo`dypbAmLNPQo-} zQCnn8x!AX9Lh)07IzSBIs^8lbunyGq@p1QK{}7l z#{z(=UVzGE1&l9&*6@{D0e=g-*7Zck*i#%uOZ;>ZUF@OXf#LS`nr@8IC3LA?@%d0a zg{@icKK3p61Ni42Zvc_*d{vOHrau62ada6hWeF&kYZbgUNUODML`{Q!ygo>4%;VAq z_pwi$X#Tm!du>7*;pQOSV&gB+kGBQsb{l_z`_cGt?(u`{l6$;m7ku;=y2nF*?YRCc zT4Rw#LAsaj16$E0T41)aH$;aR-vSyEsaXV@vX1p6*3(80ZD7hV66lB&q)plidT=H+ z$C{eG^Z@j{uE9RR%L=`;1;>Iu_Zw=Ol69m!j&fXdX{04mr!gOb>0Z##d{Decn8sE| zV>L}MMD{p^wVFy@;j)DdO_4%vZf#X;u7}yGE3Ato)##BP!%jPMNo!2&WG9UL15QPp z<%)(D8%;Yz310OAJQ1WP^#UBRK%>CzXo)4y@3@vF4Xrpo$|Fs&8r@mpuNW zpGnPi2kr4pkp8KA^ua`1V$Px2De;{_dRC+M#$nMV;&VZIUL%&YMrxzYb%)0K3qg8O zcPh3!9SZL+1?gqoB?yKRV(`+dOv4>xJNOWVrx)#YrlB4C9gGRx>rEK7M6a%esL|iT zf+o3f`A zOohhS%r={#b4$RUbgZXGmSg1EYM2_RbSbIGRxi z<^2_uri2?UGC*l8%x_rJqAZpJyqvaFE+~Fs$MM zaR$XHrEoD<`v7Ss4z5KUJz0_|U12ee%qvozkWMAuGsS##5O|-I#UaWG+N$MD0yZX&cR*#E z5lvAH_(a;@g6Dnbl%mI9h%6fDOXOI{b-?)0ij_{Gs?XZ(Z$ z<{25K_i~mnMgA!=#F3bORykuJD|xW>#kifHR7jz9xp-Z4%z;ukEI91KUtStdMa5@6 zaRqV;+8?lm)HktOw)fP@!Gj!)eTL~UE9Lk?sM#rjcEz@b$?upkU9s57nvPD(LyYY@ z5n$3GW5(eG^UZFkZ?1_gX~7wDdU|wyh;$AzniGRF&kJ%5&&N@1vTE?Pk<~_W0pg9+ z*(M{QZhD`(5eg#~*3o;upCerFVPpZix1Y6@$Ya+>T7tZg8{icpGrA-)6Dau|qx(3T z&dzXSkee9!JU{X)MGv?02r)v40GFM|VE8(lOXxjiJoZ57h>S6~%8*}S^x zO1wjeEsWs31zx?>G{Gu4TU@tZ%~X1*($x6ZV11ArSv+TfR)=A{I>^`Y^-xRbw8N;A zAxvaBlQUM(9K4P(I$zm5VOF#iKGV`jc|!}*ib#zil3&)))MRgq&Jlvmk>!c8U{MyQ z$cbr$vup?4UDf(@bmX8HaSnFS$p3%qf<&@2Ufe=RDg@HCk@s4!>+oh^)`ZJ1$J z>Xv{F>|EtxJJRhy!-AR3O)J_D^5*(>LxAm(-ofx{>lTbnGP2v=8%(nuNgPlG5nw{= zs-sgnw@aOpAM8+#TbAlh_cd%swb`Qa$f>-e5pz7I_6bLuJF%UR>!1!wa1eZWc%7WY zX{VJrc(Y+GFu~-PvP{RNpTJ}ALZ{1@RVhSpdbFlfImkJlN@Lv59@WEthp>JQqkevX zA4IC1k<&f=AgOk|O91vGL4H&x3j=ta4fhwOf6-!`j(mscF_; z(lgpOIwkqdN4yaZ3-gX3Kfybh>JN!ETU6$KkwfHX>uTS`VrgSHTZf`E7dUp!K1JQ< z{4f5df_L+a9)1B%Nta8~@rpu__wY+Npyr#43nrd3!zAxzCz2|6GV#FgT5~#%F~$eh zUJ3H6dZd9#BOz|?ava_7^&r2Y`xV*!40F!8oW_{$us6tW=?;DE4j7|L^ml^%u0|gm zr+=s!eT?*8kl)u`2PJnslsSDE;#Jm(drp|66AmD=I%8Muq8k+(dy^V_zMqz zuEV6nRhaVV5_q~n{tthNn6;sGM$HV{l|Dr)?SF&(HGgB@3z=n{DeXg@bubk+uh9`+ z{H%5E8rj_XK)3>1^*?RdkistNnhm@})ByoD?e{_cfsxq`&bK$KasT19l<2p@@Hbmy z({ZH+$5|)44hZlj&0Pt2R%S=F3qomI2{UQ0MEk`ZToJLI>tylc#OvyW zG#(ZFsTw#1NsM%z^2CvpXPDA9*=+Mfva>)l=O}q6d653jygIm*sYyYT>2lt%>99Pp zAMBU~ySXzuU5sld`5bbODKUq@@-a6nz~re`@N5%)z=l402_2bEM@syrgN> zp=3tKk?_9lfVXO})$H4XE)(Q>#PL7FMTSOVw9F*WJiSIAVRSey%t-7&MM7V4L`mrD zl7N1F^^z#dujPv8EYxR{1ZCsfjb@?{<(L}66K-L~yNm^n zZuOULqP$YIfqJy}%P>s1z(y8^b(#J_8ynO#TUZe46{mCpur;L~rR%BpHp(d7LVWTj z>a&4*>zez}Bn>+@*&NI$7ti@gWP8NP9%gVk8>1E)Vd%*f#V4oMQP#b9PM%xuq`CE_ zerPKKh5|qRjbN(4y%wmqbl{+jO>{*29Q}ssjbP>`L!;2fi!*{AG>`_Fd3otb8VqbW zX;hjvLN!Kk#YY&>$p~#2;Z8F`DfrTKZ3%WZg090oHE<&I)z8$icmgPRK;!Cx>uKm3 z${09kBaPfZL)&PfF6qx&I!yz0GY#58V^7W=w~59N+Dyl7q{-P+H_~*JXX)~6lsD3e zWv(_VNJC6CZlV4smxf%M=;ShY$o+_>?7CSCa})hy(0~mz4~)!5?Si>lu=XmKLB)vH zfziKV2{uvzZN_3eKvU`OG=sL#LVAc|^e`=>$LJh-oL16S`W-z%SK)q*R@5o>6cb{) zIK||#%!WRSb&8X%Pz%G*Gy5Usn(_pa%GSCfkGZyypJ-15GgJcVkxy^ z9gUCPH{7lj_Kw+U7|FLA6O7=@F&e?tLajE5+*7eyClLjw4z#d^VgpMy(6WJ+djl=k zk}30+hP;r~*>a?>EqGYSo9Ej|=R8WI%KTes)XAk8P{|9*0=iz+#ajYx{OBLs zK&wK2EM6#pulmh&VK}I(@mU@XXXMgly3}Z)bj4=6vN4<$@~_uj*3&ih;cP40M$FX} z*IBuZ=wR9!t=xEz+FZK9rh%H9G>uw4O1`0Eny7_+((z@S-tDSg;t?W)9TYs+w^LThWlFiR?k|iZ*929ROzp^_2FWx zSdShJm*|RHtrBNKYa6W+n@VH*gizByp;AM~#xc~|2aQi?P?r-L?DRJi8ftKd)`y4N zj0~y`kFZAAB(32I(tk3YFrSfDsWrIP8d)12t*frnU)Nwgz-)X46YCK!v&xKE%}TAp z+9pjY!9Mue_*$#9Harp2nlEiNgudlc+v+v+ zWNPJIsJ!23WiILRpVrac+4paxzX2LUvKbb~3c|*~yd+isT~`;SRX?b!N2gUkY_P4- zwP`Jn>XwOV)sO4y+~jJ*l-^9`RuKqU!Rv0~ceTjoSy}a#RUa-7`K{ayv=yKnvy4S` zvJEuCiNp zSyhHQaCh4^yR8yD>`9d@#O?ViwGtxsyy=Mc6-?4FnLJM;^w;7o+tOOzh_`G{YuOub z*^$=rcD!X*TFblfmfdMB@5Nj8B)4=>j1PceWh6Hn)<4qKQ8mGm0WGF zv-^RvX>IQH)*1z+;pTpibL#vW|BwLLf z_G^kKB)8if1a-akWjmZRISijlA8WJHj*cA8(oNOrU9z1nR&^)6a&_yRpei#L}MauJgGV;UB_!^+!GeQOk>naP}Y@@Dg>Wx2Xrc zM}6ocgukEAQ2GiKz6OnNX(sJMfbs+4qaP8p{6wn|sb9@ZH?pAHS<*(pO|0lCz@6-( z_t;IJ0)By8&O!Ea2K#YMH^{}fv^*3~qdAku15N;}0-TO}$TK;cj|V&fuo`eK=W-ox z4lm+-UcxKMeR77vOD4Z{Ev&_){)K-v4k!*?onV`-v>T z?tnc33%EZrmc^n7@CZIq9LUGv#ugC9mM)x)bnjUaIclW$JOjCwRGfj#sGH0r&D*>JvU&eFOL%|H_razjpQF z-?)bJIj*DmTvr8PC7+a|I4fhLxF9Lo5_%Xlf{*w2)_W}OM zZ+Qg2?a2kq<99s8fCKnl&m?}&vyk8SoXsD2F5nM6YxpD2{rs`#DgMOs0{`2y7w{ea z)bkmC=J^ir2mai%pTF?t@aNuq{*O1rUwVh~SKhJwU+-kVD*oC#hrjXG0M_xh-rw;5 zysP*-?@heVyP3cD?&2T3AMlUf{rr=!2k-X{7UZiG>^oivUxSdo^MvwUE?mCr0dEv; z-(AAvyB~19@cJGTKHrnV?|TaHSv)^40=|z$(D$Xt@O>>Zk&(*s2StfLM-1>6h*E#4 z80a4%2Kgt5BmGmwVE;*Ch`&(`^)D5}{O60K{MU#P{yW9d{`|6g&8f4>+T5Mo>)LmV3j0rnQ-1BZzTfujINiiv@+I4&?tlm~txCI#vN z&lDAbrDAg6JiwJ=O5l&8GH{2e3OppH23{1?0-uWML6?{jED$q;rD9g_Xu#28ZtxiT zC|D&<)yWevmclwM;v`Q{il@_1(kU+FT~NZ$33XBoCH!s>_N+&6g#LLZ(qM-N2XF~8 zG(H*{?8XC-fWvLQAaf~70a_S%kq4qCNUeeGJP4%>S{2yDN1~KPR|o#g8b6zE4BWzc zv|QR5i1JXB^687fbRLG1#TkJiJRGH-+&geMAB9qHE)MkI5h(THp@D9EG)jd$-TwuT zM5!;&^}o-fP%7d&|0_HirDEI(+{Iy(N_egRk6eaQDL>}_6^}t_5byP$&SOy;%%A#a z@i8b3$+wCpp)_7>^{wCvlqSOJ?BdBNmBYT=%u`URfR$Ozl_*VtZCT7!C{?*` zQYZ3Mjptgg#_=?b=k5<1HC^MmC(FBehQ@QxlDG0ql%~0x<(#yxAG|T;? z_?k~Z=>*RJ@ifmysoFD1{FSRw!o^52nomTj+~3GMcn(Sx{%iO)o{Q2H|DF7MJ_)5N z|NS^0PDW{(|4saTgHuqN!KdPU(+Ovz5Bv-0gU;oBM*bhE$b%b~`>BL{9%2s-lTzv$ z4~@cq)ZnDrFRQ43C#^s|;JAfGS{IiFp@71F3#l-zcdU`qxjQI-v z%US%biPLnMPuDrijN2g#{OZ#)O^$2xOc+=Y&l(~-i?C`C(i5w1q;BRRjn5HSYX6**L2Hip-w{iadNIo@EwNZ)G-+fJO zJ33HSI~Z+GTq_<;~Z%);29_GXNg!(yfRF@Q|3kEp#+O^lRH(m*%PK$(!d|L*4S!8kY+t7nNoh z8k(Ptd`1{F%QCl6{mGlSwk*pjbT1g0-9}pn=4Hp1+@nc#4`qVykVnh&Cdvm*Dr~R1 z?nW$n_q=RXx0&lU@|kT&0B40fI$xicU2rqG+h}p;^2-c4^3!t!37dc!2s1Py1FW~Y zK~h=m2yFxg)jYlUH+QVh33g8xlG@S3`+S{wFP>xI*7GIsp$J!^N7=tyVGU`$KN480xOq z+$_P(qHl_r+J)4`wEu6Mvyr>eUmk4A&|fZcR144!5zmGGn&w2vmuNa)H)&l+Z(6AX z#lGqFt8{%@mL`WU()B&l@h{f(f%N)Ib$##j`pb2FpY;0Q>-xg<`m0cXfGPYzcPUQp zV$vo_+x3Tw+URU!8E@4phYw!vbVCk8ml~~Zjp2kD?2a=*Z~ak&HuAO3&MwO_f;Hot zos?_kTA8bF&~~k7sCy_!8#*%{2+lWK{~zW+#EZ4HDKo^gexr2>V<7 zpxDjDvPE7de~)Jqam$Ic3rBDbxkQ9AMLiXZg)~GgrlZ6!DJ)v(SkX!oMU1M&5?Ub6 zr!&O`6ca0HnfNW8C05a|#YJ?j_#Is@E~XpBCA3zorn|&-^cQhGJt5Z6Q{o1CM%+j{ z#ohF(_$$3H?xjz}I{Fr|suUa9i&*tA@pmo}4DZ}D5eRe+a@PsO$3GjYB6kGNfYDee(piTlOZVuRQt9uxl)FNyC!ZJ+o?d@p_w zKS-bWUgnFRWN)!w4gnl1S(d}Es*+01k#2dC^vK14Ez&1r(l0Loyi5k{{lJCoq{6_XvEPE*nu#fDcisfOd zR35I5k%ej;UIOMn-3Pc%9<3ggBh=&YhPKI3>Ysqm%F*go8CD<5GWE3_;|j_$SGGLH)l-gh z^^xOU!{o89;c|kj4Dc8^(RHjm&NTtBT$a10$w{ucfTzg{*E~7dRVSyo7RoADR8Di9 zC8xX2lQUhb*x z-KFvr_i(_Y<*DxJ@)z!iJk9-UdAj>zd4~HM!0Y5Z_kFU)y;(~46LNw3DOvB{DHpn5 zkqz$m<(cl!Wt00`xya*^&7M3N^%Tg(oG#DCzi6knmKRxKHAKfaEpsb``u}r*wUywAOt~y!9`ry8(DEdMZ)c>28VF zWqO90=+RBDx)<<`h#o!kt-F$MLdnZju5TEp82R`V*ZaH{r2xOCKIWTI>c)H3`+N)V zGWkRG2Hy%!v-m&i1-=cX?)+cc!M7tq%@*HLfnAaz>K89fmcQXY=~n3Pe7*xza-h8x z+y=_I(A6xy6Qz8)Mf}Kjp=8Nt#HajclzPh7#2b7!O1Lb6wj{XZug{qHO z!+%AouPVh~Vz~#UBDGjdiI*B6gL_d|tYV@(UaCY4dmrjb)GP|&vI8Pi*D+3=tK6w` zzk)bQ$I{*0@6#!~4y8=@=X5;(P0!c;Upmg7o5zJ&uGcg2HsEH43&C7rJoc$aJg1cbfgE|W`{lTL?T1R(agk}m{YKrE znO3jkKcfA(mE<*1l#Q)xb%PxhqS1~SJ2WQR^SwXP=~DXo`+WPMj9KY)fC`&rXo5sN zI4#J|$jT4+`t^`vA9;VEGS1A*+eh8)X-%T+gA>t29R^>~_f)`iDuW$HwYfkQ!cHW0 zJyKfV)AJr{KXMd@#I)Ft*g9~4fwT~z@ds$NV{JWbiDu(F^6#U-e!WTCJLtHT^)Ex? zw`^j_>-Z4C_8DM1m`B(^yP%e<(YU!D%hP_b%jAaS`P55ZV6NeeFxRX4I5q=W?lsU% z2p=W%%?Gqdl(G(}sEnXawXSdSg_CfW6JYZkxys44l1=-?Rsw2bSpHcFQibG_K>fQ&`%g z$Jh-n+8Nuet9EI|66}~7E4v;{S(eRZtuv`zx<1^~W~QfRhE~`^?#7U#*73i}0$NNC ze*1{J)(mebAn3((boVm5`yQ)XEz(g%>-Yn+6dH98P;Egp8YyV>09JV&e_R%@Je&E` z>nT(k3fkA$4F93Pz)k#B*pnH~P0~fZa`W@LJE^R)d>xJV2<7V_TdNr2R4B>k;oLTI zqo?J<)vK_}@@U|(GA&PSxQDR_Xz5}3Hu1M*p^)0h`>v=Vh`NJ`}IE}B)2_@&AfF_kA)s2<`xWr-_PNL&TDTJ;iZRDrld9VRxaLa{~l6Hlum zu~YRIYZf*K%RQ>Ee^HBfw{28r*~k&@M5=~F`x2@I8k)Nte1PQztom`lgJq7;AI zA&11xD_Z#{xH<|tFbFR{^l9alM+n?pClB2dy|zo*PIo@)>qk}VWk zyFpJfg$t!jXiHGI;a_&AV*D{q5oB!x$K!O==_x6t>+v*|6o-Oz7Ea_M8r5`L8O z@pjlF0w`JBhfWkh-HJCN6Gb=OiuV?yMTTjuq5&ckrJmeCevySzZ=9)>qB}~tydCc< z>>hjRaD!*@H^9r*J;VUY7daYFjE;|%k2v=lk!$Miz#mxD9(ca^h$=)L>UzM79%+ul zkU8?2C~rURKzB2K#W3mNr?eBqyN&mQ_t#V~1F3$M5C7u_%9H)Sr-|Rw^3eCxGH$S^ zU_aKw_F>Yx8VAmHxbno0G#5l4fS-V0o!N(mA;kc#`Oz*x5V7YMM5OgQg)4fA)_|zH*}o=b|nVb4VwBXF~BZV?0`|v z0@N2D^|fGo91M~eqaJ1vmm%YH_4#5L-;tnelL1|QFL|CJpl^z#vz04)BaM3+>}HNY z9?7{Cnj_4W2!FEGH2E>1uS9e~Gr5DwAL)tfqtP(9J;C4(RO_b%gL*DDGshb&!=s=LZ7(#mKzR1Ee=^f(!OTFlNSO< z@qy%81i8Lhx}JKhp_f8Fq`{>9A^>Mmt|&(1W1ZICJurcAvssmrLlwXw^Or0ldr|R2 zh|bcH0(arZTkd3h`zjR(g#X=X4?+TbjcS9n3gcaCXxO zG--`X+RMpE3F#JU>1v$(@%P{K;59N+7868dg)E0HSKy^)2`}NOuy>1c0s%F zPBp?Cd8oom;UaAY=I2S;t1Gnp_u`PdRGujpAxZmydP+TmQ{Wrd6|Soi&eq4cOh9T3 zNi~kfs&bm7Ced_NL9^6kI#pFtjjEzE0T-!h6jRgbLcojE47wihCUrdBp-!NC)NHz6 zRnrD_B0T{3pqfjMtCQ#%z@6%3dS0DEF95!zenGFO)95w8*8x8P{7jurU#lAWNzG@s zs%8APgL73qk5CJFEH3g)QfKmMs*&fZCcXsla~T`W!oJY8KXBI+_YL6?dpfENK?s;&_CtKW;~)s=8^t`c9VtEEf*LFTDz0Q`=nUtEXHStEXL8sApV%P`ixtHCj6iuGNn7 zY6ssL#K*J|Tv;V4I)cGr9)U4Jmnh@YlI%d|rlq*LJ- z*`<3Dbq^xxZ|ZPaE{?~!bbh?9Em60D=G%3m#HkY_ap}zTmFGHj@^U2eO`UqhsZ;OJ zI=c=(ZyGA}rIQTWNk_n)(pOwwqCP@jI?2YR2#+X3DHqq_iiN&(k}s;cm(Z6^EO9z_ z7y8mkPk93m7y8mkZ@CVw`qD`s<7hoZp&w|h%=rP?jD0jA4oAn~d2v`{piVL68js8* z!n8Mr`xo$c%s-uw(wVmXaFOgRToM*hoWj_6jw`7xLBXNZ6`2g=*5tQzF9zC2oaNQO zgSI_4c(Y&tG|pQm#-IEosr%+`784(S&_A^lF(tgXEbq)VQgiY2fqwEhPjBKW@$fPp zTu#gLcu>y6zjP1z30GpBcJ`2$PF`fN;H2W8r`lo6-8#Ic#Pw|K;`{C0wp#G|$ zY5Hqw$FGz+5~32_g{Tr!0f&>DcjKqJ)5iD2@AFtphwO#{&HzNpRLsO@7CtAyJAMFg HHu3)lc-`w5 diff --git a/eclipse-workspace/MotionProfiling/build.properties.yaml b/eclipse-workspace/MotionProfiling/build.properties.yaml index 119fb0d..e423505 100644 --- a/eclipse-workspace/MotionProfiling/build.properties.yaml +++ b/eclipse-workspace/MotionProfiling/build.properties.yaml @@ -171,10 +171,10 @@ Children: Requires: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, name: Requires, type: Subsystem, validators: null, value: DriveTrain} Parameter presets: !!robotbuilder.data.properties.ParameterSetProperty - default: &id003 [] + default: [] name: Parameter presets validators: null - value: *id003 + value: [] Button on SmartDashboard: !!robotbuilder.data.properties.BooleanProperty {default: true, name: Button on SmartDashboard, validators: null, value: true} Base: Command diff --git a/eclipse-workspace/MotionProfiling/src/KinematicsTester.java b/eclipse-workspace/MotionProfiling/src/KinematicsTester.java index b308acf..210f52c 100644 --- a/eclipse-workspace/MotionProfiling/src/KinematicsTester.java +++ b/eclipse-workspace/MotionProfiling/src/KinematicsTester.java @@ -34,9 +34,9 @@ public static void main(String[] args) { createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwiceTheDistanceCoveredWhileAcceleratingCase(); createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - + createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase(); - + createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanAllJerkDistanceCase(); /* * createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase @@ -205,7 +205,7 @@ private static void createPositiveTrajectoryGreaterThanTheDistanceCoveredWhileAc m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(14)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); - + checkTrajectoryPath(myPath, kinematicsTester); } @@ -219,7 +219,6 @@ private static void createPositiveTrajectoryLessThanTwiceTheDistanceCoveredWhile m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(8)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); - checkTrajectoryPath(myPath, kinematicsTester1); } @@ -283,6 +282,7 @@ private static void createPositiveTrajectoryToNegativeTrajectoryGreaterThanTwice m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(14)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); + m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); checkTrajectoryPath(myPath, kinematicsTester); @@ -299,11 +299,11 @@ private static void createNegativeTrajectoryToPositiveTrajectoryGreaterThanTwice m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-14)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); - + checkTrajectoryPath(myPath, kinematicsTester); } - + private static void createPositiveTrajectoryToNegativeTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -314,12 +314,11 @@ private static void createPositiveTrajectoryToNegativeTrajectory2ndPointLessThan m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(14)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); - - + checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -330,12 +329,11 @@ private static void createNegativeTrajectoryToPositiveTrajectory2ndPointLessThan m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-14)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); - - printTrajectory(myPath); + checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThanAllJerkDistanceCase() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -344,13 +342,14 @@ private static void createPositiveTrajectoryToPositiveTrajectory2ndPointLessThan KinematicsTester kinematicsTester1 = new KinematicsTester(); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(14)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(12)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(13)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); + printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThanAllJerkDistanceCase() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -359,13 +358,13 @@ private static void createNegativeTrajectoryToNegativeTrajectory2ndPointLessThan KinematicsTester kinematicsTester1 = new KinematicsTester(); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-14)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-12)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-13)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -380,6 +379,7 @@ private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThan checkTrajectoryPath(myPath, kinematicsTester1); } + private static void createNegativeTrajectoryToPositveTrajectory1stPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -394,6 +394,7 @@ private static void createNegativeTrajectoryToPositveTrajectory1stPointLessThanD checkTrajectoryPath(myPath, kinematicsTester1); } + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -408,7 +409,7 @@ private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThan checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -423,7 +424,7 @@ private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThan checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanDistanceCoveredWhileAccelerating2ndPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -432,13 +433,13 @@ private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThan KinematicsTester kinematicsTester1 = new KinematicsTester(); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(4)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(5)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanDistanceCoveredWhileAccelerating2ndPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -447,13 +448,13 @@ private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThan KinematicsTester kinematicsTester1 = new KinematicsTester(); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-6)); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-4)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-5)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -461,14 +462,14 @@ private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-14)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToPositiveTrajectory1stPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -476,14 +477,14 @@ private static void createNegativeTrajectoryToPositiveTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(14)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThanAllJerkDistances2ndPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -491,14 +492,14 @@ private static void createPositiveTrajectoryToNegativeTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-4)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegaitveTrajectoryToPositiveTrajectory1stPointLessThanAllJerkDistances2ndPointLessThanDistanceCoveredWhileAccelerating() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -506,14 +507,14 @@ private static void createNegaitveTrajectoryToPositiveTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(4)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThanAllJerkDistances2ndPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -521,14 +522,14 @@ private static void createPositiveTrajectoryToPositiveTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } - + private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThanAllJerkDistances2ndPointLessThanAllJerkDistances() throws InvalidDimentionException, InvalidVelocityException, InvalidNextVelocityFromLastAcceleration, InvalidAccelerationException, InvalidFinalPosition, InvalidTrajectoryLogic, NaNException, @@ -536,13 +537,14 @@ private static void createNegativeTrajectoryToNegativeTrajectory1stPointLessThan Path myPath = m_kinematicsSimpler.new Path(); KinematicsTester kinematicsTester1 = new KinematicsTester(); - m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-2)); + m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(-1)); m_kinematicsSimpler.addPointToPath(myPath, m_kinematicsSimpler.new Point(0)); m_kinematicsSimpler.createTrajectory(myPath, 2.0, 0.5, 0.25); printTrajectory(myPath); checkTrajectoryPath(myPath, kinematicsTester1); } + /* * private static void * createNegativeTrajectoryToPositiveTrajectory2ndPointLessThanTwiceTheDistanceCoveredWhileAcceleratingCase @@ -1660,31 +1662,7 @@ private static void checkVelocity(Path Key, KinematicsTester kinematicsTester) t } for (int i = 0; i < originalTrajectoryPointsPath.size(); i++) { TrajectoryPoint originalTrajectoryPoint = originalTrajectoryPointsPath.get(i); - TrajectoryPoint deltaBeforeOriginalTrajectoryPoint = m_kinematicsSimpler - .getTrajectoryPointWithInterpolation(Key, - originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); - TrajectoryPoint deltaAfterOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation( - Key, originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); - double changeInPosition = deltaAfterOriginalTrajectoryPoint.m_position - - deltaBeforeOriginalTrajectoryPoint.m_position; - double changeInTime = m_deltaTimeFromOriginalPoint * 2; - double calculatedVelocity = changeInPosition / changeInTime; - if (calculatedVelocity == -716466.6666666666) { - System.out.println("changeInPosition: " + changeInPosition); - System.out.println("changeInTime: " + changeInTime); - System.out.println("deltaAfterOriginalTrajectoryPoint.m_position: " - + deltaAfterOriginalTrajectoryPoint.m_position); - System.out.println("originalTrajectoryPoint.m_position: " + originalTrajectoryPoint.m_position); - System.out.println("deltaBeforeOriginalTrajectoryPoint.m_position: " - + deltaBeforeOriginalTrajectoryPoint.m_position); - System.out.println("deltaBeforeOriginalTrajectoryPoint.m_timestamp: " - + deltaBeforeOriginalTrajectoryPoint.m_timestamp); - System.out.println("deltaAfterOriginalTrajectoryPoint.m_timestamp: " - + deltaAfterOriginalTrajectoryPoint.m_timestamp); - System.out.println("otherTime... " + (deltaAfterOriginalTrajectoryPoint.m_timestamp - - deltaBeforeOriginalTrajectoryPoint.m_timestamp)); - System.out.println(""); - } + double calculatedVelocity = getVelocityOfPoint(Key, originalTrajectoryPoint); velocityCalculatedFromPositionVector.add(calculatedVelocity); } @@ -1833,6 +1811,7 @@ private static void checkTrajectoryLogic(Path Key, KinematicsTester kinematicsTe throw invalidTrajectoryLogic; } } else { + if (Math.abs(trajectoryPoint.m_currentVelocity) > 0.1) { errMessage = "The point at time: " + trajectoryPoint.m_timestamp @@ -1840,38 +1819,53 @@ private static void checkTrajectoryLogic(Path Key, KinematicsTester kinematicsTe invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); throw invalidTrajectoryLogic; } + double calculatedVelocity = getVelocityOfPoint(Key, trajectoryPoint); + if (Math.abs(trajectoryPoint.m_currentVelocity - calculatedVelocity) > 0.1) { + errMessage = "The point at time: " + trajectoryPoint.m_timestamp + + " is a point where the calculated velocity and the current velocity are not equal"; + invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); + throw invalidTrajectoryLogic; + } + if (Math.abs(setpoint.vf - calculatedVelocity) > 0.1) { + errMessage = "The point at time: " + trajectoryPoint.m_timestamp + + " is a point where the vf and the calculated velocity are not equal"; + invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); + throw invalidTrajectoryLogic; + } + double calculatedAcceleration = getAccelerationOfPoint(Key, trajectoryPoint); - TrajectoryPoint nextPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, trajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); + TrajectoryPoint nextPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, + trajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); double nextAcceleration = getAccelerationOfPoint(Key, nextPoint); double currentJerk = getJerkOfPoint(Key, trajectoryPoint.m_timestamp); try { - if(Key.getSetpointVector().get(0).getm_X() == 14.0 && Key.getSetpointVector().get(0 - ).getm_X() == 6.0) { - System.out.println(""); - System.out.println("calculatedAcceleration: " + calculatedAcceleration); - System.out.println("setpoint.getaf(): " + setpoint.getaf()); - System.out.println("nextAcceleration: " + nextAcceleration); - System.out.println("trajectoryPoint.m_position: " + trajectoryPoint.m_position); - System.out.println("nextPoint.m_position: " + nextPoint.m_position); - System.out.println("currentJerk: " + currentJerk); - System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp); - } - }catch(ArrayIndexOutOfBoundsException a) { - + if (Key.getSetpointVector().get(0).getm_X() == 14.0 + && Key.getSetpointVector().get(0).getm_X() == 6.0) { + System.out.println(""); + System.out.println("calculatedAcceleration: " + calculatedAcceleration); + System.out.println("setpoint.getaf(): " + setpoint.getaf()); + System.out.println("nextAcceleration: " + nextAcceleration); + System.out.println("trajectoryPoint.m_position: " + trajectoryPoint.m_position); + System.out.println("nextPoint.m_position: " + nextPoint.m_position); + System.out.println("currentJerk: " + currentJerk); + System.out.println("trajectoryPoint.m_timestamp: " + trajectoryPoint.m_timestamp); + } + } catch (ArrayIndexOutOfBoundsException a) { + } - if(Math.abs(Math.abs(calculatedAcceleration) - setpoint.getaf()) > 0.0001) { + if (Math.abs(Math.abs(calculatedAcceleration) - setpoint.getaf()) > 0.0001) { errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point where the calculated acceleration and the final acceleration are not equal"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); throw invalidTrajectoryLogic; } - if(Math.abs(calculatedAcceleration - nextAcceleration) > 0.01) { + if (Math.abs(calculatedAcceleration - nextAcceleration) > 0.01) { errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point where setpoints are switching and the accelerations are messed up"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); throw invalidTrajectoryLogic; } - if(currentJerk > Key.getMaxJerk()) { + if (currentJerk > Key.getMaxJerk()) { errMessage = "The point at time: " + trajectoryPoint.m_timestamp + " is a point where the jerk exceeds the maximum jerk"; invalidTrajectoryLogic = kinematicsTester.new InvalidTrajectoryLogic(errMessage); @@ -1942,18 +1936,31 @@ public NaNException(String errMessage) { private static final long serialVersionUID = 1L; } - + + private static double getVelocityOfPoint(Path Key, TrajectoryPoint originalTrajectoryPoint) { + TrajectoryPoint deltaBeforeOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation( + Key, originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); + TrajectoryPoint deltaAfterOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, + originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); + double changeInPosition = deltaAfterOriginalTrajectoryPoint.m_position + - deltaBeforeOriginalTrajectoryPoint.m_position; + double changeInTime = m_deltaTimeFromOriginalPoint * 2; + double calculatedVelocity = changeInPosition / changeInTime; + + return calculatedVelocity; + } + private static double getAccelerationOfPoint(Path Key, double originalTrajectoryPointTime) { - TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0,0.0,originalTrajectoryPointTime); + TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0, 0.0, + originalTrajectoryPointTime); return getAccelerationOfPoint(Key, trajectoryPoint); } - + private static double getAccelerationOfPoint(Path Key, TrajectoryPoint originalTrajectoryPoint) { - TrajectoryPoint deltaBeforeOriginalTrajectoryPoint = m_kinematicsSimpler - .getTrajectoryPointWithInterpolation(Key, - originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); - TrajectoryPoint deltaAfterOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation( - Key, originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); + TrajectoryPoint deltaBeforeOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation( + Key, originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); + TrajectoryPoint deltaAfterOriginalTrajectoryPoint = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, + originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); double changeInVelocity = deltaAfterOriginalTrajectoryPoint.m_currentVelocity - deltaBeforeOriginalTrajectoryPoint.m_currentVelocity; double changeInTime = m_deltaTimeFromOriginalPoint * 2; @@ -1961,21 +1968,28 @@ private static double getAccelerationOfPoint(Path Key, TrajectoryPoint originalT return calculatedAcceleration; } + private static double getJerkOfPoint(Path Key, double originalTrajectoryPointTime, boolean testMode) { - TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0,0.0,originalTrajectoryPointTime); + TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0, 0.0, + originalTrajectoryPointTime); return getJerkOfPoint(Key, trajectoryPoint, testMode); } + private static double getJerkOfPoint(Path Key, double originalTrajectoryPointTime) { - TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0,0.0,originalTrajectoryPointTime); + TrajectoryPoint trajectoryPoint = m_kinematicsSimpler.new TrajectoryPoint(0.0, 0.0, + originalTrajectoryPointTime); return getJerkOfPoint(Key, trajectoryPoint); } + private static double getJerkOfPoint(Path Key, TrajectoryPoint originalTrajectoryPoint) { return getJerkOfPoint(Key, originalTrajectoryPoint, false); } - + private static double getJerkOfPoint(Path Key, TrajectoryPoint originalTrajectoryPoint, boolean testMode) { - double deltaBeforeAcceleration = getAccelerationOfPoint(Key,originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); - double deltaAfterAcceleration = getAccelerationOfPoint(Key, originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); + double deltaBeforeAcceleration = getAccelerationOfPoint(Key, + originalTrajectoryPoint.m_timestamp - m_deltaTimeFromOriginalPoint); + double deltaAfterAcceleration = getAccelerationOfPoint(Key, + originalTrajectoryPoint.m_timestamp + m_deltaTimeFromOriginalPoint); if (testMode) { System.out.println(""); System.out.println("deltaBeforeAcceleration: " + deltaBeforeAcceleration); @@ -2022,6 +2036,14 @@ private static void printTrajectory(Path Key) { } + TrajectoryPoint test = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key,15.650893188044908); + + + System.out.println("Key.getSetpointVector().get(0).getEndDeltaTime(): " + Key.getSetpointVector().get(0).getEndDeltaTime()); + System.out.println("Key.getSetpointVector().get(1).getEndDeltaTime(): " + Key.getSetpointVector().get(1).getEndDeltaTime()); + System.out.println("Added: " + (Key.getSetpointVector().get(0).getEndDeltaTime() + Key.getSetpointVector().get(1).getEndDeltaTime())); + System.out.println("test.m_position: " + test.m_position); + System.out.print("The Setpoints are: "); for (int i = 0; i < Key.getSetpointVector().size(); i++) { if (i == 0) { @@ -2030,17 +2052,17 @@ private static void printTrajectory(Path Key) { System.out.print(" " + Key.getSetpointVector().get(i).getm_X()); } System.out.println(" MaxV " + Key.getSetpointVector().get(i).getMaxVelocity()); - + } System.out.println(""); - /*for(int i = 0; i < 20; i++) { - TrajectoryPoint point = m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, 12.0 + i/20.0); - double acceleration = getAccelerationOfPoint(Key, point); - System.out.println(""); - System.out.println("The time is: " + (12.0 + i/20.0)); - System.out.println("acceleration: " + acceleration); - System.out.println(""); - }*/ + /* + * for(int i = 0; i < 20; i++) { TrajectoryPoint point = + * m_kinematicsSimpler.getTrajectoryPointWithInterpolation(Key, 12.0 + i/20.0); + * double acceleration = getAccelerationOfPoint(Key, point); + * System.out.println(""); System.out.println("The time is: " + (12.0 + + * i/20.0)); System.out.println("acceleration: " + acceleration); + * System.out.println(""); } + */ } diff --git a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java index 8d32b24..396e098 100644 --- a/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java +++ b/eclipse-workspace/MotionProfiling/src/kinematics/KinematicsSimpler.java @@ -136,8 +136,8 @@ public double getEndDeltaTime() { private double[] m_values; private int m_dim; - double vi = 0.0; - double vf = 0.0; + public double vi = 0.0; + public double vf = 0.0; double ai = 0.0; double af = 0.0; @@ -297,22 +297,57 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio * time and has been rearranged to solve for the time in which it will take to * reach the theoretical max velocity */ + double justJerkDistanceCovered = 0.0; + if (setpoint.vi == 0.0 && setpoint.ai != 0) { + double velocityCoveredDuringBothJerkTimes = Math.pow(setpoint.ai, 2) / (2 * Key.maxJerk); + double timeCoveredBetweenAiand0 = setpoint.ai / Key.maxJerk; + double finalMaxAcceleration = Math.sqrt(2 * Key.maxJerk * (velocityCoveredDuringBothJerkTimes / 2)); + double finalJerkTime = finalMaxAcceleration / Key.maxJerk; + double finalFirstJerkInitialVelocity = setpoint.ai / 2 * timeCoveredBetweenAiand0; + double finalSecondJerkInitialVelocity = finalFirstJerkInitialVelocity + + finalMaxAcceleration / 2 * finalJerkTime; + justJerkDistanceCovered = 0.5 * setpoint.ai * Math.pow(timeCoveredBetweenAiand0, 2) + - (1.0 / 6.0) * Key.maxJerk * Math.pow(timeCoveredBetweenAiand0, 3) + + finalFirstJerkInitialVelocity * finalJerkTime + - (1.0 / 6.0) * Key.maxJerk * Math.pow(finalJerkTime, 3) + + finalSecondJerkInitialVelocity * finalJerkTime + - 0.5 * finalMaxAcceleration * Math.pow(finalJerkTime, 2) + + (1.0 / 6.0) * Key.maxJerk * Math.pow(finalJerkTime, 3); + try { + if (Key.setpointVector.get(0).m_x == 14.0 && Key.setpointVector.get(0).m_x == 13.0 + && setpoint.m_x == 13.0) { + System.out.println(""); + System.out.println("distance: " + distance); + System.out.println("initialFirstJerkDistanceCovered: " + initialFirstJerkDistanceCovered); + System.out.println("finalSecondJerkDistanceCovered: " + finalSecondJerkDistanceCovered); + System.out.println("justJerkDistanceCovered: " + justJerkDistanceCovered); + System.out.println("finalJerkTime: " + finalJerkTime); + System.out.println("finalMaxAcceleration: " + finalMaxAcceleration); + System.out.println("velocityCoveredDuringBothJerkTimes: " + velocityCoveredDuringBothJerkTimes); + System.out.println("setpoint.ai: " + setpoint.ai); + System.out.println(""); + } + } catch (ArrayIndexOutOfBoundsException a) { - double initialmaxAccelerationInitialVelocity = setpoint.vi + setpoint.ai * initialFirstJerkTimeCovered - + 0.5 * Key.maxJerk * Math.pow(initialFirstJerkTimeCovered, 2); - double initialSecondJerkDistanceCoveredNoMaxAccelerationCrusing = initialmaxAccelerationInitialVelocity - * jerkTime + 0.5 * setpoint.maxAcceleration * jerkTime - + (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); + } - double finalmaxAccelerationFinalVelocity = setpoint.vf + setpoint.af * finalSecondJerkTimeCovered - + 0.5 * Key.maxJerk * Math.pow(finalSecondJerkTimeCovered, 2); - double finalFirstJerkDistanceCoveredNoMaxAccelerationCruising = finalmaxAccelerationFinalVelocity * jerkTime - + 0.5 * setpoint.maxAcceleration * jerkTime + (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); + } else { + double initialmaxAccelerationInitialVelocity = setpoint.vi + setpoint.ai * initialFirstJerkTimeCovered + + 0.5 * Key.maxJerk * Math.pow(initialFirstJerkTimeCovered, 2); + double initialSecondJerkDistanceCoveredNoMaxAccelerationCruising = initialmaxAccelerationInitialVelocity + * jerkTime + 0.5 * setpoint.maxAcceleration * jerkTime + - (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); - double justJerkDistanceCovered = initialFirstJerkDistanceCovered - + initialSecondJerkDistanceCoveredNoMaxAccelerationCrusing - + finalFirstJerkDistanceCoveredNoMaxAccelerationCruising + finalSecondJerkDistanceCovered; + double finalMaxAccelerationFinalVelocity = setpoint.vf + setpoint.af * finalSecondJerkTimeCovered + + 0.5 * Key.maxJerk * Math.pow(finalSecondJerkTimeCovered, 2); + double finalFirstJerkDistanceCoveredNoMaxAccelerationCruising = finalMaxAccelerationFinalVelocity + * jerkTime + 0.5 * setpoint.maxAcceleration * jerkTime + - (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); + justJerkDistanceCovered = initialFirstJerkDistanceCovered + + initialSecondJerkDistanceCoveredNoMaxAccelerationCruising + + finalFirstJerkDistanceCoveredNoMaxAccelerationCruising + finalSecondJerkDistanceCovered; + } /* * If the theoretical max velocity is impossible to reach due to design * constraints or user constraints that limit max velocity find when the max @@ -323,22 +358,6 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio * cruising to the halfway time which is when the maximum theoretical velocity * will be reached and pretend that you are cruising for 0 seconds */ - try { - if (Key.setpointVector.get(0).m_x == 14.0 && Key.setpointVector.get(0).m_x == 6.0 - && setpoint.m_x == 6.0) { - System.out.println(""); - System.out.println("distance: " + distance); - System.out.println("initialAccelerationDistanceCovered: " + initialAccelerationDistanceCovered); - System.out.println("finalAccelerationDistanceCovered: " + finalAccelerationDistanceCovered); - System.out.println("Total distance coverd while accelerating and deccelerating: " - + (initialAccelerationDistanceCovered + finalAccelerationDistanceCovered)); - System.out.println("justJerkDistanceCovered: " + justJerkDistanceCovered); - System.out.println("initialFirstJerkTimeCovered: " + initialFirstJerkTimeCovered); - System.out.println(""); - } - } catch (ArrayIndexOutOfBoundsException a) { - - } if (initialAccelerationDistanceCovered + finalAccelerationDistanceCovered <= distance) { double initialVelocityCoveredAtMaxAcceleration = setpoint.maxVelocity - jerkVelocityCovered @@ -459,9 +478,12 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio - Math.pow((setpoint.vi + initialFirstJerkVelocityCovered), 2) / (2 * setpoint.maxAcceleration) + Math.pow((setpoint.vf + finalSecondJerkVelocityCovered), 2) / (2 * setpoint.maxAcceleration)); - double initialAccelerationTime = (-bTerm + Math.sqrt(Math.pow(bTerm, 2) - (4 * aTerm * cTerm))) / (2 * aTerm); + double initialAccelerationTime = (-bTerm + Math.sqrt(Math.pow(bTerm, 2) - (4 * aTerm * cTerm))) + / (2 * aTerm); double accelerationVelocityCovered = Key.maxAcceleration * initialAccelerationTime; - double finalAccelerationTime = ((setpoint.vi + initialFirstJerkVelocityCovered + accelerationVelocityCovered) - (setpoint.vf + finalSecondJerkVelocityCovered))/setpoint.maxAcceleration; + double finalAccelerationTime = ((setpoint.vi + initialFirstJerkVelocityCovered + + accelerationVelocityCovered) - (setpoint.vf + finalSecondJerkVelocityCovered)) + / setpoint.maxAcceleration; setpoint.firstEndAccelerationCruisingDeltaTime = setpoint.firstStartAccelerationCruisingDeltaTime + initialAccelerationTime; setpoint.startVelocityCruisingDeltaTime = setpoint.firstEndAccelerationCruisingDeltaTime + jerkTime; @@ -474,16 +496,18 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio setpoint.endDeltaTime = setpoint.secondEndAccelerationCruisingDeltaTime + finalSecondJerkTimeCovered; double initialMaxAccelerationDistanceCovered = ((setpoint.vi + initialFirstJerkVelocityCovered) - * initialAccelerationTime + 0.5 * setpoint.maxAcceleration * Math.pow(initialAccelerationTime, 2)); + * initialAccelerationTime + + 0.5 * setpoint.maxAcceleration * Math.pow(initialAccelerationTime, 2)); double endMaxAccelerationVelocity = (setpoint.vi + initialFirstJerkVelocityCovered + setpoint.maxAcceleration * initialAccelerationTime); double initialSecondJerkDistanceCoveredAccountingForAcceleration = endMaxAccelerationVelocity * jerkTime + 0.5 * setpoint.maxAcceleration * Math.pow(jerkTime, 2) - (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); - double finalFirstJerkDistanceCoveredAccountingForAcceleration = setpoint.maxVelocity*jerkTime -(1.0/6.0)*Key.maxJerk*Math.pow(jerkTime, 3); - // I HAVE TO COMMENT THIS UP EXPLAINGING WHAT IS HAPPENING!!!!!!!! - if (Key.setpointVector.get(0).m_x == 14.0 && Key.setpointVector.get(1).m_x == - 6.0 && setpoint.m_x == 6.0) { + double finalFirstJerkDistanceCoveredAccountingForAcceleration = setpoint.maxVelocity * jerkTime + - (1.0 / 6.0) * Key.maxJerk * Math.pow(jerkTime, 3); + + if (Key.setpointVector.get(0).m_x == 14.0 && Key.setpointVector.get(0).m_x == 13.0 + && setpoint.m_x == 13.0) { System.out.println(""); System.out.println("aTerm: " + aTerm); System.out.println("bTerm: " + bTerm); @@ -520,42 +544,124 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio System.out.println("Max Velocity Distance Covered: " + (initialFirstJerkDistanceCovered + initialMaxAccelerationDistanceCovered + initialSecondJerkDistanceCoveredAccountingForAcceleration)); - System.out.println("finalFirstJerkDistanceCoveredAccountingForAcceleration: " + finalFirstJerkDistanceCoveredAccountingForAcceleration); + System.out.println("finalFirstJerkDistanceCoveredAccountingForAcceleration: " + + finalFirstJerkDistanceCoveredAccountingForAcceleration); + System.out.println(""); + System.out.println("InitialAccelerationVelocityCovered: " + + (jerkVelocityCovered + initialAccelerationTime * setpoint.maxAcceleration)); + System.out.println("FinalAccelerationVelocityCovered: " + + (jerkVelocityCovered * 2 + finalAccelerationTime * setpoint.maxAcceleration)); System.out.println(""); } } else { - double maxVelocityDistance = Math.abs(distance / 2); + + double timeTakenToAccelerateUpToAi = setpoint.ai / Key.maxJerk; + double timeTakenToAccelerateUpToAf = setpoint.af / Key.maxJerk; + + double accelertationUpToAiDistanceCovered = (1.0 / 6.0) * Key.maxJerk + * Math.pow(timeTakenToAccelerateUpToAi, 3); + double accelerationUpToAfDistanceCovered = (1.0 / 6.0) * Key.maxJerk + * Math.pow(timeTakenToAccelerateUpToAf, 3); + + double maxVelocityDistance = (Math.abs(distance) + accelertationUpToAiDistanceCovered + + accelerationUpToAfDistanceCovered) / 2; jerkTime = Math.cbrt(maxVelocityDistance / Key.maxJerk); /* - * 1/6*J*T^3 + vi*T + 1/2*A*T^2 + 1/6*-J*T^3 = distance + * Not hitting Max Acceleration: Time taken to accelerate up to ai: ai/J + * + * Time taken to accelerate up to af: af/J + * + * Trying to find jt: * - * vi*T + 1/2*A*T^2 = distance + * Distance = vi*(jt - ai/J)+ 1/2*ai*(jt - ai/J)^2 + 1/6*J*(jt - ai/J)^3 + + * 2*((vi + ai*(jt-ai/J) + 1/2*J*(jt-ai/J)^2)*jt + 1/2*(ai + J*(jt-ai/J)) - + * 1/6*J*jt^3) + vf*(jt - af/J) + 1/2*af*(jt - af/J) + 1/6*J*(jt - af/J) * - * vi = 1/2*J*T^2 + * (jt - ai/J)^3 * - * A = J*T + * (jt^2 - 2*jt*ai/J + (ai/J)^2)(jt - ai/J) * - * 1/2*J*T^2 * T + 1/2*J*T*T^2 = distance + * jt^3 - ai/J*jt^2 - 2*jt^2*ai/J + 2*jt*(ai/J)^2 + (ai/J)^2*jt - (ai/J)^3 * - * 1/2*J*T^3 + 1/2*J*T^3 =distance + * jt^3 - (ai/J)^3 - 3*jt^2*ai/J + 3*jt*(ai/J)^2 * - * 1/2*J*T^3 = distance/2 + * Distance = vi*jt - vi*ai/J + 1/2*ai*(jt^2 -2*jt*ai/J + (ai/J)^2) + + * 1/6*J*(jt^3 - (ai/J)^3 - 3*jt^2*ai/J + 3*jt*(ai/J)^2) + 2*(vi*jt + + * jt*ai*(jt-ai/J) + jt*1/2*J*(jt^2 -2*jt*ai/J + (ai/J)^2)) + (ai + J*(jt-ai/J)) + * -1/3*J*jt^3 + jt*vf - vf*af/J + 1/2*af*jt - 1/2*af*af/J + 1/6*J*jt - 1/6*J + * *af/J * - * J*T^3 = distance/2 * 2 + * Distance = vi*jt - vi*ai/J + 1/2*ai*jt^2 - jt*ai/J*ai + 1/2*ai*(ai/J)^2 + + * 1/6*J*jt^3 - 1/6*J*(ai/J)^3 - 1/2*J*jt^2*ai/J + 1/2*J*jt*(ai/J)^2 + 2*vi*jt + + * 2*jt*ai*(jt-ai/J) + jt*J*(jt^2 -2*jt*ai/J + (ai/J)^2) + ai + J*jt - J*ai/J + * -1/3*J*jt^3 + jt*vf - vf*af/J + 1/2*af*jt - 1/2*af*af/J + 1/6*J*jt - 1/6*J + * *af * - * J*T^3 = distance + * Distance = vi*jt - vi*ai/J + 1/2*ai*jt^2 - jt*ai/J*ai + 1/2*ai*(ai/J)^2 + + * 1/6*J*jt^3 - 1/6*J*(ai/J)^3 - 1/2*J*jt^2*ai/J + 1/2*J*jt*(ai/J)^2 + 2*vi*jt + + * 2*jt*ai*(jt-ai/J) + jt^3*J - 2*jt^2*J*ai/J + jt*J*(ai/J)^2 + jt*J*(ai/J)^2 + + * ai + J*jt - J*ai/J -1/3*J*jt^3 + jt*vf - vf*af/J + 1/2*af*jt - 1/2*af*af/J + + * 1/6*J*jt - 1/6*J *af * - * T^3 = distance/J + * Distance + vi*ai/J - 1/2*ai*(ai/J)^2 + 1/6*J*(ai/J)^3 - ai + J*ai/J + vj*af/J + * + 1/2*af*af/J + 1/6*J *af = vi*jt + 1/2*ai*jt^2 - jt*ai/J*ai + 1/6*J*jt^3 - + * 1/2*J*jt^2*ai/J + 1/2*J*jt*(ai/J)^2 + 2*vi*jt + 2*jt*ai*(jt-ai/J) + jt^3*J - + * 2*jt^2*J*ai/J + jt*J*(ai/J)^2 + jt*J*(ai/J)^2 + J*jt -1/3*J*jt^3 + jt*vf + + * 1/2*af*jt + 1/6*J*jt * - * T = cbrt(distance/J) + * newDistance = vi*jt + 1/2*ai*jt^2 - jt*ai/J*ai + 1/6*J*jt^3 - 1/2*J*jt^2*ai/J + * + 1/2*J*jt*(ai/J)^2 + 2*vi*jt + 2*jt*ai*(jt-ai/J) + jt^3*J - 2*jt^2*J*ai/J + + * jt*J*(ai/J)^2 + jt*J*(ai/J)^2 + J*jt -1/3*J*jt^3 + jt*vf + 1/2*af*jt + + * 1/6*J*jt + * + * newDistance = vi*jt + 1/2*ai*jt^2 - jt*ai/J*ai + 1/6*J*jt^3 - 1/2*J*jt^2*ai/J + * + 1/2*J*jt*(ai/J)^2 + 2*vi*jt + 2*jt^2*ai - 2*jt*ai*ai/J + jt^3*J - + * 2*jt^2*J*ai/J + jt*J*(ai/J)^2 + jt*J*(ai/J)^2 + J*jt -1/3*J*jt^3 + jt*vf + + * 1/2*af*jt + 1/6*J*jt + * + * newDistance = 1/6*J*jt^3 + jt^3*J -1/3*J*jt^3 + 1/2*ai*jt^2 - 1/2*J*jt^2*ai/J + * + 2*jt^2*ai - 2*jt^2*J*ai/J + vi*jt - jt*ai/J*ai + 1/2*J*jt*(ai/J)^2 + + * 2*vi*jt - 2*jt*ai*ai/J + jt*J*(ai/J)^2 + jt*J*(ai/J)^2 + J*jt + jt*vf + + * 1/2*af*jt + 1/6*J*jt + * + * newDistance = (1/6*J + J - 1/3*J)*jt^3 + (1/2*ai - 1/2*J*ai/J + 2*ai - + * 2*J*ai/J)*jt^2 + (vi - ai*ai/J + 1/2*J*(ai/J)^2 + 2*vi - 2*jt*ai*ai/J + * +J*(ai/J)^2 + J*(ai/J)^2 + J + vf + 1/2*af + 1/6*J)*jt + * + * A term: (1/6*J + J - 1/3*J) B term: (1/2*ai - 1/2*J*ai/J + 2*ai - 2*J*ai/J) = + * (1/2*ai - 1/2*ai + 2*ai - 2*ai) C term: (vi - ai*ai/J + 1/2*J*(ai/J)^2 + 2*vi + * - 2*jt*ai*ai/J +J*(ai/J)^2 + J*(ai/J)^2 + J + vf + 1/2*af + 1/6*J) D term: + * -1*(Distance + vi*ai/J - 1/2*ai*(ai/J)^2 + 1/6*J*(ai/J)^3 - ai + J*ai/J + + * vf*af/J + 1/2*af*af/J + 1/6*J *af) + * + * A term: (J*5/6) B term: 0 C term: (3*vi - ai*ai/J + 1/2*J*(ai/J)^2 - + * 2*jt*ai*ai/J +J*(ai/J)^2 + J*(ai/J)^2 + J + vf + 1/2*af + 1/6*J) D term: + * -1*(Distance + vi*ai/J - 1/2*ai*(ai/J)^2 + 1/6*J*(ai/J)^3 - ai + J*ai/J + + * vf*af/J + 1/2*af*af/J + 1/6*J *af) + * + * double aTerm = (5.0 / 6.0) * Key.maxJerk; double bTerm = 0.0; double cTerm = + * 3 * setpoint.vi - Math.pow(setpoint.ai, 2) / Key.maxJerk + 0.5 * Key.maxJerk + * * Math.pow((setpoint.ai / Key.maxJerk), 2) - 2 * Math.pow(setpoint.ai, 2) / + * Key.maxJerk + 2 * Key.maxJerk * Math.pow((setpoint.ai / Key.maxJerk), 2) + + * Key.maxJerk + setpoint.vf + 0.5 * setpoint.af + (1.0 / 6.0) * Key.maxJerk; + * double dTerm = -1 * (distance + setpoint.vi * setpoint.ai / Key.maxJerk - 0.5 + * * Math.pow(setpoint.ai, 3) / Math.pow(Key.maxJerk, 2) + (1.0 / 6.0) * + * Key.maxJerk * Math.pow((setpoint.ai / Key.maxJerk), 3) + setpoint.vf * + * setpoint.af / Key.maxJerk + 0.5 * Math.pow(setpoint.af, 2) / Key.maxJerk + + * (1.0 / 6.0) * Key.maxJerk * setpoint.af); */ - setpoint.firstStartAccelerationCruisingDeltaTime = jerkTime; + + // What if instead I used the same formula from before and added in the extra + // distance to make it work... + + setpoint.firstStartAccelerationCruisingDeltaTime = jerkTime - setpoint.ai / Key.maxJerk; setpoint.firstEndAccelerationCruisingDeltaTime = setpoint.firstStartAccelerationCruisingDeltaTime; setpoint.startVelocityCruisingDeltaTime = setpoint.firstStartAccelerationCruisingDeltaTime + jerkTime; setpoint.endVelocityCruisingDeltaTime = setpoint.startVelocityCruisingDeltaTime; setpoint.secondStartAccelerationCruisingDeltaTime = setpoint.endVelocityCruisingDeltaTime + jerkTime; setpoint.secondEndAccelerationCruisingDeltaTime = setpoint.secondStartAccelerationCruisingDeltaTime; - setpoint.endDeltaTime = setpoint.secondEndAccelerationCruisingDeltaTime + jerkTime; + setpoint.endDeltaTime = setpoint.secondEndAccelerationCruisingDeltaTime + jerkTime + - setpoint.af / Key.maxJerk; double velocityCoveredByNewJerkTime = 0.5 * Key.maxJerk * Math.pow(jerkTime, 2); setpoint.maxVelocity = 2 * velocityCoveredByNewJerkTime; setpoint.maxAcceleration = Key.maxJerk * jerkTime; @@ -567,8 +673,9 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio + 0.5 * jerkAcceleration * Math.pow(jerkTime, 2) + (1.0 / 6.0) * (-1 * Key.maxJerk) * Math.pow(jerkTime, 3); - if (setpoint.m_x == 0.0) { - + if (Key.getSetpointVector().get(0).m_x == 14.0 && Key.getSetpointVector().get(1).m_x == 13.0 + && setpoint.m_x == 13.0) { + System.out.println(""); System.out.println("setpoint.firstStartAccelerationCruisingDeltaTime: " + setpoint.firstStartAccelerationCruisingDeltaTime); System.out.println("setpoint.firstEndAccelerationCruisingDeltaTime: " @@ -583,6 +690,7 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio + setpoint.secondEndAccelerationCruisingDeltaTime); System.out.println("setpoint.endDeltaTime: " + setpoint.endDeltaTime); System.out.println(""); + System.out.println("justJerkDistanceCovered: " + justJerkDistanceCovered); System.out.println("jerkTime: " + jerkTime); System.out.println("jerkVi: " + jerkVi); System.out.println("jerkAcceleration: " + jerkAcceleration); @@ -590,7 +698,14 @@ public void createTrajectory(Path Key, double maxVelocity, double maxAcceleratio System.out.println("distanceCoveredDuringSecondJerkTime: " + distanceCoveredDuringSecondJerkTime); System.out.println("Total Distance: " + (distanceCoveredDuringFirstJerkTime + distanceCoveredDuringSecondJerkTime)); + System.out.println(""); + + System.out.println("initialFirstJerkDistanceCovered: " + initialFirstJerkDistanceCovered); + System.out.println("finalSecondJerkDistanceCovered: " + finalSecondJerkDistanceCovered); + System.out.println("justJerkDistanceCovered: " + justJerkDistanceCovered); + System.out.println(""); } + } // Needs to do this so that the last time through the code the max velocity is @@ -869,21 +984,106 @@ private void getVf_Vi_Ai_Af(Vector setpointVector, Path Key) { } else { setpoint.vf = 0.0; - double jerkTime = Key.maxAcceleration / Key.maxJerk; - double jerkDistanceCovered = (1.0 / 6.0) * Key.maxJerk * jerkTime; - Vector finalAccelerationDeterminingSetpoints = new Vector(); - - for (int i = i1 + 1; i < Key.setpointVector.size(); i++) { - Point possibleFinalAccelerationDeterminingSetpoint = Key.getSetpointVector().get(i); - double distance = Math.abs(setpoint.m_x - possibleFinalAccelerationDeterminingSetpoint.m_x); - if (distance >= jerkDistanceCovered) { - break; - } else { - finalAccelerationDeterminingSetpoints.add(possibleFinalAccelerationDeterminingSetpoint); + setpoint.af = Key.maxAcceleration; + double distance = Math.abs(nextSetpoint.m_x - setpoint.m_x); + double velocityCoveredWhileDecceleratingFromMaxAccelertion = Math.pow(Key.maxAcceleration, 2) + / (2 * Key.maxJerk); + double timeCoveredWhileMaxAccelerationIsDecceleratingTo0 = Key.maxAcceleration / Key.maxJerk; + double finalEachJerkVelocityCovered = velocityCoveredWhileDecceleratingFromMaxAccelertion / 2; + double finalJerkMaxAcceleration = Math.sqrt(2 * Key.maxJerk * finalEachJerkVelocityCovered); + double finalJerkTime = finalJerkMaxAcceleration / Key.maxJerk; + + double initialJerkDistanceCovered = 0.5 * Key.maxAcceleration + * Math.pow(timeCoveredWhileMaxAccelerationIsDecceleratingTo0, 2) + - (1.0 / 6.0) * Key.maxJerk * Math.pow(timeCoveredWhileMaxAccelerationIsDecceleratingTo0, 3); + double finalFirstJerkDistanceCovered = velocityCoveredWhileDecceleratingFromMaxAccelertion + * finalJerkTime - (1.0 / 6.0) * Key.maxJerk * Math.pow(finalJerkTime, 3); + double finalSecondJerkDistanceCovered = (velocityCoveredWhileDecceleratingFromMaxAccelertion + - finalEachJerkVelocityCovered) * finalJerkTime + - 0.5 * finalJerkMaxAcceleration * Math.pow(finalJerkTime, 2) + + (1.0 / 6.0) * Key.maxJerk * Math.pow(finalJerkTime, 3); + + double distanceCoveredWhileAccelerationIsChangingKeepingViAndVf0 = initialJerkDistanceCovered + + finalFirstJerkDistanceCovered + finalSecondJerkDistanceCovered; + if (distanceCoveredWhileAccelerationIsChangingKeepingViAndVf0 <= distance) { + setpoint.af = Key.maxAcceleration; + } else { + double aTerm = (1.0 / 6.0 * Key.maxJerk); + double bTerm = 0.0; + double cTerm = Key.maxAcceleration / (2 * Key.maxJerk); + double dTerm = -1 * Math.abs(distanceCoveredWhileAccelerationIsChangingKeepingViAndVf0 - distance); + + double timeBeforeEndDeltaTimeForAccelerationToStartChanging = solveCubicEquation(aTerm, bTerm, + cTerm, dTerm); + + double tempAf = Key.maxAcceleration + - Key.maxJerk / timeBeforeEndDeltaTimeForAccelerationToStartChanging; + /* + * Distance = 1/2*ai*(iT + fT)^2 - 1/6*J*(iT + fT)^3 + (ai*(iT + fT) - 1/2*J*(iT + * + fT)^2)*fT + 1/2*(ai - J*(iT + fT))*fT^2 + 1/6*J*fT^3 + * + * iT = ai/J VelocityCoveredWhileAccelerationIsDeccelerating = ai/2*ai/J = + * ai^2/(2*J) FinalTimeVelocityCovered = ai^2/(4*J) fT = sqrt(ai^2/(4*J)*2 *J) = + * sqrt(ai^2/2) = ai * sqrt(2) iT + fT = ai/J + ai * sqrt(2) = ai*(1/J + + * sqrt(2)) + * + * Distance = 1/2*ai*(ai*(1/J + sqrt(2)))^2 - 1/6*J*(ai*(1/J + sqrt(2)))^3 + + * (ai*(ai*(1/J + sqrt(2))) - 1/2*J*(ai*(1/J + sqrt(2)))^2)*ai * sqrt(2) + + * 1/2*(ai - J*(ai*(1/J + sqrt(2))))*(ai * sqrt(2))^2 + 1/6*J*(ai * sqrt(2))^3 + * + * Distance = 1/2*ai*ai^2*(1/J + sqrt(2))^2 - 1/6*J*ai^3*(1/J + sqrt(2))^3 + + * (ai^2*(1/J + sqrt(2)) - 1/2*J*ai^2*(1/J + sqrt(2))^2)*ai * sqrt(2) + 1/2*(ai + * - J*(ai*(1/J + sqrt(2))))*(ai * sqrt(2))^2 + 1/6*J*ai^3 * sqrt(2)^3 + * + * + * 1/2*(ai - J*ai*(1/J + sqrt(2)))*ai^2 * sqrt(2)^2 (ai - J*ai*(1/J + + * sqrt(2)))*ai^2 ai^3 - J*ai^3*(1/J + sqrt(2)) + * + * Distance = 1/2*ai*ai^2*(1/J + sqrt(2))^2 - 1/6*J*ai^3*(1/J + sqrt(2))^3 + + * (ai^2*(1/J + sqrt(2)) - 1/2*J*ai^2*(1/J + sqrt(2))^2)*ai * sqrt(2) +ai^3 - + * J*ai^3*(1/J + sqrt(2)) + 1/6*J*ai^3 * sqrt(2)^3 + * + * Distance = 1/2*ai^3*(1/J + sqrt(2))^2 - 1/6*J*ai^3*(1/J + sqrt(2))^3 + + * ai^3*sqrt(2)*(1/J + sqrt(2)) - 1/2*J*ai^3*sqrt(2)*(1/J + sqrt(2))^2 +ai^3 - + * J*ai^3*(1/J + sqrt(2)) + 1/6*J*ai^3 * sqrt(2)^3 + * + * Distance = 1/2*(1/J + sqrt(2))^2*ai^3 - 1/6*J*(1/J + sqrt(2))^3*ai^3 + + * sqrt(2)*(1/J + sqrt(2))*ai^3 - 1/2*J*sqrt(2)*(1/J + sqrt(2))^2*ai^3 + ai^3 - + * J*(1/J + sqrt(2))*ai^3 + 1/6*J*sqrt(2)^3*ai^3 + * + * Distance = (1/2*(1/J + sqrt(2))^2 -1/6*J*(1/J + sqrt(2))^3 + sqrt(2)*(1/J + + * sqrt(2)) - 1/2*J*sqrt(2)*(1/J + sqrt(2))^2 + 1 - J*(1/J + sqrt(2)) + + * 1/6*J*sqrt(2)^3)*ai^3 + * + * Distance/(1/2*(1/J + sqrt(2))^2 -1/6*J*(1/J + sqrt(2))^3 + sqrt(2)*(1/J + + * sqrt(2)) - 1/2*J*sqrt(2)*(1/J + sqrt(2))^2 + 1 - J*(1/J + sqrt(2)) + + * 1/6*J*sqrt(2)^3) = ai^3 + * + * cbrt(Distance/(1/2*(1/J + sqrt(2))^2 -1/6*J*(1/J + sqrt(2))^3 + sqrt(2)*(1/J + * + sqrt(2)) - 1/2*J*sqrt(2)*(1/J + sqrt(2))^2 + 1 - J*(1/J + sqrt(2)) + + * 1/6*J*sqrt(2)^3)) = ai + */ + tempAf = Math.cbrt(distance / (0.5 * Math.pow((1.0 / Key.maxJerk) + Math.sqrt(2), 2) + - (1.0 / 6) * Key.maxJerk * Math.pow(((1.0 / Key.maxJerk) + Math.sqrt(2)), 3) + + Math.sqrt(2) * ((1.0 / Key.maxJerk) + Math.sqrt(2)) + - (1.0 / 2) * Key.maxJerk * Math.sqrt(2) * Math.pow(((1.0 / Key.maxJerk) + Math.sqrt(2)), 2) + + 1 - Key.maxJerk * ((1.0 / Key.maxJerk) + Math.sqrt(2)) + + (1.0 / 6) * Key.maxJerk * Math.pow(Math.sqrt(2), 3))); + + if (tempAf < setpoint.af) { + setpoint.af = tempAf; + } + if (Key.getSetpointVector().get(0).m_x == 14.0 && Key.getSetpointVector().get(1).m_x == 13.0) { + System.out.println(""); + System.out.println("Key.maxVelocity: " + Key.maxAcceleration); + + System.out.println("distanceCoveredWhileAccelerationIsChangingKeepingViAndVf0: " + + distanceCoveredWhileAccelerationIsChangingKeepingViAndVf0); + System.out.println("tempAf: " + tempAf); + System.out.println(""); } } - setpoint.af = Key.maxAcceleration; - // Calculate all the new af from the af possible dtermining setpoints above + } } @@ -903,24 +1103,6 @@ public double getDistanceTraveledWhileAccelerating(double vi, double vf, double * This method takes a delta time from the beginning of a trajectory path and * returns the point where that time is */ - public TrajectoryPoint getTrajectoryPoint(Vector trajectoryPath, double deltaTime) { - // Creates a point for the point being returned to be stored in - TrajectoryPoint point = new TrajectoryPoint(); - - // This for loop goes through the trajectory vector called trajectoryPath - for (int i = 0; i < trajectoryPath.size(); i++) { - - // Assigns the point to i - point = trajectoryPath.get(i); - - // If this point is on or past the deltaTime it is the point being looked for - // and return that point - if (point.m_timestamp >= deltaTime) { - return point; - } - } - return point; - } public TrajectoryPoint getTrajectoryPointWithInterpolation(Path Key, double deltaTimeFromStartOfSetpointVector) { return getTrajectoryPointWithInterpolation(Key, deltaTimeFromStartOfSetpointVector, false); @@ -1174,7 +1356,7 @@ private double getPosition(Point setpoint, double deltaTime, double maxJerk, dou System.out.println("velocityAtFirstEndAccelerationCruisingDeltaTime: " + velocityAtFirstEndAccelerationCruisingDeltaTime); System.out.println("maxAcceleration: " + maxAcceleration); - + System.out.println(""); } } catch (ArrayIndexOutOfBoundsException a) { @@ -1201,7 +1383,8 @@ private double getPosition(Point setpoint, double deltaTime, double maxJerk, dou System.out.println("time: " + time); System.out.println("maxAcceleration: " + maxAcceleration); System.out.println("startAccelerationVi * time: " + (startAccelerationVi * time)); - System.out.println("0.5 * maxAcceleration * Math.pow(time, 2): " + 0.5 * maxAcceleration * Math.pow(time, 2)); + System.out.println( + "0.5 * maxAcceleration * Math.pow(time, 2): " + 0.5 * maxAcceleration * Math.pow(time, 2)); System.out.println(""); } } catch (ArrayIndexOutOfBoundsException a) { @@ -1232,4 +1415,24 @@ private double getPosition(Point setpoint, double deltaTime, double maxJerk, dou return position; } + // Equation used can be found at + // https://math.vanderbilt.edu/schectex/courses/cubic/ + private double solveCubicEquation(double aTerm, double bTerm, double cTerm, double dTerm) { + double x = Math + .cbrt(((-Math.pow(bTerm, 3)) / (27 * Math.pow(aTerm, 3)) + (bTerm * cTerm) / (6 * Math.pow(aTerm, 2)) + - dTerm / (2 * aTerm)) + + Math.sqrt(Math + .pow((-Math.pow(bTerm, 3) / (27 * Math.pow(aTerm, 3)) + + (bTerm * cTerm) / (6 * Math.pow(aTerm, 2)) - dTerm / (2 * aTerm)), 2) + + Math.pow((cTerm / (3 * aTerm) - Math.pow(bTerm, 2) / (9 * Math.pow(aTerm, 2))), 3))) + + Math.cbrt(((-Math.pow(bTerm, 3)) / (27 * Math.pow(aTerm, 3)) + + (bTerm * cTerm) / (6 * Math.pow(aTerm, 2)) - dTerm / (2 * aTerm)) + - Math.sqrt(Math + .pow((-Math.pow(bTerm, 3) / (27 * Math.pow(aTerm, 3)) + + (bTerm * cTerm) / (6 * Math.pow(aTerm, 2)) - dTerm / (2 * aTerm)), 2) + + Math.pow((cTerm / (3 * aTerm) - Math.pow(bTerm, 2) / (9 * Math.pow(aTerm, 2))), 3))) + - bTerm / (3 * aTerm); + return x; + } + } diff --git a/eclipse-workspace/Vision.yaml b/eclipse-workspace/Vision.yaml new file mode 100644 index 0000000..4a7476f --- /dev/null +++ b/eclipse-workspace/Vision.yaml @@ -0,0 +1,135 @@ +Version 2.0 + +--- +Children: +- Children: + - Children: + - Children: [] + Properties: + CAN ID: !!robotbuilder.data.properties.ChoicesProperty + choices: &id001 ['0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '10', + '11', '12', '13', '14', '15', '16', '17', '18', '19', '20'] + default: '2' + name: CAN ID + validators: &id002 [CANID] + value: '1' + Base: CAN Talon + Name: FrontLeft + - Children: [] + Properties: + CAN ID: !!robotbuilder.data.properties.ChoicesProperty + choices: *id001 + default: '2' + name: CAN ID + validators: *id002 + value: '2' + Base: CAN Talon + Name: FrontRight + - Children: [] + Properties: + CAN ID: !!robotbuilder.data.properties.ChoicesProperty + choices: *id001 + default: '2' + name: CAN ID + validators: *id002 + value: '3' + Base: CAN Talon + Name: BackLeft + - Children: [] + Properties: + CAN ID: !!robotbuilder.data.properties.ChoicesProperty + choices: *id001 + default: '2' + name: CAN ID + validators: *id002 + value: '4' + Base: CAN Talon + Name: BackRight + Properties: + Default command parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Default command parameters + validators: null + value: [] + Default Command: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Default Command, type: Command, validators: null, value: None} + Constants: !!robotbuilder.data.properties.ConstantsProperty + default: [] + name: Constants + validators: null + value: [] + Base: Subsystem + Name: DriveTrain + Properties: {} + Base: Subsystems + Name: Subsystems +- Children: [] + Properties: {} + Base: OI + Name: Operator Interface +- Children: + - Children: [] + Properties: + Parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Parameters + validators: null + value: [] + Requires: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Requires, type: Subsystem, validators: null, value: None} + Parameter presets: !!robotbuilder.data.properties.ParameterSetProperty + default: [] + name: Parameter presets + validators: null + value: [] + Button on SmartDashboard: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Button on SmartDashboard, validators: null, value: true} + Base: Command + Name: Autonomous Command + - Children: [] + Properties: + Parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Parameters + validators: null + value: [] + Requires: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Requires, type: Subsystem, validators: null, value: DriveTrain} + Parameter presets: !!robotbuilder.data.properties.ParameterSetProperty + default: &id003 [] + name: Parameter presets + validators: null + value: *id003 + Button on SmartDashboard: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Button on SmartDashboard, validators: null, value: true} + Base: Command + Name: TurnUsingVision + Properties: {} + Base: Commands + Name: Commands +Properties: + Eclipse Workspace: !!robotbuilder.data.properties.FileProperty {default: /Users/seandoyle/workspace, + extension: null, folder: true, name: Eclipse Workspace, validators: null, value: /Users/seandoyle/workspace} + Export Commands: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Export Commands, validators: null, value: true} + Autonomous command parameters: !!robotbuilder.data.properties.ParametersProperty + default: [] + name: Autonomous command parameters + validators: null + value: [] + Java Package: !!robotbuilder.data.properties.StringProperty {default: org.usfirst.frc0000, + name: Java Package, validators: null, value: org.usfirst.frc4905} + Use Default Java Package: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Use Default Java Package, validators: null, value: true} + Team Number: !!robotbuilder.data.properties.IntegerProperty {default: 0, name: Team Number, + validators: null, value: 4905} + Export Subsystems: !!robotbuilder.data.properties.BooleanProperty {default: true, + name: Export Subsystems, validators: null, value: true} + Simulation World File: !!robotbuilder.data.properties.StringProperty {default: /usr/share/frcsim/worlds/GearsBotDemo.world, + name: Simulation World File, validators: null, value: /usr/share/frcsim/worlds/GearsBotDemo.world} + Wiring file location: !!robotbuilder.data.properties.FileProperty {default: /Users/seandoyle/, + extension: null, folder: true, name: Wiring file location, validators: null, value: /Users/seandoyle/} + Autonomous Command: !!robotbuilder.data.properties.TypeSelectionProperty {default: None, + name: Autonomous Command, type: Command, validators: null, value: Autonomous Command} +Base: Robot +Name: Vision