From f2f99a357ca01b4931cf5deff4ee036d1a70088a Mon Sep 17 00:00:00 2001
From: Benny0Li <bennyli2011@163.com>
Date: Mon, 19 Aug 2024 10:30:46 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=20AutonomousProgramTemplate?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 RIC.zip                                       | Bin 36431 -> 37940 bytes
 .../DriveControls/SimpleMecanumDrive.java     |  19 +++----
 .../RIC_samples/AutonomousSample.java         |  49 ++++++++++++++++++
 .../org/firstinspires/ftc/teamcode/Robot.java |  16 ++++++
 .../utils/AutonomousProgramTemplate.java      |  31 +++++++++++
 5 files changed, 104 insertions(+), 11 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java

diff --git a/RIC.zip b/RIC.zip
index 3251f28271465d36cfd1b6dce55acb4e863b280e..050551c6a8c710b4e7f3ec6d8e132683f50043c9 100644
GIT binary patch
delta 8920
zcmZ{J1z1#F*Y+?&cZYOJi<ESyNQZQHcf-&f!V!^{lx~m?gYK4+mXK~GKH$^$@%i5W
zpX)j^v+uR`TKlZC*IE0z*Kj(xwh&y22Ll$90|;QK;F+>MnzLmA!Z5`5h%U^N2ZSlB
zCIN;6d=GU&p8bI!uv-t%9=yOGhz%hR0!>!N(SomeDzpa=0-fW4K*%5xkcxw`gDb1G
zk-L$)8WIRXR{YWY{&Mp~0l~m-LO`I0uP>T9s<_g){`&rs1Q=7Lp}{|K#qDrK(UKe)
zIxk*@t8t9?#|^p1yBJk57vq|UOi19gP*_^U$f!hVsAxn+U8Q7vU1xwoDc?V-;rJA!
zC8JWMX5KW+dwk0K?Xrm&f!N}ZP;`KFJo?SfCH^T%KW(P|3EINr23t13wXROfEWA<Z
z<AiI9nZo8<l?{_`#%kn-t`wEKpL-0=NQuo>s+Bi%!rnKdrrC=yIGs{%Bm|qkWGeW?
zO{wsqvwbx2`sL??lsRKZXqy1hT>nfKN+p_bf0HBoTe%Z#_d(>aaGZu0B)0Xow~gML
z-3p26WgHov08&;1?%YqrD%q5az030W<}S%GA3dA&6SnQ<fwL1M$7)&Or)+3_#@pYb
za?=jlCA+;1zjh$(TN-|)bJ9U0xA-#dWBbGFO_fWx(Kj?yVMiT(73hk0YRsdC7MFHq
zR2ky^PoKZpMfk(hoVxD_z9>A8pI>mAs`gmY;jT!OwCwLs5)$`o{l>$}5rJUovGF{>
zd?goMCN#<FhV%j>n7eKuu9i9(VX6Ko9+KX<s`+*+IM+hNoFddtfz4V6o}DLhRCcj_
zQ%px8wjd(t1-rc{T?dpllSO-ENroLJ36HWGC?4sFMrrd(CBQZ}6C|!F&Zl8q)IOBR
zHG7rJsVKmZ23A*d$jdn#6KCrU@Z?d|ThrW7QrVtBpbS2En$YdZmZ9g$4s54Gia&;@
z!67(xQo{8;OBot+v|{vbAe3i$`E5Re^;TI=r(%#rn&U(9ekOE!56bl7))(q$vMuZ8
zR?u|w&fP~prc>AR(C>6mhm5Z2oc-tZVb?-ZY}S6KM9{3cYmB(ZHCD-spDc4{eDIq2
zhl$$n=o{T7cKzvT&kA1)c)SafG(V(zTGSghOD*i*2gz{u?>(w@71)xIl~k9O`*Hm0
zOYbeBK^QVPCC3dKC@wpy*MCg1PE+zFZ_Udbt1lTc#1Mv}`sHVBS@p8OOS&tkG)u$c
zw+63>u!mD&$FtjLsJrwig1a0d8E&I0U+Ym0m}V8?9rbOplZQ%$uZ=BG__7b?=<@SR
z!}}lzC%stIY|%7mf_;rfki4^VOguhavcohw&sZCdQA_L!ZGhMK*76D#-k~IdlUBS*
zZU~OTviNywp*9&Fa*FZxLf|kxQ%y&|OKjiz&%*WdQBzGErVNSCXNW6@lJ5$5u16AQ
z@CA#&W+Kjq0_$%Ly_zH}p9u<!FgkbbaLXIxQ}nltKWmP>aaHs>$k;>bCpN}gIa9_s
z`AWik23wBO2*nEyrGgKj3bjUJkVxS|boB2v!}}cF0dRa~MwTuy!GXuhnw(WsW?vS0
z6q&Mq!&47&zBpKRCrW^_mZ^toPB?32)1zh~445svidc1$GkfVQ(OvL0+OM=OT632)
zU<IX*aktuQ&N;*9yncN|UoGo~C0!5Qf-GdXJal@W6Izl}$+v?2jz#kHw9#?dsKL3t
z$(zf!310W>Y&OT}ck>il-1yl%FEJq8y@>v>oS91mnPdN*`IAKoQZ4dKgwUDS(khgi
z&o8@|%>z@5yeJpSS4=6qa`Q2e*w87|ZH}pW(PZ{_u8-c*;~gr_*y7Gf2j?s$)*A?`
z4LG1sh(k-KFj4tJxFIrN80ZbUY%U*Imcm=MpIY?$3HOO|F;e8n*v7AFiR)5FOW`Z5
zeicL7snsi6to`o9i{~7tgmO7uDU=sWtUlUEkrN9%=2w%eS0hvM-L_g^Wf$jk-ahs#
zP92~(;*;{e)Sb65&fp7WN`I5wq+e}YjpPQWa|3nSh!WhueKA8#i#`2ecqdn!n;UVq
zMw+z6l?#jo$H<djdX1~!ip8QIvsbDiTzV|D_CeD#nrzq+$L;%?ir0jBmsU?a-OtC3
zKfvVZYm<o=-<TQRitB3jB3kOOX{@{)qqMc&wvOsr8-|dk!J{JwSUXI7h{O72e%>8V
zOg;e3<gITbvgcs_#mIs%H_u!*INRgVm#nx`qgq!q`LnNLbY;%bpSQ`UtnEyoNeGQm
z6MANH;V#RV7p%uKXRJU4OUyDOqMp||{R`c<x*c+}UNcLNJ!piqZABMsL?w<mkht3$
zrZnT8{JLZ(DnBt<Pp{$&a`;+>L2Z16KDiZCB!Te61kyTNu$tL{s{Tom{bTKg2W8K4
z@deb7EG`=n*6MSwpqM?=yog*q@IuvMCgH0bx9ZBI$K?5W?$@uH>YteUiIa_Puk2U*
zVrQE#XXkC+$);jeQAYVUXkAx=@?LQ-{{Smd>w4lY_}9U)q^|3XQu`~GnI@-Ds}R+h
zNqy$=$;dd3PTo2Gu=EpqQ5V^QEc9D@wEmTf-+A8IrpoTNYwb~!)5(<J$}<XXs>o9L
z`fE5PIS4E==>9XQilg=L=l2)<UHx=(wX$`2@=!Zf%0HT`l;4-=ngc9PMCiS`+5U%!
z*DxSZ++8K5`H#A&o2!GpgPnt$i;}a0g|m^Jnwg!Wt&ywQKZW?-*zJE8;@AXv8{(&!
zVZ)0Clm-Kjg6KmtU!Z5_V@Awj5_o4$Dv)5w=Eg|5M;}5fpc=%`5yHgrYX02se)@K>
z$EY@HpKY!!c_Yynw7c@izgUVMgn3VBmC8jIR0&~6v#)Ux#=jtq<dWqO9p;5K^;1L=
zK94@0OhzpiXf6L0M#yE)7C{}AnnRAgXDhOl<)o#rAi8hqXVTnRfg93G+sYVD!a6F_
z(-wzMz9xXG4<*+t_sFg?eZ81pgSL|tfI9WmW9Si_9yYvcAHR(3f@!u$rgM{pHphj5
zrbItdC=^yaYr@N`&W5fcy*S_VHNUU#oXV-m^lD$2ZH0YslKT}Aqe`_g!vKnpiq|yE
z2kLe|@ZN`xh7iyEA(SoPBI%uY{72~bFQzJx3Z_C`SQ}ULZ`yU;BhJQ9*PRFwF{Sp-
zK93db&qo1Ih=5vkhhL_o^=kVo+}9M1PuZ6?-1ubCpVk`)smJ?qoQU?4On4g8;!>aO
zu8*KhEJU&i#$eQZwY44>@x}8E_Ze8cc5o9b{`x|G(rml!5}`)_kt_WDDQtp*`uCCR
zZXb};5K)%H-ow4LA`dWgRt*MS02rLC3?$Ig!rz~?n2PUr49@-84CjKdnhX;JYUTrh
zXh9?ZQxHA$|9>W1C$@3Hkv^)qVk+=;n?UYoO5-msXFK|0sN#f&fya|vl2cex$C>y{
ze|OvcB{y<*)XPe*x6<H{@Wxjm0pf6<NRgh-gps?<CSAO4cNDBx$e8TIUvpk&ETTCZ
zay>t>Y4Y*zQiqnY<tII@{!y(xX4gF_|Gs!`Mp@2Y)U8T`K~u6cZuTeVqE$+{9np}I
zV!VElc6~HwjirwFVP$bj!Mb(1eag0e|GW-I3}&IBx>}i1XFZouqj#rrbSek0k(IkL
zi{(b`Mj`)Jd^RhluI&=C`)(5-tABqm?96VAt&a9+c?y)SezIkKiRDYV$ZZfxxfzZj
z%r)O56%QS_!4ct)+-&i3O+~7cu({%T*nSwMnWH@i3gRv#tp`6Na2~xY@?n0}wT15^
ztSmF#!8Ja~<Y|x;F~g!VIEhB}I&QF>V1-8`-+Jj&MR={W%VQ@=YAJug&sQ^>--_$i
zJK{{-?6|_9QG?oFlrcKWI~;;|>}|+Z$V=m=OB^J!WdxjY4Uq@fgFc7P)=I%lV$Dr$
zXG?vITxjAAjc#^Bh#6NclPC0s!HYo1f$%BclA-c{(8v~wk2}fZi4YePfeCE4fe#<H
zsn>7<(IhRj5Z%Oc=WZl@=~Ox1VA%grmZdT5gAyGKjq}(Ff^NL^FmF3u_j+q}?b+<*
z5c*=sz^-)gw$@wq844gn>0f}#w>7A413%Yi$dDn0kfHKsfGp>8%>gteB?m5xQ2x6M
zL_8$=hZ>p2K?MuvlYvCMy^Mp~s--MVSfMNOp*k`>J7(b%R(*SY&v|dJ+$!G>Za;vo
zBj4jGsPpkMQNa24^3EPLvaYM0_^Z_{Z@FiIWCOKi*E4+n`In|&vYO{LFUudN?RNC3
z2|ddokFw0O=H(RnQEk`1nETaiA*R2r%~4&q7p+hymy@f;b_pG}_51nM`}Z)do%CF5
zPXx~GnX$(Ou0P*iS*@<y-Tu&H=Q-z8qIyxMKLo8O^MimYgl;qGqm8*VHm*Yv7Ux%|
zq6r3ZhUqcBw>A=^Etw`M9kA*jXAM;`7^iWZccM*we<hWkn)ZPux$&HAfh~Q0sRLAX
zG)m?udmumKJx}CyVX7ee(b6{ai?rIZQ8ilHX9f}TWkznMFNkWB(U)w7pG8b$Q5p|v
z-b6u7LinBd`}IE|&cn4MKrkOEp{ID#A#;}Ig$G^tuwkNo_u4bX%4}fun0W40o#h;y
zpUwRRjsb<p=m;7A-DqqoytMS3amW#R1@cN*Pfyd}NuE+Sly8R@$BYJ`cTWwW@KEtk
zYjb9Gh2v%o3^rcX#+eBj)?Grjea)J_>MaA!e@h&Rr?#xlhC}~U`UiLY_iG53%m{k<
z3&K;8$tV%mN~AH~t!!I&-*203b~k1F*OwJwp88#V7808Ueg&R{jL33K7oKgBnDxRN
z=cn%lXOkF;f-qS!xknns0w+2)R)ExWXz!V+$$Tcea-{GZQ_o%XA`!u;Jl%0k|3y!z
ze^aCD06(;>>d5VQ=&-BCXK{;|GZA20(DXsNL^P}iu6VD8+N^Bnys;dH#~iQ^_cGvN
zZ_l1#p0tEdUzfyRU*<!}`8)Av^hwRl4MSu$)<<K@>b=L?{X5%lz*Huym!VTXdZ!|j
z=V~UGJw3<iNT#QdS(6JpLIP*|CI+EL`i7wqv;pt4H>8vbofDEc`M`|XMo*Mp5X013
zO!w#;-=NP4^L;7fjY4~gf|5WFi{XbFCqcuY$7a^nbe-g}RsF#m^@xE+zDGc}#PE%m
z!@8G^6;ieRgeN+U!OGgp%tn<<@#0XE5}IBzehe#uN*O0e5K;>%*8;sU@yIGPDrtwF
zW_&Q^M_%G{$wnpP)Lx18JcYq}B}|@N#<gY?3Kub!3D!XS0I!~evYN*f%U%TJocgSR
z4TDU=BGtIAB1$8ez40i~N(P*n8G*02%dW?N*|t@+jOhw|7i{l|XjP>>#mhOxv|iLc
z+8?NYqt7}0M#O}DutC2NgS3Mcnt_Y0%J)KSg@rSu)(89W88{Ky9p!raN4hH?USY|j
zO-A+zDz2vDG}ij}T#!U@b)<n2Va+~~!EUz5Pw(T(D;B55K9s^93JXtb{~8h&Mwva)
z*pCLw6b?^CHJS4vuFwZyS)drLFcMdMEl?up{*{kQHwI!^!)r``skcN;4OJU20Mu-E
zRJz_0Fs;3GYEg%0hit{X%9XQAR7okWcDq$o!ojtvX!Glnwp`CF)TPhXd7>bOiE1r-
zyF7hUXMsu8@hxQ^o{QpDsK~GJ?etsIQDL_P^*I<ghg}XHCdYAPWC70kHi=CEEu=Gf
zm-Vn`ztWkzmp(}j%lP7F`$ESCzy<mulE?~Fy*>zHvPkhMxg<|EaEWdhdTz!PKC++s
z*YosGbaq9~e$aVhG*MJ@K>gtdj~MGgma8`HX#+J3uv^+RMbA1%SOAN%#Yvt1yht))
ziNgJvjI|_FT9Xd7HZoF!Mh6<XFGP{9TrwAO_zKAKd=Ir`&N|K%w1O@sFT90yAu~!d
z@IAFYqdJ~+(>0k_frTavbV^FXm9a&-C@%+p&W?g)YmW}5<|>(*w-|Io8?-L+#I!I5
zVDY_Jy0K@nciFSo0KzG@exx%9e`gnsM!MfphMuV`=8@mdp=@FErq*@fvu3H>F0cJD
zL6xAg1xntNF}T~$c+g7woT9V#_LoQ>3LZ^ju2SOhzeuVg#)(^<dmFNrmnw_DMv9KO
z*pqCmAj|Gfg9tw#<=q?$v^L-Hu~sb9`<XfL%@dfgvM0%k6?#Y&N@F!lM#zo4d!ILl
zIVVckHsaipr&zupSF8SRw25Wd=PTS6hj!g_1i0-8Q*JMoa&2e|VNPaxCA~{BVQrr<
zsX9}23sX``=S@`lrusp}Lh5Rio?9&klsKBKNPX3qhCY({6d%91cdX)N)0lw^=}xb;
z&Qqc;<6fbR7^1iIJTRu*@Mg_i)tQfEl+fgO$Vk@3qkKb@g+~@-OE&h>CkEK*>1J~@
zr*G-)>R-_4yE;P4-%=ypUeNaa2)@3odg0H!kiRSo!^*C#LOLYOH)nurdCghYIeYcJ
zVhiO&jWWq<!4up$Yw_sx!@$W8O$vhP29nf_LGmBuSK}h^H$J4F;$PL*v+-jljTuaE
z7}KJ_?5ZSwyV@#H?eGJRhlan&Z^nw*q)!LPcIr+0*nY5w8mEo=pwxeb-gZ=%*8%Zt
z?ZwK0w0*sE4jmhP7B{<3I>L}bwdav#>*mW_bDh1iG^<HzJ)~R*-z}hFy*pgVKR;;l
zFizlXYQHEr-7F!iH|%cXeeX@P#?YcJHn-mArCBLf-lq~d=~(OT?i`t{W;gHBji*VY
zR@E$WaN_nBY9whnqG%rLi5Pp6;)PEay^ER{;cZgpwOw>u@#H#`BQ`0iYgT>#OmcsZ
zK}&gMp4qMF6aOoFR|z+)41Q0U0W694c8^I>eaA+H!Fps=eaKz9(fV^t%Ghcd%pl0&
zCsSrsi|O+s-0Qq(#|ZiYYr~o!3TJ3zlG_WxD#Zv0Q1ta2iUZOeLlX2JdsMj{!cXk|
zh%@g$WEUlxQNxK|hPm?8pqkQr*)6yVu}Gm$77wTtgn88U>nG;EzYM&KtBOv<lax)-
zjK1_lL$z7{cdK>QSaHc$!FuOKwlS=oi(NcXt_ewwGz#M-5x73+p~xK)KNJ*Mwr1!P
zZ{lFcI&e(hZ5tA-q;Z8NEq=Rp`rgy5g8<NRQX^;~NlaP$830P0`1BO7v=aApHJ?yu
z1diYYY+o(7^BSD!q&dD;V!6JW7T+4&89F;XC@=xyILV;;Lxtm)rR<u0@EQL0uHGKc
zou>*nte;ni><k7x#(W?2NdYa`J~-$xk)n1BnpWzrNVc_(88N?tLxIQAd226QZnJxQ
z{E4|KgpO;>@Ch4!P%CJ8Q)I!(Ns?v4mbegGqsIheVfjXwqbL|3k9MTBrC*F-9|J9o
zY-6@;1=$8FVK*DXou{;4(M}Y4ma&-C(2h+Nq$w3_rl}&ojHaH5Er?~GmY!osa4N1T
zpUX@YZ#n^UlxAum(-C}f`ogjUZ1r?W8GDw>9la-~P2cDccVE`pn-(FAbd*W#;!t+3
zIO#F9#v6%YMC18I2GaQ<QuleG$VQ>^ur&|iG;UdFJmzGocmSe4;#;A_L8&a3Dzrf|
z`;lqtZFHTqkGWGZy%lu)-A=47)*riOUvK)M?9FNR#>d8`P;*VQmqn?s>LDXPYCtJu
z*)A*uP^4QGB=X{grFvbrNw+4Nvw@{r%TKWQPSc2bTC}N8A)Vx1#!;eT97VQgli$Dn
znkMsxqQq<Yv=<Y!M#pPEo|Y*&9JRJN^C6`Co>k(qw)N%6*JKx|JGPBlnQ{o<5U_+J
z1-z-J%QC~cX}Afg>*K1Q(bb1y`QXGp#>VK!5+Z&bmY=E|M5;>Z@RsIvPY6XX^uW&1
zbFFf31a~4i8WgM%Zrb6E(9y|5K|lxN8VwSJ_MPE#lUxyh<?1Ap;nd1w7p4KJ7VY6x
z@s=`<rAf*rEF^A;SPSa8N*C+RETc^xTHq$mO_ATQviirv+h5~1(3z}TBwfJ}Wx7Qx
z9XAW?z`JFrX-ZPAA(vBYzG~watE7sRuc__PS1%^F>}b&<gL&$TPfFk1`Q`AUO(0qv
ziaj;6xu+-Ii*OZk+~7K;Xi0xHJbaOpfR%m%@NptVk6q7AJ&^&SK2qF5*Jk{x0axdL
zG1?4OaNYYTiz=-)Wni6!(#NqVQ($e^+9K(_2O)C)DCSGK_!1m?M9hmkp(#YbS(TQo
z@au8btMb!X?3db}<fJ|<4ay4yg^8bVpfDWtLdc0`N~Qk!&v&mJ#k`Y0J~u5aqJ~K*
zop7IK>6|cOa|0Rq04&|au_sA#dB(e*$BPW$^QU6Js?0?$Tsd}Nrg_<H_t{^gtoeRk
zVkklK*h9u*poDP?P;|ggJgE%clJda|O-7q|-_maro78M-Q@&_g!G6U*e07-*g2uoE
zyu?~+Y|hJRmebw7?bFGpQ&uDGwcj4tWWCf$qBkuSA8b~H8|<VX%UGntNVL&+&c#EI
z-Vx%XlGYg|#t?H|N+IrLIF$YAL?kQY&xwZNmve}IMN*hpFwA|!v%vogj$!2GMz0R&
z=SFgCSFF>|ax=myRB^MIu=j;jtWYbiPYd|#!qqkK?0FC#OxIaGO!F}C0i>(^6WojA
z@-co5F}IR1k{B#2>TE;g&i0ACGoLyn%>E7uT$x}*1qxL<%n5msP8pB%M{<QnZJ72l
z<LFo?W7M~^CI@Y6HelXK*9JSSeYn0Z0<0Km-i%1<j)cq}a?-+4A+xJ}(&u9Ijwu<X
zH6pmf@mM(wDRT|93?4MM2YXYUq&iwgPpxe@Xb|z1AL|(~)C%wK8&yPo4kmPd0vr&4
zq<wB6|Bz94tb!je+dpK(LtkiwSMd+eK+8w=&^(gl=l?%Sx%?Wxm4I9R)O&g&P2lAp
z<U*kK9w`_6V+eo<h2JAFLb88!=7h@bktkukKgh1|hkK+zMC~_HEn4~Dy;@B4H&Syc
zCJwn*5|VoQ0NwSj+@yjYv@WIpqa`9k0Ry>X&HxixD)_$`wE|fwxCcfJL7Lh3Ef@sa
zhXa8q?;BXM;s!28cl4Q=%aeb2HPr{?-*lQq*?;x5GSu`OX4!E46#PSzA4?l+h@NDN
z%b0~K$mE)JOm+A2BpY&7ss4helfT}tIaY=WX<IGeGF18P3kr3A=Yu1N7l6x=@8cfR
zeBFmX7GOq<iyHV{>~RPFcv{w*?~?HDm|^TsYDKABhZ=f1H|wMrm^o4(7pNhlrrDsh
znjU0MkW(n7)vUcwwAf3lDHgMYpt`?f8*egf>Jcnbj?~`LO_ol-$uY5AL$|LL%jM{Y
zz8Pam4O}W7C{ap#^<9_t_@^!W{%2NYz5z&4<7m<HCGtES$t@N<Nv=s{g|_2sZQlIp
zaP?wObW@p}Ph-{24BL59$)Ol5(lwXUvbf=rjYD}9RI5{o$6#<frkXPe&#Iz-c<1Rg
zdQ1XRh-h&AI6LJ~&oASRp79wAI91vNDB}#!gt95wE$AfRnBAv9$G+8KDj?fM!W*lb
zd9t0OaKe^aUf^NSvwI!HO!uu#@l8o#v9BLKjSYhM5nms-uKBBrvFoW=#7VsFqSkR9
z>`mVSr@?2Ifni*&f~9c7TF5g&qfs9je5%$N;U0tH;A)iybi{2&0|!CK-d@Us&`k03
zMB<;zfpRUA=8^MD;LPWho5-k!RxQI8bF{BuG96yR%(hBY(bg{Tm2)E52bxxPD*8_u
zYH|8PnB;e?manp_Zq*!1dwdmnS7-URN_S3Ir6hxww_?>&VOpDbkXiz=C{BKsor_j-
zcaS4I`4KX1_HN_6>&vk)SeTS<5;}R}P&7u@?aylVCLr5Moeb9YGHZ7B=VVp6>PXQe
zxoBOacN}p#VcwlMy-E4xdiL+NYZT$FKKOj)r%>4Il3#_bgY{5e9FZl?a3D`i*s4D~
zU{-nNY2q->CGRtdHobfHr1^SJ{k%_HIDB}S{65!l@2F-(P_iAoR$-uN&9MRj(|rn|
zdb2_98p41ml;nP>=Pmg6u-RMk?;)kn^WQ@XAkIe){oh?+;JXj?eb@KS7CulJ2>+0K
z%)Z3fS6HC`klSz|$XzPly93;O$-n~GK$5R61Q{O$jN;(|NPbib|IKw1K9J;h)HkR$
zu=dd(TmNL}|C{;#%M+6y8MuH1kn>ZAoKb*)eljAU#gFR#ewX}62r;Sum^8C@v%6>D
z@238a3Bcx02G*bkl>AM=Y&1aK-OmXP@Ca%I&ZPzXpcW8wIuO85LjfQJh=9o-1F`|)
z_h#b)NWgIPKxx2#f(!f#AOnxk1H^%P;6nz$J5b=Bxorp}c{Iof0!cVqxtobQ*t<GA
zJhTAr7Vy7;B%%0kHuvvv;4uM=LCk+gom@=r->n5A(Em;VfiOYWqKSZ0FbPl@1o!6-
zAM``(L>VA=0p_`T8}j_t|KE78Kj>ow)BM(dc(N4#*9XnNd}s>-Mj>>Ff5-7YC~d(c
z;D!eg@Rgqw{+}Q}n0t3X_$KA<5l`P;BL7-KrO=%@=|Ahex6~U#0{$d??*u>#CHf<I
zFyn{Fm7aKK;^W=L{x37xBEKgfq5XcD|FeR7qwS$2;4IMxoep7|zm|6|lnWyPH;X+8
z<HN}Qc)XVk-ATG7?j*#(f*8*KnI;rY0#TI#0g6(8-|zjj%y1Gg`g5Qq{OR9;1y8Cz
zY)KC}(7&qDLn!~`jeZ0fxK$4Dk1&O>DS`lId1?SLl1lDAZ~k4;{@y4M<$qiJCwu-?
z-|j;4#|>j78RSeA1l%f+1J;ou-~<hzG|~(`OA`c=F>*HbFmk>tlK^GhBOt&B84!;m
zgRE+TfFG)afNzut_@fTc7NrgGGy(yEx;Ox9^c`dbI7XX74opG7r2!d06iWpV#E^lt
zEr4e+2H+QVz}px@FuEh~^KRtm4(P<{f{(m_##j|_vk!n4$BF(7^bBN%2Ld(v-#uK=
F{{ZT_^U?qS

delta 7410
zcmZ8m2Q-{pw4ND^-aDh$s8OSL(TPq-5WN#!$mqR|U+)o#UI$SkqKgnBNVJI-(Q61H
z@`jtc-n(zsI{*65?q{F9=gfSkxdJj?0BN8EL++>p)L>R<aBi=|lU$(Dh907z2v~vz
zp{B{b4ra+n1Xu)eEi{O62O5MHqR0bS%*l~dm@_xzJ*do$E=3;lHg*{XTt<=H{5-B?
zFb4_(nUaA(cpw<a!1Ix(uYik<zm2gGE(pU2ZTNcl1>%Fim>U=%(9NU8Y*CNGlmh<n
zDgl?+VP?Yoq>_J@JL3K%gg*RiB6eNaCm_K06y+^vmc*i16A@S2dKO;t@jgO4;bn4s
z+^{@Sq*LYuZne?QgcRwEc;J-J*D3O|$Y<~Ttnh0WHT@>_JzU<YTh6MVyk&v5)=g_u
z-r$aP8>5ev;hoY_qX{|;dW!vC70%jDaU@$T<7F!JWp)#<YRuHjDG4_zcP50kayzOe
zDQHvlaQvGgeuqNOI~hLO;M;bpIl4A;jLT51cxYag!c+OqYMm@K%86!1hJ~td7(bNY
zwIV*YUYZ`+fM&g4cXA2N43abY<1{ssx-H|K7HBONn<O9Aok*@r?7isssrcs7S7Nse
zu3=j-J)45ttBEYlv%h$VL$A40^h4+UTiwp}=QVBj>!U3{PyMWy*pYm*cdk3nGEnvX
zASZ?A;xn9HdCB=Wl;#Pxn2>_(RvY#-<6!@9)2$B@?iTYuwG)ls^_T5>Me9E6-j|+#
zCgtBV|GV;07_HS*%-$Wt->->(+mz(Denc4A{Q!N*`h^HTc7k~exjXI1%*d+S#O|h=
zzpUFJ=pQkk=^pu(k_6KMsR|vm;pT!eom!ogB6%TjTuoI7tL!cO+BW^309~X~Z%u`x
zk4bCBK|+g(=tno&f#iIyJPMs>M)scUHI0*zLg8Uf62=b(#FLdQxB4T}6wQ*p3(;vi
zm`dhr`VB%&Z2D@Wp5N~63F<vfwiGbSC1Mhc{Z!o&@T#{OJ|q5W3%`1Kr*LlY{ja|L
z_6;j0`1-5z;^`SjSBuoG0P1s^Q7l6ymwl9o$a3W8WhT8>`}OXBjzFA`>iIP_NBxHA
zg?IzsHuJr5O*6?Cj0Xk1n<js(Y)EETpRxK~;KeNjUJ)0k(5&5)fyAOFP<fK6_yaL#
zJ#I3~%xQhwb3QxoBlwXZ7S>*FlwVPMdRS+kJzR1G$LLri{Md-ZAX~Bkf%CnT>_o%-
zR-c!E>X%i<83iL&c|qa+V&i4)Y)cKZqz(m|WYi=5R=jMA2R#ES#ltw}_M(W?5Tvs3
zJC7x)N(V$$`@-<OuM^pjck}NJlQVfiv(`Z^jV*1mB3ASF@u_E?$;yN+rj1E7obF|U
zZo{4ORaG-Z%OuZAiK95H%Z{q-GehKzwQGldazCTo>NFqJ&()R}>Uv5>TrD%Hfn_0J
zk`wAtG4KJ@%-$5Jr>T64i#8}_4ys<nYdfhdK)6zs<NbsHgFa|jc>718%WL^Ued?Lb
z+}n$g+DGNZZEwd1?=^l7fDQ9AjZ%|J3>(6^mDK|?62hgwXENM3HX}8Aj5o#Z)hs3U
z%=S&qoTTPK6rpqfUAl|a;iJRD)gkF<)mF>&>Lb!h`$-i9(;G}VQ_%;3u;hH<P6!*F
zaFW~_OA9c+wF41+!`VD{EAJH>ARW@i_8ewHC(@1WQ{A)kd}(0J`syJs@8e2xBIp-5
z*^9;&5<Xd?8OE6zPt5T<VBSvNwMS1qUYKi7<V0I=8F>>?Cz0)rL||YFA&Fe39}!Mw
z&e#N{G19N7kKk$v|B<ky$}n+ysN2dbPm8lq@lF|%6bUr7m?Qe3WhXyFeG;<qo;dPJ
z#nb>#5w;q#rJ?$xdz%V)X1hVR?QAFETMBMC2giCvqSbw}he3js=FxrTP1#h*gRgVw
zgcG;Gom#EU)^CR$nzx+&L$+8rwAKRC{bmJLyoc1=w(V6k+fv6=<Gycx#IbxiRb&07
z$$6RKp($ds@FkJsg?>)M-4Tfq-|l%mJgUjY#~W!f5a-93e2?pwavIKkyjD%x;4+jb
zi~7)oM+`l6aR*d#h4!pRr$g%7WvVCmX-t`Hl5aGW>{A@<)pq~^;+94XL=x!z*$d%L
z7**P{5v@`KvTn6be*)Ml(Z~bM`Ng|s`KU@c{t&*di~Xl+^R{NpGP5~P77nKh`36pF
zK9AG;d^*`Y8JzQ6KDw~>7JCgx!W++p3yQ&VF0apS1$MKxjoc+F5GKVyKr9%C6^x_a
zzA9$;eW^};Y5$3DfP8FWWx|jUug@%JNe;hxXsY0=PdC{|Kee)f5dpytncj%AuQ9(i
zH(C%cd3&*Ur>u+f+~1AlZ?_N=_qDfI3`DwC&zpvi)_YoY8h<sb5qur3%>f6<aCw$f
zU%4`6GYcwl=2%4RU~1|0c6$21)8@?{CQ#iM(qg)8ZKg=$aX(4v!#6rZQ~Jb!B|e6Q
zhqw;KnDTB>^-pWvWvb=d4Z4NM>$=>FFvO^z=uqcp1<r6vUqSC_VPN8cu0Qb&gyuId
ze;-ivLFeZSz{qehmK{5RaWh8bU9vzNWCP3v3L#PhU~Ftq>6~5%>746B7HQ2^k9kcd
z<yN>MKgdCva)&`6N1DJa4+k{6sMjI4=vtD9RFmA?rEbAil>`Lhkp}pAZ^MrYdL53@
z<NuwY-{@UEDb$XRf0;IibU0DfJG$?s>E@ga^r$rU+hTqA#vWCA=&>KXGMn3#Yo1T2
zxI*d1xQ;j&gt4<Msq}cMDG{_T4=sGyIl2X3uCfyOghZYy31AzrLW=gPwu1eP{rozs
zC|@=U!|!H2FwyGRZR^-H@}rk_dB<NNFvEjY9XG9SQ9L-$|EYz)k9KSGBd_s$e}n^-
zyOtoMk=UnZ%du&G!$tSpPS+Sx2xUz|6(!G?R!%-sK?xUqA5Jd5F@Y+qv4R<xpY(6X
z3Vcjz`nHG%opmXR#YfCwe`&V)ER$x}m+|gmo^Z)PpGMw^?&S#ASxhs9?u3}BLK8)i
zA9{_1qpRs2TSj6%8~3tR<UzTPqQb8iCtzxVuccO^piWx?_+{Q511K$uNinZ6-xEBP
zAqT5}qLGhZB+d(4<aV?~ig6{~*-|yj?&N}o<;OUL6`_#4_%HA2R=h9XHFdS<Z#(m9
z3HX-W7s@j~k|Fn9@~y~|!{Cz^Zi;xd6o~e-G2vqCfZmxf6}wfk>4={$Sa*J$>oq$k
zPa3JYSyac4;B)zJty2i>O)*56)auX#gDK;XrJ+)dQvw9}uT7rKV=!!Kc$38P-B)N3
z6OHWqE@n|J0ZJe7n8SoW*Qp3{_HLMZ?G5jh4n5AEMxuIGN>(38d@q(-Da`W>^`mmQ
zFaOmAWx)a*XVbeLch{gM#(LQe;<R`ob%PD3^Lko^%{0qDJ}uASKw@(r@?G+a7&b4F
zKi>@Tu#WRW6pr-aU1dhEx0^ThcG+IZQ(xL9zK#9!DR-vteeySK)Oq5VPwIWax;yZ!
z{nnHalv$A{%F;tl0_)&vfs!V7q%Q})`wadA-*Xc>5s6kRwonrkW2!&%p{D1R|6zw@
zFv}hJ{_<FvG}(whHYS~+)qS>8w6iAPd-gMRT>@`A*iS3tr9&3sLU_r~9n`NMq9u1|
zUc_8RJEBfhALVfsUziq)K3q7``wHiX^wEQG^crmsxNR@L0v#M}rK*@yR5uFm3x@2E
z$-2nGQ8K3)M_x1eohXp52`<+DCFP%Yuzc25Nk66q>rf(Rud4Q-`Ny-mUtZA10Q4{#
zW)ryu3<2CXxSbi&Y^lwow>0{b$B_3Dcw|ajZGbfBFkjPv!wav7s$e0eL3kF86JIyW
zouYjM?<6lS58HSTn()pgVrx(-&Ik+nxJr0TrmvlGOoY?MxQ}3>wvYWAQ{HdyVnztF
zV&+7t%kJ1k#S45n6eTm5U)pf8NV|vnMyawuMr+=lTST#SX62~rSej%<HlPd8tm$s0
zjF^6ie)D#a!)wCL(iM__kPF8T2~VDk_OT?cq&Q}4jNn<obAWKH$1M-~hql#2-!1fH
z;y!1rxKA2KB(5mMC7UspvQN8+obEn<F8{|5G<3T7KANrei5f0f)18l$j~LauA{$}T
zRs!oSv7N#k#w4ZDGf*j0?lihDmLUefvEXcxThhECC(q$f6G8na9!hw{14M=!-TQI6
zab5B3lt9ONQIj{$D9O3N;Bo1o!kEWo<@BzC%q-wh@pRYNuZtqd?iPY8?&iOAlu%tj
z*wWWuRHa2fm-67}2>I?G#cV@gAoEpyqXbDL?<QqlH%^;)o_+ps{<%om$xTN3bDk_p
z>hzICs}wBNb1q8xSTq}6@ir=4YIJJs$iq51jw7u6nVj0asmF*+Q7KFw78i|Zig}Ha
z2hw-_$+0&`K&6HYMbE!@wKTZJD;;bMEves27ls;se{W&+gjyiDT8;UI5-Mt~?*uO<
z_oqowOI8;1;oY(C3RfH+Pd6)hDYeS1C7Lvkb`Z$4AaG)a5_8U5_>=mV^r~(c!ImwL
zr=#sUB!NO#gGz2{O>L3fih`-ySx!79N2BNvIM`0NBHDs;f~bKwzRB+wjh*cN<<Del
zEoQIdNxd{7EDjP%UzPN=URDa)<nnUn`e4^TM{f!|`RYZf#{<Vdh&<Xi(Lh8y=||<=
z$-b0E*1q^Y*1|E73)dE@quo-wXwc${9?c*E5NG&=Y`bq%LQ=xYA{dmrrKGtFb+PcO
zp>qiYrRqBMTQ3uLoyWp3^^GCQKZvu0@0|&0m(ENrCsQok|CM6QdNnOl+ro5-M3(4z
zp1i4ujDon=R=B>5WX^-oc(;*o)-@mIx#LB9zy7Le&q_Nn3AaKJgR1aD?4lmeHK!-I
zTaC~h+eKe;6ZMZMI4~EIKK8BEGY~PfZ{34~6C$d1zXj->;qWh$Pvf`vZl8PO#3<@-
z%AXB(A5lQC&SZ|ime61}k$Az)3y+xue{p?4Hc6K()rR86LOm=M!?EKmsBsF1czaOJ
zb(TLJiV@#v;2~`rx8lcHr}Xp8-%b%%sP`b)gZ?Ne+*%g$$ygm&VC|E~n*BPbV>XC-
zPvuA}{Y6F}KVX|HNy?A<sdn}QwP7sA4BhIo+X3v>O)rc;(+$|u_hneK6gos1-fF?|
zhV}13HENyg4Fg<Nv{=BO@!FOn>%YD4Z+D7fmV12Zvkza>Oa3N;`ZP3l(ZmLnd{}im
zKK(4{I4<J(U`TS1U)77|9kZpG(F7hKK}{*I&RiuyUq!iCBZ;T#u>M4Zv7fMyi*h8`
zn!Dj0Z+q5TqS|a+j^h4?7<1ibW-~l-uEy1!pQugMECQC}SBwm?bV-a0-9H^ZJFQ7<
zKWI}MX>5T@vu8h)_|DXcd8#8igCY*#T|PB>E=6YP<@Jy-qmiCGb#*=mbG>ZqmcU3S
zj6x<|a1aKp?WuDUoKa5SIoJQ;pYd=?URyV!Yg3#mVE^o*ZPaKUmBp}YZI!e=Y{ZHC
z3`P5lQFRH*fl^{CP4`fC!?rn7`hD0T-;OCWWl{#etJNvvnL=GQ?qtJT$XSt^KD*Y^
z=3@USPAaIkbTswR1kWE|quSxgCZl+$h2Y~o)1E_rmlR&{h2;;I?tH;z1<8qslCoFL
z={i|YyXLa4a<<L9Oy;Y`Zfk3uZrY8$Rq0`2K?-rt*CI})6e3*AmJi!*72B>Eru;(e
z>E0KE2V9gnWGM=LlpE~0QgcSyvmTa4`V7D{&W1eoqxbj6Q|*szHw6gwwtNs}=_9^Z
zA7`&hgSX3RiD%aTywR{Ki6yFx*&ZavnPL1?(QW_I(Gl8yH27^|$L%Tn&yp2X_~1`9
z&C@8+?&#U@ij1q$BGPIH9H;E`%e_>&Ldf1W92WERw}(<a<^9#yP;I=u4wS95ANhO|
z%A#TH>ZfC_Uu|JXN6_=TSXta$p55SlUaMcPJR%9n@mks_(_PvbI@P`%$u-(ZYkOpE
z<SFXGSQS>!@%MYg_P?-h#(ceMJvrEBY_%R*mnskDUc2@BiPHSqNRU~BlE9Qf{IplI
zKivOuys5eSg3l188M{#<z4E~izZ@G?r-{c7i@`X_SCv6joQb;x>2blKwLvTKfA;Gy
zqtSmIGY9AF_h*9k_i)YiSJe5%n)(vcJ$zOCNDJrT3ZtYd<?dX&#37R(<*^<xGLPU5
zS|&c+<JBiKD&zxU97fvl89HWPR#N;fON4vHaldr2M(*gI5>BdaEk+ttU0@Tfl`<bN
zxO%_{hCK+hwrTo=-{X8K4|rLbZqJ6Lcpl>`fh4eFAJ{FsjB?E7&Q=a<;0MzUUi>2Q
zOj3vX_!?Nm3FdA*EgUIGw>FwHDp_rDAx+7?j9kR6a!V5EVVsvp@SV@}V%PmNpTI@&
z#3~x^t;)`d9{=VS?(~^dFymX>k}YfMm3*=2OqBd((8chD1r#t9XTv_kRhe-K)dRxC
zskjx>%+u@a%><cE!Y9bXwl4epC9Qr~<a=e@<Ntj*gt4i!{o(ZFAjJ;o7H5Q`K39A?
zuMsvIfl`APHhx58uGQ#|O=mm|pUDU7G3#Q*7Scf%p;&stMa_ZqO``MRO4F?);{F9F
z?fth*PXL>{0Xs0J&9Pyq_EtmBLu)?FC*a<XZ#q_267E;bir?d?N#kPa==%+KFu>;L
z(JXlqlpQUAVxkk59c{4~pqzon1%GE~W{D1l|32Yv-v4pc|2va}v|pHxuP@yN&$A*u
zV<X;cevJaFvx2GDh0dNdNb@;SS~2^D5`?kL(@hD3N<9&rnm$3*=%YY&a)Osw9;j#V
ziC6OWPTWJ%-c-FglUf<t#wPaQyCkcpx|>V>dkw3n8lxxPV(i^SXxH#f<LCE%a7cRa
z!JlHAW;a$jTc6Et6aDH}2_2t#c~Bu{d(`><gLlFEg1m-CagMo_hzoI(BirY;>*`}I
z+SQMy1G6I{pnDLPL@A!FwO;*ta?(xY_&wpmU^S@|j}3MS{<+)XstaODrY7+YqY4MH
zjX96fCbW6-LxWD#{)}M2J(o$hFHBEL1Fmeg&*+@R<SgCCu5j4aE`5ShUXP=HeAdXW
zw|yMq<msP<xRKSC*cy1Qbxa>m#PmaL`)Bk|2m8fE9@RfTR9Uf+>~)NsY%mhhY_R27
zdj>?2<&v<U%ZJdzV3HNmqF5*2Sl^aRv(Pr6z+ShBhNCq=i+l=jVq{k|A@(0#f1L(w
zG99YQQDU68MpaK#-SDatesJm=*xhlIO1rN|y@+t3_77C{yQGX>YdwvXPqbL>3%BUT
zs(WX4Zp;B!h;dwGow^hl?@)ZeMQNgs(;l&nLshn)LQ<n{`-zXgsXTj2jqbK*mYM=V
zp(^%>hgZ4G8?Nyccv_U0c!$r>y(6&vvW!6C2{|3!pPwRcoa`{UewPvH^p^VmX+6Ow
z14RxRc0^`rwi+we7C$}RD<&=M^djpc%{<AT`yO$WbSJ&?zJ;JideMbKOhE&QRou@^
zg=d#{Qy^kl<Cr=->(Ri~;ZT`tS8IPJ)k8z<S5xYnza=7vi{ZoWemm{Uca3dIoQU-L
z$4kz*hYJ-Ox3a2UA1C5(YT)yhJX=YL(rPl8*J;BvOzoKhN9|g+!aS8Hv4s)At*c!j
z<!1{+IJ*OR;GXHR8f?q&ZyV0v-_uU}L>^gQ!?`fbt5O#AV+t(SrJ+9Amr_f=Ye(@=
z^<L_+K%JP$UOGIvGdNOq{a|0_<a;*cHWVbM7WREFJ$PThyY0@(`}*x#1N>W6DRy`K
z9DK7c-)K<?&vdVMRYt5^Q*1u)%*1}jal3sVFLuD5nE_$jxKV%&mN^(rUduRRU+hR?
zXP0k_QB(Suh5hu@SVPN+7yfmx@Qu#M&&;4BgD^Qdhw&h|ZNVE)+8D9Ed$iA&X{+P3
z?vHuBx_h3wQxi_GNlb(p8b}?L`wVn>*0^d7{!~-*BPH|RVigOvtxBFYcaG9QY~IvW
zi8%NXZB86SgRJM=ZUi%;1DBg-GsVz$(*#A`OXzIC8u2i2!zu_9l^qh#%vDMhAhg}3
zH+z;BamdYeU)oziVv78{3rlQnAnDJD3J#V4vhai<ZsnXsxtY(0{tn-_r{9WEUxm+5
zR3`pOb8`f^N~&ksb@dc;aOINNE=jU+`{7!hUO`gW-nroa0v8GDK#}xqD0$uumVXWS
z>#75X(t6Oc8+0FeSB4P`S<wI-WTl}wXpkYN;O17qQ61=)HGxk4a|OUGmv@a`yp}Wl
zi(UY<^3B(jae2LeDeMZd*LG?ZG;Zu17XX(EwGe)Fpjz<()ZrgHL`tu&WrmcD{>mU(
zlpDYhbc~-=6#vGEq*GPKfRrl(R_a2~IYh4m8Y{_Cj{sj|C3KpKH(1GwBBYHbGZ?5e
zBLNUvl)$JN2lBm^5>}HDBk%R?1vC6MBsdp9L^6UE0dKf0#55TQ4`aAyjl)$TLAn3V
z#KJVM1;fMKi2i$(0bGQ!L5!}or~!dkC<Yq;0TjaN@m`XG(72DYn~xwsAHfd9hciOf
zNrB36OGq;%fE^(LR7NmD9?}8tB8(w*i~w__%ypU`M#89vng7+a_waM~5i~Z!0%4%x
zAw;A!gop+B5NV6C!2$ww*jWLSC<TlfRuDkS!3?yc-!0fcz*`PZG@TVtiDrZ_vIBO}
zHW;7TL4XVwGq4fOdhLWD1_tTk0C-|<L(pc?%LRgB7%}2FLBK9I3}}r}z^LQ`0Yp5s
zXa(5yqV61_v8MkEY7v2G{!U3izZIjoK_HdK&i?kwo*urBJ<&_P2|2sy+BWllaVijq
z<F76lZJ{}Qj2ozk<%1j*{F^}L1oi*=qWzCAZ%N=Fjvo8Q0eRky16{O2bc^CG475cF
z2zva-LH~^ckQ~PjDZAdkn_XgB(fF?u_WwAM1ia&5Kv)9)KYq}aeq9sjN|%X;fwTW*
z0%7sYkjZ~41fA8Mco-yA1|1zIfR(`VH?vGcYUJj5O<RF(=+#D`$A5z>1@_~(fW!pa
zzv9f-afRZD6{Mp@d(lJqKWhn-yD^`bfJ^ZA+6-M8f1^Y9k8C0g@>KptBs`G|;_y#s
z=r($XX5=Z{FbI<vA%8W{&7_?KgS=EiGw6XU6>==JwSRJY-BquXU>L%8L4cP!!!=_L
zagzjeZ^4pbkX<!EDp?3}v&A<V1-_<{0B(^)geDpwkb#D>m5+_Pmm2_1;{}$I=^zCf
zz-6)>2C)vhZuQuJm=sokIF%8yq6-M7T0=th0Ys`jB+(E!OVz~~Hw2}d(*QPU3K(@J
zAmFP7Ezp&w0I{<K{-haU)LEiQHf%r;LIGpJ3Iq_^GXir61<2h;04&`KBH;vtrdvaP
qxd30$GfQtk0kDLie1Rgs08;J`908(4VjwY41_cQ8A_$#e(Ek8E%X4D@

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java
index 1c05fad..4bf8abb 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/SimpleMecanumDrive.java
@@ -19,7 +19,6 @@
 import org.firstinspires.ftc.teamcode.utils.enums.State;
 import org.firstinspires.ftc.teamcode.utils.enums.TrajectoryType;
 import org.firstinspires.ftc.teamcode.utils.enums.driveDirection;
-import org.jetbrains.annotations.Contract;
 
 import java.util.Arrays;
 import java.util.LinkedList;
@@ -53,7 +52,7 @@ public static final class Params{
 	private final TelemetryPacket telemetryPacket;
 	
 	private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
-	private Pose2d RobotPosition;
+	public Pose2d RobotPosition;
 	private double BufPower=1f;
 
 	private final Localizer localizer;
@@ -83,7 +82,7 @@ public class DriveCommand {
 		public abstract class commandRunningNode{
 			public void runCommand() {}
 		}
-		
+
 		public commandRunningNode MEAN;
 		private double BufPower;
 		private Pose2d DeltaTrajectory;
@@ -97,7 +96,7 @@ public void runCommand() {}
 			this.BufPower=BufPower;
 			this.pose=pose;
 		}
-		
+
 		/**
 		 * 在该节点只修改电机BufPower，不会在定义时影响主程序
 		 * @param power 目标设置的电机BufPower
@@ -111,7 +110,7 @@ public void runCommand() {
 				}
 			};
 		}
-		
+
 		/**
 		 * 在该节点让机器旋转指定弧度
 		 * @param radians 要转的弧度
@@ -125,7 +124,7 @@ public void runCommand() {
 			};
 			DeltaTrajectory=new Pose2d(new Vector2d(0,0),radians);
 		}
-		
+
 		/**
 		 * 在该节点让机器在指定角度行驶指定距离
 		 * @param radians 相较于机器的正方向，目标点位的度数（注意不是相较于当前机器方向，而是坐标系定义时给出的机器正方向）
@@ -144,7 +143,7 @@ public void runCommand() {
 							.toVector2d()
 					,radians);
 		}
-		
+
 		/**
 		 * 在该节点让机器在不旋转的情况下平移
 		 * @param pose 目标矢量点位
@@ -159,7 +158,7 @@ public void runCommand() {
 			};
 			DeltaTrajectory=new Pose2d(cache.toVector2d(),this.pose.heading);
 		}
-		
+
 		/**
 		 * 不要在自动程序中调用这个函数，否则你会后悔的
 		 */
@@ -169,12 +168,11 @@ public void RUN(){
 		public Pose2d getDeltaTrajectory(){
 			return DeltaTrajectory;
 		}
-		
+
 		/**
 		 * @return 该Command节点的目标点位
 		 */
 		@NonNull
-		@Contract(" -> new")
 		public Pose2d NEXT(){
 			return new Pose2d(
 					pose.position.x+DeltaTrajectory.position.x,
@@ -383,7 +381,6 @@ public void runDriveCommandPackage(@NonNull DriveCommandPackage driveCommandPack
 	 * @return 在目标进度下机器的理想位置
 	 */
 	@NonNull
-	@Contract ("_, _, _ -> new")
 	private Pose2d getAimPositionThroughTrajectory(@NonNull Pose2d from, @NonNull Pose2d end, double progress){
 		Complex cache=new Complex(new Vector2d(
 				end.position.x-from.position.x,
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java
new file mode 100644
index 0000000..f2c64af
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_samples/AutonomousSample.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.RIC_samples;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+
+import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive.DriveCommandPackage;
+import org.firstinspires.ftc.teamcode.utils.AutonomousProgramTemplate;
+import org.firstinspires.ftc.teamcode.utils.enums.AutonomousLocation;
+
+@Autonomous(name = "AutonomousSample",group = "SAMPLE")
+public class AutonomousSample extends AutonomousProgramTemplate {
+	@Override
+	public void runOpMode() {
+		Init(new Pose2d(0,0,0));
+		robot.client.addData("Position","WAITING FOR REQUEST");
+		AutonomousLocation location = AutonomousLocation.failed;
+		while (opModeIsNotActive()) {
+			location=robot.webcam.getLocation();
+			robot.client.changeDate("Position",location.name());
+			sleep(50);
+		}
+
+		if(!WaitForStartRequest())return;
+
+		robot.client.deleteDate("Position");
+		switch (location){
+			case left:
+				robot.strafeTo(robot.pose().position.plus(new Vector2d(0,24)));
+				robot.turnAngle(90);
+				robot.strafeTo(new Vector2d(24,0));
+				break;
+			case centre:
+				DriveCommandPackage command=drive.drivingCommandsBuilder()
+						.StrafeTo(robot.pose().position.plus(new Vector2d(0,24)))
+						.TurnAngle(90)
+						.StrafeInDistance(Math.toRadians(-90),Math.sqrt(1152))
+						.END();
+				drive.runDriveCommandPackage(command);
+				break;
+			case right:
+				break;
+			case failed:
+				return;
+		}
+
+		sleep(1145141919);
+	}
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
index b0bf5d1..916f366 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java
@@ -4,6 +4,7 @@
 
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.SleepAction;
+import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -100,6 +101,12 @@ private void InitInManualDrive(){
 	}
 
 	public void update()  {
+		NowTimeMills=System.currentTimeMillis();
+
+		if(NowTimeMills-StartTimeMills>=90000){
+			state=State.FinalState;
+		}
+
 		sensors.update();
 		servos.update();
 		if(RuntimeOption.driverUsingAxisPowerInsteadOfCurrentPower) {
@@ -131,6 +138,15 @@ public void turnAngle(double angle){
 		if(RunningState==runningState.ManualDrive)return;
 		drive.runDriveCommandPackage(drive.drivingCommandsBuilder().TurnAngle(angle).END());
 	}
+	public void strafeTo(Vector2d pose){
+		if(RunningState==runningState.ManualDrive)return;
+		drive.runDriveCommandPackage(drive.drivingCommandsBuilder().StrafeTo(pose).END());
+	}
+
+	public Pose2d pose(){
+		drive.update();
+		return drive.RobotPosition;
+	}
 
 	/**
 	 * 将会把BufPower全部分配给电机
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java
new file mode 100644
index 0000000..d6a2aa1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/AutonomousProgramTemplate.java
@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.DriveControls.SimpleMecanumDrive;
+import org.firstinspires.ftc.teamcode.Robot;
+import org.firstinspires.ftc.teamcode.utils.enums.runningState;
+
+public abstract class AutonomousProgramTemplate extends LinearOpMode {
+	public Robot robot;
+	public SimpleMecanumDrive drive;
+
+	public void Init(Pose2d position){
+		robot=new Robot(hardwareMap, runningState.Autonomous,telemetry);
+		drive=robot.enableSimpleMecanumDrive(position);
+	}
+	public boolean WaitForStartRequest(){
+		while(opModeIsNotActive()){
+			sleep(50);
+		}
+
+		return opModeStopped();
+	}
+	public boolean opModeIsNotActive(){
+		return !opModeIsActive()&&!isStopRequested();
+	}
+	public boolean opModeStopped(){
+		return opModeIsActive() && ! isStopRequested();
+	}
+}