From a87fe7beb28da8a4a75cfab0f10c32a8ca5e30b9 Mon Sep 17 00:00:00 2001 From: Liam Teltow Date: Sat, 1 Feb 2025 14:32:42 -0600 Subject: [PATCH] Added enun to decide which DriveSubsystem to use, and fixed some errors --- ...rdware-Swerve-LeftFront-Drive-odometer.txt | 1 + ...ardware-Swerve-LeftRear-Drive-odometer.txt | 1 + ...dware-Swerve-RightFront-Drive-odometer.txt | 1 + ...rdware-Swerve-RightRear-Drive-odometer.txt | 1 + build.gradle | 2 +- ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 011 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat | Bin 0 -> 2048 bytes ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat | Bin 0 -> 2048 bytes src/main/java/frc/robot/Constants.java | 5 ++ src/main/java/frc/robot/RobotContainer.java | 23 ++++---- .../subsystems/drive/CTREDriveSubsystem.java | 20 +++---- .../subsystems/drive/REVDriveSubsystem.java | 51 ++++++++++++++++-- vendordeps/ThriftyLib.json | 20 +++++++ 22 files changed, 103 insertions(+), 22 deletions(-) create mode 100644 DriveHardware-Swerve-LeftFront-Drive-odometer.txt create mode 100644 DriveHardware-Swerve-LeftRear-Drive-odometer.txt create mode 100644 DriveHardware-Swerve-RightFront-Drive-odometer.txt create mode 100644 DriveHardware-Swerve-RightRear-Drive-odometer.txt create mode 100644 ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 011 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat create mode 100644 ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 08 - 0 - ext.dat create mode 100644 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat create mode 100644 vendordeps/ThriftyLib.json diff --git a/DriveHardware-Swerve-LeftFront-Drive-odometer.txt b/DriveHardware-Swerve-LeftFront-Drive-odometer.txt new file mode 100644 index 0000000..280ff51 --- /dev/null +++ b/DriveHardware-Swerve-LeftFront-Drive-odometer.txt @@ -0,0 +1 @@ +96.63815242633088 \ No newline at end of file diff --git a/DriveHardware-Swerve-LeftRear-Drive-odometer.txt b/DriveHardware-Swerve-LeftRear-Drive-odometer.txt new file mode 100644 index 0000000..e110820 --- /dev/null +++ b/DriveHardware-Swerve-LeftRear-Drive-odometer.txt @@ -0,0 +1 @@ +96.20416254778326 \ No newline at end of file diff --git a/DriveHardware-Swerve-RightFront-Drive-odometer.txt b/DriveHardware-Swerve-RightFront-Drive-odometer.txt new file mode 100644 index 0000000..6c90fe7 --- /dev/null +++ b/DriveHardware-Swerve-RightFront-Drive-odometer.txt @@ -0,0 +1 @@ +97.89397044452848 \ No newline at end of file diff --git a/DriveHardware-Swerve-RightRear-Drive-odometer.txt b/DriveHardware-Swerve-RightRear-Drive-odometer.txt new file mode 100644 index 0000000..eadf6b7 --- /dev/null +++ b/DriveHardware-Swerve-RightRear-Drive-odometer.txt @@ -0,0 +1 @@ +97.52366479043182 \ No newline at end of file diff --git a/build.gradle b/build.gradle index 73d6bb2..fbc18aa 100644 --- a/build.gradle +++ b/build.gradle @@ -97,7 +97,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() //implementation fileTree(dir: 'libs', include: '*.jar') - implementation 'com.github.lasarobotics:PurpleLib:2025.0.1' + implementation 'com.github.lasarobotics:PurpleLib:2025.0.2' implementation 'org.apache.commons:commons-math3:3.+' implementation 'org.tinylog:tinylog-api:2.7.0' diff --git a/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 010 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..82bad186f001556fbaa1c13bc52da3460d3b138d GIT binary patch literal 2048 zcmdT^J7`m36h6sK(pXvy2}SJE3qHGuvlO)2f@CN`aS$9XT3-+_lkHHgbg^hD*R3Fe z2tup~4gpaJMyP{=)JH9d4u$HXP+a6jmw3pJZh|`8A zy&>|CfG0HYU)8(x#$UV@4!AEeFF)V?vDtqom-|&8Cu%rKtQsG2eTS8Q+dU(CGxI-A zGJm_H{x$iS=n|zwuXf&SR~g>*8xjw`e`!MOfoo~u6Dhx)+KB4|f7of&`MmO~|3!9l zHDmR%UT65XnlF5FeXDUyXgY{1#Z|Y~q4#U+uZi07L>!U!y~EH#?W*)qh%R%N>UPH3 zq`x`K$6?$b8Sa}7aMXi5z>P-0qkiDH*Kn--T995(8KeP}EpQtNrj z$d4tG{EFg!Wm@KyhLFwkd$9l5+t7D$MC(DFpvCa;R)&~|ldWcKf8epZ@TG+pLFvfO z?&9;%S+l$uBODymz8+3~4S0{Xj(?CFdrh`?s6Y2@i|)JWbo5R=;5hJLm*T_HH|!qE z2OIe5VpjBkmv^b&z2iMnZ*+c`bF^Ogq3@CAZ@0Xzqc-&Mol1}NcaHB-UcN7NgFkwr z_Z0W%T0cvs13vQbeD*jFAs+Qxdbyf$=mSTeZ1Z=D+VNy+mJj-bKJL9lffxUSQq@>pkC+0-0R&h(`&|30hEZE-DfqQ`{w-BxEf8q z5SNduZn;VCH`bp=ubPM>(!Mtvk}VHPAKAKDIB>1%Hpddu-?-)DFqxa-MbiO}dXNXW z(FnNj2adajYrTyIpv)n&ONILRBOW+x)B`)y-5^rG^OUi#zhMP``FOE(S>~05kj-p`8M#qbf$^)L@7J(jZlfyeIb$2y*e(*7-Nxfjk^w!A4L?6JNc4t=ih z9<3jkk()^Onr!b-f9scZy6>jb)N%1X$AJesAMcgEVYgU5*uanG(xSJ)@Vi@8@5F<4 zsn?$$=IqxCKlCHg{Hskj4Ah1`-YGw#|8u;@=(QfH8~nndjw9S(X#Ko19q^Hd=d(Qs z2=S=j(#!QL4t?PB$t#kVZGI=;cCN_%2z^2ycPG(3KFs-(gL!cb_W*s&4f8@CXrEos z{e=zw@W$-4?1KUDgC9Mcls@4PzpKtf<-q$p*T=)s8I1?--{4~(ksoz`viDkC(gPf~ xANO^e?i>8=$2EWPlI{WGpmw96c$WhnCx~|Brk-=Zov&_J-;ZO^&)ym3{{&SCLB{|9 literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 012 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..0a682254c8aaa01a484be635f6ce6f835703afc5 GIT binary patch literal 2048 zcmdT^O=}Zj5T0aX6J?7bLDZfW)MyX-1GL}=DYQLEP(%a|L8~AJ&8bFED_$B@biEZ+ z5J3<`@DLP*;0o=*gEXjAiWfn>C{#ft)l1#^cw({=L@(kC$-Fc3%rhVFBoAq@8K+9j*2MlmCoBh5pPPFK#p>B5g2Zj`X!#h07ag#G&4={eM zqxPS9V!}(*BYeRT(>+BLFIY zm-%|bPb*(|=lWW43}`!uE60^La8)*}zcy&c6>&t`_d4?yf(hxP5MAaK%G(=DN`F(9 zkHfe>GQV#;@S`5&G50S5alkD#1j?wIUaNG@7ew&0&hx_j`NpT+ua?gD&W|J{RTSsf zF3CJn8WetOT%);l&+||lJE8TUPQX6@#vcQ)IKPv5a5D9@?GG#_3-3E{5ijk^^_8YW zV_fpyal^^Ku81GR%x=-o?zOujnAlL6%y>E;M zc;w;!d5;l~`nhe{?@(@XoR%N_RvtCdmnARWHLCkxbdS&{_#s!_nb-oZKarRh*U3GA z9~|%_Ps?r>%oBFVvkSW4Ni(=6C)X5a_4lO)*nuxTej#&!-TS6IGtJ#^Z(W`8YG*Va zewNG!9r(06-NXNbe|vZq?y1C*PaAa~S?9CixaRlw4!^>?vVBnw(Y1>Dw}rO*Ve^|y VFQ0IIL2gL!%*kA+Z+K4eKLEm_Ikf-) literal 0 HcmV?d00001 diff --git a/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 013 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..d6ef6508feab4584595a475c2ad3fff4c0df3860 GIT binary patch literal 2048 zcmdT@O-NKx6h1T0auTIsGAs#v(65Wal^Y2y#zlfL7}3VXP!a@zLkpWlrL<8rV%!N8 zvYKKB>?VCPJZCzT8JSpwxAw{_0F{S+KZqBb;v6x}FVv zFY+Ai?|&~lk!MIP~$(i!Gm|Urgn9a_xOj2U{3JozTbLMD&Cgb2hWkFZSUYppU+xUx)+k zfmNMf*x-*HOwG$Y7=U~5(^GGyPTc#yRA;t$?(?I2(|-Pz`U7`&@G*~wkG#Lxd;M9{ vbw3-<$MySpT;~t>$9k1NH?DI4eN`iZURwb0oZ$W3|2KExjLgxs5308g`*S-{ literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..061d3a973e24b681d6f4da8ac4fa6f1cfd448a18 GIT binary patch literal 2048 zcmdT^O-NKx6h3dB&1j4?Oho9S2KvKA?H!mjt7S11?OY6HMKIAUqGpjOwP>iQ2_nWq za1%)ow2;~)jG<7VIHUzp3q#u!*h>(W-g&i@nVOu;EYRG4-TxRlzhwB4 zwHnuRGL$;=e$dOUOfQ{W?jzdiwti%PWjW?Ye{#Hf&is@jayRcvKJew87A{ANO-chI-_+g5$= z#uIP5^_25~gI|dB$Xrh6lY-xF^@jH-{`^JlCp8Sc*${og z3%K=K%!~1?f%#eQQ0=kI0rNr~@aC5F{lW*nvSVgU?%M*GKk((3Gr|iz{Rr|j6g3}t zY#%)wPr(O15ub?i|J>g$)*7*ImO1+GSA^>OUVVQh>&vf8+E=D~R`hiWn&{J@p5Jqt a+Mmailey7fI8i}wS?Lw_gp|eS0ee zb%yo3ruV(^0FQIfKkA{M+roE#$Z-gH@LSu2K7Z#Ujy}N;8{HC{;04^DTFi^_+ynE= z`#Pa>z`Sq{cnfQKf8mqiNA@mHOT8s9f8eW6mu0@d)At}xOIhnt$98;l)`bszGBFq9 z|GB?i+^b{XEOQJEj|kPuLA`$!)8(H_sw>_9Qu@mZn&{n_#>a=!2D%uY6LH(hF?h3K7wyOj|sL&zQDhNe0)qqHY4u&ck7fF?3 z7r{m7P;{}7N*$CIgQ*44A!0?SSOqDqg5dk!-SahHV$i|G7m~Ys@44rG-b>!oyVUKG zkwaw8x!C1{R{BmU;xqq^k?kO|A2|UiRdQ};OO;ix-gvScpPiYBFHq&bu78SxKjXH& zV118&b2H|io*y-_g%`_xt3yP)oYptiUONG`*Z;8o;u*DFL8PwU#N&BmTdnV$D zr0?D8NfxhgA4%UVjB}gtHV4Ao-?ZklVLUhW=amP3)Pp?gt`mp@Zi6CFMo#UCjDNl$ zg5TP9UYNht?5Xz4jsLwnyF;9+7MGv6&htn}kolIl(n`nim&O`AFZG~Kz`p*$@BMaD zOGhmaPHZx&`-2;m$qznUjhBw?X&rcKE{mEsssQvgo?Be8b7>iR&5}s=nso1)6u;%u zcG-93iL_sP!1Gib>~yf7=W=Qw#`w(|e}Avw&t7QbdTQs_s2<#dKSjcixw?+n;Kw_q zAIa|+?=f~{e#+Jhe5SYkB->hVMR|Zno`k92?lIy~zik`;3;P|%^Mha8CiL0R{w8~b zKEV%Z;wp$u?$`>ok<8+~FfUtY!hFC_pL_UyFt4gS%$Zu1{bj>`DZbEF=K1M;Hlzyh z`Nw7E1)je9c|sY9N1T=S?o7L|Yw)hp)7_${4$9`h^LuI0&$~nG;LYADu=-`6>|ajn u^74|@74Mzn{t_Dac2w?r%6$hOhRq`^%G_M_S2|^%#y_6mzK6SBar`e1C0LUH literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..fb1f8b1caa20c5622d1ba09a0a202d77ac5d8513 GIT binary patch literal 2048 zcmdT^J!n%=6h6sInTr+EeD0a^H{)6`IUz0=RNhKd_nOt`gEvjY5oz5U z&6BQ-N*!rWFI?v;PrQEg>y7t$_wNZwDnG6?aY_0S(IEF<<7$oG15aJ+iC z^jYT!b%Gyz=bh0V;QEst{bHS51Ndvzo1zE(LLP8umUMn$17BW0Kc(-FdfE&CeCgr5 z@B&X?ygZ@2#v_jRTi2%(uz`<7XWIEY_qU5{Z5!6vW{$#@vQRB-*7+-%u4Z0RUCDuG mQeU5-iC&NE{klhmZTG{<$=uMlJ*)V+#{Rcl$I)HSCH^~jkWV}S literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..d998c2fc15e83909bfd6e63a1c9b8bbf77e4dfe7 GIT binary patch literal 2048 zcmdT^J!n%=6h6rdO_dZwM6?~`fm-dL;HVPo&(py{g5V%@cxV;GqM0-f(x5{_75iKy zRSH>zf>a!eE*4U$gV18IwIT{utcyYwq_heJ-?`^}N#4t#f{Pd0bH97O^PQi2Up~?Y z_s-zpUh>z4)V1Ag1FMn{I?-*3#7%z_nvcWG+|=)>4*aMGdDPut5C`5WWuT0L;?Z1i zz7T?+G0zM0=N+$lznlc`+c^?5slq&O^ro3dvjqA7C9bm6dHki1E?$y)P$y{Le&erx zyY9kK*1<}RCH4K_L~Z6%fEV)8vG%r}XZ~SX^Cp#W;E?q7u<&__=hE8y##kbG*QDI2-LYb2;6UGW?Y08{R7X^OxIAJ>mIb&f$7+4t32 zB^dg+r|e_-j&UD_b01f&p^KMA=N8S(bdK9m@ s!+HhP+v&`heq3kP*mRfReHseA2dbARnVYTt(m{#KtjKx4^UCP_0+~f$fB*mh literal 0 HcmV?d00001 diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat new file mode 100644 index 0000000000000000000000000000000000000000..c72858292953dc4a28e3f3f75e0590101aa051ca GIT binary patch literal 2048 zcmdT^O=uHQ5T0a16D7qE5o=Lcs77*7@F)`6NCfpDLGU2v5VQ(nk)CPH<){5w%2fZkyf|OQ4aOdYs+<5Q@Joq4aZ{B?K%{Q~NyF|x! z5mAr-quH664DbJb4RznTnav_k%np}BxFPD;teToP!PDr|RY^L|dKCH*7ao$n3VS?DtNa?JA;p<+{WJU%7Zm?A%C9@I;DV zOHpE8;Cr1Q&%63;_E~b>M8W|RQM-R^|mraAOWhc_PCYw9dxBlZs zop{Pcei{yTf4EoraW(J$7?HGrSe`(Lqc^b2{wU0l`qg$;bQYi?5CAN52H0DR@i zoYVyIlv_t2wY`PkGNp)ok o^HN`rpo!j&>HYi`Z}%e~?4`7CuK5>p3xcn79gu#Gj8(5i?*bJ7%)7Ckhm*m#jt zDR>bC5j+&VSV*Nk2rUh^Dx!x<>qW5^q@)%Ecizmm&2A3<;Kc{Yd-LX-Z@!tG-C6pO zy)!Vdi`;eIx!T6NZ&l+y^WPY~?L_>O6~duzpWfJ9XEh%CpD(5-#>dlhH1l8UAEEG% zG?y1{tnqhl?j=vX9&}^NlZ(ffdWbe#E$`S~Ss}HXmf2r9qjpjd+3UAVJn*HR$%6YxO`Z6D{C4HSWAKjVd$3jT=ekoyPq2UJb5IZN!Jj4Jk0@`_ z5*z$@PWh+u9^-iw&mJ?nfG>2mo?!h+)jOj+z#|XW&vT4;=;yNWUD$3}o*(=wkE(XR z$&2S2v`gP)j!-A~v2S?Tw-a1{vZG(DlWPF~LgL^X){TB454f{SGQY5aFK?O}k@rVD zmPoyC6h~xeK-O(g$;O+QW6MyIab}?V*+M|#Pl#7!VOV!n`mm?_)k-h)>5puLH+-={yDO~^>b-q ze|^7e-EvMoue!07<+Wq$6GS_#&X;VrR!Hr(uk5d#R@)Io_R>`o4}5w2g0Y7##tfcF z@Ea&f%nSU8<>z_VSV})mY@D?%FcEdQM=zgy=KNk<3uaD;%ZG)x7MH!Q^#$Obi8vyu zd#8FbwMkP)#@7qixmkESLQzxSyyoLD(>L{(l?Q(4K^}Ggv>^_-c11u&N$r_}zrP@Y zpKb0J`p+j`Klq)4a9<_U@Ou?v*|?xi4EMUYMOo=9+ZwP~WDH zTV&pqC*FPeuIZ=ZVCTalrY|RQ34>2)zT3M5fA)Nr(G%<+`W)1Qd+=vT_#?{eSYm@8 z&nf>v-eWwE(wU=17x0DQ?&GZAQN3@J2YBS+`gx8K5B*#=z6(8;<@v#{@~Cc4nY?(e zLA(4}<_L9yAA9o7WFNTxWJkYPC)WUer{}fNgMJ|oxU=grzwiTJ*}gKz{VJ*-?;-Hz zhbz)w<)<&g1KvnM;t|I?&o^w~?f60)f9L*oajorRGTY2iyjC$(tGi|XN~+7tOQI_^ p{M6JpXwXEjs&ZdVTD|w9!pYpw*P9dka`W)p>6`P;seR8({CBD@P=o*g literal 0 HcmV?d00001 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index cf48f87..6ce63d1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -76,6 +76,11 @@ public static class SmartDashboard { } public static class Drive { + public static enum Vendor { + REV, + CTRE; + } + public static final Vendor DRIVE_VENDOR = Vendor.REV; public static final DriveWheel DRIVE_WHEEL = DriveWheel.create(Units.Inches.of(4.0), Units.Value.of(1.3), Units.Value.of(1.2)); public static final PIDConstants DRIVE_PID = PIDConstants.of(0.3, 0.0, 0.001, 0.0, 0.0); public static final FFConstants DRIVE_FF = FFConstants.of(0.2, 0.0, 0.0, 0.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e33188a..475e1a3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,18 +4,22 @@ package frc.robot; +import org.lasarobotics.drive.swerve.SwerveDrive; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants.Drive.Vendor; import frc.robot.subsystems.WaggleSubsystem; import frc.robot.subsystems.drive.CTREDriveSubsystem; import frc.robot.subsystems.drive.REVDriveSubsystem; public class RobotContainer { - private static final CTREDriveSubsystem CTRE_DRIVE_SUBSYSTEM = new CTREDriveSubsystem( + public static final SwerveDrive DRIVE_SUBSYSTEM = (Constants.Drive.DRIVE_VENDOR.equals(Vendor.CTRE)) + ? new CTREDriveSubsystem( CTREDriveSubsystem.initializeHardware(), Constants.Drive.DRIVE_ROTATE_PID, Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY, @@ -24,9 +28,8 @@ public class RobotContainer { Constants.Drive.DRIVE_TURN_SCALAR, Constants.HID.CONTROLLER_DEADBAND, Constants.Drive.DRIVE_LOOKAHEAD - ); - - private static final REVDriveSubsystem REV_DRIVE_SUBSYSTEM = new REVDriveSubsystem( + ) + : new REVDriveSubsystem( REVDriveSubsystem.initializeHardware(), Constants.Drive.DRIVE_ROTATE_PID, Constants.Drive.DRIVE_AUTO_AIM_PID, Constants.Drive.DRIVE_CONTROL_CENTRICITY, @@ -45,8 +48,8 @@ public class RobotContainer { public RobotContainer() { // Set drive command - CTRE_DRIVE_SUBSYSTEM.setDefaultCommand( - CTRE_DRIVE_SUBSYSTEM.driveCommand( + DRIVE_SUBSYSTEM.setDefaultCommand( + DRIVE_SUBSYSTEM.driveCommand( () -> PRIMARY_CONTROLLER.getLeftY(), () -> PRIMARY_CONTROLLER.getLeftX(), () -> PRIMARY_CONTROLLER.getRightX() @@ -54,7 +57,9 @@ public RobotContainer() { ); // Setup AutoBuilder - CTRE_DRIVE_SUBSYSTEM.configureAutoBuilder(); + // if (Constants.Drive.DRIVE_VENDOR.equals(Vendor.CTRE)) + // ((CTREDriveSubsystem)DRIVE_SUBSYSTEM).configureAutoBuilder(); + // else ((REVDriveSubsystem)DRIVE_SUBSYSTEM).configureAutoBuilder(); autoModeChooser(); SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser); @@ -65,9 +70,9 @@ public RobotContainer() { private void configureBindings() { // Start button - toggle traction control - PRIMARY_CONTROLLER.start().onTrue(CTRE_DRIVE_SUBSYSTEM.toggleTractionControlCommand()); + PRIMARY_CONTROLLER.start().onTrue(DRIVE_SUBSYSTEM.toggleTractionControlCommand()); - PRIMARY_CONTROLLER.povLeft().onTrue(CTRE_DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d())); + PRIMARY_CONTROLLER.povLeft().onTrue(DRIVE_SUBSYSTEM.resetPoseCommand(() -> new Pose2d())); // Left/right bumper - wiggle stick PRIMARY_CONTROLLER.leftBumper().onTrue(WIGGLE_STICK.setPositionCommand(0.0)); diff --git a/src/main/java/frc/robot/subsystems/drive/CTREDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/CTREDriveSubsystem.java index c9de55d..1111c50 100644 --- a/src/main/java/frc/robot/subsystems/drive/CTREDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/CTREDriveSubsystem.java @@ -7,6 +7,7 @@ import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; import org.lasarobotics.drive.swerve.AdvancedSwerveKinematics.ControlCentricity; import org.lasarobotics.drive.swerve.SwerveDrive; +import org.lasarobotics.drive.swerve.SwerveModule; import org.lasarobotics.drive.swerve.child.SwerveX2Module; import org.lasarobotics.drive.swerve.parent.CTRESwerveModule; import org.lasarobotics.hardware.kauailabs.NavX2; @@ -64,8 +65,8 @@ public static Hardware initializeHardware() { Constants.CTREDriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID, Constants.CTREDriveHardware.LEFT_FRONT_ROTATE_ENCODER_ID ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, - org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED, + SwerveModule.Location.LeftFront, + SwerveModule.MountOrientation.INVERTED, Constants.Drive.CTRE_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -86,8 +87,8 @@ public static Hardware initializeHardware() { Constants.CTREDriveHardware.RIGHT_FRONT_ROTATE_MOTOR_ID, Constants.CTREDriveHardware.RIGHT_FRONT_ROTATE_ENCODER_ID ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, - org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED, + SwerveModule.Location.RightFront, + SwerveModule.MountOrientation.INVERTED, Constants.Drive.CTRE_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -108,8 +109,8 @@ public static Hardware initializeHardware() { Constants.CTREDriveHardware.LEFT_REAR_ROTATE_MOTOR_ID, Constants.CTREDriveHardware.LEFT_REAR_ROTATE_ENCODER_ID ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, - org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED, + SwerveModule.Location.LeftRear, + SwerveModule.MountOrientation.INVERTED, Constants.Drive.CTRE_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -130,8 +131,8 @@ public static Hardware initializeHardware() { Constants.CTREDriveHardware.RIGHT_REAR_ROTATE_MOTOR_ID, Constants.CTREDriveHardware.RIGHT_REAR_ROTATE_ENCODER_ID ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, - org.lasarobotics.drive.swerve.SwerveModule.MountOrientation.INVERTED, + SwerveModule.Location.RightRear, + SwerveModule.MountOrientation.INVERTED, Constants.Drive.CTRE_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -167,7 +168,8 @@ public static Hardware initializeHardware() { return drivetrainHardware; } - /**Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos. + /** + * Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos. * Uses the PathPlanner AutoBuilder method. */ diff --git a/src/main/java/frc/robot/subsystems/drive/REVDriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/REVDriveSubsystem.java index 61cb31f..011cf15 100644 --- a/src/main/java/frc/robot/subsystems/drive/REVDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/REVDriveSubsystem.java @@ -15,15 +15,23 @@ import org.lasarobotics.utils.PIDConstants; import org.lasarobotics.vision.AprilTagCamera; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Dimensionless; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants; public class REVDriveSubsystem extends SwerveDrive { + Hardware m_drivetrainHardware; + /** * Create an instance of DriveSubsystem *

