From e9589599b8a242db79b1be960d54d3e1244acd1b Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Thu, 6 Feb 2025 16:40:54 +0800 Subject: [PATCH 01/11] replace odometry with pose estimator(newer one) & rearray the swerve subsystem --- .../frc/robot/Constants$RobotConstants.class | Bin 572 -> 620 bytes .../Constants$SuperstructureConstants.class | Bin 1867 -> 1867 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 2840 bytes bin/main/frc/robot/RobotContainer.class | Bin 1939 -> 2161 bytes .../frc/robot/commands/TeleopSwerve.class | Bin 2684 -> 2902 bytes .../frc/robot/subsystems/Pathplanner.class | Bin 0 -> 5190 bytes bin/main/frc/robot/subsystems/Swerve.class | Bin 10671 -> 0 bytes .../frc/robot/subsystems/SwerveDrive.class | Bin 0 -> 4200 bytes .../subsystems/SwervePoseEstimator.class | Bin 0 -> 5803 bytes src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 11 +- .../java/frc/robot/commands/TeleopSwerve.java | 14 +- .../frc/robot/subsystems/Pathplanner.java | 68 ++++++++ .../java/frc/robot/subsystems/Swerve.java | 153 ------------------ .../frc/robot/subsystems/SwerveDrive.java | 67 ++++++++ .../robot/subsystems/SwervePoseEstimator.java | 82 ++++++++++ 16 files changed, 234 insertions(+), 162 deletions(-) create mode 100644 bin/main/frc/robot/subsystems/Pathplanner.class delete mode 100644 bin/main/frc/robot/subsystems/Swerve.class create mode 100644 bin/main/frc/robot/subsystems/SwerveDrive.class create mode 100644 bin/main/frc/robot/subsystems/SwervePoseEstimator.class create mode 100644 src/main/java/frc/robot/subsystems/Pathplanner.java delete mode 100644 src/main/java/frc/robot/subsystems/Swerve.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveDrive.java create mode 100644 src/main/java/frc/robot/subsystems/SwervePoseEstimator.java diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index b24d14f5e8c26e7538aeae34316be1219dab1517..01f33432cfcc6323fbc8ced3679379f7828d3c60 100644 GIT binary patch delta 199 zcmdnP@`i=$)W2Q(7#J9A8B`{6EtKQ)@$_}|@pShHiT88#b>(0XU}WIT$;?g7$xP2E znYhxLO@fC(gh6!Ts}@GZ$@z@dj4YE^Gpg2bGcYo+1C3!~;AfCz5M&U7^M!#dMvxXD zDF#%b59Tv4uxf2*5Z?&aA_*kffWiz6d<;@Rk_XIYWsqi&0rHp`WPv0TgB*iAkmdqw OS71;ATB8gWR{#L_-Wm)5 delta 153 zcmaFEvWJE1)W2Q(7#J9A85AdSEu47Tl1-F{L6AXcG8bbDquk_vMr%gq$)_1rbNCq; z8Q6genHU6sEJlzTASnzK(FgMx7+AHoGl*;iD-i>dY(QZK20jLHAjtz}voc69NCJ7x a3{pUni9wn{21s*(waYTd1LYLJ;<5mt*AT)0 diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 901da05a330c8a64a4bfd81b328028d9f1df01ef..95ddb791f998d13fa5ace1ce1bc141a4141a5bdf 100644 GIT binary patch delta 71 zcmX@jcbad*dN$K!1}=tF1|f!Y1{sD-1{H>E20eyc1}la<1{a2Wh5&|AhA4&_h7^W6 Zh8%_#hBAgWAgg`y6E-8psLA5&CIB}P5B~rF delta 71 zcmX@jcbad*dN$J}1}=sa1|fzt1{sD71{H=Z20exx1}lbK1{a1rh5&{VhA4(=po&_c YXfs0@Ln}iaL)+vhY(|Wclf~Ih05t{=@Bjb+ diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index 5f12896a2205f3066b1d416730361887eb4c65ff..248fca2b8089dc8b3ed9065c10e16bf2122c3777 100644 GIT binary patch delta 145 zcmWN}D+t0+7=Y35M=)G4?pE7;ptuP>QFNmC2tyGc&1N;5M6lR|m6?qzg3W3++O%2C z)~%g%eC_LNPVG$Mq|l@>$xtRsH&UNG6ACP`SYflK$c7SID(tCpq{fLlXPR7S5zyv_ p!<`NfdOV}y#efe(zKrvDvIYJnX7HJA-il#PoO7yT8;4r1kiVAC7HdNVCV`q*74Nf#U)8c~1l{OI_ q?)b!Xc^KhIpO;bI4EZqP%a|V%NimZQbIGxgJWGy2q+H1ciSqtaDIAUf diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 402aec187e4d5e503b1c5c8a4f826c53ff843e62..3bc0d68448f5116e2be87e307fa2224a49ab0995 100644 GIT binary patch delta 930 zcmah{O-~b16g{ug>9o^<)=JR=r7c$44$^{xq6mVt3W{2(QY&f{GYW}hOlPQZ)kL>$ z$lIIv2aG2DAefl0UAXrrxO8ufciN(`5I66g`*H3)_rAwHKfLmnUpt=xOyS;^{JJa| zFSKGwOEU~CJ}Ev<74=o!Oc|T2#!JK0HjGrR6)&!rv`GeMD?OWGP!Gz_>V}pv%=L{& zrd}kTVrtJc{dubxLu8k`_4T{Uuiy}T5{~c&n_&UYY_W1TdM$4sgFU{GSayM-Ux5gR zgfK(+e@bP9;I~M}6i9GNIL;71fV5ApjHsu@jw^6MmY{Oe7R@Jc!qPO%@Fu>i|KGbh zC}Uqc-ms;;DFtpQmh4dlCvl2*3F(d~MrE4ew1PA6@rOd6<18j6oa3*Agm4~H{JpRg zzM!B3oleZsl4mzfJv_Ij)AEZl=8%z)=3)DUa0!?Biv64G25w5oGK}$eqMvuNkf>k@ zIbITD>^6TZ-e-6DPjQ5aeAp2x13s%ZN$}8zvB3+0FufI>hsm=82O(fht)ol*gzkb+ zM^6<&!l6PPktzlVMhOm95hIu=s1@>6$fzJ#$50i?wkKJ`NIO7qOs%0ju5M$ZifP{r z1*W%g@nciQJe^5e7Z8A*M0rV2F9{lipMG@@=E=$-h&=l70AV~u1Phd{w8Y{Ht~Tq? zQrm+Jk!=ma{x+b=4Dv+xFjFvdq7Ao#H5#6<)N zk>^hBWp$@juM}I&rc*0cYcHy`orZx+nAdSxIRmutm8UiUi8(J_@e~m8(0wm^}BW(B^`IfFEuaU#XVtbX=Rg;5+udM4^7$pcd|A)SZ512F^N^aW*$pH5o_E@u9X9sGu*o}=|8jP4W>>2X#gZ9y*U6ba%Nx&epgx- jx4vLw&m%8zJmyHA;PC%jzUAGzj|ZF$<~W0gc!cr4q_${4 diff --git a/bin/main/frc/robot/commands/TeleopSwerve.class b/bin/main/frc/robot/commands/TeleopSwerve.class index af250cb97c1eb1ede23d041f1572c931a1e2ed9f..31035994793ddc51d5420d9353b87a6671730878 100644 GIT binary patch literal 2902 zcmbtW-E$LF6#v~OWZST9X^Bw4p)KVjn?M&RAW}X`n*!B<(3DDw=xmbPcH3k(>_^+E z3W7LFM(# z!tpf=b}y1r4D zj9XWddO>$M*~OFf`H{PMN;OX})l{A@PihXYDhtC>%`j}4PK^nzq>7y97+8WfQmQho zq*5i@EDDo#bkkr6XHCPg%zR$NKi3fdA$&5f^uG76p54c$41j!Z$Aprnx|m_UuRM` z_%*kX;ntLv$#aJ8q?y(7)0(9VzA6kl^E$cj-%!|1Pa-vn#8-IMb*Ls6F(zS@VfYby zYu0%(5*VlMo8!)v3F`T1RM~ukYD33{aTya5u29uB4d0+I<0(AN&_!`Bj(R8Iy2Wi< z95rgF83|XZ5$YXzCwPvV1@2fSRkZc-To_68gmE2{5^hkoO$JQeIgpXU6hp-3PMusZ z42Pl(d)_QGl~V#$v|$d`t4^AZCWeE^^roL_TE+}+)3l)2mrItJ)|P2nHIKONhN23j z@5p#w;1?nMCiS^_XVfhex#c4ny723e_Y*CVk&y+V+De*9KI^1CfD_{6&B~a=JVP7x zU)IgjK=e-`y}XD88H*e3ZV6#Qh9QJ)Yhg)RgeBoFL&t))q^Wt$m{YIPtd(KIp|DxA zSW4-0G#t1@20>*Rwj7VaI~L}O#uSFSFL;V|CB+Y;Vl=v$Ax`5h+X9lT3{a^gZ` zINH3j^;w8)P+HwFbr;uTWSniRu>fl-3q8 zr@fV?w71fX_EwtE-b(Y?TWLCbE6rwar728nfILU=7(I6o_d9tH(z*~^#m?9R>`q7U z_$v06(O<_6)Nu!59c3I64@ErUvCcA%#NuVBvBPB?iybLrC>HzyC)0s=s)8q~^kG4N zhCPTwA=pnrB_NMt1jjIr69n{0>iSdoh%Vu;s8nUL{E0MvA+UcVF#jOU zKZ*Z~29zj<>_b{D#(N~$f`GCkNaFYLfsZ^UNGPh(wU+QIKI{lob8GQ(3wsLV^l5R) j^Si@0sQM*S+@qWrNk1k>E%*eV(ki|eUc_hk0=xbNF<$ye delta 1364 zcmYjRO;B4^6#nje$$Lq>Bn^a=3KS@XByG><7)|SmQw<#WM1A)I zbnaI?y5OOQ)=FG@=^P;z2hfgjL}}|Cm_avYv6~)850>c0IMww+uStq{5D$@+o?Hb} zo*YUee*!z*kyw!yBy)e+w~=h%U;{JPX`0&@zJnv_CLU|xaW`6^h$udB_u|rZwAc|m zO~ZT)pF0#Kd+eHOwQ`Yy#Cii6IXwA&Ze?y89+P;#i?40NEr}1v&$>K5#rjr{7=mm)kCVQ@0R8X>(TNx= z*cd~CHavtW4C65NVFjbuBu%c;5B&yZ>s0wIt@sC8@~^bc-*FIsQ2ZxVjm!OWcdJ_} zyiN!`9V_Y~_zk@2E+0n;)F>4uVv2DSZw2)&a+)F+P#31@5zHugqi(@nb;EcY?@&7p W?@}WT@8NwKg#OqVF5m;=yX${Jd--1g diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class new file mode 100644 index 0000000000000000000000000000000000000000..f31464cb54e335c0b13c26ed09a81dfd612f31da GIT binary patch literal 5190 zcmb_fiGLK;9sj;0WRi6tYZ6X{A_mwbz#v}eq9r5*sEecoqVb}$yOU&Kc4s>?8>APt zm$fZbdt+1;mOPqUxQ{N}yi``y=j_Fre7 z0&orfp`l4&^@Nu(JU8tIhF?tk$Na#``NoJDOcfk6pSL^>Edm>?Y|%Jcu#E}Z^8*Pv zcKYxZBje_BW*A_X=4oaqHw)1OMark6E_CH9sqkG5{J^LB8pKy#vZ zOrT}Z%~~-uqeI7e&^4Uj$U-#=#1KbEI|RCQEI_M40Z?EIup7bJR9=SCtK!#X51tQY7yY)+d7BWKdsd3(kx1h$*k(F>g%3Z9)0Mguc* zpEDikvE2{YWi+Q@=svK*>MuEk{rtMhS)q%U(an0=NlKe6B!H^!>OpHXTN@Fx2 zkI7{3SPWZmnTAUR;?>=K>BCkg&~Z7gV2n;$!JuRM)U=D5RHk2U?(~Z}MZO7JI~l|S zlE)5#!3L7oWO*`D;vIH{sD>eds~f~a;JJ=td8`{FyIsf4yE!|fa+}A)Jq^PG>l>Vx zAvv;VNHPn|eBifk@1gnm7sF1xNILpDS}HL#G}JrRy1l!+H;8?5Z$#i=V!lwVC7nEH z#NM$^v|zuEQ5+D^83P0U4%c-oGtcBIhfKYT!~Kea{+@xV#peM>u zW-)M$0X}vVZ6|AaI&Q`-0__=<$3@Q)qBl&2dZ{Ynl%l+Tv*O{X&3RAOL`Nf|Rs(}| zXqZ9b9%X)3qhelf)D)>?lyGy6eRFj2k1#X~L?=u2Z8AR&2@L$N`BAZ53??!f(gOWa zo>LZ=*)~$@sVR4Il6KIM1zY4Y-wL>L!1NenvN;Tzj+17YWbcXeXN9d)qZPMF?o&GK z8e#)1In$P8StZtyvh&vU#a!C*4wz|&oI6u)#&pI^&z9feVoNY(vnV!b+_g1>Ihj$h z#~5!E0du+yTdpAE#WnS|Qt-LAP_>7>W+9ZNtzdb!o3%5n*N&M>XU!g#^ji{hJk6nD z$~Z#p2-q3l7@T5VxBbzAWo7;77*j4$HENhh6RvlZgMqI)M;u3W+>T?kPgV{nDh~Hs z4r$AYX7lsoF2{;iUr8dc@p%c(sxwg|90!9)D>qQ3YO>4Jcym(>ci^=eUL$aEWJNNR zIXt?c)U{uS*9)8{>z?D-Wrbi563r>gs{c_zW=qYS+^ zCS9mycTKe^Ye9@9_1SW)P|ld42>fkxJROeA^){}Ep1f8{5JvdrY?P5PHfgyzEAWmP zvPf`z?Hy~wdl_AUsvL3V0YAj)IhqZIk`lK>|*#3KBD2n0$ZZv zXTH-qK8lkZ)miJL^c=Md>eVwx5|zqx6|LHagIiZ^O-$K-pzQDoNs(nh3i+^(PvIdR z3YoE*Es)&!j@P(KofWuZ&eB}ZMM@<76&rPnUd9^cfJ(wOg>jQi0xq#Z1%BX}g}qiV z{VbHdE<3Uo}zX?&LXM~|161fHw)Wq7hgd^fLQ zj^+2HT=z(^5cW%zRN+iq(o(I|9X!A|u0=oh%@WJ`&(;psA!XT+M(w~#cC_M&I8MkL z(33iz!qY4VVF#?-1WU0Zr>**si*!oI0(>qm$<*Bm!lKeU7RMJQudnF%stm>MFgogZ zk;IqdI2nrd4IR(no6LNQRo4YdokhTi7OY__BKc?Ik|Gnl8Umq&|Ggq*{!Wayh41SK zQ0&m~y*P?-9G7c9(eWxArxAb51*Mz7FLk^JCyDn9r3CdA_??ai@etR38<$C;?~%89 zb;*$l(N?aW8vY`%wklLz9MngbhQA4{nje*AsT7L1h%z*DWN${3ydD#3k~dxT=7p8- zR$}nB&F4ZuIOtILkM_-CQQuj_$7j)b3S9&j6I^-<%LuL@c;P9m6sgmALFz%Y?cFeg zwf)av;aT*JcW#))rl z1g8`{i|hN(;KfwqNgO8cWy-jM|fd=DX+LiV%>oaI6?RxY{b2!eIF^@ z&l3+IiBIEN;(|ARY2QcTAjkK5+=V<`N@I|7fs~ryalISO|0aPZ4MO7)KGwA?B$i*c zJh{>Q^8YbDnv@FCHNc=^BON3W6q|UHj7WabW!Ob*DMCAK+|@sW>A3`YYvgPX>@ zhLolg41~Qb$v55TOYuJmWP}9<93UZQ(o1e z6yD5Gx&+H>tSgPSpL8X{JA^&0WZ{qeMDz&on zz8SoantiaUSxoJ2r2(!|td^9pE7~o>$LJ+Vlj#@Iys?hv$4fLnDQT7>rbKfaX1_G2c5D1Wj5R!m%5DtSS1!G9kBW;ehzou<^w>jFhO`k z@bhZs9pC$Z=lkCG<{Q6!<1GLiKEbBv3vav*J;~RnumB;t8h5!YL;j zPLB@RwcBADrFE$mujqBYaCHor@7wFv{22G-Gf9OX z=^W_m-g%&R>%i7sN=o~u8Jo6tdYiW7;6N&zNXO|Ic7m1`KpQqFDQy%48_#E^tK92N zR3TuXQ?O>%JY^gtK`P?#P-{3d1fkjKLX;dAA`)5IK@ z2KtLd;36R=fIieI?+lrki+KU;#$E%AG%Hn9Niser>B$Kz6=WC>M(o6JY$TY81$;Jf z69xsXONL%u2P_Qoe^w#4*Nz9*!u+ zE{vjzYU5*?VhKiSxep{2+4l=JT>!q#u`E3URN49>`f^fsJZ`1Z!T$aocHBgJVh&w^1&QU0q#Gdn!8WGkq!Cq5AF= z>~5UNl=4Zp&w60fo?4XQZWH(5UV%v^ccwSncH9ax8Cgl=)uvUbP_I9$R|EG68Vkc+ z@q{OU2k@YQ4>EP}6I6KIv4frb**qSLN3E2Jhw!kVDx!UEJY|VAdB$3ZDhtOCa#q8B zoionNvyNgy%^3#c#5NjmwzKKWinBbzc<*;QSoT6!P8?m;8ze(rcU5d(UCSK1p4+b!SDFc(VFdwpcdBK*^a5^1J zhmw{RO`G^K-ViKOd#29xcUdYAJ8Ze#f_*9$&d6b2(%_pR6?fSdME;Ep<7g?%MllR(cI05q#Ce*YKOPLfUfS`fdI>XdZxbP}9N%bDUgS zORdZ(!~$*8+A33CI!UAKqVef7TShl*+sC#@z9I!z`Yn(LvOw=#!W`4*(u|Z_rw29v z18JUUNTXIefexQJaQ+@q6bU!l>yO{|z3ng6->xj;J0|`Zf5PND8cRn~mJ_>;PIk-r znLyY186uOtlsZyoSjgDE>F9_%{Z*T#3SfU>;xF-63^jC1eMY#GW z5suT;{sw<%;BN&>)kaW!q>1k-->xC;J>mGc)tT-j`1pA?(&oI< z#6RFiv_|FPUDim-Qa;tv=r1Fm`BZa&K9M~LwfO47Z=#BIKfsSo{EOm5ou<6?mGJ(} z#5?#2g-6<2c*_-}Vlq*+1_CKPrjBB>20+Df(ah0~n~y$#xjEHL!t=H%waNXriT}Y* z>1`yWntLsW=_d&j_H@E({QsHwfBc-|Gn=Bf!}WuNm~9^`y%v?x_W<_cP8o{_&#WlV=PDXBK4 zM&=9VXLXb56|8$#UHeg`uU662G6j#e^HE1;ZPE}PdZpWtIzujEF=+POX_YG0E;i*7 zxs-*wENi-p<*xJM%QX=dt#tG^@NGjHm@elI{oOp5j)dc#sS%!v6HYdDtP~a*8=_V^ zma?LnOuAugJnb}$h7Vf}aVs(6j5b^ykY$Kgbkt-HoNaQ2DUHH9;Q~(O{*huXw0G39 z;u#-YcL65OLv0mh(qhUgVYSwD{m@Ohm9VZdB`B-i9H=V~vGUP(F~)o&aD}D9&D;XM z8NZoxu=+bw>h5A{XH}9}P|`}p>}V{)$ST*Ea;;oP6^tjN6o%klUrgDkm*+_rWPQHw zm!(c4WBB~Bh!I1aB^t4iTs4bb*&4$LPHPWP@ zz1Nf*gr&tQuHYJlDx&9|hrAkkSJS>q*~xH`*2V4y+q+z7II7NuFIUQqK7GvepTuj_ zX*@hO6b-LfE4bNLfkh*n?>SC0s4GC*c%B)yQ@o^!ru7L^WRHp8#~&!K)rTD_W;kz< zD&7}8jGs#~2mNY&1lt#oTjYQtx6+^s15-wunnp@_gQ^|M`-D21!#LC#kH^A^2u*k) zgHW!x)2EA!DoK=xSccG5YR~O8Wmxu8NwIXl$}tj-8a>Jip$eAu8G`!!+BbC2ia706 za!AGui5IQM764t5foDoWY_ht`B9LOYMgCJ6x+__u@VKn^prA$}wS`uZp^GMI_kaW9QQ&KxR$98#0%UiU{&+ za)%)wIA=4S&1_TdRPn=L{7|7I$HO{iocX1mo>%y9$%Rmx7e3@c?$n&;jm^W;%G znhwZ^}22&OF!i==^0>i7ga!toG^(d89^S z)#b9jMx81OO8%a=HtngAC)K|4F%$obXE^rh8a%^Us*N?Xv)rG{<&!L&>2g~x1M%kP zah*r;(`xqfri5gm+K|uI$Uu$kS8ZP~SxneZV4r7$xAM!TI9%C~m-NaS&FiMzC3my! zwHjq(X1BVD*%D_hN!eFrCTJMKa#CIHPF0m6b)}stnBEfe8me8}$+j5smf$@(h6?mj zsBJ^O!V~n&L+SIbX<#`;Rm#+=93|@Bmro_?+Ef4KRiXaND=Yn%H&6Uq!x_BP=I?w! z-r+OnOLg-kYMW1^ez2yo_TougdJ4;$jWbw2SbAFhoW$jmxFR%IrrydwO7#o0@qv~%a2HT^2FC|Wn?qB$pA>!_n8G7`_=tLVoRE4?<4Iz8dIBHg+p-CK@>PoG zK0L^=`zfC1D7O14YM_y~8JD0AOK>rk@@*N`p@CPb%Mrrm2xA3~VkPcHGiR^j{44PY zu41Ws4W8gd^|KuPJg&hPaV=iu)$%0PlYw@89UJf+KL05;;=9;{?_(=|$m-#bupK|a z4fq)&ySj9oha2%}qG)hchR@)$e7lcV7|-JcB7TJEPT+GifzPwO9;Ii=K#2iP-mQN2 zm(AsOc&aLI=2&xBLe*vSxRB7TF?d{0mVxt8XeE$`;xa(Eu-XXn)>5lZ)5d0(l$)mEBR2A=Y6%|VS zpC|CIdzENiA_3hwOg|pcgciDkvtZ}q-|-(LB`RI_NVe*CppqneC-7g>Asx(vRGEdu zuZ^Ruw#;}`g)hQY}0k=;n5N?vs&%TORPA?4cO zq-r|CR35^*EJA*J;E}q62vzKBFwThCQ@Zg4YFjQP*|nFpPRP8_@e;Yzc<$^wEsCyd zq6=wEz+wI#VQf4~Tume6LtAE3!eeAW*+_rB=}pMGy?ozAn^`|08|vhG^)@A)b+TO<5S?a9y7$%z zja7~4bKm;iHxBI1b0{}fJ<1gyV?2MHUqC&9wY){_Pu?TYo#(SI%CoWLhWV6LN&C-JWC3jX*n?L3Hc2+z3>iWv|+GbrNMN-$^-I2T1JKy;$zE$Bws2I5p9V)EZoicIg%kCVcX;KgnU## zJtZHPPw1y-qJH|4ydtmar;~Dup=grSF5?H5hj;*<>R616 XdI7eWmWp{nXX6V(_Y0-G1)B=;wauFKFF;XCx4gP_4j1qdWI;_St8ja}WRc z?_VAPIFH{e@CkG+SlPH`&YMo$F3sDkwxbv9c>1Ppt>{V1SkV;(1bXybDSmU=h%XqH z?Z|I=WBz&~o;3>vt(c3aD_bvWwl1)@F!!oy>tnWK6g0=Q1fr=rcDAh@6i^Cgu9Vkp zf$nSd_D#6UxFK}IDC)FiWVvS`wH++qFU7YORQvYI<~APEbj~Om&X9mV9GMjejG8$; z1V8qwcm}G1eFEX9bs~fo>}`g?eicD9DL5c-tj=c7lV&`Ng969^_=t~U5Z8w%>w>?9fXAFkWj-%4S z=LGt~M%u2k+R2;9sIJpHwPIaqzzA}~AKJ_sb;mXvh>M1Hkg@RoQdIU~=gg{Ee zFRF;6PoR_3yp|f9o0%S&Os7U>rp71dCdOvQrX@?)YRP0#Hw(IBt;T08t!U?|D6^Qz zk);?ClBH7u@o*zMB}tT|ie`kE7byyEvQ(JYtr=}T&rE4enOQAAt67E|mtO{)C4)jf zUQhdOZc??uoJ>*~yaxP9UX9F#SOSI>Q16i%G}k6V#RZHoH5PT}4L47c@{~~($;#`D zRQ@Fum+^6?CD&bEwakomlW`44>LQ#roh`brAf@0HfmjV_RjE9>q}jG%rZc_nMLM^!WLWSi-v6b<=EG-jfws?UZvnB8$v64d1!-&&z+@e_QJ1x{4dfGfQ%YowanwxWzPnvoT2| z8Sz{o^~qsbrmdXvgu{#4kR_4YeJR|K94k8q6>xK+ep*-SxUx}R zR3#NFU@6)6AwUABYnPX*VAML5YH42Ha%!LvsZ@~h3}NtZT)`@{Y!h2WfBlUy5Q-WFh^oitpk3T*`Abr)05yzP_aE`HEsse}qcL8r@97 zeyHL{xXr5JN!+`gC6hl<@g9Ea)y-s;I|BXxFG;JV<}oSD=I(8z@cv|-Vyjp0miI7G z4ZB!5mvzf9b4HdWBW;$ftbWOmEkuiF&20!?ltj{(@nTW8M)SNKbz8x2nfEmsO1kE4 z-E0SOjQ53~ocqx#`&NGQ$!^SjvV-AM;R?3H{M^maA2=uY{8V%Ud!i4~lG#A(eY8cD zzoI?ke<(lJ(6NCd=}bV50&D1UM?pCXuHl3`YLcU-HM}54_YvVyvCJBJH*oSk`v2_O zKh4h<*hc{!2+;ao{vKer(T#mb5bp)FVTy5klk+cemBdJ7ID?n@6gZ2)GU~q*wZeD9 z?Sc2u8C%CW{#^`5KS0|$#vWk&9@-w_N@jCr15;;%obK-ou4DRaQ?#>b9kUXa9}?D4 z_qc%e^_y^FOvjQwr}60UZlvZ*CAofRi1u{aAbeV?qM$AZN>TTad)`0?TQ*b1>$ZtNbZh}b^e!gACXUc}(<~}KwE(Y65`On`&YwR$M zM-C^};iT{Q#9`&`pMMg|7k9?eM=U3KYJyl!5=;LMSiVBzH<>WHnpYjR&)bS-&#CCY*F;a6Ph!(IHEPuXui Ki{IdP==>k;>2~=5 literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class new file mode 100644 index 0000000000000000000000000000000000000000..ed8605b867cf3e10d347d8c41ba28af5793409d2 GIT binary patch literal 5803 zcmbVQd0Z6d8GZ(hgX^f2P!ml|5K$Mj4&s?&jKHFziy9VD(ql8QAIs?O&Sqv7q-~lW zX`8m`eI{*_UTJzaN#)SA>3yZ9_kG`I()9kbzdqm0?y@YyssVn?H#6_|e(&+T*F5^a zbB_SH9{*KQr64?P59ziwXt}zR8+67USEL<1HYV&*any1|m*bjg!?kP`3l(e zN{;%FYYRO)Vu_4-dWSw@*oiTNPmX#-t)yR^jwi=$Yrq(zBGK8hQRoq!+|t6e$Ms&z zHC)rmv?jQ1Njjdk61kLAP{HcxNn_N|bFP`vhjW=BNk&Do*_0`4c}UDmW(+rHQ=z^} zw7Z`bB*G~(BPfeGRIqAK;b>Ey)V7C5XzBP2nS$*X#m6(+p(1y(S)Pt(OI@a5XAmb* zaB1Gn9-~>lCNDJ|A2vlQpvTC z1q(Z^ga}~)R%&=UR;aj2LH&im5W*^~Tnq)*Xt)fERMZnf7siB&1_f(pn0Hzk$2Bsp z)953*c(0<741)-JWh@lwsM_?gGs(85 zekt-s4JxWbTs=d>X6On+N#X7<0JS;XRA$k6p(?p;(XbeyT88Xa4coAtW+gWT-Qh|R zHQSv{m0;T@PuZzq2{aYgDcBkiM>FUw#mo%&7Gn+CH0;Lp3RdGAAuoKjM6^a$T+D&poXvuH%N2uRiKA2WvB0l4h_puqawnH3ef4o zVXy@Ia6rX=1y@x_5z;H&3hL-qkIDnYk-?K<$Zb<`h=&J?KF6eGd7&CT%snYHEmCH3 z#0}vHZd7qp!8(cBeX*!H*xJ&%T^}_anXsY-!UH0e6}F?H7c}@w&uJ`WO(-41aTSdE){ByI)@}_aFu;6~We$@iAi!ru zq{vg5Y!=;c_AdEzcL>ixT*Y&l&I7jfi=mF3nMw#-!_6>w#;}=6F%5+ykw{=PatCW3 z`9FSM{yY{!Cxpb}u!1K8OU)dAmgnb+`jphCr-YV@%=5+yqUkZ1xlXrLc;&~^h=!-3 zR>jHrdVqP#K?tUw$I}`vN1aUTmWC{DQBX^0&c-~8@y>A5Q_^2%TVmctj*&>lC8)G) zd81(c1@dm*O48F~8ct(e!BSf|9xty{D=-2*Uj$`|6HDSLp9+FZT-=#IyQyE6nCEMF z0ahru%sZWBJ<1|~IF}w2cAqhrqAImfYsg6T8@4I$^A8ugBPK1_eDP&L9`+IqFU9R_ zy=XMAk7}Tq<`&u*+ozo4bwa7O5MGWuRJ?-KxiY1OSK?I)YRJ3aV9IHCB391RfUQ9! zggfzC6|bRd0%Epkn;ENWcpYBP_HEd<(h-rgg`mD0!@&_8u%B&Bi2f{A;*A&WPU)CC z@n#Kgk^0DTohSVu)HD@1~J(}TmIB#m{B7>;8){suaI72c=e z{kV^aBvU_hB`%~Fwq07>dfUiy7w*^aL41fTU2E2PGT1I96Zjkhb9n`oK7x;__$WaX z0KoaSY4|ukq2LOMLa*s9x9shK&Y6#0VD1e9@1^O+;frm6X(h}d3A2YZd=h7f(Ofn` zk14o0P{^Qj=cckiNs3$Z;0s|A(<-LeJk2(4VSeai$HIDiwvZwA9o^q9pE<6P&Ptgd z*6=BOn)%#FB>KHBr$-nL=diTMumKKN)*vrNM24=3&uI88?&YM#c=EzQzOKgSnPoj6 zhVVsvS;d#;w7&&u%ch-}9DX8&;YY()@Kw$U*&Ls_>@V5^8mh7a;LV$k$RuP6*JJqw zM~G@XDpL%{h5B$pB%7pY=QTWrZ?GTa34SGz6G(|?@=aT$>85IYi{SLf?)ogXtS>gY z^`}Gl4!)=2yPSPyJ7x9=;@eKc_oZ(ZGT2X4vPE7z3)UlumYwwYC;!?Ik*wT*0#a7V0!6T+|X8x_B%urrOdnHjZC2|enu zC!sNH$WCos!*B6BIoi)WQgFW1MERXQ^8@@^krGa0)UrSf-3&aZlE$LvyX3Xtf3AIIMMP8l$`Fl zs*)qKcjd6_T{(`DYN@wg#`hfMyfxcU@5*kH*J{%6qXPe50Vs7?#ue~^<|$m+{4iDx zOyTMYtmg9Cftqmbx+!d!Ky!2RBsNW;rMdYWT7e1dAi=JIhw#iP?3qA2?>YyX&v0>u zL}#$$EV>ltkAt<*X&gR-zB=6G|4rlBbr|&jxSm2{0!eN+dGM(Tq_{aL594+9Br;su zlNjY4SI>LU)8khDKj`VS84GbW^}L3^^;l0UwV(leu$H;75&N+Yhxqh3HXx2LPN4~7 ze0M80;ST7yht&6B3tmWm(sHx{Zo`XsJ%Ny{AG+Fc@2by1{yaBqHpl*W@H)ye%OCZ}|x<&y2?DAHacSzS@G zIyr)RlDr%5$#dRIzS1Zw$XOzSj(qPVAx5`O{*d zx~cR*4?`{Jp<@p7 autoChooser; public RobotContainer() { - swerve.setDefaultCommand(teleopSwerve); + swerveD.setDefaultCommand(teleopSwerve); + swervePE.setDefaultCommand(teleopSwerve); configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index e426f25..aedce2d 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -7,11 +7,13 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.SwerveConstants; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.SwerveDrive; +import frc.robot.subsystems.SwervePoseEstimator; public class TeleopSwerve extends Command { - private final Swerve swerve; + private final SwerveDrive swerve; + private final SwervePoseEstimator poseEstimator; private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); @@ -25,18 +27,20 @@ public class TeleopSwerve extends Command { private CommandXboxController controller; - public TeleopSwerve(Swerve swerve, CommandXboxController controller) { + public TeleopSwerve(SwerveDrive swerve, SwervePoseEstimator poseEstimator, CommandXboxController controller) { this.swerve = swerve; + this.poseEstimator = poseEstimator; this.controller = controller; addRequirements(swerve); + addRequirements(poseEstimator); } @Override public void execute() { if (controller.getHID().getAButtonPressed()) { - swerve.setOdometryPosition(new Pose2d()); - swerve.setGyroYaw(new Rotation2d()); + poseEstimator.setPoseEstimatorPose(new Pose2d()); + poseEstimator.setGyroYaw(new Rotation2d()); } if (controller.getHID().getRightBumperButton()) { diff --git a/src/main/java/frc/robot/subsystems/Pathplanner.java b/src/main/java/frc/robot/subsystems/Pathplanner.java new file mode 100644 index 0000000..a19c66a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Pathplanner.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Pathplanner extends SubsystemBase { + + private SwerveDrive m_swerve = new SwerveDrive(); + private SwervePoseEstimator m_poseEstimator = new SwervePoseEstimator(); + + private Field2d m_field = new Field2d(); + + public Pathplanner() { + + RobotConfig config = null; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + if (config == null) { + throw new RuntimeException("Failed to load config"); + } + + AutoBuilder.configure( + m_poseEstimator::getPoseEstimatorPose, + m_poseEstimator::setPoseEstimatorPose, + m_poseEstimator::getRobotRelativeSpeeds, + (speeds, feedforwards) -> m_swerve.driveRobotRelative(speeds), + new PPHolonomicDriveController( + new PIDConstants(10.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config, + () -> { + var alliance = DriverStation.getAlliance(); + if(alliance.isPresent()){ + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + } + ); + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + m_field.getObject("target").setPose(pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + m_field.getObject("path").setPoses(poses); + }); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java deleted file mode 100644 index 640a7eb..0000000 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ /dev/null @@ -1,153 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix6.hardware.Pigeon2; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.RobotConstants; -import frc.robot.Constants.SwerveConstants; - -public class Swerve extends SubsystemBase { - - private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); - - private SwerveModule[] modules = new SwerveModule[] { - new SwerveModule(0, SwerveConstants.MOD0_CONSTANTS), - new SwerveModule(1, SwerveConstants.MOD1_CONSTANTS), - new SwerveModule(2, SwerveConstants.MOD2_CONSTANTS), - new SwerveModule(3, SwerveConstants.MOD3_CONSTANTS) - }; - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); - private SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, getGyroYaw(), getModulePositions()); - - private Field2d field = new Field2d(); - - public Swerve() { - - RobotConfig config = null; - try { - config = RobotConfig.fromGUISettings(); - } catch (Exception e) { - e.printStackTrace(); - } - - if (config == null) { - throw new RuntimeException("Failed to load config"); - } - - AutoBuilder.configure( - this::getOdometryPosition, - this::setOdometryPosition, - this::getRobotRelativSpeeds, - (speeds, feedforwards) -> driveRobotRelative(speeds), - new PPHolonomicDriveController( - new PIDConstants(10.0, 0.0, 0.0), - new PIDConstants(5.0, 0.0, 0.0)), - config, - () -> { - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this); - - PathPlannerLogging.setLogTargetPoseCallback((pose) -> { - field.getObject("target").setPose(pose); - }); - - PathPlannerLogging.setLogActivePathCallback((poses) -> { - field.getObject("path").setPoses(poses); - }); - - odometry.resetPose(new Pose2d()); - pigeon.reset(); - } - - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getGyroYaw()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(moduleStates); - } - - public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { - ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); - SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); - } - - public Rotation2d getGyroYaw() { - return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); - } - - public void setGyroYaw(Rotation2d yaw) { - pigeon.setYaw(yaw.getDegrees()); - } - - public Pose2d getOdometryPosition() { - return odometry.getPoseMeters(); - } - - public void setOdometryPosition(Pose2d pose) { - odometry.resetPosition(getGyroYaw(), getModulePositions(), pose); - } - - public ChassisSpeeds getRobotRelativSpeeds() { - return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getGyroYaw()); - } - - public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] positions = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - positions[i] = modules[i].getModulePosition(); - } - return positions; - } - - public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getModuleState(); - } - return states; - } - - public void setModuleStates(SwerveModuleState[] desiredStates) { - if (desiredStates.length != 4) { - throw new IllegalArgumentException("desiredStates must have length 4"); - } - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); - for (SwerveModule mod : modules) { - mod.setDesiredState(desiredStates[mod.ModuleNumber]); - } - } - - @Override - public void periodic() { - odometry.update(getGyroYaw(), getModulePositions()); - field.setRobotPose(getOdometryPosition()); - - SmartDashboard.putData("Field", field); - SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); - SmartDashboard.putNumber("swerve odometry x", getOdometryPosition().getX()); - SmartDashboard.putNumber("swerve odometry y", getOdometryPosition().getY()); - } -} diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java new file mode 100644 index 0000000..ea26258 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -0,0 +1,67 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.SwerveConstants; + +public class SwerveDrive extends SubsystemBase { + + private final SwervePoseEstimator m_PoseEstimator = new SwervePoseEstimator(); + + private SwerveModule[] modules = new SwerveModule[] { + new SwerveModule(0, SwerveConstants.MOD0_CONSTANTS), + new SwerveModule(1, SwerveConstants.MOD1_CONSTANTS), + new SwerveModule(2, SwerveConstants.MOD2_CONSTANTS), + new SwerveModule(3, SwerveConstants.MOD3_CONSTANTS) + }; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + + public SwerveDrive() {} + + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + ChassisSpeeds chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, m_PoseEstimator.getGyroYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); + setModuleStates(moduleStates); + } + + public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); + SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(targetStates); + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getModulePosition(); + } + return positions; + } + + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getModuleState(); + } + return states; + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + if (desiredStates.length != 4) { + throw new IllegalArgumentException("desiredStates must have length 4"); + } + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); + for (SwerveModule mod : modules) { + mod.setDesiredState(desiredStates[mod.ModuleNumber]); + } + } + + @Override + public void periodic() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java new file mode 100644 index 0000000..d5c1ada --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -0,0 +1,82 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.FSLib2025.vision.LimelightHelpers; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +public class SwervePoseEstimator extends SubsystemBase { + + private SwerveDrive m_swerve = new SwerveDrive(); + + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); + + private Rotation2d m_gyroYaw = getGyroYaw(); + private Supplier m_modulePos = () -> m_swerve.getModulePositions(); + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + private SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( + m_kinematics, + m_gyroYaw, + m_modulePos.get(), + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(RobotConstants.LIMELIGHT_NAME).pose, + VecBuilder.fill(0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9) + ); + + private Field2d m_field = new Field2d(); + + public SwervePoseEstimator() { + m_poseEstimator.resetPosition(getGyroYaw(), m_swerve.getModulePositions(), getPoseEstimatorPose()); + pigeon.reset(); + } + + public Rotation2d getGyroYaw() { + return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); + } + + public void setGyroYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } + + public Pose2d getPoseEstimatorPose() { + return m_poseEstimator.getEstimatedPosition(); + } + + public void setPoseEstimatorPose(Pose2d pose) { + m_poseEstimator.resetPosition(getGyroYaw(), m_swerve.getModulePositions(), pose); + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_swerve.getModuleStates()), getGyroYaw()); + } + + @Override + public void periodic() { + m_poseEstimator.update(getGyroYaw(), m_swerve.getModulePositions()); + m_poseEstimator.addVisionMeasurement(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(RobotConstants.LIMELIGHT_NAME).pose, Timer.getFPGATimestamp()); + m_field.setRobotPose(getPoseEstimatorPose()); + + SmartDashboard.putData("Field", m_field); + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve pose estimator x", getPoseEstimatorPose().getX()); + SmartDashboard.putNumber("swerve pose estimator y", getPoseEstimatorPose().getY()); + } +} From 1df0e1c71bd12e8af1fc2bb48a89f53015cf05db Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Fri, 7 Feb 2025 00:13:19 +0800 Subject: [PATCH 02/11] fix classes get instance stuff --- bin/main/frc/robot/RobotContainer.class | Bin 2161 -> 2263 bytes .../frc/robot/subsystems/Pathplanner.class | Bin 5190 -> 5292 bytes .../frc/robot/subsystems/SwerveDrive.class | Bin 4200 -> 4434 bytes .../subsystems/SwervePoseEstimator.class | Bin 5803 -> 6101 bytes src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/subsystems/Pathplanner.java | 4 ++-- .../frc/robot/subsystems/SwerveDrive.java | 9 +++++++++ .../robot/subsystems/SwervePoseEstimator.java | 11 ++++++++++- 8 files changed, 23 insertions(+), 5 deletions(-) diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 3bc0d68448f5116e2be87e307fa2224a49ab0995..a60ebbd1ded220a4bb8b3a41fc0f3be52eb0f974 100644 GIT binary patch delta 631 zcmYk3+fEZv6o$W)LMbW~f)uH=9k8gS6$G@jctG%kiq;b#*4mra(Dp_ze2D!5 zJ_48?2quQaJ72(fqi7>+DA5lPME>p)@DM8|Cq#?pz1hC1f4 ziDL@mEoiuN#>SmC>A=vL5Wm!*JIYy;ZqA8-CcKj-9!%Rp!sI+t;zSJ}#@D&=yqkjfWcAg&Sjui0T;saVqC!j@I7h{AN0+w5vgmStbKPTA=f3#qoYo%jP*|>zg>Rgb#xf*HTWm!+}a&<@cP`M$`sN4_uap1)}g^yl(IKeUE^pj+O zB?h@AE30ycYYcN!!p)XRS-;&BniW6vzBjg9%YN_>N@=4M?yw?xJrWg{(H_%d+o^MR fpUies=)Xkhkc8)7qsL9u6Q0Uwe_Q> zK7bD(Ne=`Q!?jo586SfeAg;}BluTykUx)u+Yi9jUZYA8G+Z#WCX`ZdQE1`%(QBgGG zRyNaEe*Iz5r9e7{VnpK*bvUA^NVt-6DFu;=CWhUJYuqV9^|Db1AmWalGiIOlNIe6_3Ad8W-Tdw%4CW)FL#%qsNk%KLV4ZmHdV zx47&w$7P3x5-Al*n{7L3-h}sASGa0EhX2Iwa^K;eVqy?2gf;Ut&6h}7Jv5t<=jw^c zJ8xGvh)4&pXYe`6I^KZ8SZjeU`gO;m$JZW%j<}*BSwc@pRq2rfv48v z_#hx~!s{{Vt#i7|OzNWeXV$s2CbdFbld~ot!YV`9BxI$?vY%1XGJuREWNDIPfjloL z@`_=u3rPpfa)X;YLQ}$1!h*dDTvm4Dpg*!KI?>rAD!RV8HZi&=vX`aO)2r&z$*PK!qF+(Gv8GR#0E(_QehiB| z@=dxaiW<+&*NePC%emNHQR29E7fmSWxc9U%$ujAsx2eIo1%w@CUz-4yMSluS1}Fw- z<XS`9&K$8-EnkP5+IBZ3@rna&wJujWCSTVmgS&=dsq#Ha1YKY$H z>6TT>@iy5E)31BF^&HMflM#x@^5pr+(v``oWuNR;$@0+o~nQMPV-GyjFQy4>Q#oN&+z5@W-1CrQ8?5n>nBvo$6^L^ zoX~R>M$x26li~}Fve*&ZH0C?D1&RelP5yY3ztmV{vzT%{%s)zXFomgXY376_b;O)RV)|*@{ls6v8(SKP%VfY%Z)dQ{g2@PCtvOy7bX*w@mxw3Rl zO}etWD!J0;2G%M9m34D#Q+27Tv_%7r4rfie!OAl>aihs*X=$Ed<_P9Z+^i2+zs2ql z7ShRD86JxQ?zFj!yY<2B^1ycPHQAvU(s&@Dt8L4BAI*A=eQNX}g|8-=KF4A^yKVNc zOCPc~8SOk~(xI;m7J47&3B4=$Md;B8P0rjsWAiM}>8+uO^Pi6JoT5w7tYbSpI=(Q% zi;nkxn^$O7#OhM%xz%;4$#v<|e)rCdsLt$H@(yN{$CwogwK6*+pX|!5%qwnZAxN7}Y2!4xu>{&k ziLk;ER_>O>3+T%LiY4)CNqq^qvgYEWvlJGSf^+CWeK&FIE{;7Yq^CIdrj$O6l#5eE zvq1hCDu`1_f>mO@h!U<6c|8MVqK~6V{!KPBoMuL|Lk6`*LiEZo#kthVsX+YdNJ~0} z68b#O7ltnoeGKnk5~SW^qD(=NrPvn~w+oMuF5!fE=eJuHk6VP(6`1J8aeCu8xN#yL zXISJU-IB~bZVmyVIj6XVi;o3}A5F9#>31emN2=#C7d61;T!ANIIpcO1wN_A_i1}?? zH8at|bzA5p+r~ZU?f88=z48^a&rLKcJ3^(8LBjT6c@jg9ChemuTewB+TP2`dZbo&h zs2pwrc>bWPNrRHIWVR0{FhvsZh&d!>^@-lz!ZxYxo_}hyTC*yszyb$;E z01l?Sjl-Dri~j@qUT%w!)52D?Pw-;m_n;9ph(QioX{1*zZqsgDX+=1%B2u&{6e%tD zlF(?CWS|qP(?K{itt(!`ACdXxx zZ4gHfjRZBv2zHp_7`*dbB>+6AY2$RxSrlB;_U41YtWDWYLjz{d!Qku7_9zv z7R2=p;3UVAhpO4Gk*-`#Y^<1%y3$?4HPUS1qARsi9yeKDM`^gzb8~YurnrX$rKa-n zGK{g-**KLGGs8W>U?P*$)|md<6m>eLn|m5J>lCOK?hNO2W*ivg9;mrzSuSU*s6N~i zqEKh9CMkwIN$RW~blyT(UC@Wd&GHgrzCjV;1EXT|v{;kd43=`cTIV_GUdD2r`&5}X z-FZI`s14qX$tzf?^N>csE<pn!SPL292KFwLj%c_mwB z8yw-N^7x0jUNHrX^C$Oy-8y*7;26~sZbdk9#ZX#;KZZ9=s+&08HF%Hr)n@+~*9lX= z-~O&+KJvAoDO!FV?ee!1JGyHPowJS;-#}(T zpldDNk7gbuv7m;YQ|svinbBr(fQZe~4Ihv?Caa$D z;5^LaI;P5vnNY_Z0k>LU-abhnjV!v;MG{ybDa<8N)H8|B6}2C#?0VvGF4*cGFg zMIxoKk`hWKoNRGjCQJ^>AxaSEMPYF0Xu8~yrHdaH2scXxu8kSs5kU_LLQcqK)V$U7iX|sB`i@R}{UTgfC zSj&ag^4O-1dq-u~@L)A9WP4;kk8LBywbLnIId<8B24u%hxrdBXPm-jxmx6ZDn&?`d z5Q8V>XXRn3*7B4{QX7fGah}#XD>O+;XfP}qT#_^XSNu1?3ekbWa*v)d& diff --git a/bin/main/frc/robot/subsystems/SwerveDrive.class b/bin/main/frc/robot/subsystems/SwerveDrive.class index ee55d04cd48c9d7739e5bc7ade2ef288733432fe..a75b59c8b7d047c53d9b735e35dbb4cccf7c7fd9 100644 GIT binary patch delta 1894 zcmZ`)+jCPz9R4;rCnq^ME$KBaNo#sfb6RTCQlYnSNf9h*C8@PRp|&MLY$;e;6jXwU z;*CpL9WSU@QH!@34A3xktmBh2%;1`@4Pn{l4A& z;`y;$z4^~q3jjLs#k9IJTirhxA3roXG(HkzDCg65wPV7-P`wDDg}A!`dd1cFZ9X>Q5dpurA}h??9NKg7Q` zcUZb`uYxVyA+6E3VjEeAt^tG3po%VZYuLWH9aZ5%kA_~{&*M_NEsQ<|5eA(K>#~Li z(9eI8j<~`Y5EY}usW%u5*7mDt!9yAz#x6b~CoF>+9>LJnzvKqSTa+EvFdo-1fkXVP z@`rSk!EYR-!*=eum((kX@p!=v7kV|cuz;bpwSf3}6CQ3Ypt zn_9!B`G{I&Jp(R~yvEO|6-30F8s5U&d|r)|dGL;gcX5J23mw=qH5Tic+CLnd5Ct!4 zcpo#o!G4OVJY{cZpYcEKjR^^=&=g5CSM;4`lc3(1iBpKH`_3gyTD8&4Wc#D5P*&C$5meY^2Dh8E{al92)bM zQWYB>%t6^m(V3i=36!X~+n2&>-#lEDvgA5SeahGHM9uSJNTNIiZzL*-m6U{TtYop0 zlUQS{MlE7#Nup6KucMiAwM3IxmqPn>1eOn2O-BOJD4~#eD59|#l~@fwiDv`cq@Ggj zBD)|!OGCp(gI9)4*eqb5&&p2(^*P+<^<6{h96~p+V-|Vy2uEo*ZAsz5R+-F!VtEcb zTP?m~%N%wK7-^D9sY#$~B~x)1HAAQGdQ@W_YS0Ok1x z1n?;K(rN06`+bO0!gZATF^p2MU|fO8)vF*(abwso;M3+Pp^#{H>iHPIfrBA|w2pxG z`06t-({ji)+;nPE9d)yw*ji&iOZd|9OW2&4q~eSmS7D02Oyq9CVWSem6L>OH`546s zd3dP5N4A?gW|6DAsd}^9KZmCxCr!+)%-s52UzSrL9SLZp(NA)2A_#tzCa~7P;YlaU zz>tZ`1ddZF1x`GR69hwWpu+UGc!i#0LUt$d{QrP?;dU@D-Wkk#BxHKPRJ0MARuW9d z9l*Rw=f4w}(*_uNX)<7vbQE!Y1Nwq;QQWRG(cE(>od1S;^!g3FAxMh0$_p)$xpOvw zf?~PEAF9*quHgbv_ufe}D=Cm>ZvA$<#)Nn`(M1drC0(SmEks=xX>98X%qunafq_}4 z(_O-c83LU!J8scCU{SynoCe!T(?_5lGkm;smvurf?lN1xZRzKv+bEU3O)S$Z6K2WP SgvwLxs!D)NYk(1I8hqv9ak62H79zXHa@3M-n<@I61PwHEdzZGYA6tgCBIw*pO}}r?Ov3ar?-iprKThK*v?S~A45*0H&j2+ z(;JQTbw&GP4&C zoeXt;aaFG2fqoOYiNgV1jI)4EnSNsI#NuSNXjNJr?da4%#RtT1c8_o>xwRa7&_z0o zCZ-QsqJ(r(tA``BxgFQx!o3{#;ePR}(&h+bpN72*2E`~59a^@HBZ8<1+h&Wx*l*%I z$e{W|p-`Y-hk+PJA086dY)SQDj)RDQJFM2Rob1^2;P7~4aH?-`Xd)rbXfb;jPjZZ7 zLR{DWVkg8k`!3RUhT~~GBZ?gt*sS>8u~&JKily8^F_2}`b$IbI$19ib=mG-lK4E3z?~9}T9F6I_EU zo`!oFo}l(4ym5KW98$|NTL!xm#Fju5qE5)tnJk_sJm<1q|U zz-9_Pgkd^7LJk_FqFxPQI+wuXnR>>FLLpk&sH3S+B)@Cpz3)5}6l~MWS zBw7W@S81UIH;!+jB}u`fl$?>H6UXS6MD8jk2}7pcDLj?Qe2Narl)02H{P;e({Sw@U zkFp1R4J(+A%}LCsEv^4)d`&(ZO65{XpyzKxfEp@D2m#Wl#=8bi&;vg z5zuouNf@REI;6j$Yj~dJknIJW`X89&?JzIh8D{0nbS3FrU- diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class index ed8605b867cf3e10d347d8c41ba28af5793409d2..bd3b2b43fb641fb735b9f85201f962108af00cc8 100644 GIT binary patch delta 2253 zcmZ8i30PEB6#g#*^X5G+FpPjZj<_(4D1w3}aBWpkKNw3^)6sHL`Awp!V0rtOPuR%WHvx$lk2=lkBgbJzd;_dn;J+0_4FhPMCA&fNe; zFf7+9qqW)@-nvG=r>;sDht>XD=bU6t+wiIyZ=KgalELCiD`kirTR%%z!O)c>3Rba2 zO^c7N*8PRyoeb%&G;vf-&t&Myk%CSP*5P4@m}0-DYVIUYLy4!dM%Q3LPYr$HQjp5v zTyF0gH_qp6(m9fm#?adU>ej&+%aK0&aVW4U=r2MMsVqYriEyz@p;}UN2QerGCFMgh z2MyGaj=|FN#SBTV!ijS{O`ePzPhE9JvES#dtIkO)l~~yvDm0B+!#HvNt)V#45-aJtDJh18@mPA;yatk&}2HCQ46}7&2U9osuZ*sw&n* z#yU(NlR4tyP;eQ;U~$yiL8wu^)nZ((po9bzqgdA-(hbR);7V&pbaLe!y~>}XpjBhpj1=TGR=PGT2rGpQPLi1){*sC@7ScQi;)?x*NM(H(9 zulDJ>0lSW4JsuI|(ef;haXe16Rz`2I^~KX1oAHdu)0Ky5{vf*_@j z;i`c}i_<)YT!pYhK<=6D7B#Fp4`?k2p(u^$^5I30gVePzABsJ>CR;SG``->cWm((o4EQSi1%j?1!K*W*-ZL-2iY*Nla^)gm4JE2h_ zGBK$qsRQfXZRqU|p-(H^?p;W)u)7>tZMdivL)`8lhPGma+r1N`fL4s91$hBNR@GXJ8B7M zsRby-slFOqp2Vgw^k@Q>S(QZ4GAjuEsUWsUCn2<4NOV#=qDkk7?mU~LRMR32G|vS@ zgVHo^i8^laInnru{r&z(K%vvzL1^s$4;Tk z3QVD67t>WOp&Tlud?`ad%4wtmGcna*9WYbJW74)q$odf$!3>ZYAM&y2!ehJJ9s6v2 zEck}pM&Mg~M^BrI)#La}VMP%Ka=YpIA(B!ocQKobfG7cM~a z><2Iqms->rJ2qcpU33FHjUN9X294b@i9uWLo#|4cK_`wBq$)@k)ta0X6=~>X07Di> zJah_d4B0Iwv#FQOkh5BA73VZbtgE=J$xq2+P=-&K+^?v2p^DD1OC*Qrugx@f<4~Yi z38x1~PZWq*+Ck|>90sTsqSc$D4~mJ3+vR7dw2x{r+r=4e4)4d20Ir}vLvL}>kSwMf zlf+cLNj0DgN;w8#pco%tWh}v98EgncfqjZ7O3W0i;u9^7Ns{@s9443*3}q;eU|f#x zYN)_<3WhVvvWlhW$ncd6R=Z==oVw<^g8I5fcfpt$b6hw1OBIZy2PLSLqIbxt_WC@2 zkGHW@k17U3y{Exd?{Uxes~Cf^3aS}$MV68q&oK_;#R;9yFcCEhNa9{J-^;`?8MUPM zJg?8i&^1EVs47<7;N5HFXRV5$`w-#h7O)+mSa?HbgvBWT5^>O&IfKqmgYMvs_8yxy_ zEaX^(#Ue{>Hh0Dnj-^Oth;MTFTz;`H&SB8{XM21MPFbFWQd#2LIqtxnBB&PX?#6Nj z_lR?9j^p0e9fhgvi^l5mS;~8ucr}#*H3AS)Ni|0hKagkvwUR3Y`L${bi z62y9=kwR_5OX9R~8^P@4cp1CIy2M6Z2w?@S!fdkHcFX8yie6DYylev1(dVygn5Uu~ zdpY*u6;W*(>h$1Mj@Ph}fs+j5yfeIha)3*Z*GZZRkE?#BiZ}7Lg15weQ;Fssyes}O zjp%q#=C!!VYqQUExgB!J2ONiRSWGeJ=Y7P`!MD)W)aM9Q~j zRLZS(mg7szHz_z{rmD}k*w`;(wWXB(CJtMi+COMdizAjSk(O-HD2NjU$(aELBW=ex zB+{QYO{7o8inL@RYV`z6=L20k0L)4&8O?5XwxXkRFVbpTkr6~DbvxIZ?UvkDCIV-Rx41YI#4xfn^)HOR*_*fAFlETntO&>hQBfDN>I6N+#P0rIF0 z2;f#4F?q;EH9`q#DK$!rlDI8|yTZ6{JI%xq5}=FR?ITeFF_^?FFfCXegBeC=a~v%< zN|_RXsSNyoc)#q zauCKFmiJ_A(mjm#Yb+l|Zy$*aK8+4OmxHTmXVYE*Dk<#|5g!$zifS_&74)JQk7`WE z*fy-|Bx};&949wzOoX-tl5TFFc9%8)|JO%@uPAIBzQ#AyN?ZOw&N+=A@e|I`Og2d| dktqC3FW(%T#|3nvcj Date: Fri, 7 Feb 2025 00:27:33 +0800 Subject: [PATCH 03/11] clear commit behind main --- .../Constants$SuperstructureConstants.class | Bin 1867 -> 4089 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 2840 bytes bin/main/frc/robot/subsystems/Grabber.class | Bin 1978 -> 5208 bytes bin/main/frc/robot/subsystems/Intake.class | Bin 1523 -> 4555 bytes 4 files changed, 0 insertions(+), 0 deletions(-) diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 95ddb791f998d13fa5ace1ce1bc141a4141a5bdf..e2e5569fb613e80feec5fc7ee3f5cbe7f3ee59a1 100644 GIT binary patch literal 4089 zcmbVPYitx%6#g!SU0R0nDvuIgRmAdKiaZnw?RNWMVcRWzAt=`AcG?cz&eWON0wNY4 zD1!Jv@PS%D#aCj41q#8$VB#-=#;7qy{XzVxe~iD4@!Xl&+2LA>6_UN@-1+WzzI*Pu zk6!!x%I5&q<4h2v7#4RJiIAbU=~gJBYo;Y@mN~yM)vFk$Wuy{T%22%iAVxDx+$s0T zp`@&JhT?5Im4wAGEml?C)LIp*Y6&;R8(M4Q{5M+3z@iM}Gs9ctWJ<}Q&|sL}5Utrb z6n(lpY#%8}1cwZvnx=6gT$_ZXHl&VO)t~y%N+z@Vx#_JfC#u9o@$WZ$1 z45$e+WcJENcPOE29cpK&kvp~WZkhtQ>lli>P(F16=N%r#FH``{^+29x0a0R0OIU^y z>h01MP2IiPm1Tx#OhRv03`bYWkS$f$NZvS)b#pyszcRqH&Hvr>i5A5Nq-FAXje)X( z%UgawFy}JEl$wTcMFnNEx!$*?1)1Jhe5BKC=#GFze84vQDGlY4JN@h6&NS zrtoHea;CaHevLdmlihw#m%sC-l-2#=rflX-rhCkfuL6LS-7ndOBxDu+j2RiZTi1Sj!M7wFz^ngexSBM?SYIB}_m* zhgD0c!A6R#YJG}fDeVj;#oHJDhizRUoBU+s`#Kk!5DQ{6!>Ze>%#NEMZBHt-G(icq zpu;54U17-G3d5Qah4*OI@d?#S*n$Q=A(xz?v-p4VjW7*?@?yJ|z;(1p*oymUqxR`Z zOYWpp_4F#5X`eJa$CV=_=bYOmJb(uoCYe35VMS7gp=ef2?NR9tDJhPQkdYU{LlPz; zKZu7J=y2=_jo`Sc?KhkZRu=)*3C!ib*IEV^HkdZK%mYAUVu zx~W=f9~JlVA+j+XCGD^I>h2`@WGOGKAVT33cwB&>9jbX>wJOs3St zR86T%^|UEQliZf1HJTjL6LPXeHdOB0oq5PX86C)0mum8N&j_A<^svZl)Kf-6sa83A zKF!;CC@$k~L8`r|rtxBtO;a(0ILWYN*yP&Js_Z#Rhh63*4dNX-a)!`GPsUq3N@;YR zVp~;zY zym(UyH(l^&2!5f@pGo*xLcH06KS%KA3jRDFUlH-qp0ah(jT}0Q=%Ily19(4d)T|tVkM>oIzA=r8bS|AmZFAK;s~qxiztP5ZktT@EvLF zaN%US@Esm}M+V+Xapd!8OQY=q+6&Ni5j!0_{~~n9O2fE_-5&McH1>{ml#uCC?{%p? z)V?(KxdO;^QTs$xe;WNRicA;P@5$_+xPs4$Ko{^t0g483fTrkbKAgs3pJr}5nzGukj(QhZc zS$5LGCutk?(Dv7`96Bw$fe0*YMheYzv$bLuy0Dje_u+B+e-QmRhSxBFH}F1A;4I!G zzPIruPU1RF;Rl??4ZMrra0Y)8;xBx_0{D;oC&=LY*e@g?P{Ye)=DU)*pxf delta 797 zcmZ9K%TCi#5XOIp7E0@}MQ)`~xnG0|jUb4~#bPT|D3yX5jR`SWNP`vw)s?XugImdo zdzZ$YMI_P02hfdCU%{O|fQd2AzpaRD&V2LD%r}>upG_a#t`EQ8z6Zv*vh8}d{dujl zH4&Yf-u}^Sw$#-s9ZJNOV)4azVksI{c*QmMRBc1WR3bDv8A%+7H54f;UgwFlFdxQGqTOKKz>Fcok)%sdOqc?nKd~IfAx=Et*!^%vXEJt?1Ak zMLXzH1kFno^ttH7Zx!@t+<2Vgp49ZxXIdPy&VEii7*O;|ZoPTxsId()WL`MhIut7G zq@Q8US%wrkc55}A%*0n#Hj^1A=NNM^YQ8%LY~xIr*7Et`enOf_CKR59WO{RbBb?eu zE@x8fX{&u$a|~^vi<+ten%2~y%_nC-`OPoqib|O0uCUVPn`)?ZnY@+J^R|LA zk#GnupKln|A71b0@< zGA@!-vWlM+?X1zqT?wWMv(9DibAyN6<`H*z%mcOrJ>gPb23b!2OmW4aSS5UE&mJ@X l`dGI_M642BNMM$jc(!Kd#FP-1KFAza{X7eD^2Izq{|7M3hwuOZ diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index 248fca2b8089dc8b3ed9065c10e16bf2122c3777..73f46c02dd0c0b3d49f996dd0053ea81dfb95b7d 100644 GIT binary patch delta 145 zcmWN}JqQ6|9Khkh- o$Kk<%CnF-pyqWM}%9j~GTuCsO6bnhSluW!I{ZLt={g-O}0cGQFNmC2tyGc&1N;5M6lR|m6?qzg3W3++O%2C z)~%g%eC_LNPVG$Mq|l@>$xtRsH&UNG6ACP`SYflK$c7SID(tCpq{fLlXPR7S5zyv_ p!<`NfdOV}y#efe(zKraKs=}I2k8_Qa;yRu^n zy`dMqZ|F_a(v+6s5+xxmO%Ga1>3z}CyHB5|KlCr?4}JR1?#h;J$+3UY?96=ge)l)u z=!O41^DKZn@J|iP1g;yhGf~@0TW-`Tq@78}m19oyux+H%($-KTuuTpYq7!*DI%L|8 ztG=>k`qb`d#u^(la)Z%i=~$cLNCB;o@3LIW7HEuz-}dh!~UE;SK25{aKO#*Ju}LPV3UZRUnsn&xN|cyM8x)I;C~9V?*MVWYrR{rN`E zH;tZ_jgdreS5p|9aifMU0&8cO#&WJ4mbQ+Ya5FU=H?jrUGbFGs5^E}xEg)+NqX}Cz zGz;urqUzLF$2M%IfsH{rZJ>hcWej`Lr)0)mq%q8~d{z#agET88jAECDor`o%*-;(4 zv4_DmVY(x|mSei6l@mA+iO~{ERJPq3%Ud~_^ToME$E^yvP9Z1F`(!;{joUTcCa`M} zz-&9}vevXb7>=&pX?USe7lqn|!_AExc*(jcw4(NCd4hn>oTt2+ONMtc+K6T$@ zUM|u_WjS2ts5Tw#=%8ijIWIw3zan$3y+Asvus@_@C05nph``lxYhr%fE22AftX4#E zfop;+ZfsQ*tT8euhixgHCPmh*V+~em=oQ$oa5dDB5ZF3XjoYo9;~F{F*^(^eB`dF8 z$hZYtPOsOa4=D}(0^1irrUG#}F0)&_>{a&FF@R(A(P*1pa7I|r97gs+rFuRDo!x^I zs*b&m!4ior@pMy4ndWXCwOC${qd2MK^)P6m!->|mw$8-K{@#;aJ$*filkGj-hhm5O z6Rmx*o^C2yg;B=J4VkQW!No!oaoSRk45S8@hNcA!q;9P(3F%0;iv?Q85QbUqM!l7! zF(i8p+ZdBBJwW$V;qNh?4MgrHMm3z`sVi~sCAjGX{IeKS{JX7SwVZ!7{b%@>L3vKH zbmVa_v)eZj<4533q>38lW>ne)cC11@!*tquEmtHhm#=9ab)qvBH`ATAZP~P+0Be)x zhe$!kxC)UQN+A;O8K@K^>eOi+b^fXQbi6^G+E_Z(+I=|QS?QdH2Lv`NM_r-Q(C{XK zO_kC#p_(X~%+K07yhYvbeaH6g+Oso^`|&oFE^qhl!LnPabVYj^@sN&sgcb2Sb-WAj z7Ff+P=6j)F8(vejo7HT-n7-$n>3*pE3@v#S59@d@-dC1l?3~(rI+$y7Qf$|f(0QRT z%szk*Y51VPj-?5v6v(<4kC%X${ z>}A<5WEuPoaVukFQ-*D-d9YaHj+jjE4e=Sj&TobZw~AGw-*0bIR@P-x7wKq9sbcsk z9iPS{tg56?TgEVX)sFLvxPlkb>Y1Zn2c-^uLC0h2P>6e^cKqg3QL^NE_1fpMrfel> zGS_m+yKc(J8fL8pgermpk%JYsqaBL-6FRbSPV6W;qE~l-^UO55*V_rvC^u#E*KqidY@_t6#6oL zsN+XUAmxz`zlmTcljmc@8Z!-k z$L4<4HCUo`xU!7-DmtaXmpC^0{0*I>40ReC3E zq_!bGgX_?sx)6RfsH?^sd3ksvYcWTtH;^~7DfCA6ecs5%&fDelho>nMARZ%{;8@>$ z9#=Itth<0$p2LRb=BKftp)qs;n~I2pLPhMjjA(655qmjKVP6q<@S}*#vdYaxJUSzA zn1TZ_#>kx>$U1LcAAo>&^=I%|!U%k>D%|Ji!gUd@y9(SF1GqC4;p|G%9VJ|%3S3FL zFI9z`CtW|`QdQte(tWui+(aemjuY-g6}Xagk5`48C*9qIyQd0VNjjFtz_a`4E~X+@ zD$f@2)r)vi;7Km{;kew}G(0#jJQ$Rv@L)Nt0Ag1IkrhE$Ndu6UK$5k7Y7~oj<^sM` z#B+0{G0T9~PD}HAsk$&VLY~xH3DntK#E&oHCm{@Q7US%f)3Ywle(ld@;;(_32jp0^ zEdFVfM>Fm%W4AS6w+z4aO!Pbao?|o1nTeP1$Ajznn0g5>bL_*P@E4A6#9#4u{&97b I<3F(e|LO6z4*&oF delta 847 zcmYk3NmCP15Xb+IAv4MF7)U@MtdTW|V35UyC6W+8kTsDYxKNYD7>h8eVX*YXN3g7S z^k|iLQwWGvegJRYJbU%(*|H~qa_O#q-Tmu-zy9Grtx>=IIsFP?65DUpq{~q-EW>2* ziAkk3xTL=tXOJiIyLl_jAo&AHhRSIplT{(1ibH`@K{Z3W2rDfzl`E(b?;L}&8+8gi z;=5x=s>c-}%Mm9>qutOXR^+kfW)25r7c{hTw4t4$WiB3xMrY#UgFNfIL1jGWy>0l%boMP{Iyl7a_+CW*Jpx)d*uPHKmcZ=Wk#$j&rdLM3CwL4s_yv4Z zDxvlm4WHl*9>QBfO9>q%^c>;Z#z!iwK!|K}8C8o)(6tHOM%G8Z8}%5ZJU@ft28JkN zI2gvr`NRk*km%86s0FuihcFCd{}zas7Xo~qaT|Dq z$rD62JacEk{AVmNoM1b0NxW8`-->FI-GTN?f_>+sDi9|j6-eL#`A%B17wg!V^3sy) Rcu3xjO*|s6;W3`V`xkO!lFR@A diff --git a/bin/main/frc/robot/subsystems/Intake.class b/bin/main/frc/robot/subsystems/Intake.class index 41d732bdeac946c0aedcea0f7499be3b8c450c59..3c3e4926c96c78882b9732394d42c310bc65d236 100644 GIT binary patch literal 4555 zcmb7H`+pn75&v%FU=SmbUawpY%mrT1x4=jf<58T1pEoEp6%J59+70cW3MTkYoR%)!xm1XXZPzGjnhM z@0Hg8ya)ePu#I8wlwDM9t6;gRQ!6<0jw>pTnlfE&Mkr`uI4nvvb*`$bQ@ZWA@}Hp> z&iAWDt5VU-lA2pOHK;j)p{BV-8))zZJ}1DEn(^x?p3EXyELcKljK0g6QlDHS1T~wonF^W)tRnM zO&OwK(MdHI+DvE#IviM=s|rzK=vf6~3U|)3XGnn|9CbvP!q$t!rfbhPjt)rc8FoCN zo4R{~%!$SG3@u4=B8mvE=h%hoT5%(Z_@qX06ZR;07lXP*lR0k2UcxC0m#k+v6-%Gj zW;N9y2GyKv>t;C-U*$p&u8nzU1H*ofDB4?bD?^uWXt&2ocPu+T8js>O98_?Cp|hcq z#6(%x9EWf_gEFfbH8D2DusfEDhb#-k5>ecVxPll%{}vr5Z#jCq$_a@v|pi&$O3At;O~Ovv)|D5XIxwZ+r%yrgE5V>@=V;WW;2Jcjo&+?>ix44fW5H##;kmOYmo z%bZM&JenPtNR4I4$tFsQmN}(U4GX#g(L>ur8{Us|3O>LP-+&z1b`E8QBivD{xf~ya z#&E-o_avuHiE+)=D#9i9DKt&=dx}>Ak@g~}15&x^y_WWI0@s~D{}jqnf5r-)hV?h| zzo9<__{BV|PXII4;PXpZD5SsA(+qPcN-M#L3l#LFXodab0txnENCdQ9Y8&8<0d4x(XcS6VkkrdBBko0_>Cn7YzdQ8V(It;>Dz zu*IF$>6-3OH^RDRC?&KGy6k?r&RYg$K}&2Xo|on9%N$?9GgKEzK)%w-q-pys{UcO1 z#ISSuq&M8iGvDC&raTj&S0wECqhKfPyjI|qTwl#j$^l<(FfY=qHzXmx$MJnUPinnZ z6wI#&gZu~&zgb~uu0AV<{UJBejvwMj3SMBib#;wdCCc$4o}+n{G~|V06?K;)C0n0o zq|DTxaQqZMW4K;EEI5`?b46nmIk0ZF`Z3DsWm7XKe|~{qDfng3qikZ6WLMqk2Zjw{k2N&k@Fw~3o74eM4(FGi^jy~^<&JlBrj;|~g6V~DM^ulba~ z@jCuU)t9ot_t8(b$0X<*96$Ed=B%1s6eo3=*6n_R9Fd>{bQM;XNrs!6 z%qX~xoQ=>Y;|_GmZcqQZWLfuCDwDKAemi(8jZ@xA0~4(Zoutnn`t1bR2|CT_>6<;5 z(bdy+!xija#J--Mm$9$wmdF)!*KtQAQpe%fab$Z-9sRT};GQ}X^rwyo7m@UZhXdi3 z4TVQ~UPbC`WC5d>ky}Kb^yC+CW&!V8K%tK5$(8R7dT%ViT=D+#vyraaWy~$&BE7y? z$A=g2WF4QFyhMI%!)NI?j~nUt9<-pBd_Ii5^bygEqcpawxSu`?PN5HH&<_pAP^RI1 z7RT`mDY!(@o%(tfLznID4EtkcrbFovq`$SMdWzHE1aUJG_^f5Mv$7jL{7ZKoS8+ryt0d z>iGE;{HBijYM^WgsB;QWkGjEI-k>?fdPPaUsZ#qT1Rq}>SZ{^ai({O$DOG)bQj zXRr@vL*V-Z@NIb0^X1R@3#~nLONw|4e|u;T&A)HqZCW$>=vj!GEyl E|12FRzyJUM delta 486 zcmYk1&rTCz6vcmEr!!w?_@+t=r31A5X~A~*S3yM)D#V3hg&Q$0jEjP?3#n0;COpIB zdlH%=i6%S%K7ngrz{JF*@wO0gb5HKM_ndR@x5}@f;Ns7hb6|^|BzTtc-X^c(s<+ub zYJY4hoLYTfk$&7cI1F$YGH5cIP*F}gWk++vzH%G>FnLW*v6OTs>~A;c8b%nk7w%4I z80(9QinZEqax^L9e4)PYqZl{1cs^Ms4JFFrl}_8 Date: Fri, 7 Feb 2025 01:11:02 +0800 Subject: [PATCH 04/11] add swerve photon vision --- .../frc/robot/Constants$RobotConstants.class | Bin 620 -> 572 bytes .../Constants$SuperstructureConstants.class | Bin 4089 -> 4089 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 2840 bytes .../frc/robot/Constants$VisionConstants.class | Bin 0 -> 1648 bytes bin/main/frc/robot/Constants.class | Bin 538 -> 607 bytes .../subsystems/SwervePoseEstimator.class | Bin 6101 -> 6332 bytes .../frc/robot/subsystems/SwerveVision.class | Bin 0 -> 5111 bytes src/main/java/frc/robot/Constants.java | 20 +++- .../robot/subsystems/SwervePoseEstimator.java | 17 +-- .../frc/robot/subsystems/SwerveVision.java | 107 ++++++++++++++++++ vendordeps/photonlib.json | 71 ++++++++++++ 11 files changed, 206 insertions(+), 9 deletions(-) create mode 100644 bin/main/frc/robot/Constants$VisionConstants.class create mode 100644 bin/main/frc/robot/subsystems/SwerveVision.class create mode 100644 src/main/java/frc/robot/subsystems/SwerveVision.java create mode 100644 vendordeps/photonlib.json diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 7fee06049d20edc997cfaef62c5a5ca0f9af2939..104ae0f5614feaf86da2ecd24c5b98f584e08287 100644 GIT binary patch delta 153 zcmaFEvWJE1)W2Q(7#J9A85AdSEu47Tl1-F{L6AXcG8bbDquk_vMr%gq$)_1rbNCq; z8Q6genHU6sEJlzTASnzK(FgMx7+AHoGl*;iD-i>dY(QZK1_=gnAjtz}vjTaNKpr!L Z6p&(0XU}WIT$;?g7$xP2E znYhxLO@fC(gh6!Ts}@GZ$@z@dj4YE^Gpg2bGcYo+1C3!~;AfCz5M&U7^M!#dMvxXD zDF#%b59Tv4uxf2*5Z?&aA_*kffWizw%~C*;2h3(=kYZfrYf$nn8*gKp4X_(~bPWbu;1gh zt3s7q>NIK4;(<0ccf4}Xj1C_>vZTwFJ`MwZ8S=+S9&8zm72sJRCJHl^8R138d@9OZ naTZFlRGO8FtX1Yq78}*ss=>EYAAfe5uH{$}H93M=z{2UOWL~o=?wmWR*Yd2rxj?H`oZ000YzGOB delta 145 zcmWN}JqQ6|9Khkh- o$Kk<%CnF-pyqWM}%9j~GTuCsO6bnhSluW!I{ZLt={g-O}0cG7MRBJ!g9Q%kNL00X)ES18oc!tA54uy|SmQl;;MD zyDGR;l7aNx9oaw!L(gmehFcDIYgWGeT2vINQgg{dVZN}GN@kaG$?PMB-psC9LHW|H zB^ZuuM=EirAv)2;aJar|%gVA0%Cg2_F63wO#ie3CjKmPn2)kjuU6EE*`hl|QT)nbt z!mA7AuUbW)yFu0S>k~G`juw-Tmomx4{EH&PwI)Zt;!8*Inw1PEMP8eg!m%@a)oUp7 zJ-S?=1RX&O1!bqjn}A`UDTKz(a@xu}^5`nan#)zg7Yr}=x14OXmq$|}FL;Fa)#L$uQY+>Le!|(wKGtf`;{@2*yiq3z+j^Y#s zOq{`4hREa=c4Q=-PRC0I&M{p1SEvO~?Zg?xKolo%Aq;krp)}Hpg1tw)okUBvc1(9c z67f* z;0D9!LGU(9(8NuQQBgS_w{>MQyl5t7KfvZY(%NpT)I(k-Oqe(lneL&>PfuieNRLF; zJtPd=XE?hzb6xo*QJHB-#}>YcNj#vMt4haV7}7=1M9#K{^87>y`OriR-I0f1H<3bB z%0w@^8M-sl6*+w)ioEO))4dt5!krTLWhiT96zzB%R!kq^s#lV(=jF@+`c>E#3SPsn zh*=qij&1j!;6~V;sNc-HuJBV14+0Sw$T1vi5tg!SYJ?1(IT5HiFHjhx-L%uj4xyjE z2uTJ>ZRjMK3XtagA>n!?7HBlfuG<(KF?=jRodB<7VAtAu41 zXXfX@`HT$w5Ru7EjJ}LwlP@qT8Y?ldFbFa*G6*wBGKc_aF(54tq$Pn+3P{TUX<49U LTwrw~KshM@r4}46 delta 59 zcmcc5GK)pv)W2Q(7#J9A8Kl`6m>3y^Cr@M)-l&|xIQbZ(3nTAjZYD)$DF)`r`b^T? P3=A9$%nX7IA`IdH#C!{S diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class index bd3b2b43fb641fb735b9f85201f962108af00cc8..9c2dae665ea7b287cc808bcdf1de03a962281034 100644 GIT binary patch delta 2580 zcmZ8i2Y8c37=ABF^Cy2VqiJZR14B#OBrT?;Rz?LupvWqfA`q%Vv7rRprX~@o z)oWRE?;TaS_RgivCxoJ_Lkq*PaHNZ&bWX3c(5#O(z5Fp_#iHNPX)fg$g#zI>7F$bI zc;Q#z6HUf4s|S?~qr5&D*1O9*%nF9l911Kd0;o|?%`lSYa&$-z55_Q*=>hrr%X%4f zAcqyIf`b@JdOs{ND;P&?HkjOEw=suJ5POXk!wzOpW-M4dwx+gPMI9zdQiq7~refz5 zj%;w1aHeuh!*sF2G^ua~hYfZY9eF0lEX-ywwuj;jGrh#tD?T@s^0^!iI2Fues1@H^ zbHy@SjyTHVP^}n?!#U=oSu|(OwKd`hx!FR7fVV}|*o(#bEW69MP%>Z4kqftiB@A^M z#@?(^)+2GWf}dnGZ1~(mhRqN{3Ku2p=dtl!3RiR_z^$J=kP=dN?_xwat zBo>ZSF6yO#$8fYE#E?hCb+M+$;^EHLc&Lrm$%%cC$H&7RfqC6BvbBR^+a-aO3@&ZM z|II2`Mbbv1?SbxJ=;~b8B>?bXH z9>@7uCr+|9S6s+(ktE|L6(yUrhLNZos0gB$V*`X3p{^cThf6pvMFB%rG!&C|F~wJg zW72OMIj+E!BB@rJug0|st`WP{QsZ^FUgTsKT5rV73T~of(m9GmHB+-VZo#c$S$5C< zH{wo?yKp^&N))Dr+M}V626+$1y|_>OmR)BFVhcwf;-Zc(v6#Av8C|S>f{6?GT#Fa? zb3A|@;v+uW(ukcL58+`^XIo=^6pt%-jGm<4jO*5t}2zOHYTRXd@ zT5oW?iMK?PW14?0-r;x`DF#kj4f@7{_esBoaHykA#fSJR)qz_Q9vsTX;en7e6rXU6m6o80VC+wti3M6{@NQ*G4x<$ zI%GPnk&k;bik^UNJ6Nzkff98k3j#1xs1qfFP-WT*CaF>p*D+Z+?Anff z{(hAE2T+kj6*UJWQS0B1x}ekRs_)0dBqsa)DIA(agWtamhXF~4}LtNJi!D_R(KVj1;oQ&^tDik%4e;n)-+ZXB4da4 z*pIW4IGbWmmm5=CmBP8yy,G@6{k#UxuIty?3B4vd3|gv~}drNK+O`Qbw)YEgwL z7>x!TKm|B}MpR=7YN(!#K@ZKINpt7pAiD13>2^-Q7SwB^aa|c)MpAYD zl87?OKYE5RP2p*Yu3K)`MtHROeJ@rFdT;q!n zkrSN;S%T6kWhPKseyA>$89)|H(amYZ@3g-Gf8bAQ$+*k_BQr8HQh8-H@HH{5~CK08>RJ(WW z?cRIX)>?&-sD zghPRCh>O^rBA zL6dmMnIECB7_T~DrNTq^SvD;J;Rk4`C9PoGQ0l)8~8mf7l=Fa@X%!I9CsJ>!C2m5-epX^y!i6V5A`!)@#ENf2TL1FIm~` zuMVuxmn_hI-X^cl!!XyG!Xv3sw6eJVKP8%wid|_&J316BWAOa{-5ffOW-xRUTk(=I zK^&o4Ov@2aaIEN73(`B$B`#6RGa3+*qK6s275RLG2v{=2bH))uGjBJq;K+ep!AkL> zxol()^5sn*&oKhI(qAWXoP^b4wASc2nPUys(k+lgDQ5_VWWX>H}g6H7A+iv%ua zuu`ICFDYLYt_iMa4^WbHXK{^&OL4h^%VbK6X{>M?HS+xxxKixQ=pBD4uI0E+Zr5mg zjqVHSdLq#c95>=7(aFoz2HeVV8`g^Te6jFk9;{Bn-5eWmk2sBjTHMRA37f_1nXAnG z7*G&nC=nZtd196&lTyWTpE%31fnc_A+>h;IW>%N!K|HMBA@O{c!|})-@^uY!s+!1W z;fS}hTT1d6$K!ZHWLaxmJ$Q;^2kv0tB*>y*d(w0&o*|Ly{Cc26!*h5+!SmuIYeo8t zcuB;q^GCfR&(#|WI-DK4uUIa5mE$$MF2-aR6~4(ZGQ3g`_2>?12?r&X4i1JKt2Dfg zcNM%Nm|B?a>EL)zk~EM+tt#FZmuHVv-^Pa=AK_xLEqkHq3w)*EOOb7xWWd+>MpW8% zP5a!6X8H90!0|CwXDRsJiq*t_URReMnjP?l!#Y`vr+7nG@Eb!8)tnsBnlm8{e~HyO z#YXIA$P;UF^2ClD8&kzQIYm*1EGi;tu+oQ0K%!5^Otj=hYBi`BiBUvgG({7sl`kXP zj4}68Ck_`xl;v_`({n=s}HWNrm(8-RnhQykq$6X^*E3$>t4 zCOC*q!4OnoVg<7j8kOKACPkJ+#v5G&nBah;m8KFq!kix?h6i>)n=!~5X1!|cl;Uud!3Ph*G}nuAoQeuuiD|fjR!gOMQUjb#qJjGKX z(K}^0wRu!z=ciD!4^gwr8V(jE(Dk@;5PB8?OK!OYEnPwAcg3(#CW)hOIMD?uM01CT zZrS5es&s|~ns*q{7`C>^A!)Oy^jRDa#PBE$^(LM3r0r=L#PO`{Wtp3Fk7H+x?TzI0 zTZv#-GWb9Sdyic}4=b>Uj@?KmYa$OdlV4g;hgKSC!%{3xVeLgUVI6CltSo~fq+Sr^ zIAQp)T`2&2?k63eP}($niqEK(w)~cy^BsP~PxzTe3P_4}`e*P96^2s$ir=Z0OriD< NjXHngZ~TMN{{n!W6te&T diff --git a/bin/main/frc/robot/subsystems/SwerveVision.class b/bin/main/frc/robot/subsystems/SwerveVision.class new file mode 100644 index 0000000000000000000000000000000000000000..580ff0a98543fe054e4b81467e87486eb5112ac6 GIT binary patch literal 5111 zcmbVQ`(IP%8UMbJa3CHP5Ebg(dI1EY(AjC!%1xU#5K1DBww4`|V>plt%Q-<==h|*v zySD4*u5De{bzL{R?ep0t)mU|N?QZS5`>p+L|H6LVPjlb*oDdTT$P9jx_wu~Y_4_>U z^ZNdOuU-dm5FaQg6WDSx9r319qbbX4W=73v)6x^BH*`u*Pw7L3X{3@0Tmmh6G~+!r zX?RZ>Y15LUxG_53?v11p2`w4*hGx%pX{IjVNsJgt)6$X=PVNlcQie~UBBCYqv?j1A zkV?nAlVd3>m7KCQ@DA9MZkvFltDMb&l&SZcmXXk`R9axiLg$L;c-#}oq(fG;N1rkU z)&+_!BeOAYpJt_v(MZ=?DGb4U8;N2-(7t7q{ zpeaAE#8!c2iRq{jnJ~;zBj&*dY?DfC7g*gG7}usWZ(K{pydef78S^y_NpI{_p`cPh zlfbqG6e)^#6*gg)iWamo@Zrv*BZ1E0;IS|fJtrvIWZH;ZTFl!i2VpIC#L(l>fHs}V zSiUNFu}8sffjvtRx=pG;wKYqRO{-`}hrs&2V}Y=LM5-{*KQI#PAL$Mb3R_-d7#;}+N4h)v1e)epXTv(CrxLoAp7w^*TGBk3N+&v^ zJ`WnPPr7}-z;lgrjxFkx+t7Fp`-Qb~f_*?m6+F`Uhg94ny;vOx9`6l>M>_ku{6~-Z z!^2YjP8D6~W-gn$)t8A|hK!yTkB<_+vr!P(+jtua+nH#M8LSj*m{<;mb45pw$}n?- zIcB3ZV;OO8aMEJY)Z)zPgC#c(%vU4Na8MRv$(1$113&Ipa3Ax0K6e!X91(EqiAif( zW=_9~AO=`1sAXq7ek_^RwdkO3(jC;jwn&qJVOqXr7(`e>NMOZH_HswXF|htr8Wzj3 zU0I6_^yk*e;bVAE!Elj+v%4xD!o#d$N&U1XP&X^zPUzt1xE`^53LX*g&dVoDOVb8s ze6H*d7?V0nqwW|ZFGLuoJn848zpGg0Wxg2Hw{U*O{jz;Ntcu7O3nLBRGapO0u$yo#zBQ&NXKxN)31LeGtH9%>%wOv90?E=#C63KCizO^F zp+}tvF$ zSR_;;@o^PT;4`eg1dDxNkU7(>jc_k+D;6!J9-P6GDn5s&hya^YG!rqjxIbz)1AY%a zk7pG;Bhb8bt}33x7iM7`bKtbL(Z2xh{rIAaFUgE8rveJTEU>j$R&lMC{5ql@oW)mF zd<|b`y|7Ypv7I{am}ma{!|n3sc>ymecyS4$+L@K-@J-e#nU(VPifgjW3bfurRsk_Z zD)<(=lX-ETUw^CcGQO?iReXmb(VZr9w%%`>6WjU5+1KiK@jV5v^D17RP&l2*^DCfxKeg!w|P zurqh^RQP#Ln~F(q3y2<~-->ax`_^TZelMs$w)k90k?-l8 zjI`epbp%ij(uEh)hSI5$_uz@e_Dc%?Kc6_0dfG^_-)B5RsZ2VeA2H-Jrl#<<*(O2B zg6dBu^>lY!Gfmx8@OL&gCEgT#6wP^6@T7FaY}bb^Eb-+8oeLiMisGnNz8~ygwwC-Y zll7le%kA+Bdv_(Oxl+TG?fjJ=w|En^7dR+G9ltB=<@H!?uYg_Z8m!G7=_QT))Xkvw zT`a!_F?<>IJkI$~xn6E_SYfNRiJUf5*w#F!bvA{N^7$Ogu^i6{+}(T`>zl7(!*K1E z%eeCrc5u=-JcH&Lw9VkI%lPOe_?j=mH-m#&9KNaW8d?uD(GShEtA%#8va4>h#Y*de z-RQCXBhZT@G{N!5QQHewsH}@LzPE61fb4f<(RT$82=t%Frq(MsF7P%^^hv=jS&Rgl zckw5Z#Yygs3!J<0uU*n|mu>kSwwOJfwPPbXuoHJtcsus-AtU{{+4hVtr;ZDEBZ(AM z+{n;P;uBOYO)9ChvuAQ7y;g^^8~k!95DHrXg>4qs%VgV@H^-9Zl!dq{L;vBfEHW=) zIlXuK5*|H|_c-V;-|Jej&)s6%x4-f{!VQ%<3(xFVq={KP)}UnZ*({#kU)4}$^QbtF zmM2}J-Yw_Pzm2n5d?kx-Pyi<{U&JfUJd4*N``nVD?UpxIyUrDe_t(2uyPi(%)wY)xT^6@af)QH>H;3dJM5YNO6m_tEyFixXV-_h@X^VLzR+l^IZ2J)*X|oaA%olkZRYq~Aq)d05=cVyEnk7aSD1gJt-$&HgWp Wg{*O9Yy kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + } } diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java index f0ddad1..eb0bbe8 100644 --- a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -34,18 +34,19 @@ public static SwervePoseEstimator getInstance() { return m_instance; } - private SwerveDrive m_swerve = SwerveDrive.getInstance(); + private SwerveDrive m_Swerve = SwerveDrive.getInstance(); + private SwerveVision m_SwerveVision = SwerveVision.getInstance(); private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); private Rotation2d m_gyroYaw = getGyroYaw(); - private Supplier m_modulePos = () -> m_swerve.getModulePositions(); + private Supplier m_modulePos = () -> m_Swerve.getModulePositions(); private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); private SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( m_kinematics, m_gyroYaw, m_modulePos.get(), - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(RobotConstants.LIMELIGHT_NAME).pose, + m_SwerveVision.getVisionEstimatedPose().get().estimatedPose.toPose2d(), VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.9, 0.9, 0.9) ); @@ -53,7 +54,7 @@ public static SwervePoseEstimator getInstance() { private Field2d m_field = new Field2d(); public SwervePoseEstimator() { - m_poseEstimator.resetPosition(getGyroYaw(), m_swerve.getModulePositions(), getPoseEstimatorPose()); + m_poseEstimator.resetPosition(getGyroYaw(), m_Swerve.getModulePositions(), getPoseEstimatorPose()); pigeon.reset(); } @@ -70,17 +71,17 @@ public Pose2d getPoseEstimatorPose() { } public void setPoseEstimatorPose(Pose2d pose) { - m_poseEstimator.resetPosition(getGyroYaw(), m_swerve.getModulePositions(), pose); + m_poseEstimator.resetPosition(getGyroYaw(), m_Swerve.getModulePositions(), pose); } public ChassisSpeeds getRobotRelativeSpeeds() { - return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_swerve.getModuleStates()), getGyroYaw()); + return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_Swerve.getModuleStates()), getGyroYaw()); } @Override public void periodic() { - m_poseEstimator.update(getGyroYaw(), m_swerve.getModulePositions()); - m_poseEstimator.addVisionMeasurement(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(RobotConstants.LIMELIGHT_NAME).pose, Timer.getFPGATimestamp()); + m_poseEstimator.update(getGyroYaw(), m_Swerve.getModulePositions()); + if (m_SwerveVision.getVisionEstimatedPose().isPresent()) m_poseEstimator.addVisionMeasurement(m_SwerveVision.getVisionEstimatedPose().get().estimatedPose.toPose2d(), Timer.getFPGATimestamp()); m_field.setRobotPose(getPoseEstimatorPose()); SmartDashboard.putData("Field", m_field); diff --git a/src/main/java/frc/robot/subsystems/SwerveVision.java b/src/main/java/frc/robot/subsystems/SwerveVision.java new file mode 100644 index 0000000..1fd00cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveVision.java @@ -0,0 +1,107 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.pathplanner.lib.config.RobotConfig; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.VisionConstants; + +public class SwerveVision extends SubsystemBase { + + private static SwerveVision m_instance = null; + + public static SwerveVision getInstance() { + if (m_instance == null) { + m_instance = new SwerveVision(); + } + return m_instance; + } + + private final PhotonCamera camera; + private final PhotonPoseEstimator photonPoseEstimator; + private Matrix curStdDevs; + + public SwerveVision() { + camera = new PhotonCamera(VisionConstants.CHASSIS_CAM_NAME); + + photonPoseEstimator = new PhotonPoseEstimator(VisionConstants.TAG_LAYOUT, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.ROBOT_TO_CAM); + photonPoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + public Optional getVisionEstimatedPose() { + Optional visionEstPose = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEstPose = photonPoseEstimator.update(change); + updateEstimationStdDevs(visionEstPose, change.getTargets()); + } + return visionEstPose; + } + + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonPoseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file From 0440f8ccafcd845e27a059090d66b5747682bb5a Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Sat, 8 Feb 2025 18:01:30 +0800 Subject: [PATCH 05/11] merged with main --- .../robot/Constants$JoystickConstants.class | Bin 403 -> 403 bytes .../frc/robot/Constants$VisionConstants.class | Bin 1648 -> 1648 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/bin/main/frc/robot/Constants$JoystickConstants.class b/bin/main/frc/robot/Constants$JoystickConstants.class index da3bdb4cb6f105eb8000df8dc14edafe72a09db9..b3e044601667d7ececb1b24bec7ae3fe43af8c7e 100644 GIT binary patch delta 13 UcmbQtJehez93$hW$?=RP03V$MRsaA1 delta 13 UcmbQtJehez93$h)$?=RP03UD!O#lD@ diff --git a/bin/main/frc/robot/Constants$VisionConstants.class b/bin/main/frc/robot/Constants$VisionConstants.class index 0ed5f3da93cedfbbd6f3dc6552f9041df1a174d4..1ff8d5403a481afde9ff3108053e5bc35a6818fa 100644 GIT binary patch delta 31 ncmeys^MPl>A6DKU3~~%V8Ppm6Fqkp?oy^H*#Q1Hp4Vwu7!qy7# delta 31 ncmeys^MPl>A6DM?3~~$~7}Oa)Gng@anas&%#Q1iy4Vwu7z(oqf From 1b6b58f46738351a672be9ff95784d8a8cedff6c Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Sat, 8 Feb 2025 18:10:15 +0800 Subject: [PATCH 06/11] resolved some conflicts and namings --- bin/main/frc/robot/RobotContainer.class | Bin 2083 -> 3956 bytes .../robot/commands/PathPlannerCommand.class | Bin 0 -> 922 bytes bin/main/frc/robot/subsystems/Elevator.class | Bin 4857 -> 4858 bytes bin/main/frc/robot/subsystems/Grabber.class | Bin 5453 -> 5454 bytes bin/main/frc/robot/subsystems/Intake.class | Bin 4799 -> 4800 bytes bin/main/frc/robot/subsystems/LED.class | Bin 617 -> 618 bytes .../frc/robot/subsystems/Pathplanner.class | Bin 5292 -> 5512 bytes .../frc/robot/subsystems/Superstructure.class | Bin 3681 -> 3682 bytes src/main/java/frc/robot/RobotContainer.java | 25 ++++++------ .../robot/commands/PathPlannerCommand.java | 37 ++++++++++++++++++ .../java/frc/robot/subsystems/Elevator.java | 8 ++-- .../java/frc/robot/subsystems/Grabber.java | 8 ++-- .../java/frc/robot/subsystems/Intake.java | 8 ++-- src/main/java/frc/robot/subsystems/LED.java | 8 ++-- .../frc/robot/subsystems/Pathplanner.java | 13 +++++- .../frc/robot/subsystems/Superstructure.java | 8 ++-- .../java/frc/robot/subsystems/Swerve.java | 0 17 files changed, 82 insertions(+), 33 deletions(-) create mode 100644 bin/main/frc/robot/commands/PathPlannerCommand.class create mode 100644 src/main/java/frc/robot/commands/PathPlannerCommand.java delete mode 100644 src/main/java/frc/robot/subsystems/Swerve.java diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index a02fa6a4adbbb7f3c24c915be0de8c59ba99eb0d..cbedd505f0f4d7a86a232c06de22e40a07b5ff20 100644 GIT binary patch literal 3956 zcmbtXX?N6Q6n@@P3{8Vz0T)z2MQn#9?4koGUC`f{o~A!0Jh+)f);@bhn<}6 z*jd}tcguT^UGfa8WI7631?G+!6NX+eN+bHN?3kJJ1hisC@;G*(U^)U@Qf9ua9~`&z zVastn`Bbp7V{7%CT`U@q8zHQn|ha;e3PSuiII&!&NkBTQ~N>r$IpboIWFf$VL?%!p%Tv(lYK z@!U5A-=)39jKv5YFa<7+=k51DlXYe>Q!w)a3*-4xeZ6FwTg5ckS> z{&WLdV7GhFbSBJRfy5(o_R2_&YI7_eQlkH=D!F{I?|VrJa%M#q^6 zOpT`M2FNoI@K8wecnbD-+P6XAvWUVkIb3~DgoPsWLy-no1-hmYZD1uthcq-aB1}4g ztKtA6)`pA}Z#T-G-7{)40Q7TXlazFehU4W8cQk7oPF_!&rM!_Xn4uhD+S1lY$q3GK z|9=A694}Qbtg6jhsqUQ3R>|_V3e4#28Wd>lvGb;i8R*h*C6*~zE|B~mb*f+`&%9o2 ztYFtON}jv4+B1TmDiYA8%GI=?v%hPw4FYR5%)~hg*0Z`|ZKxHvignPX$kiGY%u;ZT zz`VNFT8XRJgrronS>TG!_&C)-lYtdmHMBuhaXq>fY-6*j<3_qdkgL72UNrQ8h1EV{ zdi}n)IhN+q&S)ye3nrxp+chk}`6_P2O$z!2I;TpZ?SJWWlE8L;s0$iwK z7j987(9FL`%1cGNr|~Gl>MPiz;UX+lF^IhiZe#b0^{CobCHfF+zL^_UJ|B9YhIVKw z?nFkxT>_WHLWezABJZbv&5*0DTAo5y!)%R3A)2Y|#bv$HkBZshjW z9Mmum^A#MXn{^Gby;#K|+$&4$K7r$%k?t9BFj6eZ={OpbHNITyZEf0pA*kj1HC&8K z6g)`xoBPo)7*srfE*a{FHC&2C3LfE{jggIHeb`@B9K&NW5|2}&NhBgtVY_)!!(uFv z?dG_Kr}2!yd9La8n!`r9z#Yq9jRclAXI8gW&;g4Yzh$|~m}>*Mtau5(Az<)qWM@z!0o3VGAf@H*b$Mv|33 z<@?$g+{7e{Z{7be2xY|I(r_6rpM`h05lGY}A*_n`@PUH&1=jsLY&mm#t20-_hxkZ9 z887plXK=oCrs~UT+H)9yCxXe9=6aR$g(|dmieTDQv%Ckb^OgLAFwupnRE?}r}lvXP}6qV$(h?N z8OC{y@mVEpU?TL(J+h}@xUT6c_@QCW*R~6RbF0$9e!)cIio^e2=AcC`6?|%un*ymS z$2)0<93?(w=l3aFwNKe$eaaT-Q_g3fa%}pP)6J)BEk0$N@G0xqr%YmMzk<&#Sc+CY zbG73ADnJa<7QrvSC#KMuIE%#4j47vTEXokcT`}IJSI8PQ+Tw3CqmvMlXxl=;p4M>oZxkI63>yOc|9?S=M$57fqGu9;LVP=`R>ir zc=x35@yER1#T5fsPTyBBrYjkzRb2R1V-5c{VJ)uX{%{h2OF^uS0jsSa2VIJ zi6-$P|Lc4QTk$zLzh`Uw3ElXGW9(P-;WtWtLUb*NBR<7vzFs!CAjZ_oK+CIcP!RO~ zh@c1s1(kde3T_~kg6-0wNqqS;PM+|wrB4AwDgx0SLbTyKTIruJ&szcjt*5Z%ud)Na XYzx1(pyJ;rF-0mn^a`BD8C>)y0b`RO literal 2083 zcmbtVS#Q%o5S|UCY0^MTp_IEWXImiVzS<(uHe3paNI~!v*7i2t*7iob>j1xlAAF@d=qO#7fn zF-j9HRyG6%gV%CvK*OkrN*z$O;Rz}Ox546odLhf~kfzk&t(c~Deg97+i}_aFw$J)~ z;Xb+>UgR;?_X+HooqI!IXhj692p0&9b;qkFrH~^qN66>bv1Cew9TqrPN}MOExrm)M zPHBiude14B);5^KqqdOR!4at1`K@;QxSC#zTr$Glf-J=PjK3 zY_0J)KsRaxuH&ff+#lE;IBG4XAK<9OA`*$pdJ!jlc5b(Qi?i0+_HK6V6q@F5+j2uB z(q|93N%>%ZE>%;hBA6N2-V~K*Tq#Uu4I53t4B%AY-_d?3z+MIFCrL#UsD zv-muRUv6MD1ZDXfoPRel`vvBISlnfN4%?FnsG?p;QQ*QAdwz8Q!?hfSS`Gt2&o^^p m$YXe%!%%`-a64m|w>90)B9EbN845P`JN9`O7EzjN-1`lA_*i}b diff --git a/bin/main/frc/robot/commands/PathPlannerCommand.class b/bin/main/frc/robot/commands/PathPlannerCommand.class new file mode 100644 index 0000000000000000000000000000000000000000..f2efd3d2b4d7d19385726d341a4cb77edf45137f GIT binary patch literal 922 zcma)4%Wl&^6g?B0nkI&{0a_?fXe)}Fpjtv=RbW%8s|ZyDMWU?6_Eee5kJKLX`X)9k z00{{`fR94lYbT_rh_LXr@9{m4x%Q8rU%vx*f-MIPfqTb!=;v9G89&UDM5a;UA4)Si zjAfdte80vV*aF)sD*ZQO?H}vBF#Htj;N;lj z;jvD&c}jV_`Qtl70ee4-R1+3jE*!W5mbWupgurDN7qH-9Nx+*EcrZ=VBwokG6*!5oZ|5s^`v$gv=RnN*KQ27fP#EQiGSt#S7%r*BD&^9AY^7qdi z_#dBumek}OX&LMHBSt^ZaA>I*tw{&V$o|j{z2oy)mDh`NPj2NITF*L}^ zJX8mov#p;0+`A|8wZsN(#iCzD&_ye)D*56fA&b-v32o4QrTYcVZtoK=b?t9xzqERT z&shFQZyLBpwF8@Ko9YtrmI>=%g?Tj_tGG@b_v}KUaRcteqenRVi8|zdB?%?i3;z8AujP;=s<0aIox6YV1 Ley853rZ#^804ljq literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/subsystems/Elevator.class b/bin/main/frc/robot/subsystems/Elevator.class index 132ba3898345f8b6edd0bb3b9d7b7800042191eb..990aeb60c42bc0ecb172aa3d4146d71c95664561 100644 GIT binary patch delta 30 mcmeyV`b%}f1AeaDc+b4zlEl2^R7M7QpNRspo4Fa^2?7ATmkO`| delta 29 lcmeyR`crkn13u1N&%EN2#JuEGMh1DGi2|~lc^Kab0syda3Qzz5 diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class index 2b7dcf7d9ffbd3ce62c3491431f6e304d79a3bdd..5a242082c4cd176022329a52763c9d0ac3e7e1cd 100644 GIT binary patch delta 16 XcmX@BbxvzTCL=RfZv5tK#z(>cHZBGC delta 15 WcmX@7byjObCLt<8 diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class index 235cc2338dffedb2e5d4b1c821b829d7fee16bf1..a136520d66ee5ad42084ed2d8b87a7ead640077b 100644 GIT binary patch delta 2193 zcmZ`*2Xs?a82)bZlGnVxCXbM|bU+KVq-hIfBTz=6r6>i;DpV9mOMpO|h9*T^1Vt1P zN4XA$G6Vs&$|xPQB5o9%h+7dSiVG0+9QAGcJIZeMwAMWU*UE8ISxr+RR0SzT&WZh3{* z=M4^JFlM+)7^IO^vpp6tv{PY%na_!G@fplH?9zh@ZO5zV04swzH*!2?QqWz#V659e z*v22K75%WuYW%bkkOx^{e1T#cMj!XZcf4Ca*sdgt@5e3N(5u`S9^UyJ&?)O?sAWe zLHut+3d`k*#>D7>7%GAu#t_SAsgBMi3i2>g#s~)OBIfwA`JVEiihLCCpfTNCsA4p( z<{OPA<}oUYFqZ#gER8O-Q-X>GKY`DbIijVLy-((vneV`F>#+E(qGciubRD`h9CgTeY zOEs(%vTsz}jg>Ytz9xYNvc>R&iY?emdM9*HUG|HLJ*X$y&l<&2p(RVdGoGg~5^ouWLzV6fiSEW|bfhGt(7P+W z({)8BoXA2Z-DFu9Nbf@Q#1!;GIo)Gk^g$*1;zoj7PV@@&rw$oFbJK%{X*f2}rL&z* zhnLuyMdB%Ry<@$|j2 z?nSyCHM|+NqZvdOlSDL4A(;@>vQU;&vl?+*8$c-1IlV9O)DrPjbkTC$siRtO7sY7M z6fp-8)M`Q%?j$wio-vt?s9lS8v^=~AYq!B9)fwrbZAtGgJFTFrU7}P*V-Q80D@=x{ z#px2DmfMI;q_CO37bTAqCsJd9;a|kdSj{Lb8oG!BqsRf#>{bdZ+x|o&wo`0Rx5Y+N zBzE&?dPeBs#f7!o!n0ym2-Fw^N>dvFopu@M-Y{sR01b~x7_@*uNBs}zKC}?lg-FOf z?4#zTUv)f3CO^sZ&4USNw)k1ZOAUby$Vd!xL}@XYnTz{sVmeug?Gg delta 2115 zcmZ{ldwg4E8OOhGa+=eeo+fG5%et*=*{w~}S>_bBb%Tv|C}XUxEe^p8iuZXJFDM8i3iE$I;B!v8&5s{#KYiclc7D(G zeY!V(IA*>0&eU;W4gVaq8dZL@b=BzokH-S5)7i9hp2FV}>s1(?xq+00pRi2@wnY`Q zOrnY=-Q$~UbrsUPQ#RGq=-X6GxBDWYI-4>A7WK?EnWH!Q+SNRLqi?xA-^Qfeq)E}H zt5jQ^MI+%LiiI{oEQ`f7n=H}q`r`f;VtT26-AJ6JCT)sYLy6sq_Dmu>*uMU{WGe5Z zb6K1BU@OA;LOSbgaT3X$TMLO~%H(XtoHO>94QHM7uGHzz%6YG%WFEWv@vOTD0>%H_`WtMN9WkV%aIDTO4sR*b(csIG2?sD-@A4E;jcMrIL=#DmwIh zV{u@O&3T-!FE)BZ>ufHdQxQlcC2YOP=+ZBh2cuDZtcglpH@Nx>6`lHsQKz3&MtNSc z%%%z*dV8QYlJ3l9i^IEwHMv-^N}nvN)0wix#-)m8VVdgtO1 z2G&+fYvsq{BDT7^%k`UOR|U4&TtT1S7+7Uosc4Zl)aZu;HNg*YwaJGSiv+S-p9n<# z+esXKJ+N8@lWu?migo`hz#2E13X5ylVKS(Qd*AOXulJ`J(uc|~Sdw&=yKJ%yDP|W_ zPIqo_UDA=U=}9;{Iun^pzcfy7SFtj~*^w@e<{Hjape}e>sM71~#zcN=qCb<;gTZUd ztN57BjeK0c7CaHSmAxjn>El+5|90-sZ&eIcWR%r7V5hyD*T`3Gy1`blhqGP z!JR~5FeNaJJZkd=zNjCv`vM0zWO7iyW1s8)5@UKr<(1K|%45n)$`%JWV)GPF=*KFH zDzEE9O@S$%F`3rqhnoFg=eSOXUJiXN!hZJ^J#X_(zNNQ?JB{ZeLX3syob!UKc+uwj zj7ii|)Sg^1wZ150o^WynmkF2gy$EA77(cQ3DJS$>;Z4SmBSNf=v~Bu@B`@#SHrI1Q z*yL9cZiukQmHpo4lk5ox_?-wMh%d#TZ9dPK*!{@^^<=Nx9Oem;y%v#NrXurvCPfO|c%Pxu|cNOZuwm4uvMzRH8rN(GzTmPtgm~n-5NMm3+|; zlca>$=@OYE5_lsESwdU_&q?V0%$9RD2D2owYDu`3kp||7**r09pqcq%e3mSn1+0`0 zFBHk;w2&l5npSp+_Ild5Md-aO<#t(i`(;DkC;MlN4i3u5=EX%*sZxA|f^dyuSHzKU zn#J`nyQRX{io6>CyNoQsXQE6&kt1$YirICiiZ1S?^6uXeIecCcZdahw^W*l$HSqjI zeD2r?C*6|BeO?SfDRXUnoR9uDKx`&Z=|9r%Qlu_5!c87(kem4gzKG=x-VAC%P+gC6 zC%NU))^YB9fGW8@^%VL5WyWE@{EFJ}0*%Uzl+xgAsrE8?9LrA!ZBWMpd{*=$56a=@ zQ4)PlIJY)IeE*`(WS^3>WXfj(*dPJ;L>-c}7DOK!=Ml;6u`{_@o_2+lV5R4)wq$)e zy%3M{Wn4^oB_8GovV8v&^eZLMCtc9e*pxt53+S5v1A3IF1?*HN5*|l6CK5m6g5qOB z<`G%?CeMoO8+@DZc+_|KK{r42$RF`De(q7f diff --git a/bin/main/frc/robot/subsystems/Superstructure.class b/bin/main/frc/robot/subsystems/Superstructure.class index 4e7604ffdcb010f3734773e4168d60b362e5a0a7..7125660b5a515f8b21e467032e0ea7ae08ae467d 100644 GIT binary patch delta 16 XcmaDT^GIex8Y44TZv5s9#-BU@Huwe& delta 15 WcmaDP^H63(8Y2^D?&eI!pF99DC autoChooser; @@ -45,20 +45,23 @@ public RobotContainer() { joystick = new Joystick(RobotConstants.OPERATOR_BUTTONBOX_PORT); m_buttonbox = new ButtonBox(joystick); - m_swerve = Swerve.getInstance(); + m_swerveD = SwerveDrive.getInstance(); + m_swervePE = SwervePoseEstimator.getInstance(); + m_pathplanner = PathPlanner.getInstance(); m_elevator = Elevator.getInstance(); m_grabber = Grabber.getInstance(); m_intake = Intake.getInstance(); m_led = LED.getInstance(); m_superstructure = Superstructure.getInstance(); - teleopSwerve = new TeleopSwerve(m_swerve, m_Controller); + teleopSwerve = new TeleopSwerve(m_swerveD, m_swervePE, m_Controller); teleopSuperstructure = new TeleopSuperstructure(); + pathPlannerCommand = new PathPlannerCommand(m_pathplanner); - m_swerve.setDefaultCommand(teleopSwerve); + m_swerveD.setDefaultCommand(teleopSwerve); + m_swervePE.setDefaultCommand(teleopSwerve); m_superstructure.setDefaultCommand(teleopSuperstructure); - swerveD.setDefaultCommand(teleopSwerve); - swervePE.setDefaultCommand(teleopSwerve); + m_pathplanner.setDefaultCommand(pathPlannerCommand); configureButtonBindings(); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/commands/PathPlannerCommand.java b/src/main/java/frc/robot/commands/PathPlannerCommand.java new file mode 100644 index 0000000..efb84a3 --- /dev/null +++ b/src/main/java/frc/robot/commands/PathPlannerCommand.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.PathPlanner; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class PathPlannerCommand extends Command { + + PathPlanner pathPlanner; + + public PathPlannerCommand(PathPlanner pathPlanner) { + this.pathPlanner = pathPlanner; + addRequirements(pathPlanner); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 476f1dc..941f648 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -31,13 +31,13 @@ public class Elevator extends SubsystemBase { // cancoder private final CANcoder cancoder; - private static Elevator mInstance = null; + private static Elevator m_Instance = null; public static Elevator getInstance() { - if (mInstance == null) { - mInstance = new Elevator(); + if (m_Instance == null) { + m_Instance = new Elevator(); } - return mInstance; + return m_Instance; } // shuffleboard diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index a9732cf..38ad4e6 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -54,13 +54,13 @@ public class Grabber extends SubsystemBase { .withWidget(BuiltInWidgets.kTextView) .getEntry(); - private static Grabber mInstance = null; + private static Grabber m_Instance = null; public static Grabber getInstance() { - if (mInstance == null) { - mInstance = new Grabber(); + if (m_Instance == null) { + m_Instance = new Grabber(); } - return mInstance; + return m_Instance; } public Grabber() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 9dd1dbc..4ec64a2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -50,13 +50,13 @@ public class Intake extends SubsystemBase { .withWidget(BuiltInWidgets.kTextView) .getEntry(); - private static Intake mInstance = null; + private static Intake m_Instance = null; public static Intake getInstance() { - if (mInstance == null) { - mInstance = new Intake(); + if (m_Instance == null) { + m_Instance = new Intake(); } - return mInstance; + return m_Instance; } public Intake() { diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java index 6206e92..0dc5397 100644 --- a/src/main/java/frc/robot/subsystems/LED.java +++ b/src/main/java/frc/robot/subsystems/LED.java @@ -8,13 +8,13 @@ public class LED extends SubsystemBase { - private static LED mInstance = null; + private static LED m_Instance = null; public static LED getInstance() { - if (mInstance == null) { - mInstance = new LED(); + if (m_Instance == null) { + m_Instance = new LED(); } - return mInstance; + return m_Instance; } /** Creates a new LED. */ diff --git a/src/main/java/frc/robot/subsystems/Pathplanner.java b/src/main/java/frc/robot/subsystems/Pathplanner.java index cf40015..b0d76f0 100644 --- a/src/main/java/frc/robot/subsystems/Pathplanner.java +++ b/src/main/java/frc/robot/subsystems/Pathplanner.java @@ -14,14 +14,23 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Pathplanner extends SubsystemBase { +public class PathPlanner extends SubsystemBase { + + private static PathPlanner m_instance = null; + + public static PathPlanner getInstance() { + if (m_instance == null) { + m_instance = new PathPlanner(); + } + return m_instance; + } private final SwerveDrive m_swerve = SwerveDrive.getInstance(); private final SwervePoseEstimator m_poseEstimator = SwervePoseEstimator.getInstance(); private Field2d m_field = new Field2d(); - public Pathplanner() { + public PathPlanner() { RobotConfig config = null; try { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index a392d90..10fb24c 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -18,13 +18,13 @@ public class Superstructure extends SubsystemBase { StateMachine mStateMachine; SuperstructureState mCommandedState; - private static Superstructure mInstance = null; + private static Superstructure m_Instance = null; public static Superstructure getInstance() { - if (mInstance == null) { - mInstance = new Superstructure(); + if (m_Instance == null) { + m_Instance = new Superstructure(); } - return mInstance; + return m_Instance; } /** Creates a new StateMachine. */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java deleted file mode 100644 index e69de29..0000000 From 1b904b676ad9f58f604eb13275374759e9754588 Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Sat, 8 Feb 2025 18:58:32 +0800 Subject: [PATCH 07/11] progress rescue --- .vscode/settings.json | 3 +- .../robot/Constants$JoystickConstants.class | Bin 403 -> 403 bytes .../frc/robot/Constants$RobotConstants.class | Bin 619 -> 619 bytes .../Constants$SuperstructureConstants.class | Bin 4174 -> 4174 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2840 -> 2974 bytes .../frc/robot/Constants$VisionConstants.class | Bin 1648 -> 1648 bytes bin/main/frc/robot/Constants.class | Bin 680 -> 680 bytes .../frc/robot/IO/GyroIO$GyroIOInputs.class | Bin 0 -> 1103 bytes bin/main/frc/robot/IO/GyroIO.class | Bin 0 -> 459 bytes bin/main/frc/robot/IO/GyroIOPigeon2.class | Bin 0 -> 2393 bytes bin/main/frc/robot/RobotContainer.class | Bin 3956 -> 4021 bytes .../frc/robot/commands/TeleopSwerve.class | Bin 2902 -> 2766 bytes .../frc/robot/subsystems/Pathplanner.class | Bin 5512 -> 5577 bytes .../frc/robot/subsystems/SwerveDrive.class | Bin 4434 -> 4278 bytes .../frc/robot/subsystems/SwerveModule.class | Bin 6036 -> 6036 bytes .../subsystems/SwervePoseEstimator.class | Bin 6332 -> 6594 bytes .../frc/robot/subsystems/SwerveVision.class | Bin 5111 -> 5309 bytes src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/IO/GyroIO.java | 28 +++ src/main/java/frc/robot/IO/GyroIOPigeon2.java | 33 ++++ src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/commands/TeleopSwerve.java | 1 - .../frc/robot/subsystems/Pathplanner.java | 3 +- .../frc/robot/subsystems/SwerveDrive.java | 6 +- .../robot/subsystems/SwervePoseEstimator.java | 161 +++++++++++------- .../frc/robot/subsystems/SwerveVision.java | 12 ++ 26 files changed, 180 insertions(+), 75 deletions(-) create mode 100644 bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class create mode 100644 bin/main/frc/robot/IO/GyroIO.class create mode 100644 bin/main/frc/robot/IO/GyroIOPigeon2.class create mode 100644 src/main/java/frc/robot/IO/GyroIO.java create mode 100644 src/main/java/frc/robot/IO/GyroIOPigeon2.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..1745ba0 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/bin/main/frc/robot/Constants$JoystickConstants.class b/bin/main/frc/robot/Constants$JoystickConstants.class index b3e044601667d7ececb1b24bec7ae3fe43af8c7e..f9f12acc43d3616a192d10e598372015be8a9fbf 100644 GIT binary patch delta 13 UcmbQtJehez93$h`$?=RP03WObSpWb4 delta 13 UcmbQtJehez93$hW$?=RP03V$MRsaA1 diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index fe20a95ce0b2d3bf184de78e78146200f7419935..3aa4c18b0ec1c538909f8c656382ea9115e98ef3 100644 GIT binary patch delta 13 UcmaFO@|tCX855)AWOF7H03rti;Q#;t delta 13 UcmaFO@|tCX855(#WOF7H03red-~a#s diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index 6eb6ea2f6e301b25981dafcbe0a64bea7d7122df..c89f59254fd6cf0c4833a42dfaac53dfeba602d3 100644 GIT binary patch delta 200 zcmWN`EocG(0EXfBD+bGnEM71fmc$?k*H#4Ez_MZyW-@Fb&awFuWLnQ`7*52A=w=u< z!@{Cmn@+~2ASx`H4PJT5<0sDv1x#)TQzXJIaqc*yM3OQoDx|5BrN$Kx z+*9{#(BzR8Z8|(L^mOR*NslSd%;>XXz`r+pMjUvNk5?ItW%8ylj-tHFiZM}wFP$^j m1>efBP>vrJ_*I!dHCU?2S}iu}vDKKJKG@q!BdSX~7_g2E4LVf- delta 200 zcmWN^Jt%|$7y#hsT@02_GTw004JO^7)NK}JQx=Qm29rd%gj4d9^=>{P|KU^QXEGRd zgSdopVX`Q*foFT_*?KnZOv+Cl%WKaTx6i56)jP9x(yaxNn8G78g`w-x>X)g3v= diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index a3720490934f94f23934e54863a271d8ce99f2e5..d9cd5e05f66bc484195d45a25982bb5a2acba7b2 100644 GIT binary patch delta 1189 zcmYk5UrZcT6owD8f+TbsxTW{R$GVzJ6TbMd{3MClA>rwkWL!B%x;@1sspmT*Jdv( z6m~3=nn-r1v*}Dn!b|kp+@}-0*<>}1g9LATt9)IVgifogMcak}W1 zpZu z9(O-88q9H?u*kZ98ynPwTj^hGJgHdkzUN=(j`~X?lgm4wvf0d?K_+>|=2>#?DgTbZ z5sq3sr`RHNmAlIf+-UPW(h75=HRFsV6?N{r{%XnC#gn{X^CB;~`_1#~PAWvK3f!|mr&R1J(4D8-m}8HKe~umz z-yHiy!rVW{16R3to_MH4gYl59$++wjG9G6C6*8wwI(eMX%^Gz=D>dp!Z9$zd2d*!g z%##e2X)+lq*JRSvWHQXKCR1|EkpiQ6Mn7RJj8K^cCM7ym;BcP9zKcBFFPdliVvghe zv)aZJ@;=V3^4>^LZc9k)Ef;HR#oQJ#celj$vW-ERr6C#R6ispmo0+1Oqr`Zbow&4d zj&|Oc;rNIy=IG{^; delta 1118 zcmYk5TTEP46o$V&mot|h=~Sm6L<6a*>2Q(S3)Dg>Ob3t_S}W~PMmuznX_-O+hbc*#qLnM6zj1Hc z)j*R?GY1q_b7m})Yf*T2#A1=SwfgD+!=zv)Mt!$00?vZgo{34HBi@Dcs|v zJ8i0|2}rEV<}jNTb}XBjO81QC#2!8O;+V}7dcsp-MLA(}QqOtH z_VwA+Qe&;I&baRXYFyv+KInN?QKLiFHeK%yln0qu zYmu{glwHnp%H}zq*LS^bflIvT;|0ZD;cNA>71(0)5-*E?BhhSXB&}%Fo2@#@*!~G- zY+m8A{?59!?KMTo`P5XZel#_9uD&Og8xG>8UE#?MXC@WfyKjUZ(goj6&viwu)4fFD z?;08v8;8<9Zs;n1v${{8_J5f#caB5Oy4>;IsQ6`6Ofo9Y7!?nU%7=)$@UxpeV$WVs zpA(Q6s>=5T4gKzAd4$;~|3&uq_IuXXEoSXqTepO1$(w++9xw$(T9=9TFA^)#QJ}-K zz~ds`1&)Zg7dR>s=J*1=>s-7*JhaJx@sMrExYP+54|8fGnJAD5Z8c;flrJ@CLV63D z2=nwtG+7`Ssx)XaRAtDdWyoZhGlm?LZ4MPl7f64`xiCVFEHfd|Y>~+VlkVGG>=P|8 zeVcsWyz%Z;d0wUp@?xqbvr#IhJ}ibdh@~-!b<)Uj9^(}I8K8-?azVy9$TV#-22tMN z5OYG_6!Js5_*5=Sks~b8%~u@bTdDq$C;6FP{@?_EN~eEFC^x6oJq)O=oKcme7`tNj zBS+`lz_rtqX;<f|WQW_8P$JvJ+(ZW=)$nc}w(O8IDU_A6DMq3~~&A7}Od5Gng?lOy*=WV*EMThRp;3xV;Jh delta 31 ncmeys^MPl>A6DKU3~~%V8Ppm6Fqkp?oy^H*#Q1Hp4Vwu7!qy7# diff --git a/bin/main/frc/robot/Constants.class b/bin/main/frc/robot/Constants.class index 3b6906842c39c8d39542e7f4457608e9f2a75495..19df9baf990ea1fbe6e9b949a16db87b101145d1 100644 GIT binary patch delta 13 UcmZ3%x`K6sCljOiWG|*T035~xs{jB1 delta 13 UcmZ3%x`K6sCljOCWG|*T035*sssI20 diff --git a/bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class b/bin/main/frc/robot/IO/GyroIO$GyroIOInputs.class new file mode 100644 index 0000000000000000000000000000000000000000..9f9c6e99864eb1d231c2d1291226e0cc52399d70 GIT binary patch literal 1103 zcma)*O;6iE5Qg6+0VgKqBMnd}rGZlNQOZ)vjg|}jiewQ)AQHVf&Jq?mUTeK6&Cluq zAr+~;_eWJ7JCW#t77pI^tY@B?d3XHl_xB$F-eTLr2t)Bydt9rQGQ3vjANyL>>Z^mT z7KA+$Sx7TXox5LL?z=&U*IVbpGZw}eww3O1UmD{Jqk;q?y@;Rp0?$YlaG3zOqdFb8 z<%y6pp@8^R;zJBG10GLUAMAyuw48Pa>IEo`JPox=l6WWn$-2MZYs zGYo6@%V?v3={WRJ4q4do+d>ZG$T3VhG7$T{ZcFH+xK)O{qdeDdx?0Be@Gx!8WW=!I z+$B0SH)<3K$FN>@?vR<>68=u5X<>z-baPq9LDUOFrAh00D0Cv-`>&n|V+}J{Wmr58 zWH~y!Z$KPpaXq_bMD6xTs>J_G{sY~Rm*C?D^VfGSp)rm{Y ze1kyOd#s&g^R}pX8ri;rUOp y2E?%>DxVN$2VO%0`J;>qeP^itgZ1JT*4%$tDZET_Z-B=YCG<^fQ8%xX^UXh)o&b~p literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/IO/GyroIO.class b/bin/main/frc/robot/IO/GyroIO.class new file mode 100644 index 0000000000000000000000000000000000000000..269385745e5e936749ea63f4c7cb64671e485f6c GIT binary patch literal 459 zcmah_u};G<5Pg@11kyrFOIcVD0tpr}F;z^JK#@W-fVw-Wfm9)>YzL{I#mc}3@KJ~} z=>SqBhP(57zI*5A&#(6n0M{7%a0sJEm4&KGD-%XZc)M0rl=yH7gQa*CVIj(=FiDp( zGlX8fS_mVfa#b5mI6IG9hEE%frfG69C%DsUAqo9BFXgO$Nu_!aX~E1et};=~g~~17 zkgj>oS$y24Jt27cX8TE7!{6nE;J&I=CU0`PuG_rq(i&qkQCUhgErixm`xp|A|8v7b zzs<4YOlor{ttIUd2p#@ea3yTP~KD8E)k&1$PV|g%ejL+ UMh^NIa1EH*=NR$K0b|$VZ*~T4A^-pY literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/IO/GyroIOPigeon2.class b/bin/main/frc/robot/IO/GyroIOPigeon2.class new file mode 100644 index 0000000000000000000000000000000000000000..528c80d7f96cf0c4792ea1ad329c3b1bb7ac301a GIT binary patch literal 2393 zcmbtV3s)OO7`?-5n{;e3JW5+ZK#B{_NR0&vj|QwdB$KeX*-dA6OM}(_ zq^E=)d-MnRqdb0-q!R2R^_X+AJ2U$|?tI^!JAeH7>+b;W;wJ+w3|9&&XDWBc_06Sa zbFre_rR5c?C|&2Kfi{NDJ@HhSws4B(^3I;j`35=|xM3+uRi@zb|N|5k1M!S`qsUgCWKlU-*HSv5Jnc$uLqI`GJxQ_s=k&PRo2??w2jI zU@6Zx1IO|`vm}K_?&hphv}JOJVxFhu8`5@jmS165{=ZQJTd3FJYVkX!Ey_GYyNo9` z7~1CDyo{n1T^twDX`qK;`0Og3!zBh|aqZEasarRq=*DI3eubeozLY+0O~zN2Q%oi{ zB4Bur!@xOhJizfj2B|Sc>1TDVhvSJeHk#BChB+dL>Ifqo*K~wFia@n%-sRS&tL(fs z|B&OIx_N?Q4C4$J$$Z{*3RW>t!gm$JRA}a0&BlApiDs9ifeD7mH%oM?kL5a1Tt`I}Dz zds#ZK^owE|GY0N547|Co<`cv5na&ZT9P8!0@a2M%PXg)WD!Qe!9CMf_2DxS5Dp?0| zF7VcU%eD@LPbH)S8QQ9Pg4#x?ziG>n4};QqIiHWIUZt}A*y6SNpE-7 zwn<0g+adk^r~6qwjo>R}46M>7PwR;;8OIvd8CsSJ^gu}3rs75^;wHxQnYFS8uPgIQK8+=PbO8j+5#ATj=ZD~RYjveGk=jp_52s@Co zUdjz>EhXVfMIli%P+%B6DXS&n@0xT?l%%gJ=Bi7AP2{RM24DJ6?aN`gPG zN&a^Jq(9sEp2J3|uF)&5XVrNbf7=jIxOifq%+Mc_G$5{<>-tvUP#|=j`8v)ucU9yq z;m}eW0ZwTo0moC2aCSG{7G$*|RYp?PSc;ltHHbTWHgAc888=WlxnR`}&58SVQtLSb zU!oSNd0Tj%t{rbCK88qcTj}3U&zP2Z(uJ57Yg#)`;}&q5lN$d$zd&nrV9`m>cgf-f z`7(MJ#*Pphdxh?7XFS$>guX*u9UFUrt5x(LVyH3v;1Hwr#rP{+&mLj23Ujlua-oWk z^?0ZopH`97!_ng*1-iHSa}869o@0aw=t3J|=^``(geF09ltd2}(Ti1F!ejIi-!9`m zS@JqcEYMfb_;V~ZcJQ2b&_Zj|8;95$*B`3*?j`bv z*weGo+RQnGe~c?eh=6{&C&m;zG`Fra*8u=+3o}oerT5v%2E%*V?>8#(FU#DoIO0kI+$!0;N zX$X`oZ*vEe>j%?)d%M$aO4G=_f`-b?|D#fGuE;_&6Ic{$(=4J;(ZLqUeS&hURABxI zM8y3jFStDRfToCIMJL-N4+%=mK(I9Ai$5$_Vxqx9(-jOYsCN)_X~Jm5BlJkR&DmhZ zyq&(HPx?KmV@XXO`HEfiNp=fL{9NJY{&e>qd)3Herc~aP|F|YXRMF2964$iLxEL@4 zvLZI9i7{WXkNuJ%L4^%pXtFXUo-{wp8vT@J0XImV7R1fvP;q2`GBuP`9OPNeQijdS zka3>Vl(1NG*jx%#M-Nfu7yg2#lrqVSCgdda5sv!&V}hlXV}CBR%?O(vj*PyniBqn) zn-iK>88bu9x`Iud@GG*e!*_x=G^fmEr!4puZ%a;_znrSM@9?gPtB&&bG)uTK#94uE z*q3(88+W_*%~FaFIVbtZjH$JAKjss2N&Vt{!-Qm9uzIY$z_h4%K=Uo%nLonIgl8gp zRQzC0>KSJ{L7NqRGhKOg!&#a-r)ZvLqep9Ens$$^79RB2ZefSVqZWES_E^~Kk+P8X zIFQJ4uro2qdTXuLR!#AY$A~>XWaXas7`59YK4a??FL@lF<&I={Wj4faUrP*6TCGV& zGZ>$1?RbXQ6B$n0L~nYWng6~$J2TA(6Sm|zd}@7{9YCmN9xI4a!(wVlu*$AF?&4-P zu$mUuu!URL#;x?QmOgG{gnEu~JEysmbCx-7AEqix13%eOy2v`N*wA0?Kl#kc>#IHD zbH2zWK4r%93w!!nr9QMo4@bwIn<2j*g5-rzBgl8oj hGEAA8NYRRGy!IS#PA+;n=fCH?MSpmz$?_vb{{gq#?}7jT delta 1053 zcmYk5`*Raj6vsceh27-lhPHuV3$>w8OdDHAUOr$%3$0KMLMfsZMH47iD@4=s60nG% zg0&hiAP)iG??au8ivJ^oV9H54m(G8f_ues12)mSkc_rRJcCdCdy{T!YI}mZlPMUo=%qyHsK_l zjf#d6G#oYow8>Dz9L;7PcX`Ynb{d>+MZFIt;=aljLjp`mDX#D>GM$`ghJ^E3}IWO$olI}&b-uVu(@SloKwDXih74eL7RG-q7i zw=KG<{4D2ex8Atm14BLYLtIpt*4+cSd8>PKx&EHK=0iSq`AD&(&>6S?M5qG zOn+}Sm&tE8e8Q*JiToA*mhWA@Q!FiU_2oY>V(U$E+K-Cc3Y`htXSS)!_BV4iyctK@ zfj{k;=;FZ%)_9~-6FlP4CE-zzCnP-Sk&&>?qgTT79ytjwdF)ADVPAJ@jCQfK*rIV> z_BbTlgWco2>TyJtM}4AeoYy^$PqBxJyfu{~%XehCN9@R8k#{9_i5)LeNELZc3Z3#e zAG;uX=O?%{D!rERnfR@8o#A$-(M%;R)N%(Y?qrcX_pq4zS;B)XWgW}d#9eIRZnE6V zewNFJTY;7T=OQclO2{$!T)(htKEKKhzD7HLNb2VzZ%V`A3%(2rpS0I!RPXT1m&Vp{ z6Q!moC6ZrDUQ(!K68-T;{Pv4{bCuDNKI>hl zX2oB?#>5b9F_LK9x^QJ&X(BuK?v3Y8t1**%&iuY}&zW=2ov+QGn~YDt-@OO+@nXS< z`T_=r7%(~Qlzp51XH3pA>@545O6N^3U~2@Eb}m`W*@cuwT_|qF4Gxer8DUr>kWLl) zQlo`I1xHQN3I+w9$fU;<&5+d@*Jv2Gr|nqI&ZlEX?ZTKzj!TlBoS2+BnMvn0e4)71 z)|t?-QX|EfQ}nm5&2!nLk*QjEK&Rlm@^=LCTr-*By7SZD6}-u9om(1h&PT1)h)-pv zQzj+uILGybmUmw2z1p+azw}wHlt4$5qc4Zw&{?6ULhl@_MRY9DH?+vH3jO~=r{<^+ zH!@fzI!_?HQhZo~8>$-;SH(xO70#7uSA1iYU;K|-if^j&l= delta 839 zcmYk4%TE(g6o$9~jTHRaxA7&iT&ooH_T-{ao?It9|_a_8oALXNy{` z%k80GQDx7$HyOPN%d{ei`0iniKGtg-BcLr zE{1eY+pVsu)LES%!-~Ri$m$M_SOGnKVn*=I#XDRb%nvi#O(1%IIeKj zv`cdj(+We*XjEtXBY%baF;iBE-Ztx~a>0D(_xD^=>O)2VRFuTmP*?#UWExOLU zhZ~A*_Ip*SwNFN-LpnFPWq)-Ks-)fM=~Pc&UG~hYR16PFZupn)HG9&urRiK?s{{=z zbO)B{NpmzC^+|Lh&A>n5sRfFC<(yff_9cbBHDY~|+@2MfGh%C_X+ld>JA6l$M^sLqy#5ygEWoF8`l;(;f z5ieSh=Gvm1IiKc~m>$;3ZcvPmO8ny0(MY{Kx?QxP0~=Tt;%$ug|;a~4go1>3+3*Cpg>1-3?-D5+SD|~3$2O= zDhT?4ilE>D^~S2SwM7tbP(1Ly@d6PKypM6zw_6UGF+02auJ8Z;?|m=dE)H%F8aw{( z+yac^&sL+z(O;*JM%LVTbtoR29ZQ5JR))r37>`Y?oD!{x#Oenb^d`fgFL^rsRJkLR zm9J5fSN1hCOR z9?l4zlQnwOh8KiGb>aG&(AW#ABMpgYtlp#uuZFK79<5JION6WEpB@iaM|4IB@gwn5 zo9YwMb0deBC3C7q&vD^sU1UynB9>hj3(v`}j@8eN))gCFHPcoTInVRUD?yWGXWQvA6TrIR!g{xG3 zkn9Nra4;!=lWCU!bd52p)s>+h(M%Oe>g}4YF+v?k%20JlxvrU-H26SLKeah2C1(b+ zEP`hVVqR%!Y5sI~ak}i9{0z<@Z24xZ{YkUjb4()4Rpssxu6Y`H((#^Zox7)}mO7pJ z8aV=)u6DTt&U2}MY_EHy=4r6FoF}_@m`i1Hwx^LMorFfvUa%xN)4722)xF81QyL5! zxya;V(SJ$a@4m|9YOYaxJiFY>xKU@h+G6B6Z$hbkM*hr|+@f={M!@b{QzBXysB^sn?yKX6A_p~9x!>3htyiL%Ds_Rok!H)<}l}@w5Z{!X9hYnTn*vGJcEsN znsm{o)~Bv;Kf`l6&#F_s{hZJ9f{J=~d!P2R+8XzlOkQTEn&BJc+Tj=B4qw*L*DS{y zCU4Rrd>bQ)DY3@L*hU$y+3jbMpQV;}T0;>o}*L*pk)?M&;{JRhG-C2TBcC##@? z<2I39&_(a6c5*t%-9&+Cg`y4EL`W;^U|?Ab$>oJ@3=6(MY8NA`0;jZ7ytUwQ3aZ)| zGozD<@E@)GDD5=SI+=>J{*0LlCsM&c zW;KhcmccYIgajvXnYfpTyo_Ni7wA=lSVIZx8Oy^=pogT$?rs`}%pxi3lqi1Hh4Qq-Sxoud#!x{{=tVc%k zm91iS*d@B{kiq|9;W@|xIcpaS6AQw_bzG0bZ&-6A7Asd=5G>zu9o#Uvpp6x)NR!v9 zM_IFhB-evZxy0?V(*)!VC(CmLDa~@~X3AH>s@$UJB7?iQTeN$QvaA=)s@sF(FEVsi zYEpv??Lh+Lgn&cbUYU$g;NCXw54Q2}kM7k5FPMgizCff=BCt zAfi$R6;M$GITV3XdeDk^BX}X6cyM`(insqL`gT(glHbn0^LyXCd6^vWFZHVj&vZNw zU^xD2QiqvrDzea9K`#bxqn6ZoI?=}AK{i94H@DodcoEjk|Opgs@R#3E>$w*kRe?} zWZqvc?nRxUkBTV>=sd1skiBJPWw}%3V$WG$1+LM3*D_Rk#c`!O<=RDsoG%ACW}r$e zk%vkl>X>k`q{eZH)Z%K?D4502i%NL}SKQ{=nA6-tDPp#{I>`t_Nu1f|D%LvhFP%|k1BYC2B4qiF|4CQs0azp(y3^&u<;JhChAr!7^S08k&7)H zTk*6w!UNWA*skDNh63W~v&ZoYW^(MnMiI47cegM|wZYgd72D9p(T-+u%)Ux~0WT?d zQ7m-im|w;#VuRzjW1oiA`pCY{aR3KJqqEeqU&BF$B(GSZCEMQ8orgHyCFFmcX|8uP zZ0t(;kmDnKEWFwT=>v^~MOxmRPgR=Wqa5{Ua4Ps*LxYB8y6p>&d$7zY<0Kh05G=#D z92>Ba;=VCJ4cm_#yUa7{3x5BYDxS&xDgk7TDrzLz=H0=`F~lSI-vY_&SjrXoJ56d91u0eabh( zGCr>r1N{48??hq1J**AG_xW}sC(w%GD?2a>I8VF$0&N0m9heAoq9V|VDFF|rIvn+F zm=;&2I~*jp;c9;et^>?_F|!@lQ%V?gF!M;5uL}#Bs3RHF>}(7q;%=fogMJi14@l@v zSHeYP%b+!j*4{>34szg!k8TP-3TYjUzNkb$RMA~gUxWUbivd_jaEnP^ia~S&gJ~%H z(4-b)9m=qoQWB{|X1r4rE0dN%af@&>(aE9Gx8PQokVNQQlLDqtHChHrc8a%koO>bZbt%b1F zV<>H_=p03fhUalSK>_RO(MiWGPm-o@-wM;;NKvqo(H>}Sy9kUT0w!`hXh8DZPqkvR zzZK8?zctnHmcWh?Mycd@G#1ty&89@`1f4yB(%^T2PP+hfcO0};2aQig9CRdsj=B)^ z9OfQ$5Z2k9=vDU6fYKF%=Lp4wy$I?5T4SK#RWiPYH}Iw*y^X_o&ye275qx4upWzse p8`24UiLVUlYkY_A4e1nq!fE_WXl?>ti{<#GI2FI*cl?Re{{RvXc9sAD diff --git a/bin/main/frc/robot/subsystems/SwerveDrive.class b/bin/main/frc/robot/subsystems/SwerveDrive.class index a75b59c8b7d047c53d9b735e35dbb4cccf7c7fd9..b86b0cf576e8ce30b9bae87f5d21cae5e1066939 100644 GIT binary patch delta 1754 zcmZ{kYfxK76vzLYdvkAcSsn#Kni7yeDS3e5DU=i_khZpzPzhoqRiuS7wGS$SNYTaz zg4*g7w_34k#aF*Mnp%RQt$uRGPwI?cJJZi~#u>jWp1p(?J2o@f-Lt#roZoru{jGS> zWBmNjx7PqP;qy7;?A-6?Sf)74hBOC99=Q0G6{rp!n?%sEMRTK2M}g?Gc$NE5BxWpa zHW!NJ#S-z0rS5zFs>RwsP2Eh-x)#Z^m{VEet+! z)0QJL)x1WIR`1OKSvCYUOA*qunp%=-Q4v>?vyn6B*#%46Swrg*wf;Mt)9|5&hZQ;M1lPxn-t&J zJJc5_Uh?P{{b^RifD12iyo_nlYILiEI$jkQj57ARxNel%W^q?X_h}LyMm0bXkkGXjheZgjt%CHRLVw9N@WM#k-yvoGJZn( zGFU8$Tv8|bOWIe+LLA7T1!$>}kt|YPGvzd|uq?7h9hQ7>$U;iGin6fN~ z1oD&cL}RLy)C4x0l2uC91j3X79QQ==kJQz!$Dv7$QXt)zqv6+ZMsN^{f zPRia!l+jp+O$bvTb)(S4lvKomMl*p7A#6)=en%E`Itw|}c{l=#Xrh0+H-Vi?*mD^< z%jk&7-MG1(#J)zW|7$e3tc!>?YJQith({!k%n`_&wB?~f6%$Pfv3TgwZALAXrIB8y zOj)zZ5K6I-XY}EK$rr+7c-*W-2>p11qViFd25^w<#cRp*Aqu>ntLnGWoH$O-8Qsf_ z;{+`&bWOtuVJNhH5>KT(pC(1w97R;<(Y;F3Wn>5303wh3dMgfm>Cm>zAY)31jQmBZA;*qeo4Nq*;w{# zNu0S(HF|Ri=VX$^$t*|wJZ0*_w89D;>cYycyIZP-jBKYCYoo~9sbXPj@pfvZo$DA| zZr7~^7fr+fMe{yBNaf23C2Iu^XgZjV888|@1Q}qsgvi>%4G^q+EbPuJTm5$IHeId0 ZB$jez!AEq}f(3j`vq6e(e1gy5`WH?U34Q~(6Cz?6~t)@ffl(|@3s*FJQn@0I@cB!W8iJnWv`{q67f`Q0{N@N1#= z^X=~!05ss63GJLcZK$tv_-Lg0=;**u|LE`tgQsmGJ=b>6<^<;~gO{<0$Dv~dQW?zk z1BVAj8yRe&(f)md!TuvX{d*5ZG)Qo$$YXGmlEQnGXvl@b3Wn7>R9I9LFcgr~#Ddvk zMG;C=6!YJhTPj5v*Gx^ee3T1o9)`8<9o%KA(93&;V1Y%Jklf_~4b5oP z(ROD$EJDz(qXV4`8p*YcjSPqTk5kOu<-F5sCoA?84c+Kb5n}KXnv35w+Y}v-pqKw7 zkL7k_r{D@x3fzG}puAT@4Ib0+ICk^hO4Qt^;|cU%|5G{6GR9B*(ekbRq)_;@ju9N? z2i1S%6AV65?BTy#ESiRVJg4J%oZ^=)m)L1uYi*U!3a7iQRmSP7I$p!;{Fb#_9#ruL z&(X@*1aH(zEEm89%3FL;E3(w!JslVEK7U6G73AZxjt_8#K@Se^KXxe6er#xOWJDOd zqT?e>aV_O6)A(dcJ^PCPlCmx;LxTb{)n%qA4w_9OL&i*jrrAonHl$MQ(`Z#fv-lKt z9qHHT$3z!F3dFVzS;mgIWJh-LN;Aob&mKeikFd>yg=djN*J&O7jt(S4MJmleI;qN_ zF>6Vc(~w~4q>VZtsq-p935$C?vsmewM=q&cH3OGNy@~v=G%to23TNRCg=MjlWAGX) zMXZz<))=d(SuD*ltP{%_tS4PH;TURXQ9lF!vV*1M5-5x^awrZfXmp|&E8(Nc+KfCZ zEf;pvxd?!sK*JUzt_)4sn#8_E*eXf*v*n9$dgF)&Xn5G|xdB%k!8vqIBWoVrVcJcc z%;M2Hg^vABC5~NnW{=Yx#~#6zh?>jLS4251CXy0(ssAcbidvMR5pIL0(cthWdB{5U zVxPh2k77R}Mt=Nw5(mh=N=n947$AkUr1~HR>9Rdd+U}`LVkYV%(-8BL`xTVlN($c^11;)F++SjIV3dqA5=_G}`ZJNZ8P5=g zM7ziFY|`>6x+m%-pAtUSCN)eW)0;=u*XQ}-I2jr@u{?EZ@o(>PedUl#z&aXzRNvKv z;DalQ4F*SkLRk_+p-@I~noOy1;02r^3=snjrhmoj^&%5>IF6V82j=DbVP1JK%tk~N zdiXSKBAPm?poRy)oFo4q1asbip|>Xq6C+ndKl{B4>J`y$7s8ou&SLTh%F)|%cvl1| zT&FD5$Wr5U6xmK?l`mM~t+;_p6uJ+`CAKPCo?86t{u&e2+(exjppZ0Eoo%DgHB*gk zzl-_q8vEE__L6s>;L~IP9gxx&>0vOdU@Fdo?V{;3P>vZsUuteI(TnD0%k7tVUs?@Q biS}a2T$wOUq9$C$7c^^hu?Sz{YdHS{8bmK( diff --git a/bin/main/frc/robot/subsystems/SwerveModule.class b/bin/main/frc/robot/subsystems/SwerveModule.class index 979055ee23860b0f3ffdcce2ac32f547f10159de..e9f58e565f50f80c27567a2141a29b08b9d0bab8 100644 GIT binary patch delta 20 ccmbQDKSh7TIzjf!DXl3^t6Mj36}-<109-8y>;M1& delta 20 ccmbQDKSh7TIzjd;xm}?fANg$FDtMn20AK0}SpWb4 diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class index 9c2dae665ea7b287cc808bcdf1de03a962281034..77bf369f2775508a429c50698078ba66a2af36aa 100644 GIT binary patch literal 6594 zcmbVQ349z?8UMdrlWe9}D5V^&X$g>Qnr;b&f+>+SX-gnS$%a6BEtB0zGVSipIy0LR z6%|26MZ6U}K=45E;>ghwIlMp-QNaVn`@R*tQNjP4*<&`@ER_86+j;NJ`@Z+R@B4q( zJoL=n_X0RqtW;1Y&^T&ELzX#WIw3nfVo%zRp0GpVL%KDg?=o$DyX_bW%`q(n6#^Ud zSUPklWrRiz%XZ`_Zj2mg4MojFLQBR%;Vg@7nym{|jZIo+cfY`~J^5q0`$Ie3{wLxCO-S9uh6h z5)B!)VI~DmE_I~0iDYUba?nWXRGbmz&Ccv8)En2wL-`FEb-QO?m?I;bnnXk@XjnT$ zLlxsOrke@fu_i+Uro*$$WNVCMwTVc=jHTn!fC6iJ4rmivDD4>W&}cdtmD^~jR4Q)h zmOLbEj3qTEZBZwKrS@*!Q%r;f*XYe2ZOD__+IhsXL?o`+PVNmg@`8A|oKk*N3T!DR zl%7ScU$q5rrc_EIGHU4Yn83z)JTkZr*$K^ZVwycZVro__v_tN8HC(+l8aI-L(=Jfn z*fb8q`MO)NNgl8P(!Lnq|m&X0dimce-WesS zfjjs<9l#klQ^Do}$^`(5=j_>X7y0~Y1WN@IPFWl2H*x%h3>D@lKeIU$OThv8z9zSSlNjsi(`T)*Dx3t^^ z0-?sEp==tGm+w)r49gYt37lHw^0`ym5MuUQou`890R7TVS&~n0zO9+3gA*t*u5%_!)gWl8G?$idG5HO;4-F;BBVJL z$(yyt0*GKk0Q(S?R$+`eQZ#@skb8a-m=~Gc%#k}PZ;iI1CSSlk7lFOlT#~tWk2Vh< zqbiQa2@1v+s{s~?l>v--ia)60M4TionNU%QDjBFu6)C(_Rsp&r)4ZGKf^^tY>h;}O z#v5COgEXTnc@*A`sc%bT(+kpz6z-6U%P}dil8g($Dkt=k=MXcaJ&Ayqt9S*jV63%t z+vVjdUqCC0h+@X|yd$n9$3p!h2lS}ZR#HVd%P}uG%GqSAo zB+Hk3-YrinEu#ORl&hA8=OdWFDk^nA7M%mS3}F*`IHl_`S}asttv(=kj%&7U*d9Tf zguokAyb*6=%FRK5NzKv8ZS&FD-z6i<^YCUB*WoQ>>6iuQNuwPNqCAd80bSxsZ^Jtj zyj`HVh#xFQrsAD=m%wqOmYHztl=+&yx~M4^V^gEII%?KD|6M&bZ-LA z-R1W#1*>bAF+Qx~BlswrfS6&s9Y?S3=oWD$y5#GRsrWeR1S;v5ab2dEPsmvRNruSA zu9AM>*B{ew3qGylGq|0#c+^ZL>EAjNhS)9N+^OO&c}VS~b|~wv5~0&7W-v<%#~eIF z@mfi-_o%R$<UJ9Zl8>_e$N|$B5L} z)ifj*KBwaI_yQMF@icWhP!yjzzJ8TGU){>M_azk%;6X~_AxYq}=Y7sXT$I*SCK}3= z_Z1Zn;VR;nVbzUu@?A9^CRkkO4B!!bL&4V|fY6cWI6$ z(AMaaHSAbMa%{c>yX~K?Cu6et2!%5PdW)>7zop{aGC3?^1nrTb)6IARd>7wS@HqXb z(BfGk2aSYosrWvg5Liyq9lLgR$Td1kBIR~QUD5~7Yo zHHmCDEI$gk<1Rh!Xasz%H|*~yF+ZQ*hvK^3&|{hh)2X&1fL(B;M>sf{(*4^D zxR!kB9a);zAN@0&JGFQ`Y&d$`vMM~Y95=HX6pD(ckzS^Vs^zlURJ$3btxkT_uytDz zYC#gqHF>Nb=CysK^ncG%#By4Dk$V>^Oc6{1YfEosl=HNUbu?i)JF;4o$zq$MI@zG} z8IYeR_$-tCwEJY!>pt24xlcB8?vw2nxg6_`S8+f4CU?v}z_N@;x5$R!9f|;^S8=fUWb$26-m_KZj~gvD5xYl_D2;;8p!GtwwtH9tW=LMbHkv!SFGS7KY#F-+(ie)E8~l4@@x*yp=Ed$p=oRi9!Bdl zw(xPzG}`&%yx}{sZ5HQG<3i4QxvhU1yMn>HF+gez1(kbn>2UcRe`c{eJY10*Rpv%j z+0iuikjcK`;1sz{kNUNxWo0v~;Kj{Y^cDnsAH>q`+=&eK+NfvN)To<4pp==WV~ zW$s!laW!7+O7c3qKEwHZ(#UJAB4=qZ+TfZQTq`Yd7;nwX>49*74il!64EW?-m6P|~ zS!WPW5PKEm`V=`z3vW7t_mqKyp(FT!!2P(TM}n%iy!}>$Hms50XSP8#JB(jm zQukYr>i6#CPu}D&aw382x$OjD3RcETu$y6H58=C)zP}HrVn1OVA#5)rG$VwjMiB2O z6!)PEBh2i3aWNqr!6*)}ph{pI6EJWs;<$;k+mK{koWdNTc^@oRTWMA+6D$S};Yngn z0=nG|d{1R8a{?a6Um3z=TzCwBhxMIQF3JCEVhmJL-C7&P}8ana)9ThwYkOu4m;8&vrD-Qdn(f ziitx7D=}m_t|xa{YjA&iB5mh#Mn02Bl~rsuTvH*I8?f@OXXMiyv~|yPsiZ&J7%`m8 zA%jnL_(JWJpPL&P9(C+~;}DVSo(u+28c9%F&h(to#7^5YJX+G8;k0?Vft;NwW(84& zr+4o+4jPG~XJr#Z#e7=Ch+LtNwM<7ANm;{r!z(%jt*@5s`m3W%IAG;X+G3^4ZtN*9 zZOKZ>ja)*{4NQnBTreX&KGs)Ov{TK(+(4n4BMR%HG?`@%g}m|=O(GX{X}N(R%gkmJ z)=be6vMk}|49CkD?#Q4`&m^{pv=48?`gGRHTi!;6`grSZg;^bT#+(a6tz$Ok(9mw0 z-CN8JnogfFm?f}H-FDi@?lv4t#^J+x!=|^bLfF;u)>1e?!QnR)hK?KNV&(WcwA%+Cb({QfBk{SyH z5g;qHE3Bx!LKrQw#Q8erBBtR&hUJq7jE0K{%7hgiwvVmnE)Qb6GS+Yj6-Ei}l+09- zYruLEWozHwb?2=;H--zaQ4qgWVNrZr5GYwAKb%N;j+Gxyw(b^_H|dxMUEI^5qZ3`s zO7!y9aum05?=y^J(Y95`d@N{U0^O$L3S3FXQJccH_{^!&S}UfLvPzGRg=o~UU14p6 zaTB3i9Z(aIIT!12m5!ZADJ<^U-npy0YoKrErruQdroQdldIx&C`nq=V-Rj8vQAuHa zj*)k>voj zy49ER39)@}znS)u8VoKTY5FusrzA~@shohO1Tp%p0cJ20pq+27K zjA2*|A7Rp&FueR$!+yqy?F`GgJofT#Gg7-u^C@H+Z&;+nJSDK1rYia>+TD-gB$`Zj0eCo~>b&NjPG; z-DY}o(aL5_N5^yUT&CLW? z>H$_0wWjnI41L>u|Eht010UsN-o^q~V6+Rb|IHFJm}@Ws+oGtm71% zD(BHlb=-uTNly}cGVfWftc$lkrTt6BeYuWT2#uPlShgIp+ID0h4t%AKSK-wpv}3xy zh+m>87?BRGaJ0%d)1y2JxysBC_}1NWdc0Q0>*Tzi?XOM_dLx!AFK*Rw8{Wu+oOb)Q z-cpi(`gzCG-7C6&wI)&iX1rCyTNKW$Eve&ec)LQQWDz4bV*&vk@5XyrVh%ZWuG1WLOp{Jq6^{<0i1{qNV#L03Ag-D*^aA4T_<)WN z%6{Zj4c8Y4Kdj>;xPuTPL-3LW{d~94+&vk_qX$kR8ndS0>X* z;q2O$S6oj=7sx*;`fEWP$_M%0Ps2M+ISmh*se)-{h;kx2C06B~juFFkEjO4zip23z z9UsH}tVju05NpCB@!9z&7HIx7s_exBIzE9w=@sPqP!dbtJpNi~}NLLrPKnk(N}ZhSZb7;fp%HgfFuxwA>w%rt(au%oF=!_$t1x;cKjhCIesY zminxm9IoFe=b9}$wr-MVu4m*5vY&71_%^=7FrX|>UEY|Jsm&8&sq2vihSl7 z3dwjCqG7c^C6@i}ns8;Ly-TFR8}j5*vYru|ySZ}3}!%oTzzQHr_g zYjn2mYryYFT-X0T(jsZ3C9K74r{!=AkK>OT{xE|Qt39LRPvV!FLSjsLsX>*GuUG1q6A8yu2(aG~>vu%5>=NN?^(;KlfJm~(5 z|7m!F#!ghqR{o%Uz)W=e>M3Xp8B%tQ>IzE9hm46w3XfDn6wVBkU(Sy~S<_wKZQBQm zh2(T3tDa1n*f&}*tHo#jGFYKLtbOlvKXgP z@^p??@G8IA>TaBICvWO-CjZazAD@A<{3l{Yi*J%l>7tDMn(x9H4`Tj(Q2ocSl+!uB zgU{=I4GVnp&!VOlnzgi|X@xJL#8Q4)39L{nxdXA|XxlNYY#YbfV_410nlY?xyAK!i zH^!Ti$FN}xm$kK(uz3ty+S=~L<-i!aDbU-07j_&&-x&6A)ZgEBls88ybQIn9;A({j za4qlGmM~bt&;ziJ;y?*@Gm63AI1V-AdBGp=kKy@axSpDhaAPkT!;PGKy{yaY>JnbU z+n1Ga3rClg@R|wGdWkr25oQt9d02@i76(hvhE}Y?TCBzUHC4ybnF z#wslfDq7x)_k~)z=q7RJe9^M1g!h;5p*<5@iuo@$lV}~jqD6slsEE^Wr|&u?T|ua_ zomX*Y165vM!d+8M+EQgwL&c;ql*0Ml1W-(Bmibk%)4E%xT6P5?zp~1*=89#_IEHcG zl6!D(sQEJL70VV;vk-*Nysw0h%NpbO)KoirtL$7G#SM*hk{m;fs=X&jf zealUZyO!f7Q;y}w@F_qS-F|xc9N-I62A}ODXD!E?cID@6$4*UmIj;90*k*RUba2At zdKICVDf1GcOTIH?Q$d)s-iy$^NQ_M4L^#hmYN?~n%L_j&Y*4n5a+;{3nR@vPw5%_O zhmGAz4($Z#(4|faxy(tfP)ZLCT-B+ULHf8(KQ~A)!~mnp+~ykNB$-#=6IGt7vcNEl z++>LnUNgoTw|J+{ZSJtc-SyFEt$A!|1O&7;WNA>g)bN4liV^uo(hTXTSelQCc$)2( zL?jDuS>9kJ&9hars(K~`icu-pB29`pMF}&9{1ZovV}2Gzd?h9FY^B-{h99LIxl(0v sF@Gt^(XNJX-mCn|=DGi#X^t@&+5&;U^FtbyJIzmiQLJyS=66T&KBl-@+yDRo delta 319 zcmYk1yGw#m7>A!DnI-alpVCK4ks*XB-ER;QWl3dMvKvtbK{zF(S!SCR_F3gn+9TFE74L!+muzt@^oCt-+2c;E90xp+ z=2h&P*tU$4qDA&P3HkKzs^{;0e${^b2R$gMoDHZb=}cQ_Jo}HS26Ipr)4_{e4n9di OD!dVpiF yaw = pigeon.getYaw(); + private final StatusSignal yawVelocity = pigeon.getAngularVelocityZWorld(); + + public GyroIOPigeon2() { + pigeon.getConfigurator().apply(new Pigeon2Configuration()); + pigeon.getConfigurator().setYaw(0.0); + yaw.setUpdateFrequency(100.0); + yawVelocity.setUpdateFrequency(100.0); + pigeon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); + inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8667bec..049fc7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants.RobotConstants; +import frc.robot.IO.GyroIOPigeon2; import frc.robot.commands.PathPlannerCommand; import frc.robot.commands.TeleopSuperstructure; import frc.robot.commands.TeleopSwerve; @@ -46,7 +47,7 @@ public RobotContainer() { m_buttonbox = new ButtonBox(joystick); m_swerveD = SwerveDrive.getInstance(); - m_swervePE = SwervePoseEstimator.getInstance(); + m_swervePE = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); m_pathplanner = PathPlanner.getInstance(); m_elevator = Elevator.getInstance(); m_grabber = Grabber.getInstance(); diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index aedce2d..059699b 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -40,7 +40,6 @@ public void execute() { if (controller.getHID().getAButtonPressed()) { poseEstimator.setPoseEstimatorPose(new Pose2d()); - poseEstimator.setGyroYaw(new Rotation2d()); } if (controller.getHID().getRightBumperButton()) { diff --git a/src/main/java/frc/robot/subsystems/Pathplanner.java b/src/main/java/frc/robot/subsystems/Pathplanner.java index b0d76f0..501eff6 100644 --- a/src/main/java/frc/robot/subsystems/Pathplanner.java +++ b/src/main/java/frc/robot/subsystems/Pathplanner.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.IO.GyroIOPigeon2; public class PathPlanner extends SubsystemBase { @@ -26,7 +27,7 @@ public static PathPlanner getInstance() { } private final SwerveDrive m_swerve = SwerveDrive.getInstance(); - private final SwervePoseEstimator m_poseEstimator = SwervePoseEstimator.getInstance(); + private final SwervePoseEstimator m_poseEstimator = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); private Field2d m_field = new Field2d(); diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index 87bf31e..f8503c4 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -19,8 +19,6 @@ public static SwerveDrive getInstance() { return m_instance; } - private final SwervePoseEstimator m_PoseEstimator = new SwervePoseEstimator(); - private SwerveModule[] modules = new SwerveModule[] { new SwerveModule(0, SwerveConstants.MOD0_CONSTANTS), new SwerveModule(1, SwerveConstants.MOD1_CONSTANTS), @@ -33,14 +31,14 @@ public SwerveDrive() {} public void drive(Translation2d translation, double rotation, boolean fieldRelative) { ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, m_PoseEstimator.getGyroYaw()) + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, SwerveConstants.gyroYaw) : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); setModuleStates(moduleStates); } public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { - ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); setModuleStates(targetStates); } diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java index eb0bbe8..41377c4 100644 --- a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -19,74 +20,102 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.FSLib2025.vision.LimelightHelpers; import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.GyroIO; +import frc.robot.IO.GyroIO.GyroIOInputs; public class SwervePoseEstimator extends SubsystemBase { - private static SwervePoseEstimator m_instance = null; - - public static SwervePoseEstimator getInstance() { - if (m_instance == null) { - m_instance = new SwervePoseEstimator(); - } - return m_instance; - } - - private SwerveDrive m_Swerve = SwerveDrive.getInstance(); - private SwerveVision m_SwerveVision = SwerveVision.getInstance(); - - private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); - - private Rotation2d m_gyroYaw = getGyroYaw(); - private Supplier m_modulePos = () -> m_Swerve.getModulePositions(); - private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); - private SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( - m_kinematics, - m_gyroYaw, - m_modulePos.get(), - m_SwerveVision.getVisionEstimatedPose().get().estimatedPose.toPose2d(), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9) - ); - - private Field2d m_field = new Field2d(); - - public SwervePoseEstimator() { - m_poseEstimator.resetPosition(getGyroYaw(), m_Swerve.getModulePositions(), getPoseEstimatorPose()); - pigeon.reset(); - } - - public Rotation2d getGyroYaw() { - return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); - } - - public void setGyroYaw(Rotation2d yaw) { - pigeon.setYaw(yaw.getDegrees()); - } - - public Pose2d getPoseEstimatorPose() { - return m_poseEstimator.getEstimatedPosition(); - } - - public void setPoseEstimatorPose(Pose2d pose) { - m_poseEstimator.resetPosition(getGyroYaw(), m_Swerve.getModulePositions(), pose); - } - - public ChassisSpeeds getRobotRelativeSpeeds() { - return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_Swerve.getModuleStates()), getGyroYaw()); - } - - @Override - public void periodic() { - m_poseEstimator.update(getGyroYaw(), m_Swerve.getModulePositions()); - if (m_SwerveVision.getVisionEstimatedPose().isPresent()) m_poseEstimator.addVisionMeasurement(m_SwerveVision.getVisionEstimatedPose().get().estimatedPose.toPose2d(), Timer.getFPGATimestamp()); - m_field.setRobotPose(getPoseEstimatorPose()); - - SmartDashboard.putData("Field", m_field); - SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); - SmartDashboard.putNumber("swerve pose estimator x", getPoseEstimatorPose().getX()); - SmartDashboard.putNumber("swerve pose estimator y", getPoseEstimatorPose().getY()); - } -} + private GyroIO gyroIO; + private final GyroIOInputs gyroInputs = new GyroIOInputs(); + + private static SwervePoseEstimator m_instance = null; + + public static SwervePoseEstimator getInstance(GyroIO gyroIO) { + if (m_instance == null) { + m_instance = new SwervePoseEstimator(gyroIO); + } + return m_instance; + } + + private SwerveDrive m_SwerveDrive = SwerveDrive.getInstance(); + private SwerveVision m_SwerveVision = SwerveVision.getInstance(); + + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + private Rotation2d m_gyroYaw = new Rotation2d(); + private Supplier m_modulePos = () -> m_SwerveDrive.getModulePositions(); + private SwerveModulePosition[] m_lastModulePos = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + private SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator( + m_kinematics, + m_gyroYaw, + m_lastModulePos, + new Pose2d() + ); + + private Field2d m_field = new Field2d(); + + public SwervePoseEstimator(GyroIO gyroIO) { + this.gyroIO = gyroIO; + m_poseEstimator.resetPosition(getGyroYaw(), m_SwerveDrive.getModulePositions(), getPoseEstimatorPose()); + } + + public Rotation2d getGyroYaw() { + return m_gyroYaw; + } + + public Pose2d getPoseEstimatorPose() { + return m_poseEstimator.getEstimatedPosition(); + } + + public void setPoseEstimatorPose(Pose2d pose) { + m_poseEstimator.resetPosition(getGyroYaw(), m_SwerveDrive.getModulePositions(), pose); + } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_SwerveDrive.getModuleStates()), + getGyroYaw()); + } + + @Override + public void periodic() { + gyroIO.updateInputs(gyroInputs); + + SwerveModulePosition[] modulePos = m_modulePos.get(); + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + moduleDeltas[moduleIndex] = + new SwerveModulePosition( + modulePos[moduleIndex].distanceMeters + - m_lastModulePos[moduleIndex].distanceMeters, + modulePos[moduleIndex].angle); + m_lastModulePos[moduleIndex] = modulePos[moduleIndex]; + } + + if (gyroInputs.connected) { + m_gyroYaw = gyroInputs.yawPosition; + } else { + Twist2d twist = m_kinematics.toTwist2d(moduleDeltas); + m_gyroYaw = m_gyroYaw.plus(new Rotation2d(twist.dtheta)); + } + + m_poseEstimator.update(getGyroYaw(), m_SwerveDrive.getModulePositions()); + m_field.setRobotPose(getPoseEstimatorPose()); + SmartDashboard.putData("Field", m_field); + + Pose2d visionPose = m_SwerveVision.getLatestPose(); + if (visionPose != null) { + m_poseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp()); + } + + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve pose estimator x", getPoseEstimatorPose().getX()); + SmartDashboard.putNumber("swerve pose estimator y", getPoseEstimatorPose().getY()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveVision.java b/src/main/java/frc/robot/subsystems/SwerveVision.java index 1fd00cc..3a4dee2 100644 --- a/src/main/java/frc/robot/subsystems/SwerveVision.java +++ b/src/main/java/frc/robot/subsystems/SwerveVision.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -100,6 +101,17 @@ public Matrix getEstimationStdDevs() { return curStdDevs; } + public Pose2d getLatestPose() { + Optional visionEst = getVisionEstimatedPose(); + + // Check if we have a valid vision estimate >w< + if (visionEst.isPresent()) { + return visionEst.get().estimatedPose.toPose2d(); + } + + return null; // No valid pose found ʕ•ᴥ•ʔ + } + @Override public void periodic() { // This method will be called once per scheduler run From 692dde25a12b99a83a29d030a1a7e084979aab00 Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Sat, 8 Feb 2025 19:01:23 +0800 Subject: [PATCH 08/11] progress rescue-delete some stuff --- .../robot/Constants$JoystickConstants.class | Bin 403 -> 403 bytes .../frc/robot/Constants$RobotConstants.class | Bin 619 -> 619 bytes .../Constants$SuperstructureConstants.class | Bin 4174 -> 4174 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2974 -> 2974 bytes .../frc/robot/Constants$VisionConstants.class | Bin 1648 -> 1648 bytes bin/main/frc/robot/Constants.class | Bin 680 -> 680 bytes bin/main/frc/robot/subsystems/Elevator.class | Bin 4858 -> 4858 bytes bin/main/frc/robot/subsystems/Grabber.class | Bin 5454 -> 5454 bytes .../subsystems/SwervePoseEstimator.class | Bin 6594 -> 6594 bytes .../frc/robot/subsystems/SwerveVision.class | Bin 5309 -> 5309 bytes src/main/java/frc/robot/Constants.java | 1 - .../java/frc/robot/subsystems/Elevator.java | 1 - .../java/frc/robot/subsystems/Grabber.java | 4 ---- .../robot/subsystems/SwervePoseEstimator.java | 4 ---- .../frc/robot/subsystems/SwerveVision.java | 3 --- 15 files changed, 13 deletions(-) diff --git a/bin/main/frc/robot/Constants$JoystickConstants.class b/bin/main/frc/robot/Constants$JoystickConstants.class index f9f12acc43d3616a192d10e598372015be8a9fbf..a7c734c27fa8f1e7ea5e4239c91dc21df6e4704d 100644 GIT binary patch delta 13 UcmbQtJehez93$hG$?=RP03W9WSO5S3 delta 13 UcmbQtJehez93$h`$?=RP03WObSpWb4 diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index 3aa4c18b0ec1c538909f8c656382ea9115e98ef3..fe20a95ce0b2d3bf184de78e78146200f7419935 100644 GIT binary patch delta 13 UcmaFO@|tCX855(#WOF7H03red-~a#s delta 13 UcmaFO@|tCX855)AWOF7H03rti;Q#;t diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index c89f59254fd6cf0c4833a42dfaac53dfeba602d3..f68555e4a8449a712d1d1a261e80bbf5ba659963 100644 GIT binary patch delta 200 zcmWN`JxBrp07vonUm9FaqS3+WhNf@`1h*EUO>k*x5ovM|qEn*NA<9;r4@6B=BELY? z#lbOd3JXySF^4vr8$90jdv4GTwvP=vjgM=NxFLXrNrn@$gt_I6JI=9*ljD*+SKO24 z!P7^t0wu~gRCuOJn;IkPOlj~&lNl|(dEu8={=EB7M+bCe&{KecOoj?EQsk7eqP#1{ oOc#78!KYHpWwB6>rJh)+#99s;HThAWttRY>Uq(pD;-NpX2l=x)CIA2c delta 200 zcmWN`EocG(0EXfBD+bGnEM71fmc$?k*H#4Ez_MZyW-@Fb&awFuWLnQ`7*52A=w=u< z!@{Cmn@+~2ASx`H4PJT5<0sDv1x#)TQzXJIaqc*yM3OQoDx|5BrN$Kx z+*9{#(BzR8Z8|(L^mOR*NslSd%;>XXz`r+pMjUvNk5?ItW%8ylj-tHFiZM}wFP$^j m1>efBP>vrJ_*I!dHCU?2S}iu}vDKKJKG@q!BdSX~7_g2E4LVf- diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index d9cd5e05f66bc484195d45a25982bb5a2acba7b2..bb268f12f740868878b270870d02957cb7573477 100644 GIT binary patch delta 148 zcmWN|Eeru+9Khk(!nv!}snek)JA76xp#94|$OOlnOVysQt&WU5R?S^O_9q0f6 diff --git a/bin/main/frc/robot/Constants$VisionConstants.class b/bin/main/frc/robot/Constants$VisionConstants.class index 33772f16456ab590904adffb7b705093e92199e0..e5ab98c58b733c2b3c0f6ac64ea19601fbeae298 100644 GIT binary patch delta 31 ncmeys^MPl>A6DL93~~&=8Ppm6F_A6DMq3~~&A7}Od5Gng?lOy*=WV*EMThRp;3xV;Jh diff --git a/bin/main/frc/robot/Constants.class b/bin/main/frc/robot/Constants.class index 19df9baf990ea1fbe6e9b949a16db87b101145d1..3b6906842c39c8d39542e7f4457608e9f2a75495 100644 GIT binary patch delta 13 UcmZ3%x`K6sCljOCWG|*T035*sssI20 delta 13 UcmZ3%x`K6sCljOiWG|*T035~xs{jB1 diff --git a/bin/main/frc/robot/subsystems/Elevator.class b/bin/main/frc/robot/subsystems/Elevator.class index 990aeb60c42bc0ecb172aa3d4146d71c95664561..4895b01a9b1cd345e15248a598bd33bc5287abfe 100644 GIT binary patch delta 141 zcmeyR`b%}gFMdYF$-ns>IaL_g7*rVq7}O`b3Y>N{WME;?X5eMeVUT9f1+w%QOc?YT ztQd?KJQ<7`0)V_QARWbE#*oQi&QQQ$!B7UItAHxn7_1qlFxX5E6!hSoGCky$p`Z0(x_)p#@IaL|h7}OX97&Iok3Y>N{Vqjs=Vc=!ZWsqjjW6);MXE0$f zV6b8^X7B{E0vJpg!hm!XgE>Pcg9SqYgC#>5kgj5|W@uxuVVJ^TJ2_C$gVTY5gTWDK prqkp_f-#J)lm81wb9yo`Gk7s5Gk8u;7V=~DXAolun7mEM1pvrK8?68U diff --git a/bin/main/frc/robot/subsystems/Grabber.class b/bin/main/frc/robot/subsystems/Grabber.class index 5a242082c4cd176022329a52763c9d0ac3e7e1cd..1b119a9fc25e725aef184cd5a02c457f6f2abe77 100644 GIT binary patch delta 262 zcmW-bzb`{k9K}CZT6s-ct#>bp=uEwbSAB>di7M4<2_u`-pnm|hHQhSdzLQAAA~6^w zMk|Ryx|zCIBoZA>CbyRJIp^dIr{i_Jhwr3XkY2J7TVjx9Qmj5Mrt+$_MubTcWEf`( zACGyfD@?P^42R58BcM)}26HsY(I(`AJXfr9OMyGfdp3AtlNXA-TE4MGmlB_p`LgU$ z;g_m%*kKHaqd>*Dk)PpQbPtDp@!hISxM>NrBbTl(4eLMLP@ez0P_*u6N7~w#W9$37 kcO9q`iJGEB%%~ihP%0)a*XAH`$`EIy?Q4&785r6912LgGm;e9( delta 264 zcmW-YJuE{}97oTUlG~)!diQC9CieGm>7!C1jqmpPTCFX{*zcN5`%#l ztt1BNX0li$5*c>OjP=r4Q-nh&;yxw4`Tn^B2A$J(>Uj diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class index 77bf369f2775508a429c50698078ba66a2af36aa..b5f5de0691193bb3890b79862011636fb65a1ed6 100644 GIT binary patch delta 298 zcmWlT%PT~29K}Dkn40nkx1U!N8qpZz(e=oL8AFuGmBP&2@yKIp2zd;Jyml6{`K@f& zS@3tQEc^rh0Xr*X?`v~Topb7Z3Rc0&otTD4pPCVOBQBb#z^h-)-^3tA)Kg9i zidH=M@Y6=0Xqa|J2rxkhGjx)qixl1LVsS_h$MkY0%Pac1mh6@wcMNgQFi(WEsm6_n zPO6Z0REd66;o?cEi0V%jbK8_LO(k|dTJ19~W+`Q!I$|^sr7!BgRG=^ws@B2g&p45b|>rAcv2R_m}SpWb4 delta 298 zcmWNLODlwN6o;R;n3{44Z-1B4Bx8gayyIRPGonmJMslAQWn4nYU2Z!I*_;*1?5)(; zS*Tf9_yWFwofWcoHorRO{7%o4b+XRWv29h!i5>C!@lZn%wen{FO@+z9MF$PJZuR4V2+u^tHUn~J@+4(K z){xwrSZ`7OJ! Jg0fp4{|5ziJedFh diff --git a/bin/main/frc/robot/subsystems/SwerveVision.class b/bin/main/frc/robot/subsystems/SwerveVision.class index e37388fb493affcdf60dc57da54b7b053ad76cc6..e72f9dc04f0e044af062327835b2d2db302f9a6b 100644 GIT binary patch delta 239 zcmWN}y-PxI6vpxI4@?cLdwm1BVYJlM4Ai)bM5Od;FH(w$CW;P1f|G;L5-nCk5d47UbC&UrX|o-;{KlAn);vhBq85tYSeTT+G`jypH6)qM^hJ1ny+r9GnLiBVvM1A&L^b3}<_t~jAWkt(O$a7K%B zI$SUk_rN7%%Djkp74b%mcj|o6;9K;RCV#Z$H=_J9XVUV@mgIhfVoG)OPq?zwV}V-& Q^syM|jYwAw^>f7e2PHl$8UO$Q delta 239 zcmWNKOG^S_6o#J<$`Yy>YX+o9jHocl2{R&y$O_CZR!W0-rGbK=pjAN|X=Ansf^XBh zePGOb7H->A|Rh9*>v*!Pqu4-*C#3o~G$0Gb^e9 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0777e62..8574362 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.SensorDirectionValue; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SoftLimitConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 941f648..7a9433d 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.SuperstructureConstants; diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index 38ad4e6..68596d7 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -8,14 +8,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.FSLib2025.state_machine.SuperstructureState; import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SuperstructureConstants; diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java index 41377c4..bfc2853 100644 --- a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -6,9 +6,6 @@ import java.util.function.Supplier; -import com.ctre.phoenix6.hardware.Pigeon2; - -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -20,7 +17,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.IO.GyroIO; import frc.robot.IO.GyroIO.GyroIOInputs; diff --git a/src/main/java/frc/robot/subsystems/SwerveVision.java b/src/main/java/frc/robot/subsystems/SwerveVision.java index 3a4dee2..770c228 100644 --- a/src/main/java/frc/robot/subsystems/SwerveVision.java +++ b/src/main/java/frc/robot/subsystems/SwerveVision.java @@ -13,15 +13,12 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonTrackedTarget; -import com.pathplanner.lib.config.RobotConfig; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.RobotConstants; import frc.robot.Constants.VisionConstants; public class SwerveVision extends SubsystemBase { From cfa8cd269407a1d78ccd579ab38dfc58d0ca6aa9 Mon Sep 17 00:00:00 2001 From: Unbrainhaker Date: Sat, 8 Feb 2025 19:03:55 +0800 Subject: [PATCH 09/11] naming --- .../frc/robot/subsystems/Pathplanner.class | Bin 5577 -> 918 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class index 9a2a44856b7a2e01e4993cb07942cbd6b1c3ef25..3774436ec5d56cef928cd0c6d5a0313004d32a43 100644 GIT binary patch delta 479 zcmaiwze@u#9K~O3>5ovgty-(~N4i-Hc2FEdT|^OF3TnYcyxyfYa!rn0T5;;^eBl4# zBq->tf0USO9V;$h_$Kdt@?JjcPigw~{rV2z0M%!@OQJ`z-6BXHDUYX7Aeg;#22R6w zf_~cLN)n)UjjVQtPl-VxF!5t0cLGIcLm zNBErD8m22kf=s}*P@ZrJ=((!&waZVefFK{^f-!C@E*fB^6n6ohhqEX21XF40WOtGbncdmW%m&lT zw)U=6d)St$y_FtT1tm$rqqnWK_kG{@eIH-Hw!imgvqw4`@%x(nvh(I0zyE!_eeB<7 z9tN--|5Q#j5G__~)F_9i{wOnQ31;g9v(Mk-~xDk>DLH4_>AXxh?8EZ6hJ z$hL-$Y}DgUGHIj|`oQebPQx=5M3RTCl;<0%IA2!Ao`^)df~w?U@2Kfcl-@rem%CkS z!sPziMxNW5@|0)IL%`JlpD!QD6Sq%l}Ygm9v@)omF=AKM)*mUFhzTWR2noaMU8^WZCNfB4){iVe7BL7xDk}2A%Y9B zLd8X;%v+#K4fS}6f|BG5wzdl)FV>)j#aNo|Z^rQl*U znpLb((45~{;AA1D5!|#W=x8p`MGm{YgoZ+JB&%&2su5AKu9W|lFmeQI(I8~qpy51d z5p2X}6`K^Srn~EDhDJIF4V^*25J-(!qxwD(LS1sK;WBKY8;rP4a@T>r0n_)b)Tl=n zrH#)Gf|kAlTUA`CV9^m{!q7=@RNr}h+)VqHlhV*GT%b1XS}9+ef4^(QO%>Z|mHhsF znH0mr%pI%3Qxz=kF)Z6mH2O}X?HGy1fd2@#p+k69CwDZ*@)!d?X^ghF3`Wq69u+$k zMDweAhmV+XU&Ai+GBW7UUAEy-(nXXsze`TG*D>o{t>GHP7&+Xkps&!(^E(D@FYz+I zW$PoER9vWmX(yewEz>2of;}4cqMw0b#D$vv5AZanmq|+aGF)B9$Qlqd_ABTr!Fge@ zhH~6#+1d2B7=55DWzw&^kx_xC!slX8+)T;@I=&#tMsR>7JQ=Y>o5_jL;OrjcTB&ALpniL>iDORE@TJ_*g)A>}B7Bd}GBGr@%^%E(HK>Fk`o2%o)qKE3?l{1v3Q4SwS*{nm2H*{+S4Pr8Lw4wi-Jo+vxY9! z;j)SbmGyOay#mpjb=bBQbg)Z`Qqx{pf(@HB`c}M2#T%)yd~KRt)$nEb6? z@%<=njN+J>`>BSTaEwa)2`8j%3Vx;GE}Y=LUrHe;uY%ueIE^XJ{VpnkLhBR1Wyp(& z2#BiL?5X0f3RdNrD)NJ3?^5x11uN#?%Cr=?A}%HmQ5p1C>EZD zGBk}PJkIy~IA1OiY9u=iL|Td{mgkU`$sJ0RVEkPnfK<$hw@DVZ4b@#WjUA7)K7iJtDRdn?gFc|( zZFkYz)A)7<`@jr#VCXCk4%K6*re+$~1>P;NqjLUHgk&i>i?dZUr*ok}4D>c~|s0+`Nre2L#;`w+1xmi!jHxQwW zl!g7u|3l`=R46Km@au|-h1~U`S;dnR@SXp2O}|@`(SpT*Otegd2WN!e>>>$av1;nL zv26-3nG3L~fY0hIpTdt`L0$#LYP<@sMp-l>;wua45>%{RFoV}z(>jGW9LM>596yEI z@4|wLyUY3Ea)op-akm4mg$5P3 zDin&boTcAG`ogzsD3o6Jo+-SKlHHS+EFxDs89rT-)RJI#PP!W0j|U(ynG3R**A`(; z2bdoen1ROxm_5YYRRZ%tJVdNnLC75*#KW8^$0I~3!-w!;K6@GNpXA)9@L7CLj=q4e z;Hz@ Date: Sat, 8 Feb 2025 19:04:28 +0800 Subject: [PATCH 10/11] ? --- .../frc/robot/subsystems/Pathplanner.class | Bin 918 -> 5577 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class index 3774436ec5d56cef928cd0c6d5a0313004d32a43..9a2a44856b7a2e01e4993cb07942cbd6b1c3ef25 100644 GIT binary patch literal 5577 zcmb_g33wFM9sf-dGRZn5YX}5TX~Y0IfI&c10+t+vL>EX21XF40WOtGbncdmW%m&lT zw)U=6d)St$y_FtT1tm$rqqnWK_kG{@eIH-Hw!imgvqw4`@%x(nvh(I0zyE!_eeB<7 z9tN--|5Q#j5G__~)F_9i{wOnQ31;g9v(Mk-~xDk>DLH4_>AXxh?8EZ6hJ z$hL-$Y}DgUGHIj|`oQebPQx=5M3RTCl;<0%IA2!Ao`^)df~w?U@2Kfcl-@rem%CkS z!sPziMxNW5@|0)IL%`JlpD!QD6Sq%l}Ygm9v@)omF=AKM)*mUFhzTWR2noaMU8^WZCNfB4){iVe7BL7xDk}2A%Y9B zLd8X;%v+#K4fS}6f|BG5wzdl)FV>)j#aNo|Z^rQl*U znpLb((45~{;AA1D5!|#W=x8p`MGm{YgoZ+JB&%&2su5AKu9W|lFmeQI(I8~qpy51d z5p2X}6`K^Srn~EDhDJIF4V^*25J-(!qxwD(LS1sK;WBKY8;rP4a@T>r0n_)b)Tl=n zrH#)Gf|kAlTUA`CV9^m{!q7=@RNr}h+)VqHlhV*GT%b1XS}9+ef4^(QO%>Z|mHhsF znH0mr%pI%3Qxz=kF)Z6mH2O}X?HGy1fd2@#p+k69CwDZ*@)!d?X^ghF3`Wq69u+$k zMDweAhmV+XU&Ai+GBW7UUAEy-(nXXsze`TG*D>o{t>GHP7&+Xkps&!(^E(D@FYz+I zW$PoER9vWmX(yewEz>2of;}4cqMw0b#D$vv5AZanmq|+aGF)B9$Qlqd_ABTr!Fge@ zhH~6#+1d2B7=55DWzw&^kx_xC!slX8+)T;@I=&#tMsR>7JQ=Y>o5_jL;OrjcTB&ALpniL>iDORE@TJ_*g)A>}B7Bd}GBGr@%^%E(HK>Fk`o2%o)qKE3?l{1v3Q4SwS*{nm2H*{+S4Pr8Lw4wi-Jo+vxY9! z;j)SbmGyOay#mpjb=bBQbg)Z`Qqx{pf(@HB`c}M2#T%)yd~KRt)$nEb6? z@%<=njN+J>`>BSTaEwa)2`8j%3Vx;GE}Y=LUrHe;uY%ueIE^XJ{VpnkLhBR1Wyp(& z2#BiL?5X0f3RdNrD)NJ3?^5x11uN#?%Cr=?A}%HmQ5p1C>EZD zGBk}PJkIy~IA1OiY9u=iL|Td{mgkU`$sJ0RVEkPnfK<$hw@DVZ4b@#WjUA7)K7iJtDRdn?gFc|( zZFkYz)A)7<`@jr#VCXCk4%K6*re+$~1>P;NqjLUHgk&i>i?dZUr*ok}4D>c~|s0+`Nre2L#;`w+1xmi!jHxQwW zl!g7u|3l`=R46Km@au|-h1~U`S;dnR@SXp2O}|@`(SpT*Otegd2WN!e>>>$av1;nL zv26-3nG3L~fY0hIpTdt`L0$#LYP<@sMp-l>;wua45>%{RFoV}z(>jGW9LM>596yEI z@4|wLyUY3Ea)op-akm4mg$5P3 zDin&boTcAG`ogzsD3o6Jo+-SKlHHS+EFxDs89rT-)RJI#PP!W0j|U(ynG3R**A`(; z2bdoen1ROxm_5YYRRZ%tJVdNnLC75*#KW8^$0I~3!-w!;K6@GNpXA)9@L7CLj=q4e z;Hz@5ovgty-(~N4i-Hc2FEdT|^OF3TnYcyxyfYa!rn0T5;;^eBl4# zBq->tf0USO9V;$h_$Kdt@?JjcPigw~{rV2z0M%!@OQJ`z-6BXHDUYX7Aeg;#22R6w zf_~cLN)n)UjjVQtPl-VxF!5t0cLGIcLm zNBErD8m22kf=s}*P@ZrJ=((!&waZVefFK{^f-!C@E*fB^6n6ohhq Date: Mon, 10 Feb 2025 03:05:46 +0800 Subject: [PATCH 11/11] stole 6328's full drive code & some alterations --- .../robot/Constants$JoystickConstants.class | Bin 403 -> 403 bytes .../frc/robot/Constants$RobotConstants.class | Bin 619 -> 672 bytes .../Constants$SuperstructureConstants.class | Bin 4174 -> 4174 bytes .../frc/robot/Constants$SwerveConstants.class | Bin 2974 -> 3853 bytes .../frc/robot/Constants$VisionConstants.class | Bin 1648 -> 1648 bytes .../robot/IO/ModuleIO$ModuleIOInputs.class | Bin 0 -> 1508 bytes bin/main/frc/robot/IO/ModuleIO.class | Bin 0 -> 860 bytes bin/main/frc/robot/RobotContainer.class | Bin 4021 -> 4299 bytes .../robot/commands/TeleopSuperstructure.class | Bin 858 -> 858 bytes .../frc/robot/commands/TeleopSwerve.class | Bin 2766 -> 4454 bytes .../frc/robot/subsystems/Pathplanner.class | Bin 5577 -> 5776 bytes bin/main/frc/robot/subsystems/Swerve.class | Bin 0 -> 7597 bytes .../frc/robot/subsystems/SwerveDrive.class | Bin 4278 -> 4311 bytes .../frc/robot/subsystems/SwerveDriveOld.class | Bin 0 -> 10902 bytes .../frc/robot/subsystems/SwerveModule.class | Bin 6036 -> 4309 bytes .../robot/subsystems/SwerveModuleOld.class | Bin 0 -> 6045 bytes .../subsystems/SwervePoseEstimator.class | Bin 6594 -> 6914 bytes src/main/java/frc/robot/Constants.java | 41 ++++- src/main/java/frc/robot/IO/ModuleIO.java | 53 ++++++ src/main/java/frc/robot/RobotContainer.java | 47 +++-- .../robot/commands/TeleopSuperstructure.java | 1 - .../java/frc/robot/commands/TeleopSwerve.java | 109 ++++++------ .../frc/robot/subsystems/Pathplanner.java | 9 +- .../java/frc/robot/subsystems/Swerve.java | 163 ++++++++++++++++++ .../frc/robot/subsystems/SwerveDrive.java | 85 +++++---- .../frc/robot/subsystems/SwerveDriveOld.java | 163 ++++++++++++++++++ .../frc/robot/subsystems/SwerveModule.java | 154 ++++++++++------- .../frc/robot/subsystems/SwerveModuleOld.java | 102 +++++++++++ .../robot/subsystems/SwervePoseEstimator.java | 15 +- 29 files changed, 753 insertions(+), 189 deletions(-) create mode 100644 bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class create mode 100644 bin/main/frc/robot/IO/ModuleIO.class create mode 100644 bin/main/frc/robot/subsystems/Swerve.class create mode 100644 bin/main/frc/robot/subsystems/SwerveDriveOld.class create mode 100644 bin/main/frc/robot/subsystems/SwerveModuleOld.class create mode 100644 src/main/java/frc/robot/IO/ModuleIO.java create mode 100644 src/main/java/frc/robot/subsystems/Swerve.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveDriveOld.java create mode 100644 src/main/java/frc/robot/subsystems/SwerveModuleOld.java diff --git a/bin/main/frc/robot/Constants$JoystickConstants.class b/bin/main/frc/robot/Constants$JoystickConstants.class index a7c734c27fa8f1e7ea5e4239c91dc21df6e4704d..965439f674266feac4f624dcb9436421964ec3d2 100644 GIT binary patch delta 14 VcmbQtJehezEF%*m_vCm+6964s18@KU delta 14 VcmbQtJehezEF%-cm&x&rCIBJg1Xut7 diff --git a/bin/main/frc/robot/Constants$RobotConstants.class b/bin/main/frc/robot/Constants$RobotConstants.class index fe20a95ce0b2d3bf184de78e78146200f7419935..50de9b44b0243148ab4e8acd2454383bee39952b 100644 GIT binary patch delta 229 zcmaFOvVc|K)W2Q(7#J9A8PwPrm>3z9CQoD(p6tiSKhbEXv!qLqXP9fev%g(fmuT{jFExGIX@+pi-DOziibgrL3}bN zV+*75{$!XU|@3Kmxa029+A7XSbN delta 142 zcmZ3$`kIC7)W2Q(7#J9A8I&h-J)A7SD8VMq!ywEcGFgwYg;8PhW=3m9mdWoK9TONC z7(wDdQWU6OA4oF-X;!W63}PFB3?>E%Ajt+6lwgnqvUwm17^E1afjnjg8L%o@200+j Q1=cRlpa@i{1QwSE0OKqWlK=n! diff --git a/bin/main/frc/robot/Constants$SuperstructureConstants.class b/bin/main/frc/robot/Constants$SuperstructureConstants.class index f68555e4a8449a712d1d1a261e80bbf5ba659963..90fed52e1e7d71421f5e9da345583d7096a7a18b 100644 GIT binary patch delta 200 zcmWN^EldId0EXfBE6iDrIU<2XFfjs~8;!zdbF)}11QWqz;U}td3Ohgo0}_EmFfe2c z7z1VOh6XBT956%z12?ZdkL9sEwPPc3#~}~6c*M;UUQ+l-bAm~TET`l+=b20LTv6bf zB5_LGQYJ&)UV{p+)M(mj(WA|X4imb3*_+YlhXH%u{Q9SNCj`VXGoLRoUx>KecsWP7S|e=8-e_50jcbU;qFB delta 200 zcmWN`JxBrp07vonUm9FaqS3+WhNf@`1h*EUO>k*x5ovM|qEn*NA<9;r4@6B=BELY? z#lbOd3JXySF^4vr8$90jdv4GTwvP=vjgM=NxFLXrNrn@$gt_I6JI=9*ljD*+SKO24 z!P7^t0wu~gRCuOJn;IkPOlj~&lNl|(dEu8={=EB7M+bCe&{KecOoj?EQsk7eqP#1{ oOc#78!KYHpWwB6>rJh)+#99s;HThAWttRY>Uq(pD;-NpX2l=x)CIA2c diff --git a/bin/main/frc/robot/Constants$SwerveConstants.class b/bin/main/frc/robot/Constants$SwerveConstants.class index bb268f12f740868878b270870d02957cb7573477..ed526e750df64325c47aa35041cf9a70806e0f09 100644 GIT binary patch literal 3853 zcmbVPZE##w8GcUE+$GC3UCK6X1f(OSlR(l6wsnG(^HxkM^96jyTEprXVW82x=P#w~yL(U)`eKU9d(>JgiCoUmY7N7+hA0`djH0ABrt9h+OT78_=l@mPqe{o# zo(?63ZZKfZfeUQw6ZiQymWXB;Xo|ABrPcBxvejly(fwqi!cO~>^jW!;;I6b-v< zc+PwzZF{x5mHE^QI+`z;mgya1sBdkfld-MW zE*JsyG3>jtI0vC%K!%E52I;8KYNVvTTgGkpNB{|jw#|et3$)brX7y4CB90SWG|kY^ z8jH0JORJZv!2lnAO2+NDqmke}GCq#&406mdXN-jH*^XM^ z9rw!6MTh1YhU4qV%P8=UU~e>;r>!EswEEi$darxy3?Dcl!-PZ$x7ME;A9oFpPdg>! zqqsv_T|Hg5`>lWM;wD?hon-2IqyODsJ%0T;ZgOQD;jD|NrQ=WiF~&`^GLGV|0OlF? zZ!KGbn5(+{2^q)G)kyG@GCqaV{0g6T9K-VBX4&-I%@gr5YBhJ#nQ)02KVMs8IR$Ml*fIBQ5e(n zQw+Cma-!B)Yd$pM%lK*lU!f%Z-y2&qNXFxQ)yP+5#{hn8Q;QFR59GL?s)!vVK~sLwq3y)@6la;|IxicacZr`iAz5t z<2(2+!?iPZ$jc!q>5$Ev30G?+! z^!}T-;oV-fP@;2NFl4-dAJGjo)#K<>2E*-J+N5=vBix~vWc(N}bB8X8GZb6@PrWT} z1H<;XX&K4s@|fXdc$U-oh}(I+G^{%&$HExEt8~R|Jl7IbgoCKZ0A1kTgh|KwW>KQv z=ha6AAgI~XPTuG<`AkieGS&Ctey&aTqH0-&(_7MA*Kh;)Db?MrM2EI;Ej|IPQr5l4 zOO!mWd5hAhq~Ur4w(H>lU4Q(UbOTz@0Etk5P#vx!9IU}t`>;&?P1wH94-vk`*S~{s zxCZa6!Ob;zR}J1>gIj#~gQS1G5A*fk=)-#mf7plldT#QqcQ4_aef|3gxB74!;akzZ z?jPrO`1^t?L$Y8_(l}yHhEeh}tp!-2 zoq}nFng!D$X<}MoRQyb50Z!;f!E{1<1k;fO(+R^PCOJEjt*6D1i!_mzZ6FaXE73S@ z5i={8BPZ@(!~+X>;8i>r#;2EXruv=M_IdH`kudfx;Y%xcg1BE_#MuR$4Si@C-yESY zVLZKzZ}V^U%XpTz3wUlBFOIw-V!=OOqD%n25Q0?VLlmn$R7W}~R^1e*B-u{T3+)v8 zF@^zpo2aPZIOdVS8Km$C2C1B#z>`ShSq$MtdcrK>BrYI}Ut$!$!9Dmra`+SO#b4-2 z^fyv@2L|57IIBmIHDZEohsmym!J1KKdtkG6RM=tM$9mwgIA+)oW?2q%tVj=-`*1%N z#LD@9KJ;j;<`E*)GO{<^avnx}}sXLvYnA0M|Ds+{~$t5na7IBZ`jH5d&791amStak>#ZW6|o$JN4!>BQ^ z6Ml|nY-doLQ=_SLs}6=%j?M6CXcIHe9kl@jZAys2wK*8{^{Y#_Klt@(u3?Q$4Rh4t zVGUj4Cud`41b*AOm!lpHIwE+IV;|Ns@ZflAA{iY^kBx_)u>&!VUKxm`lgaV2KtIQT zc-mE^MsR@Rpvbzs{zDvF(4a0YWgY9j|IKUv#1Ka#xdO|be}5^~e`vd&AY zGFp>&vXnN>@ffxmn8d3buOTCjx!d&Ow)`ZnKVvPR71{Ce5=y({L8W@d_Sj6|jamGDBP z|E19^9au-Wstn&xcmvk1#5WSIE{oR?exM9LNcf>LTnpVEGS#lMCnzlqOFsT@gD67x8!=E!s^429+XhCfez{h~2S4=L+tS z*j2(ECQ4Y9I0JIXggl-oB0?4SMWp}jH`eiGD-n`WK29&!2T6pJcqbvl_bVJ zTr%U7N6fg1L^;#S!SYl{rscU$GA%0Imm$B2!{yXu4oOe7q$WKzlA2Uy(WHrC$xKn3 z&*w3c!^meCH9@TLA|}auJdddyrj#3aWq@oB*&8@L@TpYs9POvEiS{NKq_BsMTBe31 zZX$WxNZ`HX+=pE_06&K4DyHadaTKjMj&_{HZk$ID0wFA*1Mee@kI{t!_TVO>_=2*( zrtEJK!w+=lencOBK|g-Se#UT+tww@*VX_tMh|T*5nTmBgCIWjb>UGCuz=gE|jr diff --git a/bin/main/frc/robot/Constants$VisionConstants.class b/bin/main/frc/robot/Constants$VisionConstants.class index e5ab98c58b733c2b3c0f6ac64ea19601fbeae298..8844f759a09fb3792cb0d2a1376ba83d4cb7c5f3 100644 GIT binary patch delta 33 ocmeys^MPl>Z&p4=K?XTSAqI6uQ3f+cvB{ilhD?n7lWo{c0E$uuasU7T delta 33 pcmeys^MPl>Z&p5rUkq{#zZujS{xO&_{GZIpX2`_wW3mmK2>{R)3i<#5 diff --git a/bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class b/bin/main/frc/robot/IO/ModuleIO$ModuleIOInputs.class new file mode 100644 index 0000000000000000000000000000000000000000..dd9812c38e886a72f411df1f72519f98113ae30f GIT binary patch literal 1508 zcma)6YflqF6g>l#mbwC>f^Sifma44p7rrW}kW?|CO^lzmyF(eWo!QLJqW%y6h#v@v z8vX8%GTxbOB_^n(*|~RSZ_hpV-ZT5-=jU$#?qjitK8C3^9r{|el<}9A{pTvmV!pII z)pM3)l9{xK0>j8guo?JqAlv?OYlDZThyjM{O1J%37!z}&qyrLR>ThH+G(t&VH~_z) z+U=l4&~8g$krtc$rAmd3S_vWsw!kpvj+;DIp)eol_$Ak?JY+cNUeuE$7CdUI*rYv- z`AlmrO}&%QQQunvIczd5>#bD9nfaZ9q1xb4=D$mXzb15Q{7zuj{Wez}ZuAF#MVY`- zxf;zecov~Yj^T*i?m}wBCHjNYus!;N(!f0-g?YfxU!7?(6y{aL%jn0rheJ3#2!>-G ziYQs{gonZ0JLRFAduKfChgZZ|hUvW(P{sttE!>oc0~oU2w1?r`t9ls8fomQP=H5jQ zqwpAp8$$9IS*OMID?2QP(S{0xxEW|+_ua#SSr^pn>Be59L*3kG6pf$ZVzu!fu~3-H zF*nmJ;yOcRM{=)ank9+ShDXZ@*KSr%-iJJ~Lo4AX!^ziDbds2NNDVxips}v2Xrkhh zSIW4C+qUC(Xq?r?4(_VaLbfTP<(U^fvuBzN#Wk%uM7so<@af$dbVDi(XGvaU zEgor})+4n0==Q+l+BU{&Lt8j%m*aLhS?m8=+`{SF7fig_#J&Fi(H`+37Vzn%3 zb{Re(hJAEa{(@_7MsIB6)_21?&DUSd;(6D&V2Z Z{fKx2Hv40;^YoNmrUg8uil4cdg&>`MBSeFp36`wHddw=0?s_JQtC2jRc9y|QVr7!JA zir6N*KYv6Faq=h(2_(rSCpkt2KxmiBm%I en-SN4BEmQz^&uSMi1rvEk7=D!&Kdcd^1lG?Pn@3s literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/RobotContainer.class b/bin/main/frc/robot/RobotContainer.class index 379d15c0dc4a811692c710739c61ca431126f1eb..96f44d052a185fd963e0fa796dc079ea8b4a7986 100644 GIT binary patch literal 4299 zcmb_eX?xt%6@IU6tc)y(aiEX@!4Rjh2Z+ep&`APb636KzHkshYEv?ba$YVL0(Nv3r zyFnMaFM%#V_l34}1rodvTAqIH)Bn(a(x>m0Db!^*o z1y1D6a-d(STl$>kdcK_5R&n8=UUI5cqgK|7f$uvteKcBLDmu?5>^XsL3(k_~Tcyha z{q3RjvoQs!d#bY$9$^IT%qPl^gknaVXGxW*&U#l&chMXZcrZod1x1g-%&MmsqI1_; zG|8Z@GfbTl$Zo|k<#^^P>RUB@$0g^j)!Dk?&(~=p+H6yCQ>2-SXvowjS>k1E7lC~# zv9awuJ;fIjl3goHwHu6s?ah|FK*HH$1imvm@9+}T^=NxW-K!d|UpBn?qGPyayM*(yggT|&L=6Z`?yuJ{F4HmgG18-U87FfRMCn1H0;DK z1@{Z&{tqWq@BrT}@g%yZ=NmQO8(0sBXsKd1_DGQf)M0R9XgULd4{Ept-3kT;cDL1` z;k_#MVUJ{aNP~iH3LX*Ikw|U$uZoAEOEHfM?AhdP8=$#Ji-Q_6P*ohpQ3Xc??n!W4 zd|tzcaEy^uG5v|~q9wj!Kkt+BzZRyPiVx$s0=`E77T9CbF{OYL0?!Yo04FB(b51$1 z&56m}`?9^IwxJq1Y3s)|^gvTEEU+ulo7#*oxz5DolvOdE+Cddh;64>&IIVyQU@)O< zF+@}R%bSi13~#t;gq_<`#U`9k)d>x^;x-i@!8rx_&Fq{?2vYiafq}tI1f@tr0aF^b zqgTNNfxXEMw@5WAp2S7ziBFLueaDnl(n2C%{HTW8(bvZkGo#^S_;}=M!`_$^Fp2O< z(n#wO7L^>DEUY(yZ`t}>P%HTsD@Vjw2p@RtgE3HcEQt9H5^41oL=105<-QORQp_@&fG6EQmJ8*{#--?EL zSOT|uraxxR8G+4WAJ%B*q)ie{YeF8HmVr`{e#$|hs-cEm0(X_1+MHEkO^7p@RV!Py zil?Fu_7b0>qtey%dQvQP_6fQyNcu2`k-)OcrmMjNn~dI~T+01MwK_&WZ=YfROTCg- zSk!PQKClgJ8Z?R8AH%BnG(M}~GXjTh4qKMSvGvlZ;dA)BfKm^*=Nl|TgV76z4O78) zX@J}Yr90XotmyJT+DH-;(;aYMXZ+7|fmdAXS^2`lk zByb0_yS!)3sXBqzc;@V8Hf()o6<@*E6ns_S@XcAKxl7RSb$lZV{Q?8Ao+1AQt1>SE)PfVZCG)f;Uu#G6GwhdFr7Mp(j_>)d zQ9oz;^G=x&@eBM)!7tf#B*w66i_T?J&xcx*(3mqy>@b%!{2H(E+SO!B;GbzXi#a3k zGwir+(;LV;&gGz~&?Yxpj%ZddR-Zp3?k*6{ss?@!6zH4U$Xdv9@%5j`P$?@`yvUE9PS&4yI)SN3;Xld?~Ztd0kGl}^^b42u+-L(V#6$H%`a+cf_BWLFaY zS@Od_D|7f~>EvIPIXhf4YZ2`(&O2~7QG{%HIo<<^aiR;JypvtUz1erMd!}<04=!Ub z=lf>5Rxz{;P9L7hz73M>zm5ah&NUqB;&%(bZhmWcY#Ap>^~B67M%FO8j5FCaoL$Bw zH=mTOldC8!W16#*{LW-=;1a;`>2Ul6u!c`wM_HaG;*~cxh&+`NnTbRafDzRm;0G-L zr`rNB{MrM&+5#}%3c%?2NvP*f@iYGWx&95m-{SZ9Lpb{*{=(;ydvD@8-o`SiWXso& P6+Yokyn}b~H|+cmZgSKF literal 4021 zcmbtXX?N6Q6n@@P4NZgWDk{oOI|z|gbXbJ8V6o6bv072b%uL#*%uH&MDIhMmFQB+D zsJP<3;4&?Odi>-Ezxh@Fi^u0pGBX*;RF0mW$<1=_bC>sS{q3)_KLgl^pA|F+Tt47r zbjMEHp1wz3+w6j8SOwEj&?qo%*cdhRoKYCmyVJvF#uL!;Daqs5xt!?;Z0s_#Mg72t zr4Lw+>&d&El^$NDXY735C}j0?(evzr-WELGp0*EG#ghV)hwVeIXJz&a%!`N8cZ3|| zZqBCyJi-Xf?y4HU)mO9CK1eP#pE7gisNvZ(@R|sdTTHvuX69YJBV-_ZQ$98580oZh z=jwRw?ZJ0xZ$4!)Liv8EujmkW zaXf!-4O?Khd%$!?&31tWb!39?jx}mhfN==~dO8FW6G`;guGzt3$;%j0a!NimVt7L% z%mk)JU3ER==?Qo!qWa0YQs639Lzg1gYfvy* z!3_d4t6D21u3|lsQq2Z|g)Q-Ms(>Z~D>i9pf~w*sY*ny@&8CVQ=?X!vw#$0a&;}M( z^PuT<`rc+(nu}VZsTePqlpbu;FdLUDxS8c&)zsOoZ$ISNo!vdwplKIYso0KI6}Mog zf-ZrUD(897H5Dms+a<82C6d+^?Bs+d*5LT2_Gq{am#f%=y$X8kMJ7@#Qv0nF#3I7# zE4WR=99*HIANv&C!M+*mX1O6t^gG!t>P5QD=R@DEp&6QrdyrOO2wW8l9d>VtoS}d9 zkjqV8o*$2Y3NBGG2us0`z?HGUL93Uz!yGjAaKnKhVRIU$Vw#EqMikgJ(>HSBoD354 zUiwoHQw|Ra=xUgb87hi6pkS2c5Nl`c=#bc)o=fXt*K+%6?$a<6vlQG(DLUTugIZW=)scv!=gxJtpJbickIH6umEBWRVO zenP`sT&>_KPUslfNY;m)S;dohMn>W&CF(>XA{92OXEj`dd9qO**YG@E5SZ?oUb{J9 z6m#6p{H01@S)I05jYfGqwD!pcb6lnfFMH%g;8hK;VTr(H8M`oG4RY-Wo2ymGT7^MZ z#T$52!3kD54_P0n&VHTSoi1mjzJ-^qMJtyz9Sv{cZSFE@`6%C42IzVwS$xy~mk}x> z{*H$ESTGsyahH&&^Fmk^AK;XN4+U0V7`B|&?d55$;Uj!3po|px&STR^wsck3++NRN z0FwVOy69Os-3_OFW!&?`1g3UzTi#X7bA8+^cK~i6U3SLE^%;&OpTn1p-jLj>`}kA%Ef?mh8!h6W#{)PTeVNw zVSUOL=u=L1pK@&al+(?pY%M-zoA4>?*r!ZlYG26v1}s7&@41xmx)cz1(H6lke4*)cNi;e%3-R;XZ_=sox>kl|f NDjW5+^z$do`5PH)sc!%P diff --git a/bin/main/frc/robot/commands/TeleopSuperstructure.class b/bin/main/frc/robot/commands/TeleopSuperstructure.class index c2e609ddd26af96bfcbdce2acedaa0f19d64869c..f5b8661779710c4dc12a803176433bf883c4a000 100644 GIT binary patch delta 37 rcmcb`c8hJpRwiCP1{MZ>25trc1|bH)$%mLM7{w<429nZ~O_d1ZN>s}su<+FYE(cI>D)rERRVabhL3#cqP-nmVPxN?O@#t#;Mj zl~Ylm40kBdl5$JA7ihSX7OJEzAGLYImNIt6x48J7NNxtP(NSuJDg0(++H ztY*$>jv?o@#jv+%xB>^KwlE_p(6eM$T+hfBQjW1qd4C&hVsgtFm%Dn=O$2uPM9Fp( zmuy!*Pk}|vvz_FWf5ft9n2CJZ%6f)vB~o^omZr<4l4uJD;Sz}Cg2X(uwey(GrRMY# z7;eYZnyLa_PuG)D;P5tbf*h3wM~_|97qrGaYHGzyP8+yeVD^*1)~Bx8h|&gd5bC%pm!pRm6A>DiwvexLn#Gg0*4y7 zHECO}r&*pmFeBk~ZQ6O}<~8|wDTaG-zk>S&?r2S7BG6(&>UoUtqvDj*)uR!0Dk)vd zWr)cN2!Ka7;abocOwXw#W^7NBpf#3@VH^)g6ec+Pmd?p@O{h34x#`k?V8;!%*nQ-jUy(9<`leb z0~l-~Id40~z$G;55j-l5x+pNp2yS1OJkHxy%plFI@#JZx*mD(ui`zI{V8R9WIIp(%I|uk?+(Dl*6tZw&X6qr1Ara$$#XS@P&A@>t-U z2NUS63vX&Gl3g%RP_QI0vJImL&elv-VWLREGkQ+o#Ja$4dfqiwo+brbMM*+Jn3g@R zz@g7JtLvb%XcYFuE?&|6be{R+N&h2dJ*}crmo*>5GkCv(_wlN02;9)Cw7|QH58zqW zKTWxGzMk^DAGQd7osyC7oaj3E_u15FTq41TWyXJmM;lBH5eUZdF%=)jC%8~D%j~>n z+GlW+*!IcSL>R-T@M#6l2^?_Jtf6SvN7csJX7;rc1h>>zd9+=i~X@lKWNUhw!k*JoMW|#)Y@vwaM)zC+L$++9b^qXo4ii2_wRKAM;t^)|~ipdZJ>N~fir&Jx`CCZM zS8<_Vsp74-@W^~TvO@~1VycSi6E?BqMVVJiHmpxF655M|}?W;VP+K#A$q! z|4#TZ9z+#q@JmeK59It4lK3?&Keui|Uu`#L#ogm2?{bSXGm|GuE$nGkvTmjWilZ7Pk`H%ZmJ&#R6Lp-|{H z!fl8m0g*H)gl|x72aokFK4r3gk89t@k9f$RaODsM8Emh9I?+#u+`wye<5PV8jL)BA I1*_=)Cl?qo{r~^~ literal 2766 zcmbtW>r)d~6#rckvk)&Jm?~;VL3|_|VHLDm@qrMn*!Uup3f8upCJ&3;f)f5;|qte(%@zV>{D+>7UW*xpzY#$Y5s*!=A^z=iGCC_ndS2<)0sZ0&pH* zYiM9NFy-b9*U37*k#mYg)6RRwm?#LRl$jUq1EC?za75(G#(c>#rYzUTSe1%Tn2qBqg(6ul5yig z+A3PUAiHF`K0b0cL!lOE(a03U{HW=R>Sk%USTl^ej$acZ3z?D-c?OoEL>8+;BWuflXE>nse{%qgPCIm@>E3k;9qTOkm@ zw$+Y$jQ&?tiE)PTu#*>2gwVke!yblEJTb0eA4B3l10ES{q-vv|DE8uj^wUX>-A-n;KBHuJhT}z?WoR|?`B8DdY`LN+Y~N$(jo*D1qBTzg628{7Ws4i- zVph0gX0{+0+S5+XER373CFQCz?9W)_zH8mZZrX`dD4JLhxw21patXs4h8PB(k=s_4 z$C1Kis=jIAUmc;EkH-_6CunS_*eG7Yh=!|lYnvXwVL!(;yv)!>=Uf_6op8$)o+n$4 z3Tjfrbt;56{UGdG(?_bc#giiNcx++hl$SW z%h;{2VDCf>&o5*D61wW7?mFqP-m-+F@{^FCq~5xO9zD4PLqE2J6MD}Q`t|USI5iPU zW-2&c<&R1JTNTRyeb1`6_S5<8a#o|0;_9WS`mq~l&_VyA?8OicP~&w{>v!Qc4&olV zsrC=y9NFtZ}8ZdxD(VBq1qx{iT10{t5;wxbhfnB%D}5 zdSV$jD!5r&dgU>i^%(9@f=|#GBua~2!G_6sqCImJye{z|zB|xo*Xa{de>l*Kb^4^# zAC-S|^7B1rsy-4VCZsN*bjm(SVp8goN+(5Q1@;6{=_0X=QVit^r2iywbKwf!mQaTv zp}s}{0||Ogb?ieA;smrozkN84(**cUoTM3Zikj#QzM%*330=+#f&4Rp`y0XgJHht{ z$$w%{LC*sKtI&TyzPBN?Z&#SqAL63`XN-_i-hqTx1-p!oTSC>i8dO|SwIM}M`eo&J hS71=P(l&g8Psv>aJ|jmB_#9slmG88NFYyR_{sn!2%`5-_ diff --git a/bin/main/frc/robot/subsystems/Pathplanner.class b/bin/main/frc/robot/subsystems/Pathplanner.class index 9a2a44856b7a2e01e4993cb07942cbd6b1c3ef25..0fa0a1274da5706caac16bd81846d4ce1dde6b90 100644 GIT binary patch delta 2033 zcmb`Ii+@w)8OOhGlAe>CoTN$Uh0-a70!>>mAjpscw!om3Vi^ijb$%9VD213}nliy@ z-R354Zc<*np__=C&aG2XdRaSR?&fq?@1mPrbepJ~yHBU0&p9cKKVUhZ_q@;LeSXjH zdEe)}d7^gvH0#OZ!~22xe7M(oO{wzUNOAAmhHX>Cctu6Lsk%9D3e;IlpweVwZ)mJe zc!KTDnn{Xl+Rj{}sU@+z^Rn)As;Q;!|6+F2OS(v4ij9fiB(57hy zTGTSlWU3g!Z*%nVwuUgy53jpZDzS%6J#p=&;$qMt!|8*LS(>p^mEJ-eqHg z7M)ySvPm&XQWa_2jCr$kJu<#Q3CJGsT=W<{m2DAB{g zkQ~Xa+@>po)vI=Kr^y|P*oNfhWTHFQkxsO1%5`*Rl4+Z-vqKT+=vt9Yb)_;ni(TC1 zZ2E3RQM|g@4M`_6?TMDP8&Yk#x&V9lrpZ0h7c>0d;$AtMl__nja!FdEgZ6OI1G0&^ zWL8ehVg?V{?B!v7&~6RvW5A?eNl!&pO(>6d=4Cd6^yzHq;@G1K-=<`4y~REb*!++Y zeJHfk_&HCSJfXLQtGvJ9m-?RY;qc=Tc01ksTbrkNTK5(%^8Y5n(~9D_-X19lKIb@J zv^glq9~6#@z7U~rG~^YVLmbxe$a%isM?}~fnY!^+OB(x+HrKPQ(Buyhwnf9m$N- z`|s&!v_^fP=S7e8+(e;tEgYyBWL%ARh;pBd0vU#kAu9TrTr)yd>mb!b)bx`OZH8zw z`rKy z#6y|bizxMeCNr`JWhXzRw%Zm0^^)rKz4U> z?Ep9I6~*HgZ@58oPT>+59wo3u2zbOD?vb))#lAPdgHqcg|EtY%-Am;#8{J$b`Q<0;3rjzI9|u%Ehf&Zq zCqP}yALKFjJDB-q$zv`P%;uArJ;LD>(8u_ZfSoAC@jb?m#p2~B0^{MQ{7mLX`Fs17 z*nZ73JnKr&^Aaz+((gFJQCE76Kk>RN9pf$j>`H&(Z~Wbr{=vWb5AT!+TugxXu#Rcg0;%5!{#Kc4t}vNzTmt_r5#-z4w1NFRS`JUuJIkyZsSh z6n|_oKhv6n-I{mlYP%=Rw8*8GMxT;W^?h2G*R&3{Fi1D(tx=$k_U7bIi`7Mwi9sfZ zkZ)2%i9xYONwb~TyelWoAt_Zg-fFAN!jEatPorFAY2^hb<>dR(7+{fsWip5%27@(< z)L(i}dwOziGF6#OoD{3GIF!RwmTye2p$s<|rjfHCvN%!^k0k0U#xJRj)~900gvAJT zim#hCN-g*G>V2d}UR5L(kIpMhB@5%p$h^W@m&{}&V=THeR&DVW95jw9gYg>nUdE|& z7es4Q789sep0vLDB#Wb{QT@_p>QgMHGEH5S7S5QIjhAB_`&hLzt=czJW4IKaucqmJ zeX})+#VA&5^^88VnB(L-UPCV#H*Q?%Onr2&C@vq98AP15xoVp}N1taA!G819=d z5{gQBYG-;*y2%3K1`E}VMydB?68CR4MrwM!llv5v>95ioS!9q>VgCT{Vop^n{9`lf zO&U4fVyW2g_;(Z@!1)#zaG^%_yoT7~s4MUEXgrbCu{7O$AOz^FJ2gmjyN~rMFK}FTn})AGlA3R_k#>s?TGY9LEA%IM+Tba5K(N^R49}{n z;75TcY_4_Yb*se-yr_oQmA);ufTeap#>>vaYZk8y>sq@Y>s6ceUE%LoyvuuPt3Ac{ zwk_bmtn#K0O_}14EzV_G(BLDRWi~4v-xn4uSQ*s$TpTt+tMQG+ZLF8LuU%5tx7%Vf zE#li{)1qNbOeCTWmGMYpW3*8teM~$aizI5J2ES~b>Z(2%@EZK4 zwrA~VT1Zf46E=ogDG0UEb2EjZ4h{*o(x;8Sn+b_kE>{1|RA@DA46IpC`sDH!4ljF_ z%nn9|vyW(H^rN8%2!&gyoYl@m*vFduKkI0*+L?~XJtN#f)2whV;Xt63<2rEg2`+Bs z#Ikm3!TT_EZN%i2B(%{WV2J}x+a&T75uzVMrT%Q`V6OZ&kdwv7fpSJ1L=Ot-Bi|x7 zu9*Im$gxqzVKOD78Nd_JLrv!tBdTa@;qA&+CD}Z(9FP_&vTtwUPU&OA-abrsbu5`wx~aO+ z%UH`AJM@?dEIhJHA>)3g;k)2xB!`WC5XIGZ2D6Tu&9Z6%Y zcGQ_!Id)0Qvnfdn4bU{DEf7lR6Vj?)$%$JR+VTYo{dhy6O(8&dQ{GStH2n9@>}n;g z?D$J%OEdS*x#ynef6qN%{?7B4034Jl12uxiaW9kb+%Y#u_=Pe5tRL9Zeju~0V$$-mXDrTWnw}F}V+I8;pPsS2sXpsr z!8X0hvuCvmPR398>RX~j!4(Y|O5`vW2@Wl`S1u zLBUTu6L~AgB{BU*;iz|am2E1?5A@pELgLJflNfhAKS&hvPT(h|ZOiBKL`QxiXSd&5 z7GucHxfv%oD;RjKdFxVG2H+x>;EZ6wQ1_|6fv)4d-KRPS`qP6Q z{ex*i+o+bdC*A9e?c2NW&V;W4A;N_)-OJqC4csZ%zK9daT+W`bavk18VVa)remG;# zC<|{S?%ZwS^>_pKp0=iJvdCD$uoFy{nGsr)trnT31JB7%w8wD=+LfvY6vfK#Ekg$A zwCz_0kr)o)uz?Oit1{KA)6qmHT8Tbg3bFd!!1a<@1Kr%Pnz>U7Ps=Jb;E2FDGI-+d z+xPE{;~NOQPI|GB8Ique!f8y@EfPvwdPhLSw=zLaby#Z}nkBY5AaFKefCn z0oq%JlnEX(u@7wJy@E|sdM0g++s7== z;!$UNjIZ6R2(Rm%M8dZ8g=@Pl%DpbBAX{CH&=v9hQ={p5HPpb_3Il;cm=2Hn; zLUC1w()9*@p0K+T_d*^0#L7j?hB!Wqwm3e5j~V!=;I^w-9_Hf51rJ;<2di#-|a zPn!5e{E|S`MT-^N_Ns=dtd$vsf=`*a7V8ye&6)U2iA|}JRTM=N*M$@dCN3xng=+dt ziz-q@^}LDAA=PJ1d`?lVCsn$jz$dCN71`%aT(8JZ;#W+3L6KcYvgP%rqWqGHtyo`w zXoQswe+>Lu)e=MnW2I7Yynrv8_znD~;0EUH@tLd@*gc;8mV%wnob7SFj$BUA)7&+x zBXTL6F6VY7^Vc6rhT=2|j5%qe%Wy z1@dY2_D?4M8UIq^6a{g|^bBoXGpb9Ae>3s#_z!w*CRgBW>Q&!{T3+q7Ny+hFDia!2 z-S^)nzK#E()0i{SMT`|(6je*m(#|pFX6=ZmeJ%bEuNe4_;Fjg*O=>Br!jhNj58XLE z|26(+{Uo)f)X5qivE!aQUAoax+uba%u0jnlOld%4)MhC8g@Q?30nbaU`NTBk8dcCW zl?yuhrO(OpDq61WLQlDQc6lBq5JSaoaQKR#9Nzl|gQKsJd>JpVa zN@Z0fnITQ=v{XHM0?*C)iHMFu#bpOPqLS<~rCC}SGpa&z^NNoBf>jQppi<2s|sNipNN-H_9Oz?XbryIxSJ<2r7xNd?}?kAcsxq z)C@XZ%MX-X-KO-YnY9)4yfMgqEdNzkqkk1ouVU^iJG|b`hrJo$1++e_nOF~1(Ioul zvxUj3Yl&0a85C)^;AQL{N40s@hE0^)RBM8Gd)RdYpLUz+vx7-D>&GP}gNCFD42#VR zCqL^>*@>`+!N6v>qsPi{r?YH>$Z^3_m9313I%IgCmCy3>-PG&4Q-zuKm5qXmnI#L= z9li?rXa#-P$fX%qNZM)Ta%m^9+pn#c2O8uA6=f`0IVZbKccJdtCIrW1KCi;n@_pMk zWV8xkFT0-5dKHy3%lYzk#gJ3c{l4;Os90X=E^tw~MPbMoYqG_2mX`I|b5;e$x~07r z!qc+e7fF`Z8iq^|uU1&1nimXlST9_jl<^k24Bx{7v6jDU5a&NO4_Sq6RBavoWN$-1 z*_6;v7PzF{!1o$bZB*rKdAx~Z7Ov&-7LHk=md7`6%sR9@R_ob4ATPD+>u?jl+|1EV zKGn|}`3!FPG+$~cBv7xXo3LHafFkZdQ?%rmRsq;Nhg)92x{HvJ8yB#X>pAf$E~wQk z*6|6pFzV4v9a=RjQ?uQS7LI}4npQS&l3mV_yL^ia>Hyi>x_~{6AHdqiIox*XX>7QN zJ4PCx$6dez?)iM{MI0RAm&SeP@y3lk3%IX{qnzw3O%BWb9?6Tw&B*gdHm2E-r3K|(GB?aJl-{|UfbsIo;f@@ zj}NM0W*$F#5g#5|z%%psh2hqa!zYS(HhTSZ5$B`V`68Z+UN07LNxgFUFBkE}P59Ni z&*6oUT24KizJOmJ-h{6hrJ3{Tkve{OEoE_mp{ERL|vavvVW{dgQl@g#cj4Ekx>W4zuxfUh8hud-YC z7dVb@s@@+)B*xa=dW_2Tct~!>N!fuDvI}E8=(5_mt)buWdpfM?LG5PYUeDwAah8$w zIQRGi{;k!nUx%7kC}52N0th>Cfgz_||jyU-nW2?ggnSN_}}+VuA~@R(&bTx^P-H=tZ(=swmf&HyoXp z8&gHOX))a%#1<<14l3=?5T~&TQ_R&I%{tB0n54OUIx@fj3V4tPonrqlN0UCv=VLfW zlfIQrk#l%jH5c#*ihN$c+jyOPyLQc%(3mCH;6>Rc3X4RxlXHj!NurXO0V>BV+B~dRMg&xS{1bh zV_*GwMF~}!sNsYm2T&{hB(ISHImTxL$%ptmEUGbZ4^jAiEJPkUv=vYAd6Lhxyh+~7 bXO~!#A$BMD%<}m%vDrpU#${4YW9$C|(=4%O literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/subsystems/SwerveDrive.class b/bin/main/frc/robot/subsystems/SwerveDrive.class index b86b0cf576e8ce30b9bae87f5d21cae5e1066939..91e3361a211db5f653a85c65c93d8b7e83b8943b 100644 GIT binary patch literal 4311 zcmcImT~`~&8Gc3|EQmFZF~neFgB{}_39Lkpv19xZ3^raZ@U!HLNXZmEDzH zJ8hCSPMb9SNR!_5GFQ3lY10OL>eF1cr|0w^^s-mIY)|i7`poD9VHSMoX@$_v?99CL zJn#Fw@64b7{kyLKT)?Lqd;$YE?Oe>ZmMk~sRF<53j%yU1So*GE-!Ud_^Nyh*ATVs? zE3vy}Gj`Lo9apvr=F+WrEN2yq*-}21t}nfubqs+}anUR}Znl(T=UDPdTZ{>4MJrz^ z7>>a48#~RPR@1mZFD~9TOGYv4nmMjIo!pF&?^j|Q6PgKKYb>Lg*7Lc7Su)*=0{)?J zMj$X@<&8E7?9~xO3!h2OpQ#j=40|rSR3M?wq?OASGFjV{{o2d+Wy4K0IKMm8L?SU8 ztHMsqj{Q$whnuW;2W>1w2HiTI#sPtr^EHNx|6f$}Ug%1)PF3JQ6U=5G$Z4sKX`xip znoz1~WKY`7=59}C%gV&*;$5s$o#z)PJFnHav^4h9Mf- z*lxm7adw^K)ii1Jbwm&q=$)ROyq=s|oJ)<*q?6-wiP@RO>8ZJ?RGNu&!%L{khE+6N z`(A9$&X$}4HEosRc?R(~MkM2t0N zBkzobGcA{Xe*dp8|FY|?5E8hi;i^D%Go{B%$0iFpl9(p7yy@g@!!_@-9=x$T43ptq zL6#zz)$uA`6XgyQuNX$5>SKXXZ>jFM!DKk2VSzH=ITl-xNlD$%@pZgWRYuxy%a&Pk1@cdF$`)H= zPYTS|6%??elqE2-tEdWe)Ffoo-PYmMu-C&H-+i1|bgi;$7OBg8-4%6n!W0k*4NFq_ z{qle?bxdN4P-HI7o9@cxitAb>4F!P{9)W0-sgzYs^msmm63QAZf$&ZXHq|v9Z^CAo z;fXn3T9zkjWXRL?9{Z`9=<2B8j=&zc!)mV4)g z*J=$cJ$s3Na~{4ht>YcMTXl&F7f&m1m+WXtsYfIq<=?`v)44LYZHlOOfHN#-Ng z@3d91bH)`@s_&_4UaKSQIQpvC<3OqXuN5jtq_IsTF)IfD<;|74ERF$7~ z($7lC>myq}c>z&Rp2d8&vDXfr^Y*Ye$fx`S#c$B@Yc_n4m9|B_-iv+eja<};{k0`; zax}pAJzt>XPuTYeVj&V89$7;dP4L~XIObPYIuw*n&fkwNbSrC->RR-mm-A=^2icM! zdT@v>pKA57<;P*R{SaLo=0&o${U2#A$btFD8jeJ?-(%oQ9E&`{@r5sNVhzJzA{O~A zj`<&9U?J*X!}Dp`4XAFQ-VLg5u-23m3+$5uU#Z5ArSKMS^<{`NSNQT#%8l^vwwWKs9kuIDIe09-(;dAWm z4fMs6(N&B-@QJ>|pRND%A8BMeeGPpKN|cTy}g0%pd{5DjF0et6|bis1VwjH`)vJh zkF$B6L5UOfDLVNygE7kcz#0CH@>chvVslZ2VZ0HJaeM=dgw@LNZ?fg%tmlx$5?LR? zGsv+OU?}^Mhe6nq>p}R|X}E>NH1Npc_a?t&*-cBlKh|iTlk((zo%%~GtYLW-D;oj| zDKVdCwp~!j&>f1{sVnuBe@>5suV$4sz^UxJNV^^GdSzDb~dMFN8GQ7kfDN z7QW428O*n9=muoa6uQo6pA49U_nkVtzehI`|7j)X1b?p*(>3mLmA;;C!ZOgnatPmL z1SFON9$0RQ4vu{f?|EQ(@Hm$5Z;$0QVo4E8npjfAa(x>tKg37dV)+rV`0!(T#D|~Y Pr+msY{8@aAPtf}x@iHC#z}R}wnE?m8 zOwfH7{TKb*5B&wLyYC&na0IEMwb1C!z4x4b_TA^) z!$1D}=LZ1J;&&SA1iF{)RM^fh<=wEezT|8=u9gtXO~QS-dM^ooYr_gWn>l%+mhqL%l#{+J5`Ffw=cL?42srhBfYfi zcpSPw;7kEUQ^GY;SEr3NWw#MNv}<@$pl1&XJxUtTg~J*S2^=l29nULFuG8xsXlbnD z6X+Ibo}P^jE{@O6B<9Cv<`X<;g?lm)x0WJRkTbWq^Wb5ydy2s&{@MsQSMuy3lC zt-%F}_h}uUEW$Wmh9Rf=bOei2k)2Z^9RtOw6O~f}&+5>yPs5T6Rj31fJ8;fJREwU9~86e_AG`B)kR;u}i^)Mx4WW4Wk0VD%4a5uVV}s=$h(& zS*1u!T8ZhHD85cg&YsutDNHemS2pc@(zqtDH@LaHTW04XrZvO`LRC;zr11Ev;W(C) zSToJEqhkiM0v*eCK09fdne?0~`Q9+SX@N_9u~=+Rf+{cst1aBkHP7=RJ|oX_j(IEH zNNU5F*YP4)LE7BBmup_OFrQsn)r_i)?CK;~RVNk8KjcHzlb3ZQafwySG2Na1hpQ`Z zPm8i3?sv;#<{4FEs=9P!b+rrUEFfY6P0N)h#dieSl&U;dfn(JiRDj%}0=m$%>OLs4 z9I2f)7Ic+SbMgu-%4oOr+7(G~Au#&(^?yG9`?ecWA6Il-MTVDD+Hz91=~~yBv9Hu7 zfmm&_6ga4E@nQ(YOwe7!X8JxlEXcBzQ=U*>$UBy-Sk&sNzMAA$+!@ut&5^y+xKg*p zjq)B@*RcT>jsqV8Byf_~Y3)|2j)t{zP{XxIqlk<)gvGxp4V%oW61Jk=hH7I$u5VdnXRwbu|&>p>ANq#w(!(8dJ@TcIjY}6uNwK~$iIbW)F>cFfh}-vPvQd{>%Wh1 zatp_|G5i5emt#7jXgG{|I^K^C3gihq!|!1{i5UBvIUH6fB`89{h@wv*iZcb6f1w49 zbJ6ztTWAm6#aaH1_ibVF9zJ~s2k+xzvh;QvFO2y6-^b}r|6L?T0{xwVyLd^$@d!Gs zO!e@Tju6aILg`^M+>0Te$p|}UN&PtmAzFY!$GD8oDtJ-6g3qagh++|+XHY>Nk%1-J z8=~bD(%dV_=Kp^kvl_1}0TauGNUjn@JtsRtZCJSnYgz&wAkLZoV+E9+5L(r;AOr6w zq7bPYP{{hd?ImO#$kI18j>nMWuR5*@AWsxNzFor?3XWgnKB>=c9x$fko48pgy0qI5{~am|>2XE`RuDf1soFvx>Cp-^;twhAcxcnYs#lb$q2ejPW6 zLNd^Zy8ob2!xu#}=e~s3s$lu@qgcMWJC;+#a++43BbL*|65R#MHyHhHSYB6H*v1vG zY%vm2SNky<8AWx z?oFRK)Lwu4;lCc0*C>siXZ~Fv5#v0*7}Y*bSxr_z9xWZ>D1NFShZx+?@bdzF9q_e0 zL^}c+gof84`27pefWUjD3=R>yOdCh G==>ju#%bmN diff --git a/bin/main/frc/robot/subsystems/SwerveDriveOld.class b/bin/main/frc/robot/subsystems/SwerveDriveOld.class new file mode 100644 index 0000000000000000000000000000000000000000..981a5b1d501e9ecfb4c2512e83bb839142b7ebbe GIT binary patch literal 10902 zcmbta2Y6i7mHv;~lZPiumSh`)F~Z28Vi+SAh~*|*E&|CC9@$2wn9dH+=@jGuknurli@=_w)E*iZS4YMG#*Yzt(4&6YrChx)T4(H6zBL3L}J!x(2j&S zZFP4cK(>$ga|7~Ox+*)3L@MLq_^4$kj|f)IoJ6lXi3G}rB33jkxOB>FS3>^OXfSDq zgQ=0hINA1ZQ={7y{5hjtv6LN*g($9N)A5|q1LvGIY}vV47dJJ}a9-J#c-Wc)!F&_d zsPXY>p^3Axh?-g#iAC%Ug3_kuJ%Z|PN@r(!bkIun2M43XR@)s91*3a{$%uM)8><3# zFm#|Nn9%P&lwzTQ^94(PxwZ{lP+)gk;#v%Ls?jmK?7M*rNib&&*^b6t%3!mvl~_gt z+TMT7<(I8o;lmPKtXOLjENJTLzA1Px=#K_t!~TGsjKqf9oA)R=w3sMIg@NU?(A)}V zxIot2=AZ$Wn($+VU~W%u=ZgJXdUpo;J9hR5=o?xK+XCH@!L}7`tNkhMKCa0*B(h-J zeP~6S0=QDJf&kU(Gn}fRH(<4iH5nL}mtcbl1C<6g z(FHz$6RCwao7jR*!6J%gclXx){kuAL2D&@?yLxx-@7dbFb(fOTwNpGqyC|cCc0YfA zG8jum>CN#N-NT2C*ruekUEpt;%}htXT_)zhXW%NqWi#d}h@IwNKk&ZK0n#BL=S zI_u+2UU1H!paV!=)bpC;#lr`}=g9vW6KBCRu&>z8Q6w(1L9aJag}FZLN6^5H zf{RpONie7<=*%%I>F4&t%_lY#8TMzw0iR6_BE%>*l#GvV-`y3kY@1F`aVyo$j07cY zVc5Wspe`3ZwjK^y38gv{BPwiGCm2m^9R+E*A!;y z7~oc&NJJx6vb~maPMbIgns!Anq!|3wD61%!9@UgfaC0qpgQf1+o_68jg&k8 z`EWaqDg_;*oSHg2JDc}ZY^u-n-G+Nr-+hAJP1BiJKI!%u4{Y93i!yx7!~=LxU{d)V zsm<|t)Cw}Wx`;-tO|DSEZhuzK20kHZDhyo36F46p#-j!v;m+tKs9@TT`#botIUR|H zt)z*^@Hn%XkoLuN(h_NQjkpk17LFI>tcJZhr=6K+9mRy2GYrO$Z8VbjAqIt90R15XNCy#%hM*hl1jPUP#34-XT*iKpJcpu+82 zE!`IO;aMzHJJjOMbaWb)k(ke+^w=3k3)e%Q7POPO?*j(*>XK!P-c%r82l<6 zE2A|R)Af}7fH|{p>G_W4bRL)u$?Pi+t22&D#HQU73v-jseG22NIA!29LGyG{DVS(t z4C7>lin>Op+ntKa>n7gFG!OXjCf+h|nx^1EHg99!G7?OsBB?;avcf47Z{ydP(W^~Q zpD%V&9Io*PNDrim1VP59qnc@^%nw#aDII6O>ubcP=zA31;<4zv$Rsc>vBpO;hBF%g*Jc)XKC%EYLQst1{{4 zgET@W$5r~In9&W}m>gAw@)aq##A|^(kOg|@66UBr5ND*^HZ}TqAA|EmLmIW(33Qyx zf%Eo=qDZ(SZaj9&54OKlf2*>HKQ{3v_)})j;Ycc!wCu<&bh7JbX9AtGGel-Z>+O6>0v;#1HU8h8DV|PV$$!nPOqhLPsnr!}0k zluxxbdCSOSKGp1_Ph?Lrt)9B@ny6ykkMOT1{!MYBjzjMHN_hWa;y>|U6dq}7;a#90 z6_bf7LJ&yl5p|ZDF#sx-i)PMd+$Ho0%1M5v6P~-DsNLviCVq~e(%VQ#HTPIH^F$IR z?5Tt~o^B-~td3P>b}x6yV>1Rl8$Zmj)b8c`S(Ui#sF+li(EDlQOvZEXBh&fLo$&;} z_hGiEi!;7Tr73g7$M{N`c|nAtTs6DnY$jxH6w{O{nM;y(ydb!#En_xPbSTa)W-wGq z4R=qOC-By7QE5yo`<&ml&km3wb%HhztIorbzfWaMheaisZ^{B$D43VkO{Q0{=9lW) zize(@|I;ycd=eIE4~~P zQPE0Ae*@n$mKDwT;F=F$;yiuMp-e6_ zrA^LdsB-+!DcLGvtuke`tZ{On&OF4*N4>1Jh-o>(QsJhmfN#cc<{S*a(-j~bOsY)J z$|4h?gq4iM!;z2*uIo+NARDQLbRtZ72#$Kf$_BnXXZk<}=;?u3>aKYG^Fc{f@zVWe?Z*p($()H!Em;rd%z%l=G%-$1YSY zJnsnPmcl!m_f<+iBSuOWtQ+F)twzIPb&fk#DSJKoj;OxZsnck1bTAxjTq(HDQ-OJQ zRS?*{PurS7U9H*1Bh64e$qS-zN}mQrt~2rb_ygs*`n*HM4CM_{1^A)|=QBy>ps&)W zuzfxWiDd|F`ohB4l+mW9kyH+$>VGmM!_G}iM>HC7?r#<_4&@3ly}D4S@vO;$I9JyIih9ryDR11JMX5g7( zOPZ|ivdChwYbx)ljNX;3C%9bJd+|uqD!d>%lE_>Vb$X0nH^s0$Wt@fHzCj^yEp2tB z+(sL4@_FT^x0~`2xq~9)w%~|Ju*1X7r$&IxnqF6P4jmN{VGU_3!LEC<25+JS*q=MQ_7`+ZJ)1^3SA=0B_rQAEnT~tj%2F9`ARsH@l+e>i{Kj-STmUEp;NW+hF*{0?#)+3~b#CVhLkd*hh5TQwR z%Q((!nZ%NTnx@*uaa?o?%`L`hv<{R`s*jVnWE__T2Flc1*-5O@Z{_N({3I^dZx!mT z;w09pw^LY8avKLuqGKFePhkh&t{iB2jSpk!=3n19`cGjmn+M3nwWn~MbWh-h?h~l& zSw4nvtL88=PxnQJ@tbecA3aOArA zWlv&3>ljY*Z=y*lXabWbP&bJ;2eQrM_}bcXrNBkyV|Zt6Ma!a!F?{P4&Ufr?iKcY} zEqp%>>_$rG2DDHnt9i%Sj*v#aL0kVi7dcnPckmlTqn^Ht-_&-x4!?!pCMPYl``^Lu zlHO`k{yj>lLi5ER0-fD!uxckN%W_#cKoDglv#_-u-=DxA^(de;qjz?;T;ifE(cYo1 zr+^`HK1|OV!2;f9Di3LKre~2Y#Gi3(Mix5ApYy4Nqbu+iL{Z9C-nF|6U%?)Qf39OW ze>sM~p1?mSZ*f1==$ zx}6ABSZy#)OKo@Q`V**aT}-m;7q^W`ec)J$EH<8f|Gid4*D=urG$!B9oliKa-1t?i;9v47RottE@3ODq9%UMA8N>AI-{uKk_VRareZn2IF@^f z!oEv zmMa+ztSx_QO-~aJc*M5jpAEp~TO80&Y z%Nf$v@OvYk$iuuTtNcxJxyHO4KDk0Zu58Z#$s#`~ zQIQo#g702gJ$lz5(Jn|&{ZcIi`O5z2Sm%UiZ|LiWy z3t`GXujH>*uA>M# z&R(r#S|_)T$%o0>ojKN=2(*jIML@6RM4;*{d)1DslPI$^m*_wt(R&@D_vg&YA-ab| z_ZA`gpnRN`kzs}Sf}GG# zUz8W+EBfgrc|}g@r&nc6#`V*LOv;;bn$(u^_oFxSjQ!T8MOZ3t%hxF227bT83*N8G JH|1R{`hP%AJ>CES literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/subsystems/SwerveModule.class b/bin/main/frc/robot/subsystems/SwerveModule.class index e9f58e565f50f80c27567a2141a29b08b9d0bab8..ccc67fba5755791699aaace3938730d55e675d6e 100644 GIT binary patch literal 4309 zcmbVP`*&1V75?rdWRl^K7fFf@P#!@hlfp>A6bX;g43B8iKoSCh()P~GO>&dT-07V= zLGaZozO7FbYpb^SYJC+nnLyEsFRJymmcRK&xLp3EYt`lQ+xO0k^n&OQnRCxQ_k4Sw zZ-0BAo!9^U%BukO;7>sWG_0Sn6H(iWTTV2ai)YVf9b+~d9XVszbH<>R%%zMVsx>rD z>vMWErKcyOL-A=N;b>6D4<9%%@YwMFSpTsR4O&dYiaslyb@a3|rl)d7&Au0})ExTf zmWvt!rlsMYfwHasp{R%6Kh&$C#!P2&PFBNP{Xgj_e%tSU#~sVebkcY?t@hKmq;1X_ z2Mr^6(6Z0yc2YynfRW5a&t%N#glT7;=&bHcMH5!qv8`0fu%jd9Y$nC?IhHN^CcJ&b zT~We{>xmf+TmB2sk^UIZT!7_%<>c(NKziF(kg$|7VmKMgOp~*)AKav2%^Hqyr7om zqort`JI8|9sG*~5rXrwAq7}jhtg8cq ztDvw#p27;PXp^Ui!aZ0ePxmXl#e26^A%x}f?rjS7Xb7TP!;b$apdoC-4srVJJZv}? zi*=3(>;npoXbR#T8oKTbdrNpj*ohsof44$2R@Ur$Fh2O_zrXaan!SI^H4YwIW7h1w zeDfdAFZ}gs`6#B)!jEp$t$$-Qwq;cihc(ofLpVr9);w5P6yyPgK|HLX)ky0MuBggP z=d4`T)jSnX|2xM*L_VT$6vNaSqy3Pr&q$sc3{&`|M5|%IOdAj9X5)rEDnJy;fR)fw zW4dk1yzsKxnKGGh8!G~ToBXHDYBPpyT1hjp9OF0@#7Vm88+wIDaGIXwGD+PrylmCb zc^i2x5lXK_X9FHncpPmST14CX<5??}bBrTa)|Au;A&z7a3988_az2@)U|>Q+XwqE=Gp0f%Lauf~2JW49Ytdyfqx`B3ua0X|CU;M6SiXNCpkZu(Mv! ztaLJjGk9L%1zge)oRiynh|*|f{Qa((WxUiyoZI}=eW!D=cC2J1`=)eTPtXDLoO{7Z z7O$hPbCk5mF6&qsk^FZRz9*Hfsi>qv3uV&&|FanO`pYO3rjx5{P6;@h828+Knx&t{Em5WlBl%2!K8v(oUU_ND&T zt`O_#SruE`5bnh?)NmH$EPz_x)@&&F+$;@~k$ot$Kd1Z49NPFUrY);&f ztqDg(%+*-K-?g0m1$C~ynaCpUi(JQs@kMkjU{gd}z-IaBl%K9h*8;XgRxcpRZ?=us z*Viv%`vQ76+v&}AEuhz(wO~&R_O)RDBKWQ^GLOC%9B4s*9`A}&=P`7}b!LRWJ?=s7 zLp5D(Ks$Sl)$E_v(6M#cNGiho?Lh}(*n|_<%v&&wX>?+gd)Ikx;~2&`17=)$H1|n} z3Gjf&T{?M8EldKJaqSY_*}}g~*Ki^LK6JgtV{$%^NAu9@FFu1+k;2>?Ci9pU0_L&# zCJYX;T~5UBKdBvS&vJZ@^B1tcf~iXdrp}SaPq@N*lI(nvPgUFnj)9$TV2Z3gjZYWEahhuZuC3{c z(Bl;72?})FH7L16sDeRFTH3;Sj`GYx zU8|UBzlN7wwaL@>18$5x%Ie)iKSJ=+1b?*R6s@IGwBlu4CafmcdI5f%;9`0$;VI$e z1=22aD!P=nd0dgucv|x8=qx62=0>u@`LzW*q5(3~Kv7X;^59?s3BI5}UO*GgyVF3?}=@p67CCub}6*B1N;;5ROFzs2hun~88Q PeuqC$PaCMFKcenG=1wk1 literal 6036 zcmb7I33MCP6}{g!nn;;Ai6cTBmJkv^k)0?D1cE8Xv78|AhGlF5v|%KTox!q3of#!0 zl$I9K4NhrG7ogCc0;Q$k#&U34=)TbQw1w_Fr~3}w7Ye<9Mv`SbHnwz*{>;~V_y70) zTmR$FO+E_XC1SmT8iBP#Ryt;xDbtDBg_M2JcJxs@mOP+a`}Hm}Q^@KH>I9ZuqwUvX zSuHml>q%XsryT*+`@6d^n$j%+kq}twO|_dj+tG54EzsO4@pmOVjnvl7ThET!F7Pz` z3U!M>h|B9%m+6>9Jhd}zj>ar~zq>snZO81qW{t#>^0!O7R?=rIL+;b1U+>bV9ZQep z_nCUmxORJNpJrtaXq@TSvSx1A<>Xo7uF>Asoi;Oa5f|9lsb>nY z19>AhWLUNn8`YeBv9y_UEHj(ctr&?YRq2ha!K`LGZMosBE)enK9oEfJ-LVeF`b_sRHY!%%*aF>piQfya!ruNOe8&%>B;>TJOI@e(nskI)|Rx{b2aI5Kcx! zZfpakjKt${Z*NU1mSUNLjRI%*?8rw;AvEIzDa>XS%h8}-LK$}li>@$%GVh36joJV_@W5rJMK&QT6&#rf^wEtjUS8+bw522BrR>9tv)ouDo zY%W`$3DFY57PLwQUMR4Q*0G=sw0PRysUnC_5Ni-u(Scn8jmiFwj=sHJJ^ej>d)s@u zcO`aT*4Ngb=;@X=@4p7ke#jWEI3-V?uPs4bgo_n0CT#F$bDemnX6x(wbX#{AIH;FP z&?yieac7d+klw3V8vR1IsG+7=*7pJ8>fPv7&_k{lTfLXyhFx0!QuN97yUo&U_4>2P zKec`plzTq?DlP+EG3cqNVA1>UiOeEKwHl>bN_w1ydTP;%_F7()w0eIVW>EKa3@}J{ zSe9wgs4o{dO{#A0XewRl3Kdtnt`V<<;zUNlO9|i?Bl={jp>SS;ScJU-%I^Nl&)KqV za|nabq)n#;R??=YQclvbjNEX^Y%?krLzTosDu%I-X=hlMkhHskrManKOT-g!`u05? z+vQwP%|eq|gKJcbAWJuh_az28{M}%)tGIdMGLX{}a>y$%OHpE;oTjDXSRZoqpoY3iTqQDR?C_)J&-=#Xvc$OZ&Y+ z#jB5HIcTDbc*qI+Bjr&%-PsRHs z@X{Ah$kY9xj2!m}Y@IVslqH`!SjNT>?#2BI?(>zCx?~ixV}6{{p+78esh_3K&NmNF zrih(JPWKA&ek~=d%!W=gtz`!^%aHG-$vS7B!A!c&ttIEI`B{9bqi*$IF7KCmuT(|3 zw69NVIk$xN)FZIP=al9aj60`E#dX`T^o)#6-1%9)1@Z_5WR)$Ofa>1F!ejWff=>yY>04{| zhSB^23ed|$;6Bt?)u zkuGFuXVPGPq~gc;iEIcNmMe9=x4D79h0DBn8+hz#??G9Q&lSnPJ7Jq0IUV3=N#*R8akb1LsN_cZ(ByzA|< zL3l>RU*wi*DIihYenP0CM$}T6Af%KUw*%*aC#K~;pLN*8lG<@rGC zlbOSroevzQfr)w*f5$&UA|RHrNEXXkensN*6;p=DL9s#=;qn>-Iuk9Eeh8Cd8T~G4 z7HCep44FNaSI5{*(vNA!iCj)*XU(>CTM?@T*2p|R%k_+GVJJ&g%R%p}^eT$ror7DT z&B9*pc*#_0n-#H^2ItdLFw{)%DrC-g@0pmHZ1*AxYw4l2^kao1uRr+2z5|Om*2pH< z9obd5BTGJa$*1{!Ry1gWgKskPo#L; zINm8A$8hI3-b;uZi}=9CB0eDVLgR9ouZvV6-4Cq3|1nFMl`WIir_SC zdIJeeR>G*ZO(cGN{z)fMr>s7D`@SYiCs+!0t62;-@x zNAdF=f#|A05xV2KAnXmdkNyNTupG63CjmAO}M|k z5U!7K$vNSK3{NGva|tfbx85_he-`myR%Q=zrp8sNclrY^p6XGSwkJfLIIe`!Te6bX z$3$Qck zzYzUuqH9D?5&dc(^ct~JN+?bct2jn!0^OofoOod)F5-9+#~tEiv4-PS;#6@Ok9rNq Hb!hxQYhwGV diff --git a/bin/main/frc/robot/subsystems/SwerveModuleOld.class b/bin/main/frc/robot/subsystems/SwerveModuleOld.class new file mode 100644 index 0000000000000000000000000000000000000000..0cd949007a95289e6e31f968e55dafb7da19bb91 GIT binary patch literal 6045 zcmb7I33wc38GipH>~y!&G^9&u3Z)!{CY!X|gR2DErrEYEYZjPm)5(}DFrnG zb=PQnwP;$)3`e^MuhEl^fa+0n9lS|NkprPZ)6RoHQQ;+45xL0MnB$R-5k{& zYk#!YbTr2>GaFK53tVVq3}>^zy2ki?{MghlP}5H9La4@xDwbfGKy_nNe-Hx8RUC(a zd^%Z$f&~gr5on%=Jqk`0IAany<@;OZ0jK;P*s^u7^NHu~dm*s-nOt4x=t5}RW1#1M(^3ll0OChx2L}|=A7007q!3Kd-W`|7> zXX9K2=LkgRj!hNkAwbM0%J__@g4l#s1s4zxvz*vr?(5L^>giQ3O8fR_brlyf0udV7 zX65WXY29X=L}$bLbP=r~tj8AVz^wusiH>m^ZsWL_Cv;SIVgGid~FTlVqBttIibm)%?;vhnys(u z)otBj;-FtHMTbCm#NA0~Lwb*9X^acqqKBGiINwJQPwzsHf^Kph>+~K9H|*m1m!Vgl z-(?neE6<k$zM7j6Vr{Z!j6oa0Nau%ci?#3C+sC1)rOVN)rP){CO_Fn5TNh|eh znnvBby`M>XyJeXcL4Ae5>C$zxXHywUFH>=)8yc~4Dvq;eTulLfIigoq8ye>ohy~a! zpzQ3s;=J`6*M%?uO=5abU@0*@S#lDNWn_knuuZ902vrgfsTjr{mYrc;iX`rGmgbg% z^^Kl^Q`Pr;Y?o?5B?}?564$60L7HI@>y7ts_YZ@0uH)t?mx-K6$RMl0EM|#0a+;Qk zSAx~)I4zq^?-%ItfHOySd9a@c609ycTb2eI>|n2g9Mjh@#cL&gL#54lDunBy$)a+TibbeZ@Omcb>EcRoDltytiS7^Kpe#%W@g@~- z#?7)_(uOm-lh8A^Y;L@S?|+?@l%7z#HL&HEY~1)Sh+FY?1#g=bPd*Yzg@+9<)4ZsmHzO4 z+^OIWfwj|JRl3R2p%yzxI;n`M58#6gx^^>{VXALWo5_)VhOO`JF>S+Pd&lBF3(ovR z65O}qLn=Nj1us5|Lf-C2W#+hBV8g6=qGb8x#WFXBa1ZWNaIdeO^d+;99rfds0sV1- z%lwc&Gv6E{nI?7^8Qp8d`?NvXW!86?NiE&4S%!QsZq_(^3|7*!++OlnZ9n@@b<~{! zN*(@U@0H#tb@#PNE#r34o`(e1`&`uQwo!SaDyG|prKeJ7{_5^mIZ-1i}OoS;zr)HPeVDY$RH;083jietMg=4@mbW%)Ga&y5FW+n z6?{%$wQsxGof$Maq*SP@;tTj9=M6LK7^B7kI^>r5JH*dipZ=LuVxGn^wkGgp6_3db z5Vo_rp6Ya4G`1HBotMq@`Bglw;A^~x$r9el5lO|@@eNKQ+8|L_(->=t1-9ID;HjMt zgs+o*{I^wn2Tw2)^Y&u;u%+uH&wCIc@w%#g8RLkYSO`rHL~Mn4hZn8GbHjLT2W2Yws!7AJ|&wrQE=y&w3A1kzcF$ z4SvhpO{TTcY-xO^ol$rwX_49T)Pt#>%Ca};QLc4QLHpfi8D63mZ zJ>!($3HKyNk4y-XYS2Jab91u%X5iXt4 zud~*&?uRfT>KJzkGeFk}GTjUVh#?U^7 zonz=MATdCa0ZFo}{^jF%MIK2~Bn!~TU=%PqAm7Gd7qDMaUN0$ckd&{>eRD^( zG&di@mk#4AVSMxUI}oBZ(R-k9^Wi)mF47h7ExEyEINo>|PsT|VX)NG}n`$D)@yGgMLB`4leix7GvO^9B5u&Dn$8sd8QF-Tr_p zPxUB^>IqRJjw?#(DWYWg5fRu$Ny&m(KuKY-a3oR?i^j#`Ns09?#;)Y=Rdnao#O;+8 z(wCQ{%Pw)6bW9Yb*H4jt!qL(-N*|>3B&BOU(yPQ$X`wh#EaMs>1iD0nIB9DGF6MeN X*G*!DSjqJ&ajH0-SG|(!8EE)FKJfoQ literal 0 HcmV?d00001 diff --git a/bin/main/frc/robot/subsystems/SwervePoseEstimator.class b/bin/main/frc/robot/subsystems/SwervePoseEstimator.class index b5f5de0691193bb3890b79862011636fb65a1ed6..6a5f16c6c3e982a63d8d7ee06ccde0a90b541881 100644 GIT binary patch delta 2509 zcmbtWdwf*Y5&q7;?%kWS+0AB2NJvP){9xHl3=f4wo*^3`lE;z|AZjF=>?H=0tY+5` z@Uaw7@j-!}wraJYwJjBpf@U`hsY@_Y{XpmJhJf@>f5|;}X3or< zIWynP-G`=aobEV!;_w3iX0rkX1_rmNU}MA+M%%%%5))CxX&gnE&QR#C7ROEbe5Qsn zoTs3iL}G`juubmIWZ2-YS=HNK+8b&MMM|sdOKU^H%e(aI`kCKfEDjh8#XggkF`FX| zT*F*cD454EjshHKmI2Hc@0*IbkHdy^1q&FaiQ{UK*rnK2D;D8=1=XU}?9N_{8it(O zdS6pb)yky}6?Kg@6-%q@#V&KXT7^0eH|oW!=IPcY9CkPqG>QU?H@gAFRxm8%$bi#H z30uL@j0;4-vdnrBMItZV~{5!>E$rPq7fD0xEy_A zhdN6wLqEqltQXIyjg~7puEN#gZ0mfr4I4OqfQ_QxdKEF(a9m3}Ut70KuyyDWe^o;$ z5{Pt$dSpmjIev&fhBVqaDHofxT5|p)4uKzwW7=X$)lWHY#x25|)}P7nGmab4r{Q+& zRPb{KuUNvpRF_@2L%f)_-*OjzrQnxhJ0ETQHFk@=yxe*>_A1!JFh&#@#*k_=_6eIU zPi?_(IS$~Um}1K=Xu*9P18_2!13evGx`r0SWx56#irqf(q`@Wbu)4%Ho2Bd!$6*XH zSldHAJ$idY4>GU|tSG_#91lnzY3l-O{h@GY>Z(;Z!tr}NBp$bQTOP)v3LX*j(%lpP zh=UB4VB``#6420qKgsisGML=HNz0_*FC35KuVP#J#gxk@IG)5);^XulPZ$2q@eH;w z@UYAs^#Prxt9X{eU(l&{1vNZ}7Zf}%w%W^#FXAQfnEm{-=HeBOS8+9iEJjTrqKA`q z8vcdX6}%=>wB4Rnvb3{Xr>eh!HyIqHxxl}$LdwFCK=-8#M)xG2MAO?G@8Dh0@9>rW zn?dVX*BdHy2lb9gGW7p&d?4*LviIp7y}BNjF&*dlFa9Uqb4+9(i;9e#QT-ALpK^RA zl})6)Ld6%flcxsp701_jQ|K89g=vf{EKL+SCmL-mUDP>WaWks}`v{lJ$(b4J?F!3q zU_IrenyS)U*%b(f^{~RSMN?)u4VZ129#cNUSP^iJ6)$8)m?r$LolzYs4Af?icn0D1 z456fX5T!9p_81eGYNE$XkAL~RUy;u^?oX$(ys&tWVF zVz|(w+>e%K;}9)_SlQTY8llW1lw~M7yd6U;d1-6*43N74@-u+eAq2^E)gU@zSWUZK z#P!4w@_1voj1*RT;t11wRvc@IydsY1LAr?n<7w?h9<3EHQ6Jke0S-)ri|{oXlW50- zLXu5DaghvCCnbYxa4o!A78I7+M>A2dCf93He5kgPPx17=T@?s|YEsi37EurTR| zCu#GrA0|i4u=UhoYgOhTj?HJh$>S^Tq zPrI0FH{>2LsvTan>Ok-sD0@5Xpy z{KauRSSQ6zvL{L?!G9RUV{!ah_QrQ18^_<4lPHve?$geHNG5?}^s0>G<+_qbkuFV2 z68L8vZ#my1#c&+&C-C8N=SL~4Pm;{%DdtPb$PO?jJD?BwL`PfbL%kBYXeHzY==ny{Lz*3jC8Zu;v(4{pL`^quL&9)xipp?Qd; z4`MBjVm+S46?h$2;yB?{)?yA}9yIGVMU!oii(@!Jy(i)SG`=CqfN?m7x`O5@xyfP% zW+Z9|Gnk1`WuyQ>-*xi|6e+le{+ag+TXPlFQN#d2U}xh#*3Mb5WezA=FS delta 2260 zcmZ9N3tZI45y!vF9rwRG_J9L|fM5xj2nQ6ER>cPaB_=8$5#*s!igHB^LWRSGw&r4z zHnvIAHrq!`TT|0CR-2|8xdWw1Y}zJm`pByf+qAXsHhrb(yJ=!FduP<-^Z9>vXJ%*S zKeO|j*&7!;T9f(2nG+8K%T$g-ilWRksF~&|SK#QPDz`c2zQnUYF}tkX)T=`Cq&qcx zi4VmhO&Z$IVwO5AF+-_ko@I(l5^ANHR^u{FfOLljMUC0%&o-?oL35YN@cLN6*J)r72^zqPooN@m^$3dwk}8PmXTV zWRU65qPSFC7nBNlI;+cGjCTr$cvYvXKP474s9(+~8cC*_AZ7 zS~G(@ht1}wv)KI(MTuyXWccZ!($6Ng+9>)=vG=5YrzW2Qhi!^V6Ab2>N>^^FpZ=ty zYcw+{v@z_^q~h@lHLQ6TJIxZ`Qg02Tnh0Yi>TB~{tGSMGbIP~eyNUN`-pg)N;@>UI z4Vpc2a?rombf&Els}E>C$gSqEF!g*$b2}e4ucTe)`zXfY4n;*Gq542xs^(*S+)U{Q zJa=-J!#*<@$a8&)Pn%l;wZ8p)*5Pi&Ow-`Z6xDVPnCAn9-Y)Lde4hJEAbmzb7hljE zB1@6lJJdfI_S3}|ZT|P0y7Zbw4`{y35ruE-@X%0rYb4yKP#b(y@u23IHA@@sy|#6D zbRaS?JmjaIm?qAoIh4M`cbsX56N*x^C#$qv^A+w>c={sS!jWEEy@&089#Oc<8p}JZ z;H#QP`I@;VW3yb#H#Cp&xOqBbsH&fDX`bLFg&qw@)(vkNj>s>=UcMt?uN(*u_WAi9 zPdPkkv6n1UMt0kd-jPUS@94HI!@VPYnx}cj+?%<&croAC{D5&qpg$aG>Wzd)6Yl){ zh#xzgGU>r$*H3uP%nQzxIDV%2Ij2o~u(A4=3V;9j$Z&C4U%0>AM*M5dZ+Om%!;!{t z|42AIYD4**=J));JRY2*{%BqZ7H0MEXU$)%rdu?-yu2Xd{Iqe4xfeBmm%lY+O*y=( zl%u#KdyXqbxlAbgbQxzdIUvANj^-uCG92Z}WK3>il@w%T#bEE~Xn537+RV$Tb!SRT znj_go=GGj)TZ%I)!NhaMl+RRNv_IN_mlSD~SZI=p&@@%ulT^o;8=7K%jD@n*#;B8n z`tAo=KFO6a)(F=uYHMS(hC(M;C)RX?oQLS@c1_DN$%eLW_cZXm|?P(_SEQ3!|P49UJQ&WMn^;=KE)M4m#4ted5Q zVJU9uzYH!VmpK$sP6Z(r$XZV&4Fb|Dcq>;?%|_-DW?ocUvxYI=#yD?hx2W!6A$vu3 zy9~B{EMq@)OmG>c;yyQdRa}o>IGeV#xOBL(4KC|jL{^>PA=RdJL|o^u4!mdMP~6B(XYHHh0&F>R-gI9TZ@4UfLT9zi z!|0j|63aQ4SPnPyzC}WcRoC`s1oA*3c5?#?M?Rg#~ucO+<|HXd_FY}7*0wCqa hrBanw`IPOx0*PRSN>c%Oj;pv+rQ=f>DyXu_e+?XK?}PvV diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8574362..7236c70 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,6 +26,7 @@ public static final class RobotConstants { public static final String CANBUS_NAME = "GTX7130"; public static final int DRIVE_CONTROLLER_PORT = 0; public static final int OPERATOR_BUTTONBOX_PORT = 1; + public static final double DRIVE_CONTROLLER_DEADBAND = 0.1; } public static final class SwerveConstants { @@ -35,25 +36,36 @@ public static final class SwerveConstants { public static final int PIGEON_ID = 40; - public static final double STEER_MOTOR_KP = 0.008; - public static final double STEER_MOTOR_KI = 0.05; - public static final double STEER_MOTOR_KD = 0.005; + public static final double DRIVE_MOTOR_KP = 0.0; + public static final double DRIVE_MOTOR_KI = 0.0; + public static final double DRIVE_MOTOR_KD = 0.0; + public static final double DRIVE_MOTOR_KS = 0.0; + public static final double DRIVE_MOTOR_KV = 0.0; + public static final double DRIVE_MOTOR_KA = 0.0; + + public static final double STEER_MOTOR_KP = 0.0005; + public static final double STEER_MOTOR_KI = 0.000015; + public static final double STEER_MOTOR_KD = 0.000001; public static final double STEER_MOTOR_WINDUP = 0.0; public static final int STEER_MOTOR_LIMIT = 0; public static final double DRIVE_MOTOR_GEAR_RATIO = 6.122449; + public static final double STEER_MOTOR_GEAR_RATIO = 150.0 / 7.0; + + public static final boolean IS_STEER_MOTOR_INVERTED = false; + public static final double DRIVE_WHEEL_DIAMETERS = 0.0964511800486235; // meters public static final double DRIVE_WHEEL_PERIMETER = Math.PI * DRIVE_WHEEL_DIAMETERS; // meters public static Rotation2d gyroYaw = new Rotation2d(); - public static final Translation2d[] MODULE_TRANSLATOIN_METERS = new Translation2d[] { + public static final Translation2d[] MODULE_TRANSLATION_METERS = new Translation2d[] { new Translation2d(WHEEL_BASE / 2.0, WHEEL_BASE / 2.0), new Translation2d(WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), new Translation2d(-WHEEL_BASE / 2.0, -WHEEL_BASE / 2.0), new Translation2d(-WHEEL_BASE / 2.0, WHEEL_BASE / 2.0) }; - + public static final SwerveModuleConstants MOD0_CONSTANTS = new SwerveModuleConstants(); static { MOD0_CONSTANTS.DriveMotorId = 1; @@ -61,7 +73,7 @@ public static final class SwerveConstants { MOD0_CONSTANTS.CANcoderId = 0; MOD0_CONSTANTS.CANcoderOffset = -0.272461; } - + public static final SwerveModuleConstants MOD1_CONSTANTS = new SwerveModuleConstants(); static { MOD1_CONSTANTS.DriveMotorId = 11; @@ -69,7 +81,7 @@ public static final class SwerveConstants { MOD1_CONSTANTS.CANcoderId = 1; MOD1_CONSTANTS.CANcoderOffset = -0.123047; } - + public static final SwerveModuleConstants MOD2_CONSTANTS = new SwerveModuleConstants(); static { MOD2_CONSTANTS.DriveMotorId = 21; @@ -77,7 +89,7 @@ public static final class SwerveConstants { MOD2_CONSTANTS.CANcoderId = 2; MOD2_CONSTANTS.CANcoderOffset = 0.205322; } - + public static final SwerveModuleConstants MOD3_CONSTANTS = new SwerveModuleConstants(); static { MOD3_CONSTANTS.DriveMotorId = 31; @@ -86,15 +98,26 @@ public static final class SwerveConstants { MOD3_CONSTANTS.CANcoderOffset = -0.119141; } + public static final SwerveModuleConstants[] SWERVE_MODULE_CONSTANTS = new SwerveModuleConstants[] { + MOD0_CONSTANTS, + MOD1_CONSTANTS, + MOD2_CONSTANTS, + MOD3_CONSTANTS + }; + // motor configuration public static final TalonFXConfiguration DRIVE_MOTOR_CONFIGURATION = new TalonFXConfiguration(); static { + DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimit = 40; + DRIVE_MOTOR_CONFIGURATION.CurrentLimits.SupplyCurrentLimitEnable = true; DRIVE_MOTOR_CONFIGURATION.Feedback.SensorToMechanismRatio = SwerveConstants.DRIVE_MOTOR_GEAR_RATIO; } public static final SparkMaxConfig STEER_MOTOR_CONFIGURATION = new SparkMaxConfig(); static { - STEER_MOTOR_CONFIGURATION.inverted(false); + STEER_MOTOR_CONFIGURATION.smartCurrentLimit(30); + STEER_MOTOR_CONFIGURATION.voltageCompensation(12.0); + STEER_MOTOR_CONFIGURATION.inverted(IS_STEER_MOTOR_INVERTED); STEER_MOTOR_CONFIGURATION.idleMode(IdleMode.kBrake); } } diff --git a/src/main/java/frc/robot/IO/ModuleIO.java b/src/main/java/frc/robot/IO/ModuleIO.java new file mode 100644 index 0000000..ced430c --- /dev/null +++ b/src/main/java/frc/robot/IO/ModuleIO.java @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.IO; + +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.math.geometry.Rotation2d; + +public interface ModuleIO { + @AutoLog + public static class ModuleIOInputs implements LoggableInputs { + public double drivePositionRad = 0.0; + public double driveVelocityRadPerSec = 0.0; + public double driveAppliedVolts = 0.0; + public double[] driveCurrentAmps = new double[] {}; + + public Rotation2d turnAbsolutePosition = new Rotation2d(); + public Rotation2d turnPosition = new Rotation2d(); + public double turnVelocityRadPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double[] turnCurrentAmps = new double[] {}; + @Override + public void toLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'toLog'"); + } + + @Override + public void fromLog(LogTable table) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'fromLog'"); + } + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} + + /** Run the drive motor at the specified voltage. */ + public default void setDriveVoltage(double volts) {} + + /** Run the turn motor at the specified voltage. */ + public default void setTurnVoltage(double volts) {} + + /** Enable or disable brake mode on the drive motor. */ + public default void setDriveBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on the turn motor. */ + public default void setTurnBrakeMode(boolean enable) {} +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 049fc7d..bb46f7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import frc.robot.subsystems.LED; import frc.robot.subsystems.PathPlanner; import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Swerve; import frc.robot.subsystems.SwerveDrive; import frc.robot.subsystems.SwervePoseEstimator; @@ -26,18 +27,18 @@ public class RobotContainer { private CommandXboxController m_Controller; private Joystick joystick; private static ButtonBox m_buttonbox; - private final Elevator m_elevator; - private final Grabber m_grabber; - private final Intake m_intake; - private final LED m_led; - private final Superstructure m_superstructure; + // private final Elevator m_elevator; + // private final Grabber m_grabber; + // private final Intake m_intake; + // private final LED m_led; + // private final Superstructure m_superstructure; private final SwerveDrive m_swerveD; private final SwervePoseEstimator m_swervePE; private final PathPlanner m_pathplanner; - private final TeleopSwerve teleopSwerve; - private final TeleopSuperstructure teleopSuperstructure; + // private final TeleopSuperstructure teleopSuperstructure; private final PathPlannerCommand pathPlannerCommand; + private static Command driveCommand; private final SendableChooser autoChooser; @@ -46,22 +47,32 @@ public RobotContainer() { joystick = new Joystick(RobotConstants.OPERATOR_BUTTONBOX_PORT); m_buttonbox = new ButtonBox(joystick); - m_swerveD = SwerveDrive.getInstance(); + m_swerveD = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); m_swervePE = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); m_pathplanner = PathPlanner.getInstance(); - m_elevator = Elevator.getInstance(); - m_grabber = Grabber.getInstance(); - m_intake = Intake.getInstance(); - m_led = LED.getInstance(); - m_superstructure = Superstructure.getInstance(); + // m_elevator = Elevator.getInstance(); + // m_grabber = Grabber.getInstance(); + // m_intake = Intake.getInstance(); + // m_led = LED.getInstance(); + // m_superstructure = Superstructure.getInstance(); - teleopSwerve = new TeleopSwerve(m_swerveD, m_swervePE, m_Controller); - teleopSuperstructure = new TeleopSuperstructure(); + // teleopSuperstructure = new TeleopSuperstructure(); pathPlannerCommand = new PathPlannerCommand(m_pathplanner); + driveCommand = TeleopSwerve.joystickDrive( + m_swerveD, + m_swervePE, + () -> -m_Controller.getLeftX(), + () -> -m_Controller.getLeftY(), + () -> -m_Controller.getRightX()); - m_swerveD.setDefaultCommand(teleopSwerve); - m_swervePE.setDefaultCommand(teleopSwerve); - m_superstructure.setDefaultCommand(teleopSuperstructure); + m_swerveD.setDefaultCommand(driveCommand); + m_swervePE.setDefaultCommand(driveCommand); + // m_superstructure.setDefaultCommand(teleopSuperstructure); m_pathplanner.setDefaultCommand(pathPlannerCommand); configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/TeleopSuperstructure.java b/src/main/java/frc/robot/commands/TeleopSuperstructure.java index 2a5163a..45ba34d 100644 --- a/src/main/java/frc/robot/commands/TeleopSuperstructure.java +++ b/src/main/java/frc/robot/commands/TeleopSuperstructure.java @@ -1,6 +1,5 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.ButtonBox; import frc.robot.RobotContainer; diff --git a/src/main/java/frc/robot/commands/TeleopSwerve.java b/src/main/java/frc/robot/commands/TeleopSwerve.java index 059699b..8001b0b 100644 --- a/src/main/java/frc/robot/commands/TeleopSwerve.java +++ b/src/main/java/frc/robot/commands/TeleopSwerve.java @@ -1,70 +1,71 @@ + package frc.robot.commands; -import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.SwerveDrive; import frc.robot.subsystems.SwervePoseEstimator; -public class TeleopSwerve extends Command { - - private final SwerveDrive swerve; - private final SwervePoseEstimator poseEstimator; - - private SlewRateLimiter xLimiter = new SlewRateLimiter(3.0); - private SlewRateLimiter yLimiter = new SlewRateLimiter(3.0); - private SlewRateLimiter rotLimiter = new SlewRateLimiter(3.0); - - private double xSpeed = 0.0; - private double ySpeed = 0.0; - private double rotSpeed = 0.0; - - private double reduction = 1; - - private CommandXboxController controller; - - public TeleopSwerve(SwerveDrive swerve, SwervePoseEstimator poseEstimator, CommandXboxController controller) { - this.swerve = swerve; - this.poseEstimator = poseEstimator; - this.controller = controller; - addRequirements(swerve); - addRequirements(poseEstimator); - } - - @Override - public void execute() { +import java.util.function.DoubleSupplier; - if (controller.getHID().getAButtonPressed()) { - poseEstimator.setPoseEstimatorPose(new Pose2d()); - } +public class TeleopSwerve { + private TeleopSwerve() {} + - if (controller.getHID().getRightBumperButton()) { - reduction = 0.3; - } else { - reduction = 1; - } + /** + * Field relative drive command using two joysticks (controlling linear and angular velocities). + */ + public static Command joystickDrive( + SwerveDrive drive, + SwervePoseEstimator poseEstimator, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier omegaSupplier) { + return Commands.run( + () -> { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), RobotConstants.DRIVE_CONTROLLER_DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()); + double omega = MathUtil.applyDeadband(omegaSupplier.getAsDouble(), RobotConstants.DRIVE_CONTROLLER_DEADBAND); - xSpeed = xLimiter.calculate(-controller.getLeftY() * reduction); - ySpeed = yLimiter.calculate(-controller.getLeftX() * reduction); - rotSpeed = rotLimiter.calculate(-controller.getRightX() * reduction); + // Square values + linearMagnitude = linearMagnitude * linearMagnitude; + omega = Math.copySign(omega * omega, omega); - // square the input to inprove driving experience - xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - ySpeed = Math.copySign(ySpeed * ySpeed, ySpeed); - rotSpeed = Math.copySign(rotSpeed * rotSpeed, rotSpeed); + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); - swerve.drive( - new Translation2d(xSpeed, ySpeed).times(SwerveConstants.MAX_MODULE_SPEED), - rotSpeed * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED, - true); - } + // Convert to field relative speeds & send command + boolean isFlipped = + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red; + drive.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * SwerveConstants.MAX_MODULE_SPEED * 0.3, + linearVelocity.getY() * SwerveConstants.MAX_MODULE_SPEED * 0.3, + omega * SwerveConstants.MAX_MODULE_ROTATIONAL_SPEED * 0.3, + isFlipped + ? poseEstimator.getPERotation().plus(new Rotation2d(Math.PI)) + : poseEstimator.getPERotation())); + }, + drive); + } - @Override - public void end(boolean interrupted) { - swerve.drive(new Translation2d(), 0, false); - } -} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Pathplanner.java b/src/main/java/frc/robot/subsystems/Pathplanner.java index 501eff6..bbb1999 100644 --- a/src/main/java/frc/robot/subsystems/Pathplanner.java +++ b/src/main/java/frc/robot/subsystems/Pathplanner.java @@ -26,7 +26,12 @@ public static PathPlanner getInstance() { return m_instance; } - private final SwerveDrive m_swerve = SwerveDrive.getInstance(); + private final SwerveDrive m_swerve = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); private final SwervePoseEstimator m_poseEstimator = SwervePoseEstimator.getInstance(new GyroIOPigeon2()); private Field2d m_field = new Field2d(); @@ -48,7 +53,7 @@ public PathPlanner() { m_poseEstimator::getPoseEstimatorPose, m_poseEstimator::setPoseEstimatorPose, m_poseEstimator::getRobotRelativeSpeeds, - (speeds, feedforwards) -> m_swerve.driveRobotRelative(speeds), + (speeds, feedforwards) -> m_swerve.runVelocity(speeds), new PPHolonomicDriveController( new PIDConstants(10.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java new file mode 100644 index 0000000..089f65e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -0,0 +1,163 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.IO.ModuleIO; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +/** Add your docs here. */ +public class Swerve implements ModuleIO { + + private static Swerve m_instance = null; + + public static Swerve getInstance(int index) { + if (m_instance == null) { + m_instance = new Swerve(index); + } + return m_instance; + } + + private final TalonFX driveTalon; + // private final TalonFX turnTalon; + private final SparkMax turnSparkMax; + private final CANcoder cancoder; + + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnAbsolutePosition; + + private final Rotation2d absoluteEncoderOffset; + + public Swerve(int index) { + if (index < 0 || index >= SwerveConstants.SWERVE_MODULE_CONSTANTS.length) { + throw new IllegalArgumentException("Invalid index: " + index); + } + + driveTalon = new TalonFX(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].DriveMotorId, RobotConstants.CANBUS_NAME); + // turnTalon = new TalonFX(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].SteerMotorId, RobotConstants.CANBUS_NAME); + turnSparkMax = new SparkMax(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].SteerMotorId, MotorType.kBrushless); + cancoder = new CANcoder(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].CANcoderId); + absoluteEncoderOffset = new Rotation2d(Units.rotationsToRadians(SwerveConstants.SWERVE_MODULE_CONSTANTS[index].CANcoderOffset)); + + TalonFXConfiguration driveTalonConfig = SwerveConstants.DRIVE_MOTOR_CONFIGURATION; + driveTalon.getConfigurator().apply(driveTalonConfig); + setDriveMotorOutputMode(true); + + SparkMaxConfig turnSparkConfig = SwerveConstants.STEER_MOTOR_CONFIGURATION; + turnSparkMax.setCANTimeout(250); + turnSparkMax.configure(turnSparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + cancoder.getConfigurator().apply(new CANcoderConfiguration()); + + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getSupplyCurrent(); + + turnAbsolutePosition = cancoder.getAbsolutePosition(); + + BaseStatusSignal.setUpdateFrequencyForAll(100.0, drivePosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition + // turnVelocity, + // turnAppliedVolts, + // turnCurrent + ); + driveTalon.optimizeBusUtilization(); + // turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition + // turnVelocity, + // turnAppliedVolts, + // turnCurrent + ); + + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble() / SwerveConstants.DRIVE_MOTOR_GEAR_RATIO); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble() / SwerveConstants.DRIVE_MOTOR_GEAR_RATIO); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = + new Rotation2d(Units.rotationsToRadians(cancoder.getAbsolutePosition().getValueAsDouble())).plus(absoluteEncoderOffset); + inputs.turnPosition = + Rotation2d.fromRotations(turnSparkMax.getEncoder().getPosition() / SwerveConstants.STEER_MOTOR_GEAR_RATIO); + inputs.turnVelocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(turnSparkMax.getEncoder().getVelocity()) / SwerveConstants.STEER_MOTOR_GEAR_RATIO; + inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); + inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; + // inputs.turnPosition = + // Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + // inputs.turnVelocityRadPerSec = + // Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + // inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + // inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + } + + @Override + public void setDriveVoltage(double volts) { + driveTalon.setControl(new VoltageOut(volts)); + } + + @Override + public void setTurnVoltage(double volts) { + // turnTalon.setControl(new VoltageOut(volts)); + turnSparkMax.setVoltage(volts); + } + + public void setDriveMotorOutputMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + // public void setTurnMotorOutputMode(boolean enable) { + // var config = new MotorOutputConfigs(); + // config.Inverted = + // SwerveConstants.IS_STEER_MOTOR_INVERTED + // ? InvertedValue.Clockwise_Positive + // : InvertedValue.CounterClockwise_Positive; + // config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + // turnTalon.getConfigurator().apply(config); + // } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveDrive.java b/src/main/java/frc/robot/subsystems/SwerveDrive.java index f8503c4..6c46a7a 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrive.java @@ -1,74 +1,87 @@ package frc.robot.subsystems; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.ModuleIO; public class SwerveDrive extends SubsystemBase { private static SwerveDrive m_instance = null; - public static SwerveDrive getInstance() { + public static SwerveDrive getInstance(ModuleIO FLModuleIO, ModuleIO FRModuleIO, ModuleIO RLModuleIO, ModuleIO RRModuleIO) { if (m_instance == null) { - m_instance = new SwerveDrive(); + m_instance = new SwerveDrive(FLModuleIO, FRModuleIO, RLModuleIO, RRModuleIO); } return m_instance; } - private SwerveModule[] modules = new SwerveModule[] { - new SwerveModule(0, SwerveConstants.MOD0_CONSTANTS), - new SwerveModule(1, SwerveConstants.MOD1_CONSTANTS), - new SwerveModule(2, SwerveConstants.MOD2_CONSTANTS), - new SwerveModule(3, SwerveConstants.MOD3_CONSTANTS) - }; - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + private SwerveModule[] modules = new SwerveModule[4]; // FL FR RL RR - public SwerveDrive() {} + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - ChassisSpeeds chassisSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, SwerveConstants.gyroYaw) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); - setModuleStates(moduleStates); + public SwerveDrive(ModuleIO FLModuleIO, ModuleIO FRModuleIO, ModuleIO RLModuleIO, ModuleIO RRModuleIO) { + modules[0] = new SwerveModule(FLModuleIO, 0); + modules[1] = new SwerveModule(FLModuleIO, 1); + modules[2] = new SwerveModule(FLModuleIO, 2); + modules[3] = new SwerveModule(FLModuleIO, 3); } - public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { - ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.02); - SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); - setModuleStates(targetStates); + @Override + public void periodic() { + for (var module : modules) { + module.periodic(); + } + + if(DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } } - public SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] positions = new SwerveModulePosition[4]; + public void runVelocity(ChassisSpeeds speeds) { + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = m_kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, SwerveConstants.MAX_MODULE_SPEED); + + SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - positions[i] = modules[i].getModulePosition(); + optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); } - return positions; + } + + public void stop() { + runVelocity(new ChassisSpeeds()); + } + + public void stopWithButton() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = SwerveConstants.MODULE_TRANSLATION_METERS[i].getAngle(); + } + m_kinematics.resetHeadings(headings); + stop(); } public SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { - states[i] = modules[i].getModuleState(); + states[i] = modules[i].getState(); } return states; } - public void setModuleStates(SwerveModuleState[] desiredStates) { - if (desiredStates.length != 4) { - throw new IllegalArgumentException("desiredStates must have length 4"); - } - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); - for (SwerveModule mod : modules) { - mod.setDesiredState(desiredStates[mod.ModuleNumber]); + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getPosition(); } + return positions; } - - @Override - public void periodic() {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveOld.java b/src/main/java/frc/robot/subsystems/SwerveDriveOld.java new file mode 100644 index 0000000..06ea175 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveDriveOld.java @@ -0,0 +1,163 @@ +// put here as a tribute to the old swerve drive code programmed by CHenry +package frc.robot.subsystems; + +import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +public class SwerveDriveOld extends SubsystemBase { + + private Pigeon2 pigeon = new Pigeon2(SwerveConstants.PIGEON_ID, RobotConstants.CANBUS_NAME); + + private SwerveModuleOld[] modules = new SwerveModuleOld[] { + new SwerveModuleOld(0, SwerveConstants.MOD0_CONSTANTS), + new SwerveModuleOld(1, SwerveConstants.MOD1_CONSTANTS), + new SwerveModuleOld(2, SwerveConstants.MOD2_CONSTANTS), + new SwerveModuleOld(3, SwerveConstants.MOD3_CONSTANTS) + }; + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); + private SwerveDriveOdometry odometry = new SwerveDriveOdometry(kinematics, getGyroYaw(), getModulePositions()); + + private Field2d field = new Field2d(); + + private static SwerveDriveOld mInstance; + + public static SwerveDriveOld getInstance() { + if (mInstance == null) { + mInstance = new SwerveDriveOld(); + } + return mInstance; + } + + public SwerveDriveOld() { + + RobotConfig config = null; + try { + config = RobotConfig.fromGUISettings(); + } catch (Exception e) { + e.printStackTrace(); + } + + if (config == null) { + throw new RuntimeException("Failed to load config"); + } + + AutoBuilder.configure( + this::getOdometryPosition, + this::setOdometryPosition, + this::getRobotRelativSpeeds, + (speeds, feedforwards) -> driveRobotRelative(speeds), + new PPHolonomicDriveController( + new PIDConstants(10.0, 0.0, 0.0), + new PIDConstants(5.0, 0.0, 0.0)), + config, + () -> { + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this); + + PathPlannerLogging.setLogTargetPoseCallback((pose) -> { + field.getObject("target").setPose(pose); + }); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + field.getObject("path").setPoses(poses); + }); + + odometry.resetPose(new Pose2d()); + pigeon.reset(); + } + + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + ChassisSpeeds chassisSpeeds = fieldRelative + ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(), rotation, getGyroYaw()) + : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); + setModuleStates(moduleStates); + } + + public void driveRobotRelative(ChassisSpeeds robotRelativeSpeeds) { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(robotRelativeSpeeds, 0.01); + SwerveModuleState[] targetStates = kinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(targetStates); + } + + public Rotation2d getGyroYaw() { + return Rotation2d.fromDegrees(pigeon.getYaw().getValueAsDouble()); + } + + public void setGyroYaw(Rotation2d yaw) { + pigeon.setYaw(yaw.getDegrees()); + } + + public Pose2d getOdometryPosition() { + return odometry.getPoseMeters(); + } + + public void setOdometryPosition(Pose2d pose) { + odometry.resetPosition(getGyroYaw(), getModulePositions(), pose); + } + + public ChassisSpeeds getRobotRelativSpeeds() { + return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getModuleStates()), getGyroYaw()); + } + + public SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] positions = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + positions[i] = modules[i].getModulePosition(); + } + return positions; + } + + public SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getModuleState(); + } + return states; + } + + public void setModuleStates(SwerveModuleState[] desiredStates) { + if (desiredStates.length != 4) { + throw new IllegalArgumentException("desiredStates must have length 4"); + } + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, SwerveConstants.MAX_MODULE_SPEED); + for (SwerveModuleOld mod : modules) { + mod.setDesiredState(desiredStates[mod.ModuleNumber]); + } + } + + @Override + public void periodic() { + odometry.update(getGyroYaw(), getModulePositions()); + field.setRobotPose(getOdometryPosition()); + + SmartDashboard.putData("Field", field); + SmartDashboard.putNumber("gyro (deg)", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("swerve odometry x", getOdometryPosition().getX()); + SmartDashboard.putNumber("swerve odometry y", getOdometryPosition().getY()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 1343af6..b2a049f 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -1,101 +1,123 @@ -package frc.robot.subsystems; +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; +package frc.robot.subsystems; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.FSLib2025.math.Maths; -import frc.FSLib2025.swerve.OnboardModuleState; -import frc.FSLib2025.swerve.SwerveModuleConstants; -import frc.robot.Constants.RobotConstants; import frc.robot.Constants.SwerveConstants; +import frc.robot.IO.ModuleIO; +import frc.robot.IO.ModuleIO.ModuleIOInputs; +/** Add your docs here. */ public class SwerveModule { - public int ModuleNumber; - public SwerveModuleConstants ModuleConstants; + private static final double WHEEL_RADIUS = SwerveConstants.DRIVE_WHEEL_DIAMETERS / 2; // meters - private SparkMax steerMotor; - private TalonFX driveMotor; + private final ModuleIO io; + private final ModuleIOInputs inputs = new ModuleIOInputs(); + private final int index; - private CANcoder steerCANcoder; + private final SimpleMotorFeedforward driveFeedForward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; + private Double speedSetpoint = null; + private Rotation2d turnRelativeOffset = null; - private PIDController steerPID; + public SwerveModule(ModuleIO io, int index) { + this.io = io; + this.index = index; - private Rotation2d lastAngle; + driveFeedForward = new SimpleMotorFeedforward(SwerveConstants.DRIVE_MOTOR_KS, SwerveConstants.DRIVE_MOTOR_KV, SwerveConstants.DRIVE_MOTOR_KA); + driveFeedback = new PIDController(SwerveConstants.DRIVE_MOTOR_KP, SwerveConstants.DRIVE_MOTOR_KI, SwerveConstants.DRIVE_MOTOR_KD); + turnFeedback = new PIDController(SwerveConstants.STEER_MOTOR_KP, SwerveConstants.STEER_MOTOR_KI, SwerveConstants.STEER_MOTOR_KD); + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + setBrakeMode(true); + } - public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { - this.ModuleNumber = moduleNumber; - this.ModuleConstants = moduleConstants; + public void periodic() { + io.updateInputs(inputs); + + if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { + turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); + } + + if (angleSetpoint != null) { + io.setTurnVoltage( + turnFeedback.calculate(inputs.turnPosition.getRadians(), angleSetpoint.getRadians()) + ); + + if (speedSetpoint != null) { + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getError()); + + double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; + io.setDriveVoltage( + driveFeedForward.calculate(velocityRadPerSec) + + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec) + ); + } + } + } - steerPID = new PIDController( - SwerveConstants.STEER_MOTOR_KP, - SwerveConstants.STEER_MOTOR_KI, - SwerveConstants.STEER_MOTOR_KD); + public SwerveModuleState runSetpoint(SwerveModuleState state) { + state.optimize(getAngle()); - lastAngle = new Rotation2d(); + angleSetpoint = state.angle; + speedSetpoint = state.speedMetersPerSecond; - steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); - steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + return state; + } - driveMotor = new TalonFX(moduleConstants.DriveMotorId, RobotConstants.CANBUS_NAME); - driveMotor.getConfigurator().apply(SwerveConstants.DRIVE_MOTOR_CONFIGURATION); - driveMotor.setNeutralMode(NeutralModeValue.Brake); + public void runCharacterization(double volts) { + angleSetpoint = new Rotation2d(); - steerCANcoder = new CANcoder(moduleConstants.CANcoderId, RobotConstants.CANBUS_NAME); - CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); - cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; - cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; - cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - steerCANcoder.getConfigurator().apply(cancoderConfig); + io.setDriveVoltage(volts); + speedSetpoint = null; } - public void setDesiredState(SwerveModuleState desiredState) { - desiredState = OnboardModuleState.optimize(desiredState, getModuleState().angle); + public void stop() { + io.setTurnVoltage(0.0); + io.setDriveVoltage(0.0); - desiredState.angle = Math.abs(desiredState.speedMetersPerSecond) < 0.01 ? lastAngle : desiredState.angle; - double error = getModuleState().angle.getDegrees() - desiredState.angle.getDegrees(); - error = Maths.constrainAngleDegrees(error); - double steerOutput = steerPID.calculate(error); - setSteerMotor(steerOutput); - lastAngle = desiredState.angle; + angleSetpoint = null; + speedSetpoint = null; + } - double percentOutput = desiredState.speedMetersPerSecond / SwerveConstants.MAX_MODULE_SPEED; - percentOutput = Maths.clamp(percentOutput, -1, 1); - setDriveMotor(percentOutput); + public void setBrakeMode(boolean enabled) { + io.setDriveBrakeMode(enabled); + io.setTurnBrakeMode(enabled); } - private void setSteerMotor(double speed) { - steerMotor.set(speed); + public Rotation2d getAngle() { + if (turnRelativeOffset == null) { + return new Rotation2d(); + } else { + return inputs.turnPosition.plus(turnRelativeOffset); + } } - private void setDriveMotor(double speed) { - driveMotor.set(speed); + public double getPositionMeters() { + return inputs.drivePositionRad * WHEEL_RADIUS; } - public Rotation2d getSteerAngle() { - return Rotation2d.fromRotations(steerCANcoder.getAbsolutePosition().getValueAsDouble()); + public double getVelocityMetersPerSec() { + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; } - public SwerveModulePosition getModulePosition() { - return new SwerveModulePosition( - driveMotor.getPosition().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, - getSteerAngle()); + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); } - public SwerveModuleState getModuleState() { - return new SwerveModuleState( - driveMotor.getVelocity().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, - getSteerAngle()); + public SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } -} \ No newline at end of file + public double getCharacterizationVelocity() { + return inputs.driveVelocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModuleOld.java b/src/main/java/frc/robot/subsystems/SwerveModuleOld.java new file mode 100644 index 0000000..b2068c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModuleOld.java @@ -0,0 +1,102 @@ +// put here as a tribute to the old swerve drive code programmed by CHenry +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import frc.FSLib2025.math.Maths; +import frc.FSLib2025.swerve.OnboardModuleState; +import frc.FSLib2025.swerve.SwerveModuleConstants; +import frc.robot.Constants.RobotConstants; +import frc.robot.Constants.SwerveConstants; + +public class SwerveModuleOld { + public int ModuleNumber; + public SwerveModuleConstants ModuleConstants; + + private SparkMax steerMotor; + private TalonFX driveMotor; + + private CANcoder steerCANcoder; + + private PIDController steerPID; + + private Rotation2d lastAngle; + + public SwerveModuleOld(int moduleNumber, SwerveModuleConstants moduleConstants) { + this.ModuleNumber = moduleNumber; + this.ModuleConstants = moduleConstants; + + steerPID = new PIDController( + SwerveConstants.STEER_MOTOR_KP, + SwerveConstants.STEER_MOTOR_KI, + SwerveConstants.STEER_MOTOR_KD); + + lastAngle = new Rotation2d(); + + steerMotor = new SparkMax(moduleConstants.SteerMotorId, MotorType.kBrushless); + steerMotor.configure(SwerveConstants.STEER_MOTOR_CONFIGURATION, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + driveMotor = new TalonFX(moduleConstants.DriveMotorId, RobotConstants.CANBUS_NAME); + driveMotor.getConfigurator().apply(SwerveConstants.DRIVE_MOTOR_CONFIGURATION); + driveMotor.setNeutralMode(NeutralModeValue.Brake); + + steerCANcoder = new CANcoder(moduleConstants.CANcoderId, RobotConstants.CANBUS_NAME); + CANcoderConfiguration cancoderConfig = new CANcoderConfiguration(); + cancoderConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + cancoderConfig.MagnetSensor.MagnetOffset = moduleConstants.CANcoderOffset; + cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; + steerCANcoder.getConfigurator().apply(cancoderConfig); + } + + public void setDesiredState(SwerveModuleState desiredState) { + desiredState = OnboardModuleState.optimize(desiredState, getModuleState().angle); + + desiredState.angle = Math.abs(desiredState.speedMetersPerSecond) < 0.01 ? lastAngle : desiredState.angle; + double error = getModuleState().angle.getDegrees() - desiredState.angle.getDegrees(); + error = Maths.constrainAngleDegrees(error); + double steerOutput = steerPID.calculate(error); + setSteerMotor(steerOutput); + lastAngle = desiredState.angle; + + double percentOutput = desiredState.speedMetersPerSecond / SwerveConstants.MAX_MODULE_SPEED; + percentOutput = Maths.clamp(percentOutput, -1, 1); + setDriveMotor(percentOutput); + } + + private void setSteerMotor(double speed) { + steerMotor.set(speed); + } + + private void setDriveMotor(double speed) { + driveMotor.set(speed); + } + + public Rotation2d getSteerAngle() { + return Rotation2d.fromRotations(steerCANcoder.getAbsolutePosition().getValueAsDouble()); + } + + public SwerveModulePosition getModulePosition() { + return new SwerveModulePosition( + driveMotor.getPosition().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + + public SwerveModuleState getModuleState() { + return new SwerveModuleState( + driveMotor.getVelocity().getValueAsDouble() * SwerveConstants.DRIVE_WHEEL_PERIMETER, + getSteerAngle()); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java index bfc2853..4e6e76f 100644 --- a/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/SwervePoseEstimator.java @@ -35,10 +35,15 @@ public static SwervePoseEstimator getInstance(GyroIO gyroIO) { return m_instance; } - private SwerveDrive m_SwerveDrive = SwerveDrive.getInstance(); + private SwerveDrive m_SwerveDrive = SwerveDrive.getInstance( + Swerve.getInstance(0), + Swerve.getInstance(1), + Swerve.getInstance(2), + Swerve.getInstance(3) + ); private SwerveVision m_SwerveVision = SwerveVision.getInstance(); - private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATOIN_METERS); + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(SwerveConstants.MODULE_TRANSLATION_METERS); private Rotation2d m_gyroYaw = new Rotation2d(); private Supplier m_modulePos = () -> m_SwerveDrive.getModulePositions(); private SwerveModulePosition[] m_lastModulePos = // For delta tracking @@ -74,9 +79,13 @@ public void setPoseEstimatorPose(Pose2d pose) { m_poseEstimator.resetPosition(getGyroYaw(), m_SwerveDrive.getModulePositions(), pose); } + public Rotation2d getPERotation() { + return getPoseEstimatorPose().getRotation(); + } + public ChassisSpeeds getRobotRelativeSpeeds() { return ChassisSpeeds.fromFieldRelativeSpeeds(m_kinematics.toChassisSpeeds(m_SwerveDrive.getModuleStates()), - getGyroYaw()); + getPERotation()); } @Override