From 1c0d58c599aa51b7f35d3ca10060e1c9a5d12458 Mon Sep 17 00:00:00 2001
From: NamelessSuperCoder <Marius.juston@hotmail.fr>
Date: Mon, 5 Mar 2018 22:48:37 -0500
Subject: [PATCH] fixed issues

addeed more wait time before spitting out cube.
crossbaseline now uses splines.
change comp bot robot width
changed center right and left endpoiints
updated mottion profilling
---
 PowerUp/lib/2974Splines.jar                   | Bin 27028 -> 27997 bytes
 .../usfirst/frc/team2974/robot/Config.java    |  12 +++---
 .../org/usfirst/frc/team2974/robot/Robot.java |  39 ++++++++++--------
 .../robot/command/auton/CrossBaseline.java    |   6 ++-
 .../robot/command/auton/DriveSwitchScale.java |   4 +-
 .../robot/command/auton/FindCube.java         |  17 +++++---
 .../team2974/robot/subsystems/Drivetrain.java |   2 +-
 .../frc/team2974/robot/util/AutonUtil.java    |   2 +-
 8 files changed, 46 insertions(+), 36 deletions(-)

diff --git a/PowerUp/lib/2974Splines.jar b/PowerUp/lib/2974Splines.jar
index dfaff5ffd2688890947a5e6b093f7102e8ba4cb7..6822bbaf76cc9a8e3f3b3cded81b9fb0d74522b1 100644
GIT binary patch
delta 7985
zcmZ9RbyQVdw}<KO?(Qz>Lkk=_r9tA*&7ot{(sgK%Zd9ZjL|PgY5J71UAdMghc$N3N
zcf8--`;WQjv*wy>?>Y7wYmE8JTtHk~LB!QjM@At*z`(#jaD53$$F)TMYuxD{O&T9k
z@|(tYS;&7qm@$e35;VFQhLVi**KcM<$3i6h^Pqw7uS*Ulh%N;4#6Sb|q1I3cpRKF4
zk58q(o5#EkNf;jqmIXTTGL*lsJW6xFs(>ob$L@1xbOe|^mO?huv{AiNQ!TGw8Hlot
z7p9t$`Bk2@N7f`@p^^DzyRfKeuW(PI{rchO&5l$!sdSv8g5>}b8AM~e<26G>q<|?7
zI3^=sj%PTHEKDLc=@js{TZP0Oe>~VZAdYRMU23;@^DEXvum&>C>#-mxNn1bo_L*02
z_SLj!NKUXjEX`g;*y1?&&6aV|V#AvBO&YE9GrKsCoO;N;(>vyVjx7IQ9-o71&qLH5
z0xrE<9_B&^_px54`rHtjfeVgZ@?}jLI}MN83bNZ>L}+iB&}o2HF`OCiERgNDS(>PJ
zk+o~ruUxw-wwi|K3!VqD4%khI<wMDBtlq%exgi(P_IuyhTH&G03*Wk{Z!0a#UTf}N
zgC{{BISYy7m-F{=66zIO&3eAj?Ta<Qc%J%q7dg3oQTV-mavJ$uUyu`TKU0m}-*5yJ
z`{K8DoekNwq4@w{ND8hnsE8S77)}!hQ7H~V&9$0@I`3weTX%~fw=BJ<9$Xa9Uiiy&
za1J+54?9r~C$n}1VRp68kF=!Wq-o=aDF2M7Y*a*w5TD*cqNw8RQnuu&M8Co;wT+h5
z8xl|y<?{Xx+HepQd)L4~V!*-av?0tinua0+#RAg5ev$#Oay%u*7A!MJX|oBd(9_7t
z_EHEyKh--m&Q&Gfpy%?~bjY{yZ<FFOJ=^E5P*%gEXY`8;AmGp=NJ5RE!i+qFizU1V
zNp|Aaz7Xss+_yHDMUfKEGh!rqi=WF*xPiE@If*Xw!Ur+Z9l_n&#&(xnM;#r*fzSH4
zH4*{>2Py&{!hikQFaZob;81Ttj^v?@A%b9=Y2AWQUcJ7>T#2x!jmudVg=aHM#6qow
zMT=+%{S<V!NAF-j!<#hy6G0|(O9EVpQ?7Ujl?44d8vpEbbH&mv3xxhcd`=-jdL;fy
zZnGr;@3KCQ@r?~aR8p)lPP2Hq4T^(F;Z}9|E2)O(Nuyq5<Ob$L0IPD2p~yx;LcJ=7
zDpv=a&K_@`yS6z)QPWFvSHqd$ww-0lFT%KD_RHCO%Qrnx8(2l->!B)}q{Wni?0pLo
z3M`|p4t`#pg160jB@z(R^XU)X%971NY})y4nQri#mg8J$el8aYUBoAzPwKn*QCbSJ
zSzjty`dff!Mz|)+fa>kUx>~PiJQ>znZr2AHnKztYo_r2Bzdl+I%J<dp+E3!_;A7xs
ztC;_Y9)hLVu*HLhg2wGHXE(jfox<2qpy4^{2YcAN<+d1<`Mxw8-<#A$x;iTmQGn&{
z)RCMS3tpY`%o!#*=FD9bh%&*wHmMM--ay<%eZOOu^AVJA4G^S?NRJSsYCn-4j6|cQ
z#B=8ub51iRp}0RuCR-ka3_p_=rZ0Gs)46X<-H<60@$ey=V6Dqqb?JyQZsE<8(Zlqe
z+dx**p+wgPEA2%Vi=AmpLob99t7W%Bto`*u`Nny0ufaS)&Slk#g;m0(tOV`|db<|K
zfXb(hJ9;neY`}@Dgy4j;6Mj=%P$37Js-9XU;-oU)pX$jJQa5YlF4Q<Td#H|WHXqdR
zb;fw5=f}t1?kzgWIB!@Tf2-~u?;Tq%{VI%P&1};H<Ef->QKcNQZgE;RJMDBj+41tj
z7iY>1(;|c3Y?l;UDJQ1@QIDA~FYkVV4F~L9md~xU4uH9GsY?pM*AC5<Cq3`>&&4mq
zTKJRsx-fiahgDlzPMwr$&=_S389d+h3&g<DYtrFH(M*RmCgmm==-|b4+#x9%rdNWo
z34_~&j>Cdba!^RrW^e%AP2?<^Gxl<p&Dd`^+JR`vBq=?rVkjbRfhuo_V6Nl0IT8AI
zYXh_9a)5fcY2wwPBlmM{&O!H&`mrZ-95U-AKjU=IFo|t&u{VWZ`WHV>;19ccDIYuh
zoq9h+I)!c%WoRhgh8EmZyHUqH=Ns4GJWFa!axWkWF2tgbncMTBJTp@E${Z0@ID$q=
zE7<b$JJ$aGbE=?Y$n;nB+9M+%)M6q0vllV`_n3j9;3xwo=pp(`pJeyIi?AIB$|S_R
z^xTbg#N0F|DFW9?f#vn*wzgwLO$6_owpE3=HC2=s(%77o6~~+l*h;6E5Lp;Rn%(?_
zSE`)mdmSVfz$4eP*C1SKR?+*jbof`F6VUal&kX3|Cgz$xv@lNVQ{BZros43>aAj>)
z)pSgiWeLz+k+)YC9N0JQ(9Kc2H!&F*(v?b$;+$z;nt0Fw-h0YN1!3aJwvSKD@ot`;
zfLb1xp}Bj_iMx|J#!Pcth{&Vac4~Ug5Uv^`wPJr{oOO<nL_HvKOqN?idQIy1Qf?0m
zS|qoJ4ULc^$J!=!>|GsX*GWyh?~x}%5^C3_zyo$K*p7X@$A&{`vh!3CuP>fTs-Oz8
zX|9eV^L?E(i=KOp<hBsl{mPu@dYMbJUIWnB<>OpWy<kVVtq>_D;<Px}p0K;v5$bq9
zUQG6#8*(H3e%x^WJ3(~)Q^{iAxzV3g5pS|O56z*JfL8utW#>R+_#}|R{$PU?36zV7
zfq{#_(a*A)jK<+_q0Q#*)jGXTEBe&)%_=)B`9ca{9!ocP<S{o|8;98E30Bba9P<!z
zzy!@d5#xlA>5$gJs_))GosO$>dHDL=-Kmc$y07jcGzIlWKLsi>buLbpY5ESfm^6Kj
z{rk0~?0Ok@Kp%`g^MTRnO8mx}U6yIx5@0`ph4sUTwX`H}!eRIyh8Wpp54H!j9`a%I
zKIdCnVeiMaC2u&{WskNSq;9CS9;#r)UgynPVOoCD{8QMH{WTZrl0f3PBi@Vqxfn(d
zp50WzPE+Qy=Paum$v+P6g!CgM+*5<M!d_h|MHSqi)re8p=g$$vqdB()D4wZ`15_sX
z(pImktV3}o`U3Q+8jHY(M|PWRh}puQgQ-JgBED2Q=@M@s=v(%S^%3%DczoVo`~I3T
z4qfp{pX#z0LGtb3S^kL5&=NHcdj1_b?oWA<eh>vl@D{S}X{#%W+4P5DzO=15RgUe{
z#g>iB+Z%@O@N1Q*bUM#foU_Z?UqHJO5+ac4q{hee+Js}-1HbNYIf~a3`eu?)qrCa&
zcZ@4eC`gh34;_Nu5Yxl^HHMmS7$d;md-POVzBnnr$BSQo7~=}F>ie)QZh)d&y0MPC
zV5Bg!AYs4MaWdN7*<{|+d)?o=&+Ql%H}TOwDn7$gbjq4+4UA%n@#;_z!2<-LZkN9n
zR^t?|u0=-gbv>k!ZUUROmxz*v^|={le=@2w2l}yM1qRV9+OH{GmN>)!E;JsSsmjcA
zTK(E*Iv|{Zr<8V;8tJ?b95u;Jbk9KDx#SDWr7H{z%lfXRctX7!$JR{z8rm8&E4cZ#
zk=7V22<bnAOL7vxo@r08D*@b_*MXxZ1D*mh#kH)}Y&!zyRo1xWL^ALC1m>f<Nm@|`
zA4riy4|hHsD&}&!$N%gp)$$PGuyvWI`$iGHy-Tw-wL?p4Y4lblX{;9#r^ZXaU#SeO
zu1nV=wySQhuAYMWkdhuM+pIO!hbZNyaofSEF*crj`E~z5M>8*2$PSDxuQwXkgYbN_
zroS!Wf1$Mt+OTw_z#(vqY3%q$??#MmywHeo2N_ZOaw;m*?^u$rcPu2k{f)p#F>v_#
zb6aPfw3zrx999V+?cDWHn{zg@Fo_nX!V!&l6J*wVpHMN~P&3;}d&jY`pBkO%+eR-d
z;c&AX$x|Gm!=f<L!46d8XrgB=+1f0Z_XQYt>Chl9lIo>&pdz7i`n*+RNl<Mj?RQ&-
zKPeud(^K6Ue4Fjf7N`qL;{L%VXgx!I9$a1(7!ERvadc0Y<?(j8cDuaBsUd)4jgSp?
z4hfW|5nP-{H5%4ZU}VK#I+W(%Twbrw#xlw~?+T7lyE=kgDu8|EmLJNYRZ4~7JYM2K
zJie2os|vpC;@@!xJZBUx5P90+O*Tbc4cxTif}XMSGOc6e$wM}@0uNt5ih8WHGWF-=
zB({u}A9S&jW>uxchU?o-FLJNQ06e1i+Y&yxT`#&1b@2qM6LzL2+os10Mg6DEt0q!!
zT7Hw`m%D)^27zTxA2hG{fvzoP7`}wG@5fqigFP?V4<AbOstehwUx6N~?e0_$Di(Gs
zE>eDhqlAk-T04wc58e)^(~7bT`n9T{mC5|xELr}(T*_jfC{GE^?X#b4-<LuIY0pqx
z@_-y>PvF;Rp*fZ!aKxb55_(U@P0#810UYuuEQ~0kOhDwP;ccJ1hw23h$nDhS&TaWF
zO=-G>9S)Adn=6+DM%Pcy4>j16zjx+VCBMU@z5|6R@|}i8ecKkxt-@>Yj(o_L{}wIs
z6TRsd<BIJmK5CG02|eazoSZC~V}qP5p<}U}EU9C^oGdZiD<X_<))whr!AntA<Y&$u
z_e;R-6!7@jju!f|&w7?HXlb2^uJYmp=K=H^(Wz(W*+j)AH}=Q}QDZ{+Jz1(d(ck0i
zvKLm+hx!3vs~;bb&=G`dp2~a7(FGi{K6GRfL9Bn1`J%jU8#p=rhWmj2a~r^#qGX1m
zqxGVMZUATfxVL1zuPy+b2Y%JL`ZB|Yc!AVe;JP$cGyQG9t2zO>kow4SW>c41)VG~$
zoG>c^H9c3bJMCpGi3=J~k}l6bMrqqIv>tEn==jPh)*iVb-(o^o<b_<<qy*}RoDo*d
z2zv_CQW5uos5D%+OwpCu1c+~v2<v<j=~I47mPTWWILM~U(QgAo)zZl8#SzL&$<Ucu
zV8n$klR|S~c@`P^`r|Ach8b}yI9Aj|N`6U8q(x}MFF{1V)oBb#nDMRQr`{}PKXHw%
z7b7zx5zlK>jzz;buF329aV~J&x}Nb(fycz`vmqEWQ7(BV_Mf(}+|axu^B}wEh);A8
z-5Xi;D{N=rd5|dV)4&AxT{Fy)K;elXK#DX*&F3u*O!5}3c*fMd{Wkj}R)EeBx)FSL
zb^U<aXZX2~Ct}LD-mpnZ*UGklzG?MH?WK27pobyCjK0xyU4)>nNHfo`!MK<=Y8JlZ
zTv63RmVQ@_wdAg17x~;v9`C(sey8X@O&9-of6^-SUBqZEoPFQf@z~L_QOVl?0749(
zML0D$>KU%YW$^XT(YeEoQVtSq$F-q40x)BiS<Nvk?)-cbY%LyR2{&lRm<!v*C^C3Q
z_~1^uuwVKUzZj*(o4%{sJimGm@M~2p9-Zd8dy<M>EaWIw^Jp(Wyl{p*v*(ILF^zfE
z<{Uk@JYG^dv0~8*btz*pdi`%8Kn>lZdSyoC%cCzkkyjh2Ovq=K)rHS1s&M%@@Km<1
zuf`p6Xcjsg`;!!AV;V_Ss(&+$Mama1O4o92z=?tnPU&SzOE~lu6%%m}O|wM<vl5Vn
zxWT#=!vi%MRHd&PoHU4IPle#js~xM{>kO~33{?(4Xe~s`0oKpX0_NY|0v@bWH;DyC
zBJKQ!O0En{Os>;`yn{kS6Lk(a!3uR<q@Vq783&%zur@K1;Y6d~YH)i984^+*;g5eZ
z;5E$^l02*kNm{D8oyjaWG?UECEnRJsIYSD3Yf|cPcu_{t9=N2)`y=^l?vHUnwsY;Q
z@vDT^GLoAE<;^dGtS@qD>wpz$!(ic?RR1%-7M~}wwv|XdaK-6{+=*<=uemhpY0)#(
zu?RbfzY)rE&uykU=#C3--76~=^3I2y4o`=t?%wStj<Lt)V$2hMe#^*~pi|qkKK5~J
z<LKo21>=fIC(daNCwFBHePr=D4t<0yruKFE#1wt+>gC)7wrogV)hqyl@)K%S*>L(J
zkg9)ZxP9^Zfw2CY%a32L!umgzZ!%UPhd+^w6Xc?LlDet(Bh@4zwcZler1FC!*&r23
z?gxjYK`NKVj~O%D?cO?I{2<-BdL|gP--x$o=@s3N!h?R74d!14paa2A{Z40R*$1N$
zWC=tqvP_$w3{>$ZkLx7?x#_yW%x|IL=9pYHE9%OTSoN-$-bOSPWp#SNkrWu^;R2HK
z!-mr+$+cQ~U<*3?Q9t@BueHiRFx814J%hHhfpan&v>P=)?pp<l=9@aLG|QMZb{4qO
z{sfW8)7x6ie75^blq_{XFVbGsD!oK)zn;i|_kI#(DoDxVVAK(ia^hJL9@8SOjkDiM
za6qc7B(5#`;6yi7dIP26eOD79S(AQS6JOR6*S28Om6<(@XR0QqBfO?I0p~N-y4J;}
z>(2^f0xqkh3GweU!>S&Xy0mL?x#4>RHB}6OK~iv!N~?9k^UOHFDS-ldKE?)^zpg{u
zYj&V3$;Ub5dkT~w-#h!7=f&sCykQ76kC0T-uK*zFmN@fG-`p}rJhbo*4o^q+xDK{7
zzKi&_KfMpSeR4li_8Eg_z}xVsI`SY2>b;~B1j^KY;0byo0wQ<aYA+Esl;O&~JP*<x
zPiAWR5ndFW<CfL^*}?@~`kIo?$j~q}Tnn70IRcjSB?nGc)T>F0iz<XC^Mns+4BwA3
z)<KMQEh2ZHbrKu!PnjfOSa3@1TC3DDgD;9J8o`og?@-w&YCe?~<F2Jm1>C8DB-@^_
zIhCFuJYlVA{2<8zP9$pSP>W5(pCgiR`)=x)T~ZmfOZ2gDBZ!|jD*byE<o%%ch9m^*
z)8}lRmqGvvK@)2&>Q+oP<8%8aaIK;4itk6?Pps_|h}_!<i56)lwS3DN3{n(G^~spd
zzpUJCEt!03TSEL5A^LaLXEYsghR3Eax)<X=E4X&&gc%^7DqWZ!fe+xQ;;h2bs`bI4
z6k6)qzE`&)Q*^=hMoa4!7AhwPLjIhUoTJ+6^ZCG6Q{5RgpO0&1E(rP7DT`N&UEI=s
zR+z6716KP?RWM%~Z8sXBe2kvh6AVG$kBov^zb59U+mk=PO)wxo6@NuK>fSZpV|&;O
z?Uc?LlW)OP^81SR<?X8Nb=a7*Y)FQ0i?-bh@Z6LM=W$3`J!=cyR<oxZ=8yHHN%PP}
z^IjFeE;zekao=k!L)&=##So+aoK5QB7LMwcuy4)2b)IjxKJ#*k1y9!Ag4(;#3Vn)2
zst8<5CEN8&Hr+z4GDPw5DbHN9^}Du#&u-N|aNCwxzEbS{t=t<D3$)PyVGAw)1=2W=
zVrAD`oXttYI94eFsW-7c-LWaNt*;h*%@vA)Eie?MT~2+&vNB%1z$=~VcgEC}wKVUx
zs@U$6dA!cOuk{sdwJo);-c!&wi9fs6Q|x!K1eXa*INtM^kGi7)4e`&@g}uBw|I&hg
z4CzN}DAO#j@ROvSrGlrmWzc<^E*~`I2UWRMj<(WXM*HF3Qp3S+?4Zguu(nJ0oGc(`
zwS+e(yrO>j>WVf<{ugg=O|QzgrsS#|x1Gy6H_=}E`tl{W3wUrF>SdK1YcIqsxsO<k
z<YW=QxEq(~26?py&*`4<cdsU1AwYMJ>BHyQnDKx!hQN<|NiOI9p}lq9%J8{>>jc~*
z{QDh^l{$dFy-t4P9E6^xFKzR|0}04@J8+4QgLAoS@>|L7f!v;J{D;i_to_3Nx8Ie+
zD}-IjK<a~iv!;{+^|_4P_XX%}s@88q_gjo!7fH*bFd8Akx><+)2Jv3GW(rqfk-)u|
z(6)U{QGuf};-UEhpN<!6^ga2OksgigoD736=6FykAz2Cyi-@f)=t^GPNOXWh#Ud)R
zLAZ6<`=4S#=DloPR202^rtO`}S-s0yrZf<~HWtoYO9NyR9xZ<N50<MHWa{&Yy`K|%
zoxKWw+NDZOfmNsuqEp}K^L-KKno?Jx(wnHvm<mV$u@-r$Iv%jajq?A<%+A)T3D+ja
zZmBiyq?BM|)FWqV%Vz4zW)=db70k^MLQNBv+@kni$5s{M(_+d=y`N3w4Y<(W#B3O4
z{wjaMC>&Z$2dNsR{7mx&j_z9RFP{1^g-17aV?NDSG>E!7gx0XFAXB`i=5vw}HxbHt
zk7RdX#At<4S5~m+L+Gd|#`zBKRt`!`6B4eZ2qg|#H*gg12&8sU92@{<>euB&?sQks
zWv<qf#NhUx&FG!SQZ2a9D;><#DxVF~t6A|P#f#<fo9&G(D3bV8wVq6ILI=XlF-qlU
zt)^b}n6^$<sF1skBZR@BY|jAAJ5GlfcU&%jXXirPSATG*#XU%<d%@tr_H{h7|2MTI
zFv^jzB9x0dYu4#jyB$zNTbbXW4u)>GeU_I7W#R4kU8gaG<-T7t3<+n#JQyu>bOdDb
z1&vtt%M8Zk=#IizUb9v!%3^%g-y2Vx#1zTXu5d0=a5y(G#Y;ViQNi^_b=y_z%BWxW
zs^5S(K-TWr4v(3%S&CHP%2d37$=jjXGCk$8Gn?L-c{tQ~0XXU&cMoQmidUTdnaT5E
z3te|{gzvcy-ODO-TabPc#?OdL>^$f}6MVbUs#^SBGN+Cu4^EOrI`10zvZg16gode|
zWw=Wz7M$R3*qDd0@9v+0CiK35%RQ5tp+_0RgE6xqo{jOl#@?{OxVj^sm_wtN+M=)P
zkPjFp%V+F63q&#OjJ)U0=-8+cg+|zeS10A2N*4LRL-H#$5%tYRHFpX<v_vn6R=g*U
zL@b8g#mBxzwcui^EqY1`Wv)RZR7yQ&-k*yz3AMiFNQ+4v4_}d`2oC1^?nc(esOEUx
zAsvg}c`{t~xu$VNsL9!Y&zjX-j#_iur>4+I7xzPd5O7=pSw$^Bs=_9GV;jPE6rN`}
zLC&BNywC7!Kk%y@zmMFwH*%62CFvl_FbE36jpaB-EK|=xY^Yep1x;;bb?2%6`dq%M
zDG?c3>n-*~6_t&{zd4G`<PvW_AhD<?<3$U;PN(?fsr{gA<S)|~S}<8m6K#J!qC%$U
zRt}IQUVth7nY4u`TUZ1nToiGIq-F|v*{t3u2+G<az2DBvux;cnDDUG;U4?~0+8u=4
z8dLP1s0-z6p7$NQKx2U#${h(^uiVs)Gq&|zj&JZeC|6uAhh23du1`w@TV{FOI!-o{
z`)a|UFOExG*;iWbbx+LV>9gfqQSt60k$2<^tUw56(fXF!wzs_b3{eAd*mEN3nz(Dd
zfaE=fuV*cWn6qdwxIWTkIaYw51%%JHg{laGU(=$;d^bV2{lo>1!-s58gJQuIiKp9#
zkzc(I6ij7aaTwnjbTI1Aa}T-CYRhUELFr4h$*uA#X5wI4^M=Dx8Xd;#^_momx1Tp%
z0F3~Rj5oY})6sia(|Z~LP4AWp-twK|vW3`WxbyUlM^|Iq1risroldct^POrD7crjl
zvYB(9@)5UTPnl^hvz@xJt+JgSv8{5P(z5j!uS;nL)V{M4*szc2cYL4!&PvFd>?a>_
z?2g5ggAA5X39!Gc^bn(E{+W{WTF3`oEud(6N1~@z#!h(CC0p#Nl7QHZoVkG5(w21v
z(UKt}e5`kkXVKTEJCbWyO}>Upvu2v>fmZKyCzOO%B|n8(vpU~lw`1zXTj5L5<^aEx
zsjGCiH}Xwhm-b)AM2nM*JumIu><Y9u^nF}+30-4}r8HZs7mQ}L-%%fv1*Uj25p#CG
z=u5c2F6{>fqJJa*?H5ZCarAGJA|Qkd{-<C3*e?E~ICe4KA^lMvVR*cP&HF5kD1TMR
zLQYTQzZQ?z>(P%8jQ$(bj4qP$s7}IiMW6if)8Dbe4jm|(al|<B{|<{OOW^#q-g>H-
zkD_L;neZPUhAzU|Om5DD_Q;fN*dDEijpCziwc&rXdp0tE^iUYPt<j?!VGI5{b<)=C
z(dDwUepEyMr9mOc!3yn^#r~y)>Zl_kks$n^-m3BM9=YtlWK;x%b1Yc8AOp+_gAw-K
z4u|2d&id~HiTqdaFy4QH31F%aqQ7rt4xvH9Wc?3CgOxzY{?d*=6rJOdUj0Mx>}ik=
zc^-*|J=NbB$3MhE;E~AiV!__p|1B7-?+=|7`9t|(*ba|`(t!r)nb>2TkpuZ-3A`Mr
zk#;5iAW~S5C?@Qc1J>iBe*)<wAL*e3%U?q1NP`p$ekA6~#615l3N+eI<IcaD@)`pH
zp@R?sf%U(>bHMO%Qm!N2<9eNr)JWJ?|73O~{G0jVPiFal2)Q|#=HJX%2!Hg4KiU4j
pQjPgvnKK~>2%1j~<axnb%KSR&sAzvj$^YC*=m-eie+Cf{{s&MV#D4$)

delta 7252
zcmZ8mbyQT}79Lu<J0%9`kOq;ip}VBJkpU&Ilt{;rLxYsG3`#2?AR?Ut!qD9yA@P*o
zdTYJkyZ5g3oqhKH_S$!!b^h7k&smhzaTI)Q2pT#W=)r>rpa-w)lJPCj{(=YNJtXm>
zV!f*WoQC$-f*GTWqtZYdVCeCvXjrf88d$J!Q2s7pcR0GhmhQMV*{w$`x||G!w<U2Q
zLTz9<>=7?d4LNb$n?Se)1Oe7ec}Iwpyy&C;c!ZKO+J%fGM9Pcj!So0CCERm0%QiyJ
zSLYjo&RNfIFPeYRe-=CD{DAc0ie}*q5U65Kh=?JM6Qr>f7+Nlmu@xIyMt_=X^-~oC
zCY5L7ZQ28n4e?7<7SV6$=P2vSs&yh{8a}$U++c?)%=#T4Q3iB2@bnGM<~cWccYl!T
z<0X4b`bmjqPVH7leQZ{*)ILBD)iybRlF%L<p5mv8<EELNp2HzMsOBg;OrbKK`vJc_
zX_OweVQ){$&RKw2LjgUHe&zS${aYCKW)0WY54&8z<kTogN7%Z|DLKx|g`O|Hg!}t!
zO+V&pKzYoL_|)Kb>EjAy*sH4SqyRm}BM!WF^*SnIs_I3QWN}YQdU*@}EaXBwxI6J|
zqviG9OrbT`2Z3!pFP&~Sb3-r1ZiBbN=G}3xceowA<vVj;`*S-r^)rVn;(x4TC8}U+
zP2JZ4+UA;ymw5L%nrfF&7+gk@*O0HnaxkNnQAzLGM^T@c8@6p`MP7tmre-HJMh9$>
zQq_23k5<#ZhW@-wM&Fy&y*c>4NRczpl2=S?@l7BfB3G*TJD?maA6nCPN+Ajj;MNVS
zHBzHlNwE-a+2KmQWiK23vU!Eg_OeVM=Z<9`u!=85#W~7LPn5dzfsr^BUgl(3f)y*d
z+Ake-*PXaT6m>a527qa=9M?=wb^K6+*`bZKfT3P-h0-M<9iZP=C*gVOTzq0t;J`J0
zp$b}~2@ZnvOx4+cY>Cd<dh*wz_JBamnnky3mLJ_*ze;z5f8c`Q$pQN$DErLX6%Yq1
zl+O*e1Z+%)*EkpbLs=TV*F872F@J9{Pe3SHMt}5|5f}2y1&PU8l>yc`qlDVU^|cQa
zjfig58;Jap-N3YOre`#aZSlwhzp{)(qHS^o8iy|jzNEVe8{vej&7?Yfy_?~L8x?Y=
zGh)a?u&aS2Q#mvK(T>7kj#u=Zb<IG;5Xd)e&uHYCoEBTRG(!8e=LmSgCgWuk_7OwI
z7c40<#;vwMLcO5lhD91X<DriDp}z1<c1;;|ywn|)WR1+qgkD3Y+o7)8&qcdYqfFkM
zL1djt!}b%F)ZNlFt=R5`w5t|0ghydR>6Y3MtOt6M7(tF`AW#Jk2=w;`Q~q;>|J+7(
z(EmI_Faaz9fY;W|+Q+BS6gx<Na`NfMpxwg2sy&I^E9`6(*@l8>r!;J4GY}d!b97Y<
z0nA#^L6FoDjDFe>3#o=YtE4zDNqbjb{+;N{JNL$YNTr`8qWY)1Yolpp+5xa1*uWYG
z4tgpL47K02<_7LJZ{!3H;SL8Mf3;PB#)&j$?yqJ6x^z4}0=hv%@yo`teX0Tj8jDu>
zLmD2O4*93w)0bb04lavJ<Rig}lTVCd{5|r9aiJFFJ`LIH$IPMz$2PJMsPw3Yg>&3G
zMOLf+Ixp!2XSv46b6Iw5(}$aOvRoLAxZ6a~m;!?^LtIl9r|JUVuqoX0G8~D)LD~x_
zwU|6`b=`H?YeV2w)+sCMxtw$gP4mheo@h1$E4|$m5Zhj-I>#w?#E#j?NlJd3v%<|m
z0eO*s>dw0qvcT+AN#+RgPUyU2i5(WcI1xO;9lKPtUWVxAu-k6LTJd^~{|G89;DanG
zdR&nJiZQVzO<#l)tM1Gb@48V1?{qnMVaq_}bK#8E^^W8$do=4U9P#-yxhu}(t4UpR
zmY$yH$Xe>4>Y?doj;?83AIl80d51%MkB(%|l-@z+F0+WQ0n_uiLm@u>H)$J9mxdi0
zF>{e4^b(6(a3iU;;)HdUwi<r}k(XWjqnEVL-}FcT;~E<c!^qaR3%ora@jB*^f#|@3
z>mfnK!Q+dNa7O-@Wu1g-ee#%3-41!f^)Sou7u*kDbq3;vd_lHjK6N?#h#&GD*@qv}
zg*^BgbD(zgBsYKJk|X!iu-ylR;?9e%H&2rvfHUXa>+QrS_72%wMmOW4M6qDrfY>+a
zOIL0Nm(6qKlzJzr55(iB(XTIQNvQ|`*6apS$H&e;D>Iu+J_~{!&#z@<yjsbs-?7lx
zaBiQmx7Mv7rXJs9-{1%Ld%b-~6pMMad}Jl`qmGFL7aCPuxQ;XlmL=lxAdD)ywX>A^
zih!a+68OIGg4tK<RamJ!jYEGr=7YLw7ObS`cF_BHl9Uo9LR`7YaC);!$(YB0bWx0*
zTx(xC$EL}|Z@F_}&iV}b^h*rF&l1lSi>*yMcQV!779wehdmg-WIA%==!BegLk$?Jl
zGR`XZdun4-l{8?d$W5HC7iTqWEcfL=^=hTiO7NqjiKkSPINc5efm!^}H@UG}+ms`i
z>)JZG%b4zAUIdll3yg3R4~4q`K+-Iwv*{^y_;s8Au`51%N)|DZ@gaviOS2yvPIG|U
zhc4;JP1%iOWIF3UD^+6Oa%C7wkNgckI^`Oe%Rc4|Yo)-@!?LQyZvftlH;K8`3xCfG
z*m#T4XhJb0d7e<wCoGFSq<b`jU9D?o6>Pz$&r5>%{jAT{`xw1+Dl9-7_^gl}Ww2vv
zT<2q5McQjPMU#M;aen4T_F7bFL~oP^_kyMJ`lt@Er|I~fE6zuhfQ8pJBk~79GlsCI
zZ<Aia{uW~q(STAwIdN`<$J+_J_e%%*l&HKeE>nqLMC>O|i3cOhA9l-FqJR9cK#S*R
z14~%i6?2ruvT+?^P1Sg31!(bcdnPR@v=&<PGkVu0BTH9tbS#@wyvn)8A8D|LcQ(0H
zZ5gBY`nZ?8ZbHat>+AV)@ENB}qqsC<yuV{w`!SoXQ{az5Ey)zf>#lW$_S5~=LQ!|4
zY*G@dKmF18R(v!fYnCuC=S0d?HLqvoA#veGPV7UCfp#)q)5keSGJqhb0c-TeD7}sc
zg8T(Y*U+}a9Caa@=fb^5?h~84qk;xp8qYOJQCI4P&OGzV(7dYL46Z`hZIeBN)vyi=
z+>9bFW_}jUOCfk0#V4vEPgSEU)__j4>U*2jNSv|f!s)k;1x+^z;4A6l70@ca2x3TB
zx2nf%D-nx3no8z_7+{|JVT!!ivrvL`ey`LE;Zx{~^1%!`$CoZqEBSl3&|2ZWbR#!W
zA{z-k|Ly*FL13zpnJObYwvl~hyD3e$F+D+vzpQgRx7rCSEcy$gn1(iMir~f64V{%N
zITxLgyYu%wkp^$@I0jbR++A$c=m)wq+uhAxS@WX$TjbYOJwRmG%{R`ial63NzVB()
zGimnLD#?r1)oT@pGF$tyae^z&@RhXh4t@O&-B&%y?jo$xejod;mP3)tbu+t}g{%o=
z;eyVh_?f39?QUawr_W#=%3APK$$){kMw`yC4#gMQrz|3e1eAE!)f+b27oKC`r%9@d
zrG4rluk#~I2m$O4^;t+FJhcchG@NxU_31No<|}XX)(CNv1<Z0aBVsQ@BP7z&1}@Q7
z)PKsDc}oBF74WF?pfFG)#9)L#widfZYh)*EpYoJNmhiz<eL-$m2=gYf>{D`t^2jim
z=&3CX82U7FnE}3w0o=z{P9szg#<9Sy%?cZ$>LD-y2p^gAE1gU!F^D3a%-($=v#|)j
zmJAtKT|8lg-WK-*%TEabF>Qe@;X~Ql@%F=MNSG@2U1f1+NFxS=E9|Zd5V@>H!WJpu
zQHdkinWuP&h`U}ubju__a|6h{CQup0ErRf=9dSOvTysrO44=HQru(XPLZbIB+ePD+
z{S$z@k+-B0wpI7Es7dhh$!&(}BIPjy?_+;%TJWdgq^VDlgM^n)!2s)~?bh$w0<E_*
zQF1Kt8XB;!P=B}{m!L3ZMkc(5y3~+cZ(cW5#X2Uyh&zSSxJhNz!eB9!dJSQYnfQB3
zP2&pXW@JLjl$s=V-OFYZv&GQEr8uG*#~naa7`WI%-K#3%Nz>bP^_^FUm)96NMAC{U
z5@rVZ<7uy4csFsCCDz<lUOxvCt~mMoCcsXlC7BVAY?XWy4(Y^FsVDsis!KJLSow=*
z)s_wV9wI3q&!dH>;@~<@#GLbrH`hBn3!VuTw*;mYX}b%j7wRcLc}i3lHMiFy{1`xG
zAUo<bX`B=lvsJRhn4!xNDsE8oHe{mwAojhaJIfDTo@1M9##Ze!k$iG&2u&R<AG*u5
z)-u|vkXR*yu?s}JSPiY9bQ67%&F(HmI`?o#QifcyN!#SnY(|n`rg>Hdp@b{kKv%ek
z3fm^?OH_z%d98SommC%b*SbR6TpZxlP}*!#Z)NqFKz613Nvh#)Fh98BP=3!c;wHm>
z+n%C$V4_TTa@=FOI*T~k7^AT~PW|De5e*IZk|5$F->uCf5<_J586&Lqb7Yt6XNr%w
zCMVY_3XwOFrQa<UCG<lbAU4TmhI?ko0>6-$Ho^V7FJN9())+y`Bn+&Z2PgnqF)nna
zpnWVxhO&2Jm$5pQ34$5ickB>$b@O<+q8qS}2xG2{!yClkwf!->O4&nU#}-ng65H*)
zCB)*NUz6EOxMV*<Sz8~e{8?6iE$w+N?ul+&_iKYdNrbOep-N>55?g(aA@#$u21B|F
z_<&5H7Dvu_i8aN@h?y?t*BM}p`P0prK2?+|S+AVCUdYz+mu8Pstxi@Jo1o;54@b0I
zBK;GaLq&eA!i~)%77f1((HP8ShwJ1oMEGCzgxd%5>pq>at?~*|*EBffK2oWdcrWGz
z#ilfjj6kTA461jA=f2oGcv#J+on>SSEk<s1mq6?nHc{o1<Ucb;MyvsPfsA>~Yon%K
zQaR!${i@NP2EDY7JZwn_27hHHuq74=v$K)gcxi-_TJ`Z8mT{hpPaj38BApT+*~R_#
z{o&oY`|#wk(K+=A`|`H5Pt#gMRQA)rmrG@Y<76$c4p{|Qxtt(*0Z#OsL3?-2LBr1P
zy(^T~1?OoM!#7*jR`Gs7VLU?NjD=74j3ID=<=qtP3mxHOS_76CBVj=-1C~(TGd(|A
z;;MJ~^{x^9KW_5+4ln{2V*TRv)hBCbPKf2+T^R&^ns$@6Fgi0Hk#h(=W>;uSRqKyj
zp!aR<FQ0?-xW0f%Dzf<|kYx#@>6;p@sT13N2~U1cmNrezL1T~)l*W`YpC(weW^hg$
zl9xEg^)+PoI+(8ProX6G)+P^G(Gx1HkyZ?#H`G!eWvyT@+a#)})Du$NuHcZ+-*)DJ
z^PX9ANSJTK$tKmFWfWauO*)6fJZCc~%2CvW%h<2VS4Ou%r8G)O%hT7#l7;dU3Na>>
zaGgnciabo!u}$TGjge@mbmK?~BDJQ_9?H@v$2ZHAzjpfgQc{pST77fl6!+}Bk6z8l
zxX{ujy)<e5BdO2Fky@rpX0~Zdv*#Gq%+J4C^6qSRp2bnOZoQ$K^H4;RO;k?!O-@bh
z`mz1kQ=0(8nQvZe1PG`M1SBfbd}6`Xeo9Um2rrVJg6Eb1`}Gx;ik@ShZg5<FWryY+
zKONN@kZ=`)8lOrJk#$0zBxD#r{DNmwJuMML(O%SVrz@UZOmX_%J;6JVB5@dd{vw#S
zva)hfL%HPZ!#$zF`CcPw1!W^TSn06IlRlB=?TExDP`Yk?hm!L8AsBN$nMTZmXOFlX
zY!%bEH2R(bvc=;<%1CSqyi4fj`g~*3Ak}rRzb|)oM(JhfPXv?V$@Xsi7=mAM(09yj
zK|a>*<LJ(3lNbuZZOCW}-89t`Z%G~!0@H|%IO2(oIho*IbeZ1d(h}+$WuGu2Z%*dv
zgBO~<Ej)68StB1V(IX9g-<NPa4<kJk<5P6M%dmX_oHsYv^|a8#;Y;}814{&&?_~Q}
zWADhFC7Njulgxcy8Tw(3<fQDm91x(Jwhhg{OX#eW_+>e1U9H>V{V2o~uDSKA`4z!O
z)5?}$%n@j1W6Rt|&5;CJ8UnoTixuR+R?T%H;VSp8oBWU_6h*@KmF9^*|MD5$$ZMIb
zcXAfv0E2+NqqqxPb5W_VrLmPM?3sS{kB3amvSm)D!LOrVIa#pfhWe3i>W?WUeLK56
zz6!EP{El-WW%1)17Ox_Cd+9W33NvUjlTdh`qn$yk6hr=bO!JZc1-R(Z7Fl@zSy$4s
z5RHW^^Vz61f?B#dKOubBM9#6>HCLW}yczLY8qjhap>*QP#rotY{Y;s1IqkQgMN#Zw
zfO*_GlH}zz#JVG+bG5dBa7&H1sIn=pX5AZ#ul3gTl4!OUABx5Z5k1C+l71W?45Inj
z^%KekBzvrIZ4q>&@2{F|W(TqV>^UF%dCk9vL}^K6EXlp5dGZw~h`OK;F!mYebtfr7
z05r4J^bj=D$9toOV-|bR0DZ~Wj%qd?&yqgG@c7DX<Y|PT__ph2;OyQh>zA9<Us6x1
zZ{T^O3nd$f#4Uei;`Y_@w;A*+I$EE9gFVVAhrGGm3l_T>capxEonl?FEWEc-#<)?v
zbQ;%D0x+M4MSa}lli~F`w!Pv?LJ7$$0X~M#cah)Ve?`}NF|rscwcH{|K%jCh3A=`o
zlQuR^OU~@%Z;HcDwKg3IqBIX~qt;y+M@z1xrL$su1GLHj@v6HwzecIXs-)LU7Rv~^
zEh|E)r7XWZDZ=@XkJI?(h$ob*^_Bwe`hy>}!ClCt3aX0Iz0h%xwoB9&_E<yK8Q52-
zr?N|p#D0=LuyZUSFWtTK4%<IIQ}7Xnx+^E1KWzG^yM_^d1MN3Ae7{vfKRe8U>dq2p
z9D6JSc}%YN;m<C^pSjctN>U9m;#JD7siX;;<mBWi?s-*dSO}Zvt*xzc>vkKE1ij>H
zOIxUa^Az42n_Y8kXkZXu8_Gu!=mV5*xIea$4O2>6?x^%y89(Y`45iJF!a7%wO6IZ>
zD3ciwK@Q1})ICB}GR~eVZgv}tgk=|EoR7D@`L&?jc7}4gvsiHYuwD5M<?4;{D7#ed
zFYn?jN0IOB^}Vz+h0qHEf7WL9VrQB3(zIi0V=jp{6p^dKP5#IkjF=H?uR0)6nuH#c
z@nF2Ks2jQNepS24f8ie~ts`;$z@|4?W(~O|p+I`h3=U5GAc$rVR;y=%Ml%pE6h*(w
z%9pPIYkp83BdSgPD=?*(i(d5WkwBV+gW;G?Rf9DaeuK6T*l9-IQ4k-S#`re!S+qoj
z^B_VifQ-_yBFHN78Bbvs{wZMK|GTC7g)phqw_-<sNMgXJx%ZyDQ=1PIPb?SQjDFqt
z5(EdG>)N@~AC%^iDUPyplJO`BY<CIuR8f)~G+bI4Dkntx5wVu~V5|zl!QO0>C&qA$
zgXXyQ)eUU2byY+b`?%ua+YE2vww;MKn5BEZ`P*ofBbM@^N0)CraSMSSijo=mm2p-H
zU|D1=cdmbHCfUB5&8G>R=(fi6-n2&w)8sMaLnqjb_5IJ}cDh(id4}Ag-JVnjzI7?`
zv98Vvs)U5p*sZkK<Ovyt^wI{y*o!eT%V&Ltr$vEM&T6ZyqU0^|FZH0(1S#1V?I)uO
z9!1Vl*)Eqa;n5$#eZ9b_f1fweFsnw#>RiRQU?z+{bv{TzWc93eq$%sjdzuwF>9Q}V
zXsL0X1LCgwnM?@;2LRhy@C_Kgo@RyGAs{7l^M>shZq^dK?kDlHvedIbd(F4!ZRIEK
zR8h*@hm9eJyF8H&=W`7jqfg7G<C#Fo+lD$WcJl7xCOp~;1=PUQr#Ik38~6~U9EEz;
z%|Z#%O#Z7SXXsTt_^{19?ELcie!-kSIsTzUK>GAJB<Nkr8hGZ1?mOi3HR`DpG&olt
zO3?u39-pHWelVp3Po4wnUYJ8{S(fIx%%`4lA>>cSVyQv4m(JV)XhK7!>>pRz+!>$a
z8|^o;Toj&_3ta-gV}pImm{T~qce!?*nhaP+UsvJ|e&$1vmE)Fkt#YB_{XR9VtwA?p
zJ)>yD=xMy6K(t(q8ZhhCbJ88B1wgpA=%msZP&SHeB@J+{QF+@H&M<;X>VET8pHT#k
z)@KhvlG90pVNMjDtl?-x0DA(}+lsbmm!??wg}TeigK8i&@Rv_2GhMOteu<BG0+-68
z<N*|KDYG`roX=unTEqwbUW8h5Z7WW(Bn4>T(PDIC1!1pkIF2QJldJ}nQ)#*BRUcX!
zQ@JF8$C||mk*ra40gIt=C+QIYu6q>KqZPJT&)ROji+P4<hf0(D!sSGcb#UK`Yfu^l
z^>nR;@c|#|vPvPGyp4zDLeC733~)ui<qYsh$sUqZOkr3$FR41Wyjym`Wn{jf!PO$Z
z7RF`dyfDSpq72H7B%};#kIa=j*`e@zu;8#H(kW{EG7m}1GtYei#^s|3YKY8u(9zQc
zN1E|`5A$r`=RPPp_8Er;!uMyyzdZc4Aw{g!Py@W5N6I4y&d*D)l6qd6p0sgj)l+J%
z$?x?vcnhS6?r;YBkXv|^Otmzc7dqRNFbO?@l=o{+K1(Sh52m0uN>AW|Z179BeRh_P
z%q=XdSeiwR&1Eq&?>obwkW~8c5|dHKs$S%MkMeW*&>pF<9xt2tcTzF?x{rmfO20PD
zzv5tCP@Pl#eLuEM-_-~(AP0f``TrT){xh}xBdU<IU8DXHS76Tk0<h1{*bN$w%F+Ld
zE$7@`Xn)a}-}~O47mB!#)39WNCGg|Im|Sriaz*t}?**C@ap6C)$)DL8v?OE+|I$)a
z#qs{)pn(eZeF&B1<G;3sr4Hu3U1$%!$8YxX_ekWxcaL%o(to5MSeS#+y*clo`iBN%
zcQm^fd?FpKiT{q8|91-g6n5^Y1gmtSVEXI%-yaqFpMf+8bdC#Ka*|`Bf!e9_#ENtm
zp@Kj`m>>}Ezk+HW!j=fYFbEqCfi?sMl??RXM!`az=}@a!|9F{TpPVV~(=cV<VpIK@
zh5KNEK-~XIlf-c!$j(FX|7iqV=ulC4?!BHa|D@RhCG~RssfH6B1mgKu8ef6?Ks;eW
z?tfQfr{0@<#3KHOB>RWN@h{Tv$M+;ASIYYiO1RRZ#)|yeB8O>7VEunNp{{hO-T(Dc
z!M?lxJvG?upIt7c|8ig*s(3Iaw|ft-8y%{H?!D*OfaJa^9MB&r;K85o|0Dpae^pgy
zc27dM5#P720tr*O#sGnc@&3R3^?w4d+?emnXLP4!5w`(>G!*q^c~mu(__QGye@+bq
QqWJSN{As+^A14U(A2?J-8UO$Q

diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java
index 3855dca..f707cf1 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Config.java
@@ -14,7 +14,7 @@ private Config() {
 	}
 
 	public enum Robot {
-		PRACTICE(0.7800, 0.000409, false), COMPETITION(0.7239, 0.0002045, true);
+		PRACTICE(0.7800, 0.000409, false), COMPETITION(0.7800, 0.0002045, true);
 
 
 		private final double robotWidth;
@@ -105,7 +105,7 @@ private Input() {
 	public static final class Elevator {
 
 		public static final double TOLERANCE = 0.1;
-		public static final double INCHES_TO_NU = 775; // TODO: FIXME
+		public static final double INCHES_TO_NU = 775; // TODO: improve number to improve accuracy
 
 		public static final double HIGH_HEIGHT = 72; // inches, this gets the scale
 		public static final double MEDIUM_HEIGHT = 24.00; // inches, this gets the switch, exchange top, and portal
@@ -116,7 +116,7 @@ public static final class Elevator {
 
 		public static final double NUDGE_DISTANCE = 1; // in inches
 
-		public static final int CRUISE_SPEED = 2000; // native sensor units per 100 ms
+		public static final int CRUISE_SPEED = 2500; // native sensor units per 100 ms
 		public static final int ACCELERATION = 2000; // ^^^ per second
 
 		// motion control constants
@@ -162,10 +162,8 @@ public static final class Path {
 		public static final Pose R10 = new Pose(0.83147, 5.80987, 0);
 
 		public static final Pose C0 = new Pose(0.19177, 0.42835, 90);
-		public static final Pose C1 = new Pose(1.29388/*testing values 1.29388*/,
-			3.22791/*testing values*/  /*previous 3.12791*/, 90);
-		public static final Pose C2 = new Pose(-1.29388/*testing values -1.29388*/,
-			3.22791 /*testing values*/  /*previous 3.12791 */, 90);
+		public static final Pose C1 = new Pose(1.59388, 3.12791, 90);
+		public static final Pose C2 = new Pose(-1.59388, 3.12791, 90);
 
 		// we can do this because every point is measured from the center line.
 		public static final Pose L0 = negateX(R0);
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
index 7a50dc2..d8f1c81 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java
@@ -10,13 +10,11 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.usfirst.frc.team2974.robot.command.auton.GamePosition;
-import org.usfirst.frc.team2974.robot.command.auton.SimpleSpline;
 import org.usfirst.frc.team2974.robot.subsystems.Drivetrain;
 import org.usfirst.frc.team2974.robot.subsystems.Elevator;
 import org.usfirst.frc.team2974.robot.subsystems.IntakeOutput;
 import org.usfirst.frc.team2974.robot.util.ElevatorLogger;
 import org.waltonrobotics.MotionLogger;
-import org.waltonrobotics.controller.Pose;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -35,7 +33,6 @@ public class Robot extends IterativeRobot {
 	private static String gameData; // for ease of access
 	private CommandGroup autonCommands;
 	//    private static SendableChooser<Config.Robot> robotChooser = new SendableChooser<>();
-	private SendableChooser<Boolean> doNothingChooser;
 	private SendableChooser<Character> startLocation;
 
 	public static Config.Robot getChoosenRobot() {
@@ -71,11 +68,8 @@ public void robotInit() {
 		intakeOutput = new IntakeOutput();
 		elevator = new Elevator(elevatorLogger);
 
-		doNothingChooser = new SendableChooser<>();
-		doNothingChooser.addObject("Do Nothing!", true);
-		doNothingChooser.addDefault("Please move!", false);
-
 		startLocation = new SendableChooser<>();
+		startLocation.addObject("Do Nothing", ' ');
 		startLocation.addObject("Left", 'l');
 		startLocation.addObject("Right", 'r');
 		startLocation.addDefault("Center", 'c');
@@ -84,8 +78,8 @@ public void robotInit() {
 //        robotChooser.addDefault("Competition", Config.Robot.COMPETITION);
 
 //        SmartDashboard.putData("TEST AUTON", SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 1, 90), new Pose(1, 2, 0), new Pose(2, 2, 0)));
-		SmartDashboard.putData("6m drive straight",
-			SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90)));
+//		SmartDashboard.putData("6m drive straight",
+//			SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90)));
 
 		updateSmartDashboard();
 	}
@@ -96,7 +90,7 @@ public void disabledInit() {
 		gameData = null;
 		drivetrain.reset();
 		motionLogger.writeMotionDataCSV();
-		elevatorLogger.writeMotionDataCSV();
+//		elevatorLogger.writeMotionDataCSV();
 	}
 
 	@Override
@@ -107,6 +101,7 @@ public void disabledPeriodic() {
 
 	@Override
 	public void autonomousInit() {
+//		drivetrain.cancelControllerMotion();
 		drivetrain.startControllerMotion();
 		elevator.startZero();
 		motionLogger.initialize();
@@ -114,11 +109,11 @@ public void autonomousInit() {
 //        drivetrain.shiftDown();
 		drivetrain.shiftUp();
 
-		if (doNothingChooser.getSelected()) { // if should do nothing
-			System.out.println(">:( Nothing has been chosen. Scrubs.");
-			autonCommands = GamePosition.DO_NOTHING.getCommand();
-			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
-		}
+//		if (doNothingChooser.getSelected()) { // if should do nothing
+//			System.out.println(">:( Nothing has been chosen. Scrubs.");
+//			autonCommands = GamePosition.DO_NOTHING.getCommand();
+//			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
+//		}
 
 		while ((gameData == null) || gameData.isEmpty()) {
 			gameData = DriverStation.getInstance().getGameSpecificMessage(); // "LRL" or something
@@ -126,6 +121,15 @@ public void autonomousInit() {
 
 		char startPosition = startLocation.getSelected();
 
+		if (startPosition == ' ') { // if should do nothing
+			System.out.println(">:( Nothing has been chosen. Scrubs.");
+			autonCommands = GamePosition.DO_NOTHING.getCommand();
+			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
+		}
+
+		//TODO remove this when we have the elevator able to reach the scale
+		gameData = gameData.substring(0, 1) + "..";
+
 		System.out.println(startPosition);
 		System.out.println(gameData);
 		System.out.println(GamePosition.getGamePosition(startPosition, gameData));
@@ -155,7 +159,7 @@ public void teleopInit() {
 		}
 
 		drivetrain.cancelControllerMotion();
-		elevator.enableControl();
+		elevator.disableControl();
 		drivetrain.shiftUp(); // start in high gear
 		drivetrain.reset();
 	}
@@ -188,8 +192,7 @@ private void updateSmartDashboard() {
 		SmartDashboard.putNumber("Right", RobotMap.encoderRight.getDistance());
 
 		// Selectors
-		SmartDashboard.putData("Start Location", startLocation);
-		SmartDashboard.putData("Do Nothing", doNothingChooser);
+		SmartDashboard.putData("Drive Team/Start Location", startLocation);
 
 		// Elevator
 		SmartDashboard.putNumber("Elevator Position (inches)", elevator.getCurrentPosition());
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java
index ed3e3af..7a0f495 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java
@@ -8,6 +8,7 @@
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.usfirst.frc.team2974.robot.Robot;
+import org.waltonrobotics.controller.Pose;
 
 /**
  * Command for crossing the baseline. Usage is: new CrossBaseline().left();
@@ -45,7 +46,10 @@ public CrossBaseline right(double yMovement) {
 		isOptionSelected = true;
 
 		// drive forward x meters
-		addSequential(new DriveStraightByDistance(VELOCITY_MAX, ACCELERATION_MAX, yMovement));
+
+		addSequential(SimpleSpline
+			.pathFromPosesWithAngle(VELOCITY_MAX, ACCELERATION_MAX, false, new Pose(0, 0, 90),
+				new Pose(0, yMovement, 90)));
 		return this;
 	}
 
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java
index cac8bc0..022921e 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java
@@ -31,7 +31,7 @@ public DriveSwitchScale left() {
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, L5, L7, L8));
 		addParallel(new ElevatorTarget(LOW_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, L8, L9));
-		addSequential(new FindCube());
+		addSequential(new FindCube().left());
 		// TODO: gather cube too
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, L9, L10));
 		addParallel(new ElevatorTarget(HIGH_HEIGHT));
@@ -54,7 +54,7 @@ public DriveSwitchScale right() {
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, R5, R7, R8));
 		addParallel(new ElevatorTarget(LOW_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, R8, R9));
-		addSequential(new FindCube());
+		addSequential(new FindCube().right());
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, R9, R10));
 		addParallel(new ElevatorTarget(HIGH_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, R10, R6, R2, R3));
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java
index a6ea8c3..e390f03 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java
@@ -1,17 +1,22 @@
 package org.usfirst.frc.team2974.robot.command.auton;
 
-import edu.wpi.first.wpilibj.command.Command;
-
 /**
  * Finds the power cube and positions itself so it is in front of it.
  *
  * TODO: make the class function
  */
-public class FindCube extends Command {
+public class FindCube extends AutonOption {
 
-	@Override
-	protected boolean isFinished() {
-		return true;
+	public FindCube right() {
+		return this;
+	}
+
+	public FindCube left() {
+		return this;
 	}
 
+	@Override
+	public AutonOption center() {
+		return this;
+	}
 }
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java
index 03c1ada..0612ab4 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java
@@ -28,7 +28,7 @@ public Drivetrain(MotionLogger motionLogger) {
 		driveMode = new SendableChooser<>();
 		driveMode.addDefault("Tank", true);
 		driveMode.addObject("Cheesy", false);
-		SmartDashboard.putData("Drive Mode", driveMode);
+		SmartDashboard.putData("Drive Team/Drive Mode", driveMode);
 
 		motorRight.setInverted(true);
 
diff --git a/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java
index 0efcd1d..2dad267 100644
--- a/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java
+++ b/PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java
@@ -16,7 +16,7 @@ public static <T extends AutonOption> T driveToSinglePoint(T auton, double eleva
 		Pose... splinePoints) {
 		auton.addParallel(new ElevatorTarget(elevatorHeight));
 		auton.addSequential(SimpleSpline.pathFromPosesWithAngle(false, splinePoints));
-		auton.addSequential(new WaitCommand(0.5));
+		auton.addSequential(new WaitCommand(1));
 		auton.addSequential(new DropCube());
 
 		auton.setOptionSelected(true);