@@ -43,6 +51,7 @@ public REVDriveSubsystem(Hardware drivetrainHardware, PIDConstants rotatePIDF, P PolynomialSplineFunction throttleInputCurve, PolynomialSplineFunction rotateInputCurve, Angle rotateScalar, Dimensionless deadband, Time lookAhead) { super(drivetrainHardware, rotatePIDF, aimPIDF, controlCentricity, throttleInputCurve, rotateInputCurve, rotateScalar, deadband, lookAhead); + m_drivetrainHardware = drivetrainHardware; } /** @@ -81,7 +90,7 @@ public static Hardware initializeHardware() { Spark.MotorKind.NEO_VORTEX, Spark.MotorKind.NEO_550 ), - SwerveModule.Location.LeftFront, + SwerveModule.Location.RightFront, Constants.Drive.REV_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -103,7 +112,7 @@ public static Hardware initializeHardware() { Spark.MotorKind.NEO_VORTEX, Spark.MotorKind.NEO_550 ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, + SwerveModule.Location.LeftRear, Constants.Drive.REV_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -125,7 +134,7 @@ public static Hardware initializeHardware() { Spark.MotorKind.NEO_VORTEX, Spark.MotorKind.NEO_550 ), - org.lasarobotics.drive.swerve.SwerveModule.Location.LeftFront, + SwerveModule.Location.RightRear, Constants.Drive.REV_GEAR_RATIO, Constants.Drive.DRIVE_WHEEL, Constants.Drive.DRIVE_PID, @@ -160,4 +169,40 @@ public static Hardware initializeHardware() { return drivetrainHardware; } + + /** + * Configures the AutoBuilder so that PathPlanner can use its built-in commands when running autos. + * Uses the PathPlanner AutoBuilder method. + */ + public void configureAutoBuilder() { + AutoBuilder.configure( + () -> getPose(), // Robot pose supplier + (pose) -> resetPose(pose), // Method to reset odometry (will be called if your auto has a starting pose) + () -> getChassisSpeeds(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> autoDrive(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new com.pathplanner.lib.config.PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + new RobotConfig(Constants.Drive.MASS, // The robot configuration + Units.KilogramSquareMeters.of(1), + Constants.Drive.MODULE_CONFIG, + m_drivetrainHardware.lFrontModule().getModuleCoordinate(), + m_drivetrainHardware.rFrontModule().getModuleCoordinate(), + m_drivetrainHardware.lRearModule().getModuleCoordinate(), + m_drivetrainHardware.rRearModule().getModuleCoordinate()), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } } diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 0000000..00d3f73 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2025.0.2", + "frcYear": "2025", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2025.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file