From 056c982ab16270857be85c831b8d38ec9337affd Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 5 Nov 2024 21:29:26 -0600 Subject: [PATCH 01/54] Update to 2025 --- .gitignore | 6 + .wpilib/wpilib_preferences.json | 2 +- WPILib-License.md | 2 +- build.gradle | 5 +- gradle/wrapper/gradle-wrapper.jar | Bin 43462 -> 43583 bytes gradle/wrapper/gradle-wrapper.properties | 2 +- gradlew | 7 +- gradlew.bat | 22 +- settings.gradle | 2 +- src/main/java/frc/robot/Constants.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../swervedrive/SwerveSubsystem.java | 153 +-- .../robot/subsystems/swervedrive/Vision.java | 958 +++++++++--------- src/main/java/swervelib/SwerveModule.java | 27 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 12 +- .../encoders/SparkMaxEncoderSwerve.java | 14 +- .../swervelib/motors/SparkFlexSwerve.java | 276 ++--- .../java/swervelib/motors/SparkMaxSwerve.java | 248 +++-- vendordeps/NavX.json | 40 - ... => PathplannerLib-2025.0.0-beta-3.1.json} | 14 +- ...enix5.json => Phoenix5-5.34.0-beta-2.json} | 28 +- ...enix6.json => Phoenix6-25.0.0-beta-2.json} | 91 +- ...EVLib.json => REVLib-2025.0.0-beta-1.json} | 16 +- ...{ReduxLib_2024.json => ReduxLib_2025.json} | 14 +- vendordeps/Studica-2025.1.1-beta-2.json | 71 ++ vendordeps/WPILibNewCommands.json | 2 +- vendordeps/photonlib.json | 57 -- 27 files changed, 1054 insertions(+), 1032 deletions(-) delete mode 100644 vendordeps/NavX.json rename vendordeps/{PathplannerLib.json => PathplannerLib-2025.0.0-beta-3.1.json} (80%) rename vendordeps/{Phoenix5.json => Phoenix5-5.34.0-beta-2.json} (88%) rename vendordeps/{Phoenix6.json => Phoenix6-25.0.0-beta-2.json} (85%) rename vendordeps/{REVLib.json => REVLib-2025.0.0-beta-1.json} (87%) rename vendordeps/{ReduxLib_2024.json => ReduxLib_2025.json} (82%) create mode 100644 vendordeps/Studica-2025.1.1-beta-2.json delete mode 100644 vendordeps/photonlib.json diff --git a/.gitignore b/.gitignore index 5528d4f6..375359c2 100644 --- a/.gitignore +++ b/.gitignore @@ -169,6 +169,8 @@ out/ .fleet # Simulation GUI and other tools window save file +networktables.json +simgui.json *-window.json # Simulation data log directory @@ -176,3 +178,7 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +# clangd +/.cache +compile_commands.json diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 9a94617b..77a1e114 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2024", + "projectYear": "2025beta", "teamNumber": 3481 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 43b62ec2..645e5425 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2023 FIRST and other WPILib contributors +Copyright (c) 2009-2024 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index dbfd1afd..ec028da5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" } java { @@ -45,11 +45,12 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = true +def includeDesktopSupport = false // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index d64cd4917707c1f8861d8cb53dd15194d4248596..a4b76b9530d66f5e68d973ea569d8e19de379189 100644 GIT binary patch delta 34592 zcmY(qRX`kF)3u#IAjsf0xCD212@LM;?(PINyAue(f;$XO2=4Cg1P$=#e%|lo zKk1`B>Q#GH)wNd-&cJog!qw7YfYndTeo)CyX{fOHsQjGa<{e=jamMNwjdatD={CN3>GNchOE9OGPIqr)3v>RcKWR3Z zF-guIMjE2UF0Wqk1)21791y#}ciBI*bAenY*BMW_)AeSuM5}vz_~`+1i!Lo?XAEq{TlK5-efNFgHr6o zD>^vB&%3ZGEWMS>`?tu!@66|uiDvS5`?bF=gIq3rkK(j<_TybyoaDHg8;Y#`;>tXI z=tXo~e9{U!*hqTe#nZjW4z0mP8A9UUv1}C#R*@yu9G3k;`Me0-BA2&Aw6f`{Ozan2 z8c8Cs#dA-7V)ZwcGKH}jW!Ja&VaUc@mu5a@CObzNot?b{f+~+212lwF;!QKI16FDS zodx>XN$sk9;t;)maB^s6sr^L32EbMV(uvW%or=|0@U6cUkE`_!<=LHLlRGJx@gQI=B(nn z-GEjDE}*8>3U$n(t^(b^C$qSTI;}6q&ypp?-2rGpqg7b}pyT zOARu2x>0HB{&D(d3sp`+}ka+Pca5glh|c=M)Ujn_$ly^X6&u z%Q4Y*LtB_>i6(YR!?{Os-(^J`(70lZ&Hp1I^?t@~SFL1!m0x6j|NM!-JTDk)%Q^R< z@e?23FD&9_W{Bgtr&CG&*Oer3Z(Bu2EbV3T9FeQ|-vo5pwzwQ%g&=zFS7b{n6T2ZQ z*!H(=z<{D9@c`KmHO&DbUIzpg`+r5207}4D=_P$ONIc5lsFgn)UB-oUE#{r+|uHc^hzv_df zV`n8&qry%jXQ33}Bjqcim~BY1?KZ}x453Oh7G@fA(}+m(f$)TY%7n=MeLi{jJ7LMB zt(mE*vFnep?YpkT_&WPV9*f>uSi#n#@STJmV&SLZnlLsWYI@y+Bs=gzcqche=&cBH2WL)dkR!a95*Ri)JH_4c*- zl4pPLl^as5_y&6RDE@@7342DNyF&GLJez#eMJjI}#pZN{Y8io{l*D+|f_Y&RQPia@ zNDL;SBERA|B#cjlNC@VU{2csOvB8$HzU$01Q?y)KEfos>W46VMh>P~oQC8k=26-Ku)@C|n^zDP!hO}Y z_tF}0@*Ds!JMt>?4y|l3?`v#5*oV-=vL7}zehMON^=s1%q+n=^^Z{^mTs7}*->#YL z)x-~SWE{e?YCarwU$=cS>VzmUh?Q&7?#Xrcce+jeZ|%0!l|H_=D_`77hBfd4Zqk&! zq-Dnt_?5*$Wsw8zGd@?woEtfYZ2|9L8b>TO6>oMh%`B7iBb)-aCefM~q|S2Cc0t9T zlu-ZXmM0wd$!gd-dTtik{bqyx32%f;`XUvbUWWJmpHfk8^PQIEsByJm+@+-aj4J#D z4#Br3pO6z1eIC>X^yKk|PeVwX_4B+IYJyJyc3B`4 zPrM#raacGIzVOexcVB;fcsxS=s1e&V;Xe$tw&KQ`YaCkHTKe*Al#velxV{3wxx}`7@isG zp6{+s)CG%HF#JBAQ_jM%zCX5X;J%-*%&jVI?6KpYyzGbq7qf;&hFprh?E5Wyo=bZ) z8YNycvMNGp1836!-?nihm6jI`^C`EeGryoNZO1AFTQhzFJOA%Q{X(sMYlzABt!&f{ zoDENSuoJQIg5Q#@BUsNJX2h>jkdx4<+ipUymWKFr;w+s>$laIIkfP6nU}r+?J9bZg zUIxz>RX$kX=C4m(zh-Eg$BsJ4OL&_J38PbHW&7JmR27%efAkqqdvf)Am)VF$+U3WR z-E#I9H6^)zHLKCs7|Zs<7Bo9VCS3@CDQ;{UTczoEprCKL3ZZW!ffmZFkcWU-V|_M2 zUA9~8tE9<5`59W-UgUmDFp11YlORl3mS3*2#ZHjv{*-1#uMV_oVTy{PY(}AqZv#wF zJVks)%N6LaHF$$<6p8S8Lqn+5&t}DmLKiC~lE{jPZ39oj{wR&fe*LX-z0m}9ZnZ{U z>3-5Bh{KKN^n5i!M79Aw5eY=`6fG#aW1_ZG;fw7JM69qk^*(rmO{|Z6rXy?l=K=#_ zE-zd*P|(sskasO(cZ5L~_{Mz&Y@@@Q)5_8l<6vB$@226O+pDvkFaK8b>%2 zfMtgJ@+cN@w>3)(_uR;s8$sGONbYvoEZ3-)zZk4!`tNzd<0lwt{RAgplo*f@Z)uO` zzd`ljSqKfHJOLxya4_}T`k5Ok1Mpo#MSqf~&ia3uIy{zyuaF}pV6 z)@$ZG5LYh8Gge*LqM_|GiT1*J*uKes=Oku_gMj&;FS`*sfpM+ygN&yOla-^WtIU#$ zuw(_-?DS?6DY7IbON7J)p^IM?N>7x^3)(7wR4PZJu(teex%l>zKAUSNL@~{czc}bR z)I{XzXqZBU3a;7UQ~PvAx8g-3q-9AEd}1JrlfS8NdPc+!=HJ6Bs( zCG!0;e0z-22(Uzw>hkEmC&xj?{0p|kc zM}MMXCF%RLLa#5jG`+}{pDL3M&|%3BlwOi?dq!)KUdv5__zR>u^o|QkYiqr(m3HxF z6J*DyN#Jpooc$ok=b7{UAVM@nwGsr6kozSddwulf5g1{B=0#2)zv!zLXQup^BZ4sv*sEsn)+MA?t zEL)}3*R?4(J~CpeSJPM!oZ~8;8s_=@6o`IA%{aEA9!GELRvOuncE`s7sH91 zmF=+T!Q6%){?lJn3`5}oW31(^Of|$r%`~gT{eimT7R~*Mg@x+tWM3KE>=Q>nkMG$U za7r>Yz2LEaA|PsMafvJ(Y>Xzha?=>#B!sYfVob4k5Orb$INFdL@U0(J8Hj&kgWUlO zPm+R07E+oq^4f4#HvEPANGWLL_!uF{nkHYE&BCH%l1FL_r(Nj@M)*VOD5S42Gk-yT z^23oAMvpA57H(fkDGMx86Z}rtQhR^L!T2iS!788E z+^${W1V}J_NwdwdxpXAW8}#6o1(Uu|vhJvubFvQIH1bDl4J4iDJ+181KuDuHwvM?` z%1@Tnq+7>p{O&p=@QT}4wT;HCb@i)&7int<0#bj8j0sfN3s6|a(l7Bj#7$hxX@~iP z1HF8RFH}irky&eCN4T94VyKqGywEGY{Gt0Xl-`|dOU&{Q;Ao;sL>C6N zXx1y^RZSaL-pG|JN;j9ADjo^XR}gce#seM4QB1?S`L*aB&QlbBIRegMnTkTCks7JU z<0(b+^Q?HN1&$M1l&I@>HMS;!&bb()a}hhJzsmB?I`poqTrSoO>m_JE5U4=?o;OV6 zBZjt;*%1P>%2{UL=;a4(aI>PRk|mr&F^=v6Fr&xMj8fRCXE5Z2qdre&;$_RNid5!S zm^XiLK25G6_j4dWkFqjtU7#s;b8h?BYFxV?OE?c~&ME`n`$ix_`mb^AWr+{M9{^^Rl;~KREplwy2q;&xe zUR0SjHzKVYzuqQ84w$NKVPGVHL_4I)Uw<$uL2-Ml#+5r2X{LLqc*p13{;w#E*Kwb*1D|v?e;(<>vl@VjnFB^^Y;;b3 z=R@(uRj6D}-h6CCOxAdqn~_SG=bN%^9(Ac?zfRkO5x2VM0+@_qk?MDXvf=@q_* z3IM@)er6-OXyE1Z4sU3{8$Y$>8NcnU-nkyWD&2ZaqX1JF_JYL8y}>@V8A5%lX#U3E zet5PJM`z79q9u5v(OE~{by|Jzlw2<0h`hKpOefhw=fgLTY9M8h+?37k@TWpzAb2Fc zQMf^aVf!yXlK?@5d-re}!fuAWu0t57ZKSSacwRGJ$0uC}ZgxCTw>cjRk*xCt%w&hh zoeiIgdz__&u~8s|_TZsGvJ7sjvBW<(C@}Y%#l_ID2&C`0;Eg2Z+pk;IK}4T@W6X5H z`s?ayU-iF+aNr5--T-^~K~p;}D(*GWOAYDV9JEw!w8ZYzS3;W6*_`#aZw&9J ziXhBKU3~zd$kKzCAP-=t&cFDeQR*_e*(excIUxKuD@;-twSlP6>wWQU)$|H3Cy+`= z-#7OW!ZlYzZxkdQpfqVDFU3V2B_-eJS)Fi{fLtRz!K{~7TR~XilNCu=Z;{GIf9KYz zf3h=Jo+1#_s>z$lc~e)l93h&RqW1VHYN;Yjwg#Qi0yzjN^M4cuL>Ew`_-_wRhi*!f zLK6vTpgo^Bz?8AsU%#n}^EGigkG3FXen3M;hm#C38P@Zs4{!QZPAU=m7ZV&xKI_HWNt90Ef zxClm)ZY?S|n**2cNYy-xBlLAVZ=~+!|7y`(fh+M$#4zl&T^gV8ZaG(RBD!`3?9xcK zp2+aD(T%QIgrLx5au&TjG1AazI;`8m{K7^!@m>uGCSR;Ut{&?t%3AsF{>0Cm(Kf)2 z?4?|J+!BUg*P~C{?mwPQ#)gDMmro20YVNsVx5oWQMkzQ? zsQ%Y>%7_wkJqnSMuZjB9lBM(o zWut|B7w48cn}4buUBbdPBW_J@H7g=szrKEpb|aE>!4rLm+sO9K%iI75y~2HkUo^iw zJ3se$8$|W>3}?JU@3h@M^HEFNmvCp|+$-0M?RQ8SMoZ@38%!tz8f8-Ptb@106heiJ z^Bx!`0=Im z1!NUhO=9ICM*+||b3a7w*Y#5*Q}K^ar+oMMtekF0JnO>hzHqZKH0&PZ^^M(j;vwf_ z@^|VMBpcw8;4E-9J{(u7sHSyZpQbS&N{VQ%ZCh{c1UA5;?R} z+52*X_tkDQ(s~#-6`z4|Y}3N#a&dgP4S_^tsV=oZr4A1 zaSoPN1czE(UIBrC_r$0HM?RyBGe#lTBL4~JW#A`P^#0wuK)C-2$B6TvMi@@%K@JAT_IB^T7Zfqc8?{wHcSVG_?{(wUG%zhCm=%qP~EqeqKI$9UivF zv+5IUOs|%@ypo6b+i=xsZ=^G1yeWe)z6IX-EC`F=(|_GCNbHbNp(CZ*lpSu5n`FRA zhnrc4w+Vh?r>her@Ba_jv0Omp#-H7avZb=j_A~B%V0&FNi#!S8cwn0(Gg-Gi_LMI{ zCg=g@m{W@u?GQ|yp^yENd;M=W2s-k7Gw2Z(tsD5fTGF{iZ%Ccgjy6O!AB4x z%&=6jB7^}pyftW2YQpOY1w@%wZy%}-l0qJlOSKZXnN2wo3|hujU+-U~blRF!^;Tan z0w;Srh0|Q~6*tXf!5-rCD)OYE(%S|^WTpa1KHtpHZ{!;KdcM^#g8Z^+LkbiBHt85m z;2xv#83lWB(kplfgqv@ZNDcHizwi4-8+WHA$U-HBNqsZ`hKcUI3zV3d1ngJP-AMRET*A{> zb2A>Fk|L|WYV;Eu4>{a6ESi2r3aZL7x}eRc?cf|~bP)6b7%BnsR{Sa>K^0obn?yiJ zCVvaZ&;d_6WEk${F1SN0{_`(#TuOOH1as&#&xN~+JDzX(D-WU_nLEI}T_VaeLA=bc zl_UZS$nu#C1yH}YV>N2^9^zye{rDrn(rS99>Fh&jtNY7PP15q%g=RGnxACdCov47= zwf^9zfJaL{y`R#~tvVL#*<`=`Qe zj_@Me$6sIK=LMFbBrJps7vdaf_HeX?eC+P^{AgSvbEn?n<}NDWiQGQG4^ZOc|GskK z$Ve2_n8gQ-KZ=s(f`_X!+vM5)4+QmOP()2Fe#IL2toZBf+)8gTVgDSTN1CkP<}!j7 z0SEl>PBg{MnPHkj4wj$mZ?m5x!1ePVEYI(L_sb0OZ*=M%yQb?L{UL(2_*CTVbRxBe z@{)COwTK1}!*CK0Vi4~AB;HF(MmQf|dsoy(eiQ>WTKcEQlnKOri5xYsqi61Y=I4kzAjn5~{IWrz_l))|Ls zvq7xgQs?Xx@`N?f7+3XKLyD~6DRJw*uj*j?yvT3}a;(j_?YOe%hUFcPGWRVBXzpMJ zM43g6DLFqS9tcTLSg=^&N-y0dXL816v&-nqC0iXdg7kV|PY+js`F8dm z2PuHw&k+8*&9SPQ6f!^5q0&AH(i+z3I7a?8O+S5`g)>}fG|BM&ZnmL;rk)|u{1!aZ zEZHpAMmK_v$GbrrWNP|^2^s*!0waLW=-h5PZa-4jWYUt(Hr@EA(m3Mc3^uDxwt-me^55FMA9^>hpp26MhqjLg#^Y7OIJ5%ZLdNx&uDgIIqc zZRZl|n6TyV)0^DDyVtw*jlWkDY&Gw4q;k!UwqSL6&sW$B*5Rc?&)dt29bDB*b6IBY z6SY6Unsf6AOQdEf=P1inu6(6hVZ0~v-<>;LAlcQ2u?wRWj5VczBT$Op#8IhppP-1t zfz5H59Aa~yh7EN;BXJsLyjkjqARS5iIhDVPj<=4AJb}m6M@n{xYj3qsR*Q8;hVxDyC4vLI;;?^eENOb5QARj#nII5l$MtBCI@5u~(ylFi$ zw6-+$$XQ}Ca>FWT>q{k)g{Ml(Yv=6aDfe?m|5|kbGtWS}fKWI+})F6`x@||0oJ^(g|+xi zqlPdy5;`g*i*C=Q(aGeDw!eQg&w>UUj^{o?PrlFI=34qAU2u@BgwrBiaM8zoDTFJ< zh7nWpv>dr?q;4ZA?}V}|7qWz4W?6#S&m>hs4IwvCBe@-C>+oohsQZ^JC*RfDRm!?y zS4$7oxcI|##ga*y5hV>J4a%HHl^t$pjY%caL%-FlRb<$A$E!ws?8hf0@(4HdgQ!@> zds{&g$ocr9W4I84TMa9-(&^_B*&R%^=@?Ntxi|Ejnh;z=!|uVj&3fiTngDPg=0=P2 zB)3#%HetD84ayj??qrxsd9nqrBem(8^_u_UY{1@R_vK-0H9N7lBX5K(^O2=0#TtUUGSz{ z%g>qU8#a$DyZ~EMa|8*@`GOhCW3%DN%xuS91T7~iXRr)SG`%=Lfu%U~Z_`1b=lSi?qpD4$vLh$?HU6t0MydaowUpb zQr{>_${AMesCEffZo`}K0^~x>RY_ZIG{(r39MP>@=aiM@C;K)jUcfQV8#?SDvq>9D zI{XeKM%$$XP5`7p3K0T}x;qn)VMo>2t}Ib(6zui;k}<<~KibAb%p)**e>ln<=qyWU zrRDy|UXFi9y~PdEFIAXejLA{K)6<)Q`?;Q5!KsuEw({!#Rl8*5_F{TP?u|5(Hijv( ztAA^I5+$A*+*e0V0R~fc{ET-RAS3suZ}TRk3r)xqj~g_hxB`qIK5z(5wxYboz%46G zq{izIz^5xW1Vq#%lhXaZL&)FJWp0VZNO%2&ADd?+J%K$fM#T_Eke1{dQsx48dUPUY zLS+DWMJeUSjYL453f@HpRGU6Dv)rw+-c6xB>(=p4U%}_p>z^I@Ow9`nkUG21?cMIh9}hN?R-d)*6%pr6d@mcb*ixr7 z)>Lo<&2F}~>WT1ybm^9UO{6P9;m+fU^06_$o9gBWL9_}EMZFD=rLJ~&e?fhDnJNBI zKM=-WR6g7HY5tHf=V~6~QIQ~rakNvcsamU8m28YE=z8+G7K=h%)l6k zmCpiDInKL6*e#)#Pt;ANmjf`8h-nEt&d}(SBZMI_A{BI#ck-_V7nx)K9_D9K-p@?Zh81#b@{wS?wCcJ%og)8RF*-0z+~)6f#T` zWqF7_CBcnn=S-1QykC*F0YTsKMVG49BuKQBH%WuDkEy%E?*x&tt%0m>>5^HCOq|ux zuvFB)JPR-W|%$24eEC^AtG3Gp4qdK%pjRijF5Sg3X}uaKEE z-L5p5aVR!NTM8T`4|2QA@hXiLXRcJveWZ%YeFfV%mO5q#($TJ`*U>hicS+CMj%Ip# zivoL;dd*araeJK9EA<(tihD50FHWbITBgF9E<33A+eMr2;cgI3Gg6<-2o|_g9|> zv5}i932( zYfTE9?4#nQhP@a|zm#9FST2 z!y+p3B;p>KkUzH!K;GkBW}bWssz)9b>Ulg^)EDca;jDl+q=243BddS$hY^fC6lbpM z(q_bo4V8~eVeA?0LFD6ZtKcmOH^75#q$Eo%a&qvE8Zsqg=$p}u^|>DSWUP5i{6)LAYF4E2DfGZuMJ zMwxxmkxQf}Q$V3&2w|$`9_SQS^2NVbTHh;atB>=A%!}k-f4*i$X8m}Ni^ppZXk5_oYF>Gq(& z0wy{LjJOu}69}~#UFPc;$7ka+=gl(FZCy4xEsk);+he>Nnl>hb5Ud-lj!CNicgd^2 z_Qgr_-&S7*#nLAI7r()P$`x~fy)+y=W~6aNh_humoZr7MWGSWJPLk}$#w_1n%(@? z3FnHf1lbxKJbQ9c&i<$(wd{tUTX6DAKs@cXIOBv~!9i{wD@*|kwfX~sjKASrNFGvN zrFc=!0Bb^OhR2f`%hrp2ibv#KUxl)Np1aixD9{^o=)*U%n%rTHX?FSWL^UGpHpY@7 z74U}KoIRwxI#>)Pn4($A`nw1%-D}`sGRZD8Z#lF$6 zOeA5)+W2qvA%m^|$WluUU-O+KtMqd;Pd58?qZj})MbxYGO<{z9U&t4D{S2G>e+J9K ztFZ?}ya>SVOLp9hpW)}G%kTrg*KXXXsLkGdgHb+R-ZXqdkdQC0_)`?6mqo8(EU#d( zy;u&aVPe6C=YgCRPV!mJ6R6kdY*`e+VGM~`VtC>{k27!9vAZT)x2~AiX5|m1Rq}_= z;A9LX^nd$l-9&2%4s~p5r6ad-siV`HtxKF}l&xGSYJmP=z!?Mlwmwef$EQq~7;#OE z)U5eS6dB~~1pkj#9(}T3j!((8Uf%!W49FfUAozijoxInUE7z`~U3Y^}xc3xp){#9D z<^Tz2xw}@o@fdUZ@hnW#dX6gDOj4R8dV}Dw`u!h@*K)-NrxT8%2`T}EvOImNF_N1S zy?uo6_ZS>Qga4Xme3j#aX+1qdFFE{NT0Wfusa$^;eL5xGE_66!5_N8!Z~jCAH2=${ z*goHjl|z|kbmIE{cl-PloSTtD+2=CDm~ZHRgXJ8~1(g4W=1c3=2eF#3tah7ho`zm4 z05P&?nyqq$nC?iJ-nK_iBo=u5l#|Ka3H7{UZ&O`~t-=triw=SE7ynzMAE{Mv-{7E_ zViZtA(0^wD{iCCcg@c{54Ro@U5p1QZq_XlEGtdBAQ9@nT?(zLO0#)q55G8_Ug~Xnu zR-^1~hp|cy&52iogG@o?-^AD8Jb^;@&Ea5jEicDlze6%>?u$-eE};bQ`T6@(bED0J zKYtdc?%9*<<$2LCBzVx9CA4YV|q-qg*-{yQ;|0=KIgI6~z0DKTtajw2Oms3L zn{C%{P`duw!(F@*P)lFy11|Z&x`E2<=$Ln38>UR~z6~za(3r;45kQK_^QTX%!s zNzoIFFH8|Y>YVrUL5#mgA-Jh>j7)n)5}iVM4%_@^GSwEIBA2g-;43* z*)i7u*xc8jo2z8&=8t7qo|B-rsGw)b8UXnu`RgE4u!(J8yIJi(5m3~aYsADcfZ!GG zzqa7p=sg`V_KjiqI*LA-=T;uiNRB;BZZ)~88 z`C%p8%hIev2rxS12@doqsrjgMg3{A&N8A?%Ui5vSHh7!iC^ltF&HqG~;=16=h0{ygy^@HxixUb1XYcR36SB}}o3nxu z_IpEmGh_CK<+sUh@2zbK9MqO!S5cao=8LSQg0Zv4?ju%ww^mvc0WU$q@!oo#2bv24 z+?c}14L2vlDn%Y0!t*z=$*a!`*|uAVu&NO!z_arim$=btpUPR5XGCG0U3YU`v>yMr z^zmTdcEa!APX zYF>^Q-TP11;{VgtMqC}7>B^2gN-3KYl33gS-p%f!X<_Hr?`rG8{jb9jmuQA9U;BeG zHj6Pk(UB5c6zwX%SNi*Py*)gk^?+729$bAN-EUd*RKN7{CM4`Q65a1qF*-QWACA&m zrT)B(M}yih{2r!Tiv5Y&O&=H_OtaHUz96Npo_k0eN|!*s2mLe!Zkuv>^E8Xa43ZwH zOI058AZznYGrRJ+`*GmZzMi6yliFmGMge6^j?|PN%ARns!Eg$ufpcLc#1Ns!1@1 zvC7N8M$mRgnixwEtX{ypBS^n`k@t2cCh#_6L6WtQb8E~*Vu+Rr)YsKZRX~hzLG*BE zaeU#LPo?RLm(Wzltk79Jd1Y$|6aWz1)wf1K1RtqS;qyQMy@H@B805vQ%wfSJB?m&&=^m4i* zYVH`zTTFbFtNFkAI`Khe4e^CdGZw;O0 zqkQe2|NG_y6D%h(|EZNf&77_!NU%0y={^E=*gKGQ=)LdKPM3zUlM@otH2X07Awv8o zY8Y7a1^&Yy%b%m{mNQ5sWNMTIq96Wtr>a(hL>Qi&F(ckgKkyvM0IH<_}v~Fv-GqDapig=3*ZMOx!%cYY)SKzo7ECyem z9Mj3C)tCYM?C9YIlt1?zTJXNOo&oVxu&uXKJs7i+j8p*Qvu2PAnY}b`KStdpi`trk ztAO}T8eOC%x)mu+4ps8sYZ=vYJp16SVWEEgQyFKSfWQ@O5id6GfL`|2<}hMXLPszS zgK>NWOoR zBRyKeUPevpqKKShD|MZ`R;~#PdNMB3LWjqFKNvH9k+;(`;-pyXM55?qaji#nl~K8m z_MifoM*W*X9CQiXAOH{cZcP0;Bn10E1)T@62Um>et2ci!J2$5-_HPy(AGif+BJpJ^ ziHWynC_%-NlrFY+(f7HyVvbDIM$5ci_i3?22ZkF>Y8RPBhgx-7k3M2>6m5R24C|~I z&RPh9xpMGzhN4bii*ryWaN^d(`0 zTOADlU)g`1p+SVMNLztd)c+;XjXox(VHQwqzu>FROvf0`s&|NEv26}(TAe;@=FpZq zaVs6mp>W0rM3Qg*6x5f_bPJd!6dQGmh?&v0rpBNfS$DW-{4L7#_~-eA@7<2BsZV=X zow){3aATmLZOQrs>uzDkXOD=IiX;Ue*B(^4RF%H zeaZ^*MWn4tBDj(wj114r(`)P96EHq4th-;tWiHhkp2rDlrklX}I@ib-nel0slFoQO zOeTc;Rh7sMIebO`1%u)=GlEj+7HU;c|Nj>2j)J-kpR)s3#+9AiB zd$hAk6;3pu9(GCR#)#>aCGPYq%r&i02$0L9=7AlIGYdlUO5%eH&M!ZWD&6^NBAj0Y9ZDcPg@r@8Y&-}e!aq0S(`}NuQ({;aigCPnq75U9cBH&Y7 ze)W0aD>muAepOKgm7uPg3Dz7G%)nEqTUm_&^^3(>+eEI;$ia`m>m0QHEkTt^=cx^JsBC68#H(3zc~Z$E9I)oSrF$3 zUClHXhMBZ|^1ikm3nL$Z@v|JRhud*IhOvx!6X<(YSX(9LG#yYuZeB{=7-MyPF;?_8 zy2i3iVKG2q!=JHN>~!#Bl{cwa6-yB@b<;8LSj}`f9pw7#x3yTD>C=>1S@H)~(n_K4 z2-yr{2?|1b#lS`qG@+823j;&UE5|2+EdU4nVw5=m>o_gj#K>>(*t=xI7{R)lJhLU{ z4IO6!x@1f$aDVIE@1a0lraN9!(j~_uGlks)!&davUFRNYHflp<|ENwAxsp~4Hun$Q z$w>@YzXp#VX~)ZP8`_b_sTg(Gt7?oXJW%^Pf0UW%YM+OGjKS}X`yO~{7WH6nX8S6Z ztl!5AnM2Lo*_}ZLvo%?iV;D2z>#qdpMx*xY2*GGlRzmHCom`VedAoR=(A1nO)Y>;5 zCK-~a;#g5yDgf7_phlkM@)C8s!xOu)N2UnQhif-v5kL$*t=X}L9EyBRq$V(sI{90> z=ghTPGswRVbTW@dS2H|)QYTY&I$ljbpNPTc_T|FEJkSW7MV!JM4I(ksRqQ8)V5>}v z2Sf^Z9_v;dKSp_orZm09jb8;C(vzFFJgoYuWRc|Tt_&3k({wPKiD|*m!+za$(l*!gNRo{xtmqjy1=kGzFkTH=Nc>EL@1Um0BiN1)wBO$i z6rG={bRcT|%A3s3xh!Bw?=L&_-X+6}L9i~xRj2}-)7fsoq0|;;PS%mcn%_#oV#kAp zGw^23c8_0~ ze}v9(p};6HM0+qF5^^>BBEI3d=2DW&O#|(;wg}?3?uO=w+{*)+^l_-gE zSw8GV=4_%U4*OU^hibDV38{Qb7P#Y8zh@BM9pEM_o2FuFc2LWrW2jRRB<+IE)G=Vx zuu?cp2-`hgqlsn|$nx@I%TC!`>bX^G00_oKboOGGXLgyLKXoo$^@L7v;GWqfUFw3< zekKMWo0LR;TaFY}Tt4!O$3MU@pqcw!0w0 zA}SnJ6Lb597|P5W8$OsEHTku2Kw9y4V=hx*K%iSn!#LW9W#~OiWf^dXEP$^2 zaok=UyGwy3GRp)bm6Gqr>8-4h@3=2`Eto2|JE6Sufh?%U6;ut1v1d@#EfcQP2chCt z+mB{Bk5~()7G>wM3KYf7Xh?LGbwg1uWLotmc_}Z_o;XOUDyfU?{9atAT$={v82^w9 z(MW$gINHt4xB3{bdbhRR%T}L?McK?!zkLK3(e>zKyei(yq%Nsijm~LV|9mll-XHavFcc$teX7v);H>=oN-+E_Q{c|! zp
    JV~-9AH}jxf6IF!PxrB9is{_9s@PYth^`pb%DkwghLdAyDREz(csf9)HcVRq z+2Vn~>{(S&_;bq_qA{v7XbU?yR7;~JrLfo;g$Lkm#ufO1P`QW_`zWW+4+7xzQZnO$ z5&GyJs4-VGb5MEDBc5=zxZh9xEVoY(|2yRv&!T7LAlIs@tw+4n?v1T8M>;hBv}2n) zcqi+>M*U@uY>4N3eDSAH2Rg@dsl!1py>kO39GMP#qOHipL~*cCac2_vH^6x@xmO|E zkWeyvl@P$2Iy*mCgVF+b{&|FY*5Ygi8237i)9YW#Fp& z?TJTQW+7U)xCE*`Nsx^yaiJ0KSW}}jc-ub)8Z8x(|K7G>`&l{Y&~W=q#^4Gf{}aJ%6kLXsmv6cr=Hi*uB`V26;dr4C$WrPnHO>g zg1@A%DvIWPDtXzll39kY6#%j;aN7grYJP9AlJgs3FnC?crv$wC7S4_Z?<_s0j;MmE z75yQGul2=bY%`l__1X3jxju2$Ws%hNv75ywfAqjgFO7wFsFDOW^)q2%VIF~WhwEW0 z45z^+r+}sJ{q+>X-w(}OiD(!*&cy4X&yM`!L0Fe+_RUfs@=J{AH#K~gArqT=#DcGE z!FwY(h&+&811rVCVoOuK)Z<-$EX zp`TzcUQC256@YWZ*GkE@P_et4D@qpM92fWA6c$MV=^qTu7&g)U?O~-fUR&xFqNiY1 zRd=|zUs_rmFZhKI|H}dcKhy%Okl(#y#QuMi81zsY56Y@757xBQqDNkd+XhLQhp2BB zBF^aJ__D676wLu|yYo6jNJNw^B+Ce;DYK!f$!dNs1*?D^97u^jKS++7S z5qE%zG#HY-SMUn^_yru=T6v`)CM%K<>_Z>tPe|js`c<|y7?qol&)C=>uLWkg5 zmzNcSAG_sL)E9or;i+O}tY^70@h7+=bG1;YDlX{<4zF_?{)K5B&?^tKZ6<$SD%@>F zY0cl2H7)%zKeDX%Eo7`ky^mzS)s;842cP{_;dzFuyd~Npb4u!bwkkhf8-^C2e3`q8>MuPhgiv0VxHxvrN9_`rJv&GX0fWz-L-Jg^B zrTsm>)-~j0F1sV=^V?UUi{L2cp%YwpvHwwLaSsCIrGI#({{QfbgDxMqR1Z0TcrO*~ z;`z(A$}o+TN+QHHSvsC2`@?YICZ>s8&hY;SmOyF0PKaZIauCMS*cOpAMn@6@g@rZ+ z+GT--(uT6#mL8^*mMf7BE`(AVj?zLY-2$aI%TjtREu}5AWdGlcWLvfz(%wn72tGczwUOgGD3RXpWs%onuMxs9!*D^698AupW z9qTDQu4`!>n|)e35b4t+d(+uOx+>VC#nXCiRex_Fq4fu1f`;C`>g;IuS%6KgEa3NK z<8dsc`?SDP0g~*EC3QU&OZH-QpPowNEUd4rJF9MGAgb@H`mjRGq;?wFRDVQY7mMpm z3yoB7eQ!#O#`XIBDXqU>Pt~tCe{Q#awQI4YOm?Q3muUO6`nZ4^zi5|(wb9R)oyarG?mI|I@A0U!+**&lW7_bYKF2biJ4BDbi~*$h?kQ`rCC(LG-oO(nPxMU zfo#Z#n8t)+3Ph87roL-y2!!U4SEWNCIM16i~-&+f55;kxC2bL$FE@jH{5p$Z8gxOiP%Y`hTTa_!v{AKQz&- ztE+dosg?pN)leO5WpNTS>IKdEEn21zMm&?r28Q52{$e2tGL44^Ys=^?m6p=kOy!gJ zWm*oFGKS@mqj~{|SONA*T2)3XC|J--en+NrnPlNhAmXMqmiXs^*154{EVE{Uc%xqF zrbcQ~sezg;wQkW;dVezGrdC0qf!0|>JG6xErVZ8_?B(25cZrr-sL&=jKwW>zKyYMY zdRn1&@Rid0oIhoRl)+X4)b&e?HUVlOtk^(xldhvgf^7r+@TXa!2`LC9AsB@wEO&eU2mN) z(2^JsyA6qfeOf%LSJx?Y8BU1m=}0P;*H3vVXSjksEcm>#5Xa`}jj5D2fEfH2Xje-M zUYHgYX}1u_p<|fIC+pI5g6KGn%JeZPZ-0!!1})tOab>y=S>3W~x@o{- z6^;@rhHTgRaoor06T(UUbrK4+@5bO?r=!vckDD+nwK+>2{{|{u4N@g}r(r z#3beB`G2`XrO(iR6q2H8yS9v;(z-=*`%fk%CVpj%l#pt?g4*)yP|xS-&NBKOeW5_5 zXkVr;A)BGS=+F;j%O|69F0Lne?{U*t=^g?1HKy7R)R*<>%xD>K zelPqrp$&BF_?^mZ&U<*tWDIuhrw3HJj~--_0)GL8jxYs2@VLev2$;`DG7X6UI9Z)P zq|z`w46OtLJ1=V3U8B%9@FSsRP+Ze)dQ@;zLq|~>(%J5G-n}dRZ6&kyH|cQ!{Vil( zBUvQvj*~0_A1JCtaGZW|?6>KdP}!4A%l>(MnVv>A%d;!|qA>*t&-9-JFU4GZhn`jG z8GrgNsQJ%JSLgNFP`5;(=b+M9GO8cg+ygIz^4i?=eR@IY>IcG?+on?I4+Y47p-DB8 zjrlar)KtoI{#kBcqL&4?ub@Df+zMt*USCD_T8O$J$~oMrC6*TP7j@H5trGV$r0P6I zV7EZ{MWH`5`DrX*wx&`d;C`jjYoc_PMSqNB290QXlRn_4*F{5hBmEE4DHBC$%EsbR zQGb7p;)4MAjY@Bd*2F3L?<8typrrUykb$JXr#}c1|BL*QF|18D{ZTYBZ_=M&Ec6IS ziv{(%>CbeR(9Aog)}hA!xSm1p@K?*ce*-6R%odqGGk?I4@6q3dmHq)4jbw+B?|%#2 zbX;ioJ_tcGO*#d0v?il&mPAi+AKQvsQnPf*?8tX6qfOPsf-ttT+RZX6Dm&RF6beP3 zdotcJDI1Kn7wkq=;Au=BIyoGfXCNVjCKTj+fxU@mxp*d*7aHec0GTUPt`xbN8x%fe zikv87g)u~0cpQaf zd<7Mi9GR0B@*S&l&9pCl-HEaNX?ZY8MoXaYHGDf}733;(88<{E%)< z^k)X#To3=_O2$lKPsc9P-MkDAhJ~{x<=xTJw2aRY5SSZIA6Gij5cFzsGk@S)4@C65 zwN^6CwOI9`5c(3?cqRrH_gSq+ox(wtSBZc-Jr5N%^t3N&WB|TT_i4!i3lxwI=*p)Y zn7fb%HlXhf8OGjhzswj!=Crh~YwQYb+p~UaV@s%YPgiH_);$|Gx3{{v5v?7s<)+cb zxlT0Bb!OwtE!K>gx6c4v^M9mL0F=It*NfQL0J0O$RCpt746=H1pPNG#AZC|Y`SZt( zG`yKMBPV_0I|S?}?$t7GU%;*_39bCGO*x3+R|<=9WNe!8jH- zw5ZJS(k@wws?6w1rejjyZ>08aizReJBo%IRb3b3|VuR6Uo&sL?L5j(isqs%CYe@@b zIID7kF*hyqmy+7D(SPa^xNVm54hVF3{;4I9+mh)F22+_YFP>ux`{F)8l;uRX>1-cH zXqPnGsFRr|UZwJtjG=1x2^l_tF-mS0@sdC38kMi$kDw8W#zceJowZuV=@agQ_#l5w znB`g+sb1mhkrXh$X4y(<-CntwmVwah5#oA_p-U<_5$ zGDc%(b6Z=!QQ%w6YZS&HWovIaN8wMw1B-9N+Vyl=>(yIgy}BrAhpc2}8YL-i*_KY7 ztV+`WKcC?{RKA@t3pu*BtqZJFSd2d)+cc07-Z#4x&7Dnd{yg6)lz@`z%=Sl-`9Z~*io zck_Lshk9JRJs=t>1jmKB~>`6+(J z@(S}J2Q{Q{a-ASTnIViecW(FIagWQ%G41y?zS)gpooM z@c<2$7TykMs4LH*UUYfts(!Ncn`?eZl}f zg)wx@0N0J(X(OJ^=$2()HLn)=Cn~=zx(_9(B@L04%{F_Zn}5!~5Ec5D4ibN6G_AD} zzxY^T_JF##qM8~B%aZ1OC}X^kQu`JDwaRaZnt!YcRrP7fq>eIihJW1UY{Xhkn>NdX zKy|<6-wD*;GtE08sLYryW<-e)?7k;;B>e$u?v!QhU9jPK6*Y$o8{Tl`N`+QvG ze}71rVC)fis9TZ<>EJ2JR`80F^2rkB7dihm$1Ta2bR?&wz>e`)w<4)1{3SfS$uKfV z3R=JT!eY+i7+IIfl3SIgiR|KvBWH*s;OEuF5tq~wLOB^xP_Dc7-BbNjpC|dHYJrZCWj-ucmv4;YS~eN!LvwER`NCd`R4Xh5%zP$V^nU>j zdOkNvbyB_117;mhiTiL_TBcy&Grvl->zO_SlCCX5dFLd`q7x-lBj*&ykj^ zR3@z`y0<8XlBHEhlCk7IV=ofWsuF|d)ECS}qnWf?I#-o~5=JFQM8u+7I!^>dg|wEb zbu4wp#rHGayeYTT>MN+(x3O`nFMpOSERQdpzQv2ui|Z5#Qd zB(+GbXda|>CW55ky@mG13K0wfXAm8yoek3MJG!Hujn$5)Q(6wWb-l4ogu?jj2Q|srw?r z-TG0$OfmDx%(qcX`Fc`D!WS{3dN*V%SZas3$vFXQy98^y3oT~8Yv>$EX0!uiRae?m z_}pvK=rBy5Z_#_!8QEmix_@_*w8E8(2{R5kf^056;GzbLOPr2uqFYaG6Fkrv($n_51%7~QN<>9$WdjE=H}>(a41KM%d2x#e@K3{W|+=-h*mR&2C01e z2sMP;YjU)9h+1kxOKJ+g*W=&D@=$q4jF%@HyRtCwOmEmpS|Rr9V_2br*NOd^ z4LN#oxd5yL=#MPWN{9Vo^X-Wo{a7IF2hvYWB%eUCkAZq+=NQ=iLI9?~@ zr+|ky4Rgm7yEDuc2dIe941~qc8V_$7;?7|XLk6+nbrh}e&Tt20EWZ@dRFDoYbwhkn zjJ$th974Z0F${3wtVLk_Ty;*J-Pi zP0IwrAT!Lj34GcoSB8g?IKPt%!iLD-$s+f_eZg@9q!2Si?`F#fUqY`!{bM0O7V^G%VB|A zyMM>SKNg|KKP}+>>?n6|5MlPK3Vto&;nxppD;yk@z4DXPm0z9hxb+U&Fv4$y&G>q= z799L0$A2&#>CfSgCuu$+9W>s<-&yq3!C{F9N!{d?I|g|+Qd9@*d;GplgY5Fk$LOV+ zoMealKns!!80PWsJ%(}L61B!7l?j1_5P#LRrVv%NBhs{R`;aufHYb&b+mF%A+DGl5 zBemAHtbLFi++KT(wv9*?;awp>ROX~P?e<4#Uf5RKIV{c3NxmUz!LYO#Cxdz*CoRQp zSvX|#NN06=q_eTU5-T!RmUJ?Ht=XQF8t)f+GnY5nY5>-}WLR1+R5pou?l@Y|F@KEX zk=jh-yq=Rn9;riE*;Slo}PfNKhXO#;FrZCf%VZ9h7W z<63YWE^s_SlAVQh6B(En9i<9%4AT|2bTQ4Ph2)pI?f2S`$j?bp`>_3(`Fz&?ig-FJ zoO7KAh@4BDOU>sBXV84Eajr9;>wlbW&OSUt&dug?oAV;`+3oBzpI18%%1wA4blzmb z-{QPYJmn_2-F$A5JI!a8+-p8Bk*^U?^f5j7uZ}jEz0E3;XbahB2iZwS&l4jj4WRS6 z3O&!w=ymQSl~7LUE99noXd2y1)9E>yK`+ouR%sTOQ@Qjt@<;lErGLk1wrw7r zV)M})+amJXs_9hQa++&vrqgU&Xr8T)=G&5Vy6vOnvt37L*nU7&ws&ZO-9`)TGA**t zpby#0X|df;etRud+s~#Y_7zlPZ=_oLg%q&wraF6s>g@;VO#2sUseO=^+3%&Z?61(- z_IKzU`+Kw;Blil&LR#qv&{rzQnG|%i(Q3zLI@gh)2FE^H;~1dx9G|AOj(e%mSwT(C z71Zp!jar*i3S|_ik_3{n0L4KavYWWZ2x3MhyU!66E$h=L+A&-s$9X_w9Q_e;+`-{ZW# z^Zn2H_I~`}!vGeFRRY^DyKK#pORBr{&?X}ut`1a(x__(dt3y_-*Np0pX~q39D{Rns z!iXBWZO~+oZu>($Mrf0rjM>$JZar!n_0_!*e@yT7n=HfVT6#jbYZ0wYEXnTgPDZ0N zVE5?$1-v94G2@1jFyj##-E1Um(naG-8WuGy@rRAg)t9Oe0$RJ3OoWV8X4DXvW+ftx zk%S(O8h?#_3B9-1NHn&@ZAXtr=PXcAATV*GzFBXK>hVb9*`iMM-zvA6RwMH#2^901uxUFh&4fT% zmP?pjNsiRIMD)<6xZyOeThl_DN_ZJ*?KUIHgnx{vz`WKxj&!7HbM8{w?{Rued(M1v zKHsK{_q=YI88@Bf0*RW@cIV@=<{eGsG21xrTrWycT7*KBd!eD2zb1R(O@H~k7>Duv zHPwp=n8;t#1>7~fuM9IaD5w%BpwLtNCe_Sq9eal4oj2DB1#<+(MGR-P&Ig%3t%=!< zS$|KxI1a~an2Q>L$s;1$9nQJal4dk)Box$YsAKgCiEGni##jr|%So6Y4J@pYBF!;~ zhXwpKhc7&QZ$=e~Sb&ABZ4o)&U~N*dSU`2G^eQh-WCe9tA}~Ae369btLlB{GjOKB@yEDH!C7Q&df^#X zi~?{rCuAE|kAjKzt+r#t6s)1h840@A<%i5(O;$Q&tD(opg0)yzgm#=ucf4CSqkqYS zaTdivk5I~#=1Z9K5M*uV6H??6s9*ynT`vzr2@%Tkr4k+Tr_ib40$fPP7$yLA$cwJ@ zF@`94=op)$x^0t+QAsNY$pi!4e7hp~gO=|yD=^8JTvTiC(HAamYEQ}t z+hR~QoKTOz%)IHEg&6iC4vP=3mw&u4wvcSwi$vNBGQE5RoSUs^l+u{A+6s~aMMkXG z+1g4wD8^Y27Oe4f``K{+tm76n(*d6BUA4;pLa26`6RD6?Rq?2K1yMXVAk`&xbks*~{+``Mhg4cQEuw+aM zaI9{}9en8DCh*S9CojIk)qh|k?#iNiCQ}rAmr&iYRJiND ztt+j*c+}Fv&6x&7U~!(Sb1eAz1N@Nf`w?YxGJdhy+seiNNZEYIG1_<^?&pm^P8W?d ze(p@$nWC`Pxqpf8d&AIGNJn#Ty)j z1NbA^Y}pNQ>OfTdiAp+WR>C6390IrFj;YZglitGH8r7(GvVRpWjZd7|r24M{u66B) zs#VS$?R*!1FT&sO-ssvW8s5jh$-O=^9=7^y z75||~QA6zLW}Lu!YOZh1J$j46m zNH|;^a$U_RKgla5h>5(igl^ek(~2nL5a_0}ipvA_Xf0k*E-ExJNld0{LZ;F^DzqAL+IZGJ7<3i1szf zxMRkQ(|@;wj9%I7h{c*{;?g%giylU}Dz{iwb(1vGK<-vlnKs!|Mb9}iTt)Rl&NZka zkkugrMiY(ng3QseY!npaOf1jo3|r35nK+eTYh*`DHabuv@IFy zG7@V!LWE0&)bvqgQ8=-L-(vt#Z-&xaOj3G@Nqw1FfbNQ`!bFEl@z)0)+#Z5e#_hQ|Rd!KrEoRn^aFz zkzYzz%hher>ixcg6fW`=rr>Nx@enQ!sQqYR{<2^|eUfw?e8;B_`T)Kxkp8${U>g?k*VhCd zp^yYLvi}<#5TDjrx@{0U$jx*tQn+mhcXsq2e46a@44^-Sd;C6S2=}sK1LQ_OUhgO` z^4yN+e9Dv9TQ64y1Bw)0i4u)98(^+@R~eUUsG!Ye84 zFa7-?x3cqUXX)$G<2MgYiGWhjq?Q-CE(|sm-68_z>h_O2vME5nX;RodIf)=No(={I z_<&3QJcPg8kAI}_Vd+OH4z{NsFMmjv3;kunMSh94VNnqD?85uOps%nq=q?kU_JT5@ zwih;eQlhxr)7d^K#-~InWlc&<*#?{A(8f^+C_WmRR{B&Yh3pxhLU9-toLz%rCPi}} zE!cw^pQlXB3aACUpacU&ZlBUl(Jo4fxpbDVwDn^m{VG||ar9B)9}@K`(SJxmAWro& z_3yzfUqLoXg`H($!I;FTudPdo6FTJm2@^S|&42H(XbSRW7!)V&=I`{;mWicu@BT7z zQs!)F9t-K|aFaMsoJ_6z-ICrzjW5#yJRs>~)bugki)ST$8T%!D4F@EBliCNSA5!fl zN;OuKbR3m0rj=rrq}5`nq<<%iHIl|euXt6QA}$hFNqV)oR?_Rm4oPnoLy|ru_DQ-= zJTDFa;zjY2p{sg zWqz0I5y>-U{xR1Rl4r{NQ?6Ge&y@N7t~Vsll=-(^?@FF2^Y6JnkbgW==09{7N}eh4 z?h`%x-LM8D}+*41ZA#EG0D9KQjc2#z59Pq zO9u!y^MeiK3jhHB6_epc9Fs0q7m}w4lLmSnf6Gb(F%*XXShZTmYQ1gTje=G?4qg`Z zf*U~;6hT37na-R}qnQiIv@S#+#J6xEf(swOhZ4_JMMMtdob%^9e?s#9@%jc}19Jk8 z4-eKFdIEVQN4T|=j2t&EtMI{9_E$cx)DHN2-1mG28IEdMq557#dRO3U?22M($g zlriC81f!!ELd`)1V?{MBFnGYPgmrGp{4)cn6%<#sg5fMU9E|fi%iTOm9KgiN)zu3o zSD!J}c*e{V&__#si_#}hO9u$51d|3zY5@QM=aUgu9h0?tFMkPm8^?8iLjVN0f)0|R zWazNhlxTrCNF5d_LAD%TwkbkKL>+-8TV4VSawTAw*fNnD^2giQT{goNRR~OwAH5%vorH%=FNNm``;VB z_N`CeB%?_hv?RK-S(>S)VQBau{&NwD>j_ zF-Hwk*KNZb#pqexc5oKPcXjOO*cH#{XIq~NkPxH{TYm*Rtv_hwbV2JZd$e=Z)-pN0 z^PH`XkLz~lpy{|;F6Sq&pjD@}vs!0PGe z6v$ZT%$%iV1Z}J(*k7K8=sNv;I#+Ovvr?~~bXs?u{hF!CQ|_-`Y?!WYn_8|j3&GBu zl|F+DcYh8nxg49<-)ESHyI0Vo;oInYTMcVX9@5;g9>>x1BRMQ@KPJc%Za)^J6|_nr zKQ#*4^Z(G>Pt6Lgrp6!zX?X+rXibm;)WBbN1WBP~{Iw45)a0toTeof%G+Oh5Wryxb zN@p5YCm&YsN!Jd$jG8^|w^_Wo-1ad{*|(#*+kcnS97j-dxV>sGIk+cCchX&K1yxY6 z`dB};!Xf&3!*LyHut$Qlnc5WEME3}4k)j3H$aVHvxg78Y3_E@b3u@5wjX7b zPLz^7h65uMRj8d}5Y1tP55ozK;r0{r?;WHL>g4laujaX3dTd*h+xuy|LOa-f%M7RA zuz#V1WlscYXGzO0Xsu-c>6UPEVQ}o>+w7v~meKw6 zfS|`8k|tL(5VDPt0$*C)(&lVYGnVeCrsb+>%XBrvR5fz~VkMmn-RV#V&X1#`XH?fx zvxb>b_48WV%}uD=X5}V20@O1vluQ2hQ-2>^k+tl+2Al20(<||vxfpIJ~|9`dJ zVH^pxv&RS97h5DqN9ZW4!UT{rMgsH>#tHOouVIW{%W|QnHohN<4ZE5RR@l7FPk$#A zI?0%8pKlXW%QH2&OfWTY{1~5fO3=QyMi3vb*?iSmEU7hC;l7%nHAo*ucA`RmedXLF zXlD(SytNYn`{9Rs;@fw21qcpYFGUH*Xmdk{4fK z0AKh-FGJC#f0Ik!{d{T7B7elr2J8>e z4=VKi^h2D=Q8&0_LHc1j$T9pQ7-FcHxZj3w-{RF}MXBm@?_X&zG?V%-Bet=g# zgEZn=6W?w3jeoQ(!&ECWHqJ zs;lJ@+Tf9MhC9~LX7*WT*0A%cJEpn#(bX;0i-*TF1j2A3zeOFlEi7~=R7B$hpH(7@ zc$q9Z%JU#Am8%BTa1gvUGZPX)hL@#()Y8UP?D?tiCHan51waKUtqypCE-ALn&``k4jkeO@}6ROkhI5oJaRd?*oW z5XmD5>YOZAT4pPd`M`dOKE|;8c#wXMeqKQ__X$u$!F<91^W0T4GtRNpyh;fxIv+8{ zOV!mig|0Jq`E}FfEGH;5uUHx|3whm^-h~cRG|loa&)cs`#D7mW5K(xZ?6+)vAgAZC zD+2J-T)KRUZh~%1{k&VASQx^y`SF+OS6KX4kyjRJJpeT){PgS47=e2L=`KjGaKL_s zUIno%SwM4WAF(xl=4hpof(h_9QEfU}Rt7%rCFq{-h?=0}Z_#HJdX0XYPezSbpFe{d z0C)YJ60>{(bbnZJLT@3P<#<0>aI5md?+Lo2+D-Fke_x?5v0p-So~;%rL+cL|`Xc=y zDo2?BXJ-XJpB{>GjhRUa08Q0fc~|Te5H?$jM>&XZG_?d?@$c3DX04&{U<}^Kj^=z zll8%>K>i=dqr$~=S9jB6O9hsxyPZc556Zw=j_nVDRZX|_LS7YaUr=}9egcpXb&Lyu z)YmbNGJh^0d;nj66%_}BAGOYHUX^~)0N68LkJ^TyJHrdKncoeHWg@5uMJ!*CaF?vi zs}inQ2`7nFmB(0lPrqn_`mS~KaI)&6rO6}?TrFA@(Ja=?UzYTXI{;CnCeCzb>5&FP zU9f&`4m+(A>lG0a8$bbgJoRdhk?tvg@Ikz#RDUy9`Bv_`)Mkhjai_S8ErG{n6Y!ZX zjPs#^rE8v{eXb(WZW}1zS0~dl)qaDzZc6#Eb{ck_GRA z#30&5L=j;Tg=w(=Im_LHt$@}KL1QA*~192~ak5Zap zUm99S=A}`1@@=9=5f6x7EHE6dJZ-x$j_M#N`oWZ#8SoMRTSbJEkaI_E1S`LPb#u`l za~4L#=6*e^6>@H+e`vvSoIfb`u^orz|9^Gmf4h-i>_^V46i#@Dxdo?h3>Vd9UB7Q1 zd*h%uq=*CJ?O?Lm(&(J#sK(r_I|5=@p*QJ8=tPJL3W(!iGFv{}j#xpF;@rMTpd4td z<_1}s1;k09u3T^?RJY`6H5?F+aq(TFbgz!+$2p?$R`cYY_JBwWirgNmvn*Q5HGe{f z-XaT1oDGR#3t6;+$vF}g;7xCzl>r&9Od6(sppYNY?IXMuZ9`V@!`mKeeSE_wM4Gd+URu(#jex(s}ep9w1GC3 z7Kw+jq#o_EXrxGYA1~6D%cM+Ge1B+?9*7ocTWaW4s-L{|jmQn!kxEX{y*KxIy1Xsk zjnC7@NQ-xSD&Z?q_a#!IA$;sPe$gu?Z@nHJio8s36Lg7G@2AP18uG-3n|dSD^zhIP z+Lua-$Q13Lqz^#~2=HF178_n9HXiZ3Ovmd`>ukdKrc^2!X-ZAeBT)7dg@2>+{JWz! z=p-xnDEg15lCRLp=uPi))DZP-pCqq%wfcyWMMo@`orpju`U#jwh%@+&z~1$+@gb_i z)6qj`VXXJU%FkkS64rkme)%TMc?)t4l%`DCsP&j<&wVcTDtWIqWv3~3;0Bqggf}`x z?`&K}p9&;=Aun6(T&k=7S$}GZhkTxv`XW6!32V~_TI%bru-U&74|$7pp-A6@^%t>z zik|j#`C5GOo6l26yv4Vpk#1d>ruU>0Sp1{7@3N40)z%`t|2VeC&_KN}@=GU4?^hP}~YUu?KOKHT)vA#ce-FMp(9pP!wPTFk%# zEwqky;$|C=p1Ezu@6K6!t$>6N_Ie-e^%}k#xcn}ovllZSv|SPDuQ-}tU^i{{+`l1; z+iYOZMxq` zyNmevH37(cCUt;!hJWefMf#0t`kVyL=P%JpzSQp?pS<i{A@amJ0F;?aT#H3gGL(m+ zMd2x(2y7PxEPwgIW>H_-O1kRG@$x~jQ_UiPlcvRrqG+t>u>Js>8_Xp<>`syJiiA&! ztVK|;R}+4AD**Ck_Nds%Xh&S}{}jiCxVtDeH;a2t6-Dft*jg0#%HQsyNF;oXVK{$( zQQY6LPpMO5t9niY*so`U_cqrfS%ttA> zMrrXr{mf-r8(+hNdUxQONMdM>QWS?n{+OpF2q5te-AZ?0^44=hA%DU`#Rc;$`A425WvPKyy?$o4V#Hc#hepIh#q zrzgc`^ts)D{=4V}+2@w~FVe?kpIh#KoUY0~x7_FGtMoP5=a&0# zq5$MRx9AIxXym?ZxgQhVvd=B|)8ZMaXDKe4fFb_31FMfwok)^Lq|q0WrRvD@ZBR=G z2pQ0I&-V@h0C*ge;YJ*jtBNjvYflqF6o%gs=t3z%xd|2&*IQdyR=^LH8WYpRgrrep z4Mx6Aw}fxhSE$jN_`x6Gk20R2MM&C)-R$h{nfE#GnVgwFe}DZ3unAM( z^yK7C>62cU)*<-~eOtHo^)=lJyq4q2*a>{Y3mU}nkX(`x@nlm*hSem0>o7{ZNZ;O< zZbWN(%QigOG8~nI>Q5dw>RYT0OXvK4;<_A&n$p-%65n=wqR{bejviAOu@}cn>s#w3 zqd~{|=TQiObS+3ii(WV`2`mPoZQ7x1xMY3^WvfM@Sq*HPLJh+LQwQ=`ny&P1^Hu$T ztXM-zVD=*VoC&`n>n>@37!?>fN*sy>#GXLvspC8GGlAj!USU^YC|}skAcN~^Xqe0( zjqx#zAj>muU<=IUs~34|v06u2ahGbSeT-uAG|Vv*Bw$#pf8#qXFt zMfw|VuC{UeT)2WpJ6&O+E6jF;;~n9>cf~Ip6j-_@&PGFD0%Vu*QJ@Ht`C7Og!xt#L> zmqlJGEh<%*ATJUmZc(FfNSB##fy_`Y-70r{Iv3jEfR|~Ii!xC44vZ(KNj#>kjsE86 zE3FB*OayD~$|}3Y&(h6^X|1 z(TcJ}8{Ua3yL1loSfg!2gTekntVO7WNyFQCfwF2ti$UvL8C6{{IPBg01XK~$ThIQx z{)~aw>(9F2L#G36*kRDPqA$P*nq=!@bbQ#RzDpVIfYc*x9=}2N^*2z1E%3epP)i30 z>M4^xlbnuWe_MAGRTTb?O*?TCw6v5$6bS)qZqo=w4J~*9i;eVx4NwO!crrOjhE8U( z&P-ZZU9$We^ubqNd73QDTJqqV55D;u{1?`JQre~$mu9WZ%=z|x?{A;q|NiAy0GH5U z*nIM2xww(4aBEe#)zoy#s-^NN%WJl5hX=Oj8cnY%e+ZYt5!@FfY;fPO8p2xj+f6?; zUE_`~@~KwcX!4d}D<7hA<#M$$MY^)MV_$1K4gr3H8yA&|Ten>yr0v!TT@%u$ScDfR zrzVR=Rjj3cjDj)fWv?wQanp7LL)Me^LS6EzBMR%1w^~9L%8&g(G;d3f4uLKFIqs5J zYKSlle?R1Fyx?%RURbI;6jq>Nh+(uYf`e8J=hO2&ZQCoTU^AKRV>_^&!W{P-3%oVM zaQqOcL1!4cYP)vuF~dMQb1#lKj_HWu4TgBXPYuJQYWv&8km~(7Mlh=5I8HE}*mJ#? zmxhx%#+9e>eorO0)eg#m6uhb7G^KSg`Cbxlf9XizZH9>B@hZcqJ*7VTp6)w1tHLB1 z1}(?)MI0$rLIUS0;Z^atECLmzzb6FE#PKdBl;L{}$M%UdWEi4$AS4ew$#8O?ZRr(G z4syuHkcGi8a#*gRz@QP|7R93=j*A$L;eA}9id+JyWjkK`Mod00;{&DlA!QJFR3&lj zf1vI*O1ec{(V=0QA?ELLVls-W``ELsu7M`3`vI4MzhVcpJ!9#^KGjq|#b-J`!F7h$ z{dUEFmBLuMbYu>nV^(S3q+UC;7s@e_qZG#+N=oo0o$G1>6Y0a{9@&9;EU2+8k|7P6 zp?HMh|8#X5UnwpxGbHw;%WXHXn_~8nedvw09V+G$(lhoq7L}=qb+OaPSD&;$TuUtG(4;py( zh)8|Nord(*d1ZH-Dmw1MqU&RKiI)26r-hE(pqnmo4uixe^`qea7(_HA_R2KjdJ4$g!)7ve&Q^b1Tf+{(Vd6vInCd>i725IomG^(Ez(D8L!4qlUAX=)EV9!3JfWLB4n1z)!ums&0UuuVLUH zP)i30*5f6tnvk?lbhL{|8I78X7|_cA3p(L9<~X5y1L3{K8Sf*xL|5gToDT;aYig?m8z^z zQ`XdEMJqC#*O|ho!7x~+MzT<5g$turF~pS;RSY&GR;6TxR)3Q+&%yG`3&ngIwR*qK&t{TERu@0|fDrKKw3=RE&t-)Xh-$i& zl5|>BSn5)z)hg3d?<~8msU=ye>CHWR!9yT;PU|$KP*qADf(V?zj^n^g~nykv^I)Uz3{78Ty81{n~ zZsS&7WH)#Ach3%UyVD1s=Ahvw9*%Wt z<42vTt%|niux3Zww13+oK)-d~G>VKHM0ov>KXKaUH(Cc)#9GFVSc4EoUbnRudxi}T z8J!VNY=4g*Y7C*Ho7#^wUVt&67&ea4^1oBw%@h^ z+YZ+eK^VI5573*KZosq?pMj(u5257?^lBu&LF9`ao`sYf9&zx;uK2iv&$;8{ z4nFUSFF5$3JHFuHORo5YgFkV{CmcNEicdQDvO7NM;484|f=_+6!)x%g1CL;L9DE%% zT=1xaKZ8v-+-@x1OZ;|0_a9J82MFd71j+6K002-1li@}jlN6Rde_awnSQ^R>8l%uQ zO&WF!6qOdxN;eu7Q-nHAUeckHnK(0P3kdECiu+2%6$MdLP?%OK@`LB_gMXCA`(~0R zX;Tm9uJ&d7>n z%9A~GP*{Z zrpyh7B^|a-)|8b<&(!>OhWQ08$LV}WQ`RD4Od8d3O-;%vhK7#W<7u;XvbxQo0JX@f zY(C0RS6^zcd>jo287k@<4tg;k3q5e5hLHE@&4ooC)S|`w7N|jm>3tns$G}U4o!(2g=!}xLHp?+qF zvj$ztd<%96=4tCKGG@ADSX{=mNZ@ho6rr?EOQ1(G2i@2;GXb&S#U3YtCuVwc*4rJc zPm$kZf2+|!X~X6%(QMj{4u)mZOi!(P(dF3hX4ra9l=RKQ$v(kJFS#;ib+z9K^#Gle z6LKa>&4oMFJ4C&NBJ7hhPSIjcOno$M6iq+l;ExpH9rF68@D3-EgCCf}JJSgVPbI1$ z?JjPPX!_88InA}KX&=#cFH#s3Ix<6LeY==wf5DK*jP`hqF%u+|sI)3HfyywfAj=0O zMNUX2pLR;T(8c+$g&}Z#q9L>(D~t~l&X^VFXp@&w92f8tq+KXMZ&o!an%$#uo^hJh z^9-RjEvqE_s%H8{qw(juo4?SC{YhO*`|H*ibxm%ZF6r=2QC)bE`d3oZ(~?;a-(mX)b!|i%p!VVP>DN6tg*Ry97gUPUJj<}OxaYL1nXE}h zxs-O{twImUw z43Eo6nJ4_RTDIQALB8H!3nq37cE6>oNG;jZZhXh!vORPsMKfzJ8_*?O7DfGmcrL8A z(_NAhSH+JE?u?`xR1|ZThDb;2Dt`9hC;UQ%94^20-MA*;<$KO0{3b&9y(ENIe@&xj z6>X23)Ftc?ax=4pL5FZ06CPOjgG%2*lbx;+sVm6EHifaku2RZ6dm2zO1s^4+O| zX?^Rl!e{47y>uJGVh+yEaNe$4U2tTYyJ3nqt9nkQP8+X`9>;yxHT1=;SB4=QU*?nq zndTZfT|OzWa_zE$8FPQtuK2+Z>H-NyCcc=wWX>wq$q7{vij#xqCQBclE;KU_SpRHh zW?)cb0G=uW2QHH@&UKOjUxp5p-v+$&z!*iIUwCrEeC5gh!qSr;%oC7--UiJO%g(@H zgQD=VC|Kd1c_uQ*S7+LyC@PW!E7G5DDhEzd%(QbXn4J;PQoYKo1+C zI4^v%{X#z$(3LimCoU9YO4kMJJG0PS25}<7q9LXMM{Esm6)13%7{fk7Wdx5wm$C1R5emYB+b4!_g{ zCYC2a7ogf;<2t!#hh+G05lGD55CT^#LlBoxIEo9C9q6 zV^AjZEfZsU6$%s=ojiXT+hlLxY4o6EhgiZ7JP-%P5cLSCVgnh(`W^-bB@{)=b3uwG zE!U6%u3dpFT>%EaE{d8bl@K+c6+w`+ju^dTU{F9&yQvzYmVNS(GoZm{D-R;bE=#wApMmV(yJpr(t7y*s2{B8_zE)_ yL|YQw3&NAZiu6_*%Ye#&V4x{Sc^DWpP)tgl235p9dFD!GE+Jk92JyL|;s5}0b2K*q delta 34555 zcmX7vV`H6d(}mmEwr$(CZQE$vU^m*aZQE(=WXEZ2+l}qF_w)XN>&rEBu9;)4>0JOD zo(HR^Mh47P)@z^^pH!4#b(O8!;$>N+S+v5K5f8RrQ+Qv0_oH#e!pI2>yt4ij>fI9l zW&-hsVAQg%dpn3NRy$kb_vbM2sr`>bZ48b35m{D=OqX;p8A${^Dp|W&J5mXvUl#_I zN!~GCBUzj~C%K?<7+UZ_q|L)EGG#_*2Zzko-&Kck)Qd2%CpS3{P1co1?$|Sj1?E;PO z7alI9$X(MDly9AIEZ-vDLhpAKd1x4U#w$OvBtaA{fW9)iD#|AkMrsSaNz(69;h1iM1#_ z?u?O_aKa>vk=j;AR&*V-p3SY`CI}Uo%eRO(Dr-Te<99WQhi>y&l%UiS%W2m(d#woD zW?alFl75!1NiUzVqgqY98fSQNjhX3uZ&orB08Y*DFD;sjIddWoJF;S_@{Lx#SQk+9 zvSQ-620z0D7cy8-u_7u?PqYt?R0m2k%PWj%V(L|MCO(@3%l&pzEy7ijNv(VXU9byn z@6=4zL|qk*7!@QWd9imT9i%y}1#6+%w=s%WmsHbw@{UVc^?nL*GsnACaLnTbr9A>B zK)H-$tB`>jt9LSwaY+4!F1q(YO!E7@?SX3X-Ug4r($QrmJnM8m#;#LN`kE>?<{vbCZbhKOrMpux zTU=02hy${;n&ikcP8PqufhT9nJU>s;dyl;&~|Cs+o{9pCu{cRF+0{iyuH~6=tIZXVd zR~pJBC3Hf-g%Y|bhTuGyd~3-sm}kaX5=T?p$V?48h4{h2;_u{b}8s~Jar{39PnL7DsXpxcX#3zx@f9K zkkrw9s2*>)&=fLY{=xeIYVICff2Id5cc*~l7ztSsU@xuXYdV1(lLGZ5)?mXyIDf1- zA7j3P{C5s?$Y-kg60&XML*y93zrir8CNq*EMx)Kw)XA(N({9t-XAdX;rjxk`OF%4-0x?ne@LlBQMJe5+$Ir{Oj`@#qe+_-z!g5qQ2SxKQy1ex_x^Huj%u+S@EfEPP-70KeL@7@PBfadCUBt%`huTknOCj{ z;v?wZ2&wsL@-iBa(iFd)7duJTY8z-q5^HR-R9d*ex2m^A-~uCvz9B-1C$2xXL#>ow z!O<5&jhbM&@m=l_aW3F>vjJyy27gY}!9PSU3kITbrbs#Gm0gD?~Tub8ZFFK$X?pdv-%EeopaGB#$rDQHELW!8bVt`%?&>0 zrZUQ0!yP(uzVK?jWJ8^n915hO$v1SLV_&$-2y(iDIg}GDFRo!JzQF#gJoWu^UW0#? z*OC-SPMEY!LYYLJM*(Qov{#-t!3Z!CfomqgzFJld>~CTFKGcr^sUai5s-y^vI5K={ z)cmQthQuKS07e8nLfaIYQ5f}PJQqcmokx?%yzFH*`%k}RyXCt1Chfv5KAeMWbq^2MNft;@`hMyhWg50(!jdAn;Jyx4Yt)^^DVCSu?xRu^$*&&=O6#JVShU_N3?D)|$5pyP8A!f)`| z>t0k&S66T*es5(_cs>0F=twYJUrQMqYa2HQvy)d+XW&rai?m;8nW9tL9Ivp9qi2-` zOQM<}D*g`28wJ54H~1U!+)vQh)(cpuf^&8uteU$G{9BUhOL| zBX{5E1**;hlc0ZAi(r@)IK{Y*ro_UL8Ztf8n{Xnwn=s=qH;fxkK+uL zY)0pvf6-iHfX+{F8&6LzG;&d%^5g`_&GEEx0GU=cJM*}RecV-AqHSK@{TMir1jaFf&R{@?|ieOUnmb?lQxCN!GnAqcii9$ z{a!Y{Vfz)xD!m2VfPH=`bk5m6dG{LfgtA4ITT?Sckn<92rt@pG+sk>3UhTQx9ywF3 z=%B0LZN<=6-B4+UbYWxfQUOe8cmEDY3QL$;mOw&X2;q9x9qNz3J97)3^jb zdlzkDYLKm^5?3IV>t3fdWwNpq3qY;hsj=pk9;P!wVmjP|6Dw^ez7_&DH9X33$T=Q{>Nl zv*a*QMM1-2XQ)O=3n@X+RO~S`N13QM81^ZzljPJIFBh%x<~No?@z_&LAl)ap!AflS zb{yFXU(Uw(dw%NR_l7%eN2VVX;^Ln{I1G+yPQr1AY+0MapBnJ3k1>Zdrw^3aUig*! z?xQe8C0LW;EDY(qe_P!Z#Q^jP3u$Z3hQpy^w7?jI;~XTz0ju$DQNc4LUyX}+S5zh> zGkB%~XU+L?3pw&j!i|x6C+RyP+_XYNm9`rtHpqxvoCdV_MXg847oHhYJqO+{t!xxdbsw4Ugn($Cwkm^+36&goy$vkaFs zrH6F29eMPXyoBha7X^b+N*a!>VZ<&Gf3eeE+Bgz7PB-6X7 z_%2M~{sTwC^iQVjH9#fVa3IO6E4b*S%M;#WhHa^L+=DP%arD_`eW5G0<9Tk=Ci?P@ z6tJXhej{ZWF=idj32x7dp{zmQY;;D2*11&-(~wifGXLmD6C-XR=K3c>S^_+x!3OuB z%D&!EOk;V4Sq6eQcE{UEDsPMtED*;qgcJU^UwLwjE-Ww54d73fQ`9Sv%^H>juEKmxN+*aD=0Q+ZFH1_J(*$~9&JyUJ6!>(Nj zi3Z6zWC%Yz0ZjX>thi~rH+lqv<9nkI3?Ghn7@!u3Ef){G(0Pvwnxc&(YeC=Kg2-7z zr>a^@b_QClXs?Obplq@Lq-l5>W);Y^JbCYk^n8G`8PzCH^rnY5Zk-AN6|7Pn=oF(H zxE#8LkI;;}K7I^UK55Z)c=zn7OX_XVgFlEGSO}~H^y|wd7piw*b1$kA!0*X*DQ~O` z*vFvc5Jy7(fFMRq>XA8Tq`E>EF35{?(_;yAdbO8rrmrlb&LceV%;U3haVV}Koh9C| zTZnR0a(*yN^Hp9u*h+eAdn)d}vPCo3k?GCz1w>OOeme(Mbo*A7)*nEmmUt?eN_vA; z=~2}K_}BtDXJM-y5fn^v>QQo+%*FdZQFNz^j&rYhmZHgDA-TH47#Wjn_@iH4?6R{J z%+C8LYIy>{3~A@|y4kN8YZZp72F8F@dOZWp>N0-DyVb4UQd_t^`P)zsCoygL_>>x| z2Hyu7;n(4G&?wCB4YVUIVg0K!CALjRsb}&4aLS|}0t`C}orYqhFe7N~h9XQ_bIW*f zGlDCIE`&wwyFX1U>}g#P0xRRn2q9%FPRfm{-M7;}6cS(V6;kn@6!$y06lO>8AE_!O z{|W{HEAbI0eD$z9tQvWth7y>qpTKQ0$EDsJkQxAaV2+gE28Al8W%t`Pbh zPl#%_S@a^6Y;lH6BfUfZNRKwS#x_keQ`;Rjg@qj zZRwQXZd-rWngbYC}r6X)VCJ-=D54A+81%(L*8?+&r7(wOxDSNn!t(U}!;5|sjq zc5yF5$V!;%C#T+T3*AD+A({T)#p$H_<$nDd#M)KOLbd*KoW~9E19BBd-UwBX1<0h9 z8lNI&7Z_r4bx;`%5&;ky+y7PD9F^;Qk{`J@z!jJKyJ|s@lY^y!r9p^75D)_TJ6S*T zLA7AA*m}Y|5~)-`cyB+lUE9CS_`iB;MM&0fX**f;$n($fQ1_Zo=u>|n~r$HvkOUK(gv_L&@DE0b4#ya{HN)8bNQMl9hCva zi~j0v&plRsp?_zR zA}uI4n;^_Ko5`N-HCw_1BMLd#OAmmIY#ol4M^UjLL-UAat+xA+zxrFqKc@V5Zqan_ z+LoVX-Ub2mT7Dk_ z<+_3?XWBEM84@J_F}FDe-hl@}x@v-s1AR{_YD!_fMgagH6s9uyi6pW3gdhauG>+H? zi<5^{dp*5-9v`|m*ceT&`Hqv77oBQ+Da!=?dDO&9jo;=JkzrQKx^o$RqAgzL{ zjK@n)JW~lzxB>(o(21ibI}i|r3e;17zTjdEl5c`Cn-KAlR7EPp84M@!8~CywES-`mxKJ@Dsf6B18_!XMIq$Q3rTDeIgJ3X zB1)voa#V{iY^ju>*Cdg&UCbx?d3UMArPRHZauE}c@Fdk;z85OcA&Th>ZN%}=VU%3b9={Q(@M4QaeuGE(BbZ{U z?WPDG+sjJSz1OYFpdImKYHUa@ELn%n&PR9&I7B$<-c3e|{tPH*u@hs)Ci>Z@5$M?lP(#d#QIz}~()P7mt`<2PT4oHH}R&#dIx4uq943D8gVbaa2&FygrSk3*whGr~Jn zR4QnS@83UZ_BUGw;?@T zo5jA#potERcBv+dd8V$xTh)COur`TQ^^Yb&cdBcesjHlA3O8SBeKrVj!-D3+_p6%P zP@e{|^-G-C(}g+=bAuAy8)wcS{$XB?I=|r=&=TvbqeyXiuG43RR>R72Ry7d6RS;n^ zO5J-QIc@)sz_l6%Lg5zA8cgNK^GK_b-Z+M{RLYk5=O|6c%!1u6YMm3jJg{TfS*L%2 zA<*7$@wgJ(M*gyTzz8+7{iRP_e~(CCbGB}FN-#`&1ntct@`5gB-u6oUp3#QDxyF8v zOjxr}pS{5RpK1l7+l(bC)0>M;%7L?@6t}S&a zx0gP8^sXi(g2_g8+8-1~hKO;9Nn%_S%9djd*;nCLadHpVx(S0tixw2{Q}vOPCWvZg zjYc6LQ~nIZ*b0m_uN~l{&2df2*ZmBU8dv`#o+^5p>D5l%9@(Y-g%`|$%nQ|SSRm0c zLZV)45DS8d#v(z6gj&6|ay@MP23leodS8-GWIMH8_YCScX#Xr)mbuvXqSHo*)cY9g z#Ea+NvHIA)@`L+)T|f$Etx;-vrE3;Gk^O@IN@1{lpg&XzU5Eh3!w;6l=Q$k|%7nj^ z|HGu}c59-Ilzu^w<93il$cRf@C(4Cr2S!!E&7#)GgUH@py?O;Vl&joXrep=2A|3Vn zH+e$Ctmdy3B^fh%12D$nQk^j|v=>_3JAdKPt2YVusbNW&CL?M*?`K1mK*!&-9Ecp~>V1w{EK(429OT>DJAV21fG z=XP=%m+0vV4LdIi#(~XpaUY$~fQ=xA#5?V%xGRr_|5WWV=uoG_Z&{fae)`2~u{6-p zG>E>8j({w7njU-5Lai|2HhDPntQ(X@yB z9l?NGoKB5N98fWrkdN3g8ox7Vic|gfTF~jIfXkm|9Yuu-p>v3d{5&hC+ZD%mh|_=* zD5v*u(SuLxzX~owH!mJQi%Z=ALvdjyt9U6baVY<88B>{HApAJ~>`buHVGQd%KUu(d z5#{NEKk6Vy08_8*E(?hqZe2L?P2$>!0~26N(rVzB9KbF&JQOIaU{SumX!TsYzR%wB z<5EgJXDJ=1L_SNCNZcBWBNeN+Y`)B%R(wEA?}Wi@mp(jcw9&^1EMSM58?68gwnXF` zzT0_7>)ep%6hid-*DZ42eU)tFcFz7@bo=<~CrLXpNDM}tv*-B(ZF`(9^RiM9W4xC%@ZHv=>w(&~$Wta%)Z;d!{J;e@z zX1Gkw^XrHOfYHR#hAU=G`v43E$Iq}*gwqm@-mPac0HOZ0 zVtfu7>CQYS_F@n6n#CGcC5R%4{+P4m7uVlg3axX}B(_kf((>W?EhIO&rQ{iUO$16X zv{Abj3ZApUrcar7Ck}B1%RvnR%uocMlKsRxV9Qqe^Y_5C$xQW@9QdCcF%W#!zj;!xWc+0#VQ*}u&rJ7)zc+{vpw+nV?{tdd&Xs`NV zKUp|dV98WbWl*_MoyzM0xv8tTNJChwifP!9WM^GD|Mkc75$F;j$K%Y8K@7?uJjq-w zz*|>EH5jH&oTKlIzueAN2926Uo1OryC|CmkyoQZABt#FtHz)QmQvSX35o`f z<^*5XXxexj+Q-a#2h4(?_*|!5Pjph@?Na8Z>K%AAjNr3T!7RN;7c)1SqAJfHY|xAV z1f;p%lSdE8I}E4~tRH(l*rK?OZ>mB4C{3e%E-bUng2ymerg8?M$rXC!D?3O}_mka? zm*Y~JMu+_F7O4T;#nFv)?Ru6 z92r|old*4ZB$*6M40B;V&2w->#>4DEu0;#vHSgXdEzm{+VS48 z7U1tVn#AnQ3z#gP26$!dmS5&JsXsrR>~rWA}%qd{92+j zu+wYAqrJYOA%WC9nZ>BKH&;9vMSW_59z5LtzS4Q@o5vcrWjg+28#&$*8SMYP z!l5=|p@x6YnmNq>23sQ(^du5K)TB&K8t{P`@T4J5cEFL@qwtsCmn~p>>*b=37y!kB zn6x{#KjM{S9O_otGQub*K)iIjtE2NfiV~zD2x{4r)IUD(Y8%r`n;#)ujIrl8Sa+L{ z>ixGoZJ1K@;wTUbRRFgnltN_U*^EOJS zRo4Y+S`cP}e-zNtdl^S5#%oN#HLjmq$W^(Y6=5tM#RBK-M14RO7X(8Gliy3+&9fO; zXn{60%0sWh1_g1Z2r0MuGwSGUE;l4TI*M!$5dm&v9pO7@KlW@j_QboeDd1k9!7S)jIwBza-V#1)(7ht|sjY}a19sO!T z2VEW7nB0!zP=Sx17-6S$r=A)MZikCjlQHE)%_Ka|OY4+jgGOw=I3CM`3ui^=o0p7u z?xujpg#dRVZCg|{%!^DvoR*~;QBH8ia6%4pOh<#t+e_u!8gjuk_Aic=|*H24Yq~Wup1dTRQs0nlZOy+30f16;f7EYh*^*i9hTZ`h`015%{i|4 z?$7qC3&kt#(jI#<76Biz=bl=k=&qyaH>foM#zA7}N`Ji~)-f-t&tR4^do)-5t?Hz_Q+X~S2bZx{t+MEjwy3kGfbv(ij^@;=?H_^FIIu*HP_7mpV)NS{MY-Rr7&rvWo@Wd~{Lt!8|66rq`GdGu% z@<(<7bYcZKCt%_RmTpAjx=TNvdh+ZiLkMN+hT;=tC?%vQQGc7WrCPIYZwYTW`;x|N zrlEz1yf95FiloUU^(onr3A3>+96;;6aL?($@!JwiQ2hO|^i)b4pCJ7-y&a~B#J`#FO!3uBp{5GG*Cni@K85&o0q~6#LtppE&cVY z3Bv{xQ-;i}LN-60B2*1suMd=Fi%Y|7@52axZ|b=Wiwk^5eg{9X4}(q%4D5N5_Gm)` zg~VyFCwfkIKW(@@ZGAlTra6CO$RA_b*yz#){B82N7AYpQ9)sLQfhOAOMUV7$0|d$=_y&jl>va$3u-H z_+H*|UXBPLe%N2Ukwu1*)kt!$Y>(IH3`YbEt; znb1uB*{UgwG{pQnh>h@vyCE!6B~!k}NxEai#iY{$!_w54s5!6jG9%pr=S~3Km^EEA z)sCnnau+ZY)(}IK#(3jGGADw8V7#v~<&y5cF=5_Ypkrs3&7{}%(4KM7) zuSHVqo~g#1kzNwXc39%hL8atpa1Wd#V^uL=W^&E)fvGivt)B!M)?)Y#Ze&zU6O_I?1wj)*M;b*dE zqlcwgX#eVuZj2GKgBu@QB(#LHMd`qk<08i$hG1@g1;zD*#(9PHjVWl*5!;ER{Q#A9 zyQ%fu<$U?dOW=&_#~{nrq{RRyD8upRi}c-m!n)DZw9P>WGs>o1vefI}ujt_`O@l#Z z%xnOt4&e}LlM1-0*dd?|EvrAO-$fX8i{aTP^2wsmSDd!Xc9DxJB=x1}6|yM~QQPbl z0xrJcQNtWHgt*MdGmtj%x6SWYd?uGnrx4{m{6A9bYx`m z$*UAs@9?3s;@Jl19%$!3TxPlCkawEk12FADYJClt0N@O@Pxxhj+Kk(1jK~laR0*KGAc7%C4nI^v2NShTc4#?!p{0@p0T#HSIRndH;#Ts0YECtlSR}~{Uck+keoJq6iH)(Zc~C!fBe2~4(Wd> zR<4I1zMeW$<0xww(@09!l?;oDiq zk8qjS9Lxv$<5m#j(?4VLDgLz;8b$B%XO|9i7^1M;V{aGC#JT)c+L=BgCfO5k>CTlI zOlf~DzcopV29Dajzt*OcYvaUH{UJPaD$;spv%>{y8goE+bDD$~HQbON>W*~JD`;`- zZEcCPSdlCvANe z=?|+e{6AW$f(H;BND>uy1MvQ`pri>SafK5bK!YAE>0URAW9RS8#LWUHBOc&BNQ9T+ zJpg~Eky!u!9WBk)!$Z?!^3M~o_VPERYnk1NmzVYaGH;1h+;st==-;jzF~2LTn+x*k zvywHZg7~=aiJe=OhS@U>1fYGvT1+jsAaiaM;) zay2xsMKhO+FIeK?|K{G4SJOEt*eX?!>K8jpsZWW8c!X|JR#v(1+Ey5NM^TB1n|_40 z@Db2gH}PNT+3YEyqXP8U@)`E|Xat<{K5K;eK7O0yV72m|b!o43!e-!P>iW>7-9HN7 zmmc7)JX0^lPzF#>$#D~nU^3f!~Q zQWly&oZEb1847&czU;dg?=dS>z3lJkADL1innNtE(f?~OxM`%A_PBp?Lj;zDDomdg zn+lVJBnzA5DamDVIk!-AoSMv~QchAOt&5fk#G=s!$FD}9rL0yDjwDkw<9>|UUuyVm z&o7y|6Ut5WI0!G$M?NiMUy%;s3ugPKJU_+B!Z$eMFm}A**6Z8jHg)_qVmzG-uG7bj zfb6twRQ2wVgd)WY00}ux=jqy@YH4ldI*;T^2iAk+@0u`r_Fu(hmc3}!u-Pb>BDIf{ zCNDDv_Ko`U@})TZvuE=#74~E4SUh)<>8kxZ=7`E?#|c zdDKEoHxbEq;VVpkk^b&~>-y`uO~mX=X0bmP!=F1G1YiluyeEg!D*8Fq-h=NyE-2S;^F6j=QMtUzN4oPedvc*q(BCpbg~*As!D@U z3(sz|;Pe1hn08P_cDQ(klZ6 z;P`q(5_V?*kJYBBrA1^yDgJD|)X1FV_*~sO>?8Sy~I9WdK5K8bc7aeNC zDb{Fe>y3N^{mrD1+GyH{F?@9}YQ2Om3t`nt zQ(}MS8M?6Vk>B=*j*yibz6QCdR=ALgTUcKx61){O@1WkPp-v$$4}e#KgK`HG~2@#A?`BF8em`ah6+8hH-DNA2>@02WWk9(fzhL_iz|~H~qEViQ(*{ zV;3tjb<%&r!whm6B`XtWmmrMWi=#ZO&`{h9`->HVxQ)^_oOS{W z!BzVRjdx5@pCXl#87ovlp<^QU;s<*d$)+|vI;Ai(!8Tjll^mi6!o~CpnlgZAK>6=V zm38^kT`D$_$v@UYeFyVhnsMZI1m`E&8<{V07>bBEI1=fg3cji*N?7pBzuamD`X|^^ zm!)2v?s|6T&H-_^y`KM&$!0!9tai9x&)5<(&sY6B`3D{$$KMAX3@&`SW;X0 zB-}obt^I;|#o_bR>eOv?P>=UC6CGTXIM+lSu?Uy+R9~O;q|c2+FafBP;E)B5M9HJgRIpF|GvRi*E+JTBI~T?T*X}r) zefUd*(+3n_YHZZS(g8)+7=pNV9QR^>Qs8t+iEpbJS!9;wio&9rn=19C0G#Ax zM-tWHp_YlJvXWsUqJUr^`OYFA4wkgL`cSOV;w4?tp>GT1jq}-qPoN zp&G}*;+#+Zh&vqDOp>gRL#^O7;s2yWqs+U4_+R4`{l9rEt-ud(kZ*JZm#0M{4K(OH zb<7kgkgbakPE=G&!#cNkvSgpU{KLkc6)dNU$}BQelv+t+gemD5;)F-0(%cjYUFcm{ zxaUt??ycI({X5Gkk@KIR$WCqy4!wkeO_j)?O7=lFL@zJDfz zrJJRDePaPzCAB)hPOL%05T5D*hq|L5-GG&s5sB97pCT23toUrTxRB{!lejfX_xg(y z;VQ+X91I;EUOB;=mTkswkW0~F$ zS%M}ATlKkIg??F?I|%gdYBhU(h$LqkhE!Xx$7kPS{2U4wLujF_4O+d8^ej{ zgSo(;vA)|(KT8R_n_aQ$YqDQaI9Stqi7u=+l~~*u^3-WsfA$=w=VX6H%gf!6X|O#X z*U6Wg#naq%yrf&|`*$O!?cS94GD zk}Gx%{UU!kx|HFb+{f(RA2h+t#A!32`fxL}QlXUM{QF3m&{=7+hz@aXMq*FirZk?W zoQ~ZCOx>S?o>3`+tC&N0x4R`%m)%O$b@BkW;6zE+aBzeYi47~78w$d~uypaV*p$kQ zJf34Q+pp~vg6)yeTT&qWbnR2|SifwK2gA7fzy#W(DyM^bdCjnee42Ws>5mM9W6_`j zC(|n5Fa&=MT$$@?p~)!IlLezYa}=Uw21^Fz-I#?_AOk(7Ttxm;#>RDD_9EloqhvrS z&7fpbd$q_e21Al+bcz|o{(^p}AG>jX0B}ZZRfzk$WLbNLC{y|lZ|&a(=bOE6Mxum{ zM=Nd+-I2A-N&2giWM2oAH`O&QecJn6%uYl0GWlpx&2*)BIfl3h&2E(>#ODt4oG}Dq z__73?sw2-TOWq@d&gmYKdh`a}-_6YQ5```}bEBEmWLj))O z?*eUM4tw0Cwrr+4Ml^9JkKW9e4|_^oal0*sS-u_Xovjo8RJ18x_m7v!j$eR@-{2(Y z?&K4ZR8^T{MGHL#C(+ZAs6&k}r07Xqo1WzaMLo9V;I<9a6jx2wH2qeU?kv25MJxoj zJKzX`Un|;_e&KY%R2jU~<5lm-`$EjIJLDP~11_5?&W#t3I{~+0Ze++pOh2B4c1Mde zSgj$ODQQm7gk&w{wwfE1_@V(g!C=2Hd%Gwj{{-_K4S|nZu+vk}@k(?&13iccsLkQo z_t8#Ah$HVB-MRyzpab*OHOp zl`$tEcUcF9_=3*qh8KTaW$znGztA7Obzb`QW5IQN+8XC=l%+$FVgZ|*XCU?G4w)}! zmEY+2!(!%R5;h`>W(ACqB|7`GTSp4{d)eEC8O)Mhsr$dQG}WVBk$aN1->sTSV7E)K zBqr;^#^bZJJX4E_{9gdPo8e?Ry>ZrE&qM)zF5z20DP0`)IIm_!vm&s2mzl z2;EPI{HgFH-Mp&fIL^6f74>19^>o^AOj`uyL0+Nb##Slvi9K4LQSs>f+$j?cn9Z__C zAkyZ9C;#uRi3cDYoTA>AT<|*pt{K70oZKG*S1F$r?KE=$4~W3!u53yUvh~(kMrClS zXC?Dmgv4iS`>~wBPJJFL_C8x2tEg*PCDX2=rHQ@z+Zs)Kkr;FYG`GnbUXqdipzvHE z1aZ>G6|e`}Q#)Kru0)(SZnUCN#dN2H zd1}r&xGsaAeEed9#?|0HzMGA7pl2=aehy_zsRV8RKV6+^I8woDd%4J8v9hs$x{ zl*V61wSumovRVWtetd1eJ%i^#z`_~~^B;aeuD`6LgHL66F0b^G5@om^&_3REtGmhz z%j^9{U`BH7-~P_>c_yu9sE+kk)|2`C)-ygYhR?g~gH`OK@JFAGg0O)ng-JzSZMjw< z2f&vA7@qAhrVyoz64A!JaTVa>jb5=I0cbRuTv;gMF@4bX3DVV#!VWZEo>PWHeMQtU!!7ptMzb{H ze`E4ZG!rr4A8>j2AK(A0Vh6mNY0|*1BbLhs4?>jmi6fRaQwed-Z?0d=eT@Hg zLS(%af5#q%h@txY2KaYmJBu>}ZESUv-G02~cJ-(ADz6u8rLVECbAR7+KV~a!DI83H zd!Z(Ekz%vjA-|%4-YpgfymMzxm_RjZg%ruo zT4^x)f*%Ufvg_n`&55cK;~QChP6~Fy_Z67HA`UtdW)@$Xk-2+|opk6A@y0~3Qb;V% z%+B@ArKl|Q^DJW&xuBZD#~SurH7XXf*uE0@|ccNd&MA%Ts*1 zg7TU!xY}~*AOY+tAnFR(Fu)e@^9V!Rm65$;G$-?6e%7w7p9WT098%-R?u#J+zLot@ z4H7R>G8;q~_^uxC_Z=-548YRA`r`CsPDL!^$v0Yy<^M=Jryxz5ZVR_<+qP}nwrxzi z-)Y;nZQHhO+db{>IrD$#DkHP%swyKhV(qn`H9~3h0Bd33H*DAP0S!ypZqPF^1^tZJ z{z;HN?$WJ5{0jQNzYOc|KbJ(Pr42~YhW5ohNdY*rEk=({8q+F}hy)&ziN(@q1;>jL zBN<9(k1N!p2D%uHF0NxFut`XwEMc@ZH-|95>U)PY@}C=bmV_*dakL}J5DUpNZi-y& z+{i0>H@c-g|DBO)HJ>7$VVtn)z3X}H`FuN-t>gcqLas?Lk@MJb5?u@BTn0Q}E(}S~ zXrNX`ysRv*iOn1v@fBDeSDvvR>+;o>kj ztRqEZOWN!fqp(`XQ3ppvC)c{AeyS6b_8pN1M*~0=$U;P31!~Px`Obrz;GNs(8RrJvONy<{Dk1x0z zJJzhQBt{J@&DP6cHugB!q?xi~O`yJYHUsTI zmgulx%I<*?vPSl(!tj;LL$K*k zH(*d31iyB9aYAzw49W&qDi0>f;b5kA31nz(%2W`QFJqaX0&hM`KP1gfdRw?7@}$XB z!^cUI%C!?X!QVQxbqEFSbuP0>_3MTCof6!e4LMAfGRd0;Lt+w0WK@b4EkGHRqX!h{ zrYxwwH&-fM67X7zP&Qpup&vAOaKH|S*pcbI{ksFg@tfw)paaK)5khkys0GSTnAtfC z{mVJkCXt|G-SYwt0O4dM8Hf{L*&^nOeQ271ECyc5Y&z5R0%hCq6~} z$XW$kcz!nnCTAl}NyB0#ikwyg_M};inG%*x38`EYJ%FXdj&A`g)-wJ(R=C`O^r{W` z8$1r{G0X4g`uD+}vw4`H5!*B8TTsmeaYGk3x0{&aar7ocO6?dlGbyV480<#{%^93y zF(ei<%{OYi?n?L9#HL_R-00#zRzbbwVnJ0zt}4f|KNBkT6&=Kb=$E(@aC03vU~p)7$XA@ zq5*`*4Y&u*=Ju>+x}q&Xxsjn;Dd)6Otudner9zi z<*LpeG}*vJ58#P4|qXF-ul1|u*;=-@oGPtmBnQW6VY9(s`5GMsO@!;s_PKo_? z3HbGokZ|vaAA-guf5W0JDwpV}1u8;7XJ=wD;NgcLIJW8S5w!c%O*zU0%~)0M)`!Al-+OFsmPW1zniB%fqF;klqxz`Y z2@srWa3e?B3ot|nhE|Q7VIjr+$D7F^n?wm5g8w?Ro0i72K3u^g)&&F^9~@eHd33YY z9LR!!orc0vq$sd~eR~hW{4?R3Di;~mz{^G1X?#-!|Cli(#0-sm|GHYpcab`ZA=zi3 z5*m>sJyOij{!PgIJa?A0%wL*Ur1fLJdJW$a>&Xj5p_IO=SwyTp@nn&@6L4vIfT79aPyo{LQ4DhIz1 z5g*+hII!(cLGHc5ROH&^^o=02r*x>MxMPx{JFMmNvzJ?AI8p!u_H8L1a`{6~bF@L* zxszth=`>%Vi`=E{jJKd-+6pf^vo93EzqFfTcr)A&V{rERu__UAQVyE1imol78AFmB z7T;pNFxW^M+O3#;Tz^e*`AqsD?M*wPT6pnBFPA^kOTnZYHr@O(JUQ^#6bD&CC*?HG zRAKSXYv9DU)L{V(wM=te@V@Db3}97Sn9r2nroOz06!qV=)+%EKB^MR_K}p$zM5OD1 zzhYv+?%A`7dBrU(#&1hXF;7lzH`nENZKP2I{qp^NxBA8~N>?1H@uZ~Do{d+|KYx9I z_z)J7O(;xu0%0n3o4y7LnJKRPK?RV@_v_YLogYPH;}`>cZmDVyO#%-IMQVq6z9r>@ z?*AQC$=?|aqrY8xGx%vfk0ZeByTz18IrP0XTVlJyRx5!NALYPyjcn|)U5jl^<)_KZ z2C?1|dkBZ;h8e#)3gUPfdf80xu^8evspE%Xf~x zs%phX&YuB{y}>%PuOG>s&EW}5Y0`dyseV)!C|`1(U{Nd4c4>07ZFmdTJS2T3+dEw8 zK%f_x!O?H8+_Qd>$DsYNY!?tC^H;N+!fQS{!4-9c^;uXx)D3|joo_FlBTTdDM4nx{ zPve})D_u{PG>&^G=>$2N-dZ!eMx?9X7FmPNo)7|>Z|A-mNZ0{+884L6=f-{Q4bN3y zAWL{oJIh(js2$bDTaV&bh4Fn=4^M?@N~+$IXxytdnI4{RkYA$8j(}sb2TO$~49JHz z0$K$WB@axSqKsyG>m7&3IVR+?xXLfs7ytuJHH8{`ewhkH;?H7#an)*hPiBLi22jAI z{|tZ;dU=nDUVyfIurEm0VoB6kiaK#ju6RV?{3qaV`NQ4&$)fc4AAVKiXu_1$86nxh zX)Mif*|y>N;S~7UCXQhs3-%nqNuTu>=8wqtp$-#tC?bwc-{&k&0>0nRBku-b5X931zqll&%fn$1$->@El+EIA;L zfEYJY)kaTI%H z{A%hpZ?Xt=;#(++B0e)B>4_a3E7h#8upWz!G;VQBX0rjzKvy9N2LECS2@wrBoS;4G z1PgI50DD!wtwsZ&JoAGuum9s&+0NI&_n}!kUTvpD{tyG9jlSXyQ)m9H8VXoDY$j!w zo;imjJKl;E5u|n4Q?HQsy`*&=VY`SG+YFUqG*+;A9(wKfm_|6^SWh_6>1u63)H3zEGm5Uk)#z>J0XC1L+&pzieqnAo+7zlr$M4kl;-h zjo^h7U5Y3tbY@(_{#h1et^{nbOP9Nw*tJOD;WejSG-4d{(2X$tDM@-rK8SbUqMe}%IPqxOV}m#%mq0)auvNwT2R9)$1-o(2o zpIS;qwy8m^tEBC99O}bYKd7ALbB~$d<=eGd>WML+U0aAl>{Uc8CB|oVWMt zbPe9+6&V{l2Th1)Jx`K64?gUC_<>x#Wk*SOSA<&A=j2q zo_M`Lznpsg1h-W546hm(q@Rf=xL@w5QJ;HxIp?O`;sOMovgc4n%D5`kiDO6%Rhe2^ zzPa=8pd(2&HN-=5JzsiJ^(ZlLVpZD^5!$(rt0PVLQCzh7s#6_N1dRKtQv_vTgSQT5 z63+e@K`67zjbb@QdwMNF8G29tcxAl36SZAGxolCj9aS%>(Tl*6a0eW@3j4!&d!12v z%+~Xc=>VJqBcW!D#JX3#yk4O^;#|O3!ol;J%t8>wc!*6`+`~%?-QE_M{wa&vg14R~ z(M1VT-&l-M(N1>3pNjVfvCIk}d|H4&*7{*8!W-;^tFgD31O%~NtUaK_*-m7CSEt}T zm^Z02X#cQ$Mcw}TG{>1I`vmvNoxujnPra4aSwP55x37=0VvyV<)68QB-b$o-h7p*V z#QQ8?A7`=m`*+dTfYdm=;i1ptR|In}rUF^r&{bKbI@5DT$JEo;?-N}Z13}n16v?G2 z{?@ny^7|!rg(on8b97#GupiPA<(g=o;@P`4 zEx06)SiGKkIKFHzK1M`ctf?vQV#b-{ws=+0U^*LYoTK*pu;A#NB$$I=Tv{LLVQin~ z@aGTp?J<(c_1M!Jr8MK;XA8fcB+*DkFF@oAhQ=B1o*$<@;ZdGs_5O!BKi8XjF2L4n zA&(?SaRDWm+p0UTFXj1prs!*v$(q+s=8S1h(*H8pd5*8%HGN0mgw3yvfsxr4QYT)o zzdjal^6zA56|Z@csYH^3Qr2~ZR#p|Huuh0Yt|$~>oQZJDF75aeH%UlQv)fQ=3P{i1 zRt99gL`$b61Q`pdos?W6yd&%2IWK#}$wWOa9wJW&($J4h0M|9sFtQu9k)ZtYEQ#vu zS+uD(3`7T~t?I;f%z8N~nG&FVwxGXrTL!k9s#LB}FSo;a+V-j}H^myGwQq@jTIycD zP5A{w+a;^kOQW^C%9W{j^&o@)3!v~U(?wx42E5G*bd82&a1p6ax|pk)#8nG9risCw zOERH8;tq?Q4ymxf*9_aF-sTpLvETwD#sB#ID1D+WohEt0s557Ij5)ldexY+diQJ*l ziBo;1v*vx(F|lI8udAo450QIQTmPqf(7oULr5*0dE9i>i#D&k%WyfM*4{*?_%9k>g zg1_1%x?#`Xm7M@YZ?!zJs$AxS&8sBLI@c|-vSiG<*OZyw>CL*p6#N~p z#VywqpWdZ;{ylc5d7W8E7Jx_H+5e#N$h#{ni@#TlGqz`yah-qCC_;P8?N*>CPJ03b ze(YVDvbIR$#lJEkuf}L7F8q$fKCWz&>{uFg9JgTOmA*Rux-{|#+pO`!s!!4;PlE%9ys+;|)oK%&V$*FH!G2%|y(zz>X zUwdXer0HIIJkelANg_W!ofsyiN{zi2=}G1UL{`V81}1D1Sz zviLV^w-$RE9fE4@H+ys>u;OY!sgqe&V-oFE9Fn$P9HbpOI{}esLIvc zV5S-9(XjFzn1qzo2owwg_d%7_)cR*!d&%@S&D($cFFMXXd!GdUxw5tZ_W@zRbjVfU zzx13(Hc!$teqA2WOYo^+SHpRz16DOcYqaXHSMZl2Ax$)f^WC??al8lfX9)O_p9#Ml}LB(N8yJ! zj&_UD9K54Rt#yqvhklEMZ3bRC&)(^h`#kzq-#_QN?J6eLT$ zMWG-mP;HkB@5;2*lAP&1*4C)HWEs{gtp15Y%y|*%(3UOMu*v4kTi0@pWvg2Y%7yI* z%XNlZa$@AZ(Z#Elv`5MUei~VFCjF8El)@g&>(v;E; z;laavf&ANfk9*0LA@oP4QmbCBF-lB^Mj~wo)eGG57gqAKC>Hd80Eb+7b;iJzV5RsL z8>ddQH8PnC;l{M(t4c$M=q78GW6=*d#c`-jK$q#-{9c)UNO4eLm9c!DWcCth4O-FU zboSKPhL-lq3q<)m8Xw7+l=Z)H=rGgMI0H?KrPjc;iDzY5g|Ve$8?SE`8*sb1u*>dm zD~f9~j2H~6Oo2`_1 zq@_mmUbFQV25E7XJ)zBRQktT12@qHHy-@aCdAFWv4iZVN0B3}E;k(jg>X|eqOrqgM z4yBUuA*BHdnN9v;5>3#L$NFREyHW&Q*rWYa_q zhC~>M&bMFgXC6AeQ`P-s<}Ot_x^cb51r7ArPbRRs&Dd_TEeugnjR(O#V5i6OYjzRF zw1@Rvo;_wEfQA@P%I^9ljrhxxuqf9g^cWSKq~+kiVxa`&EBDqmB=C1G+XB7`TQeiV zR_k?`$&W&+ntIPeEtM9hqcj|yfW>x7&1Ht1@;!d#Wo%1hO+^Q{E?VD|`-OvV9G?tp;6{sI%L-u)Hw z;|`uN6~VqZ!g~K#B@W7?wDcbO?XS4hnW9kS1Hbi=U_m*~7`N~3oK;qFTX$$LQ#CkL z6I?a(HkF8SKJU8mT{K35ekfP3`05!M{gmrV0E-=IyqP=N;K<&jOnPcjdXrbk$%)z9cUe|#I0unK5^+qGx8#2 zz_!bmzVG*Uat*&f4P>&sV2RswlITV}wPz?_;(S;19}e}54fP|K5l_c2kU5(-Zh!7t zz=B2HktD~ap{s%*CDEl?x6o+91T-xH895-S1}M=*KhFM7Nm&1$OB++Robv0T`OBcJ zXNX%Xio0_ryjr)!Osc7au35UM`B}Ru4zN_o+C!+s&e7|}Zc;5?whP$@J@DE`>w-XH zlVmbrI4|-Z^2^I^EzuYKD+JA@8lx%>aLFZq7KT1~lAu}8cj$<-JJ4ljkcSA;{PNr)d-6P5Z!6Q=t!t*8%X)a|;_92=XXN=WMV))*gWR-wHzU(G6FPTfSjd9) zm8e1mfj4qFmlXO*a3};$&jgc$nfG>NR&iao(jYk`%E75h=K~dJ{Jqs%UH|aGHL8)-1MOyS2B?OJsyeA_YbGMDpE+>=NFcyoI;N z>1>3G4QR2~EP{L{x2e@E1U0jGGV5H$aeigDq&Dr zQ3FwJ+& zndX7VK+XD)t06uUY=)Cfo!ke%uDpOmq^bpEB`iv6(CKTGgEZUi4ddfNXJi_z4;)ob z?R+qj2SYX*zi8z=DXChEEDW+Cy>w-0agE|A7MoRJ4}-(|go-rP#sr%a(5k%wV z&Jllj+6XuSoIfZX9|mK!bbd)7TuaHBvoa(`9C$*XUh}hH1;Q7cTJQR)c>h}Hfr$aS z64c7#D^f{mN3s#2=SEf1$(*Vj{vZjF6Qc{a=VbTske7L^EY&A1I1sgXaYSH7(lF1V zZ<7`Rq33WZuu`!HK$wRr1=uE}#&JMftnZ&(P17gWF;>$TA&$ZQnIz>blTrW@49Z&H9yhgLBpFw(57K1dbIQW4fn1X(IiFWEKmPzV8gAa|ak)HAsmcQ7stP|q0hEzBNL=4YdXEkyfS zF+K+CVB#~(qd7eeZqR-VKIYJVmK2ePk``4I^PfQ*C7NUR z`w9lb?iHv2$4_p-+a+O}Fq6SnPiz>aV!~d=l3VdgDuwAPMR9eR`)b_`lg~{oX0lf1(zbBrnj4+-q zOl^#`)XKn=`()B-jExviKVTYrAKa27KAg3cboG+}D6*R;<`GC-b?i=e;aV7n(}XDS zK5xAEV=T^r#eThV+3C<^H>SuvAP&fw;Yn67eY%4=Y(p$~!`~h12 zQHM|f0#pQP_s$Q+TtMMvBdjQbLWw9cW?gl_+P z)2T94UJaYG2!yXITYjYl-@#5_47g{N|5=P~m|e}-F)*^L+{7O$#wv2e##5Y=A{>jN z6NhQSor9ulwP3gfxTF?V`P7AJ#E)ij$I`gc2fnmp&9w6qS2-Ct}6 z$#O%mKtP>I2VUBMt^Xm3LjP*D=xEyV?|8Psb91ZEj=gM(C3^Kcfvbx*$NK+MhP>W;OneZ{Q>eFEmxv}%ZCJ32=zr_OZd>6~v@ z6+3JzX%9qOvKS393r&R9O+te&#?{Q9nLkOV-eLg9!{WK}WyUWLZ7bQ5u26*u9c*T1 z_s1)j1k5&b8&5@YnmtS{tsmQaLW2%8D*8G-9w#PcVQh6sQY`!tBpU=8EZR!zfB{f{ za<+Err#ZNM4JEx5n9!zuC#KmeI*%tRXP}jpswzymT7J{YpXdzA{J7K)j1tBF8B3DL zZXkec{`rT_{__t_`!E7veO1rg1tFzVeUTBjut*3ZOq}A$r%sWXn4v4|rA+7uMvy9n zL~2WHKLg$BeD2Wq%?frTUM^c}?K?3#L+Q2-?PR+e1Fn-XUThl8^}8JOyDZz-wcFh5 zYJCJ%J_Pf~bX(0A?Z4hGw(mY?J$j#Vo&@9O>in*f)*`H6&(Z-5xx5}$V@dR)-lxgN z=DMA_EJO4+^w_+D7N>4=%{6AbvpDG<(b)xE5Ezo~oEg~cEM?mwyY?3ZtFE;RyDS`u z(^sa_s%B<)vktqh=1|?Uv6DXsA`D^B9%_mXqx1C=a#KurOE?49)P_ixiHAA)D)oqEjQ6_v0UC9mTtMu&kf8&7uRiiigPD{$Cf(&DuOj0 zr*5{zPyO@Kq(|Ttu@wxKanV=^OPOjh-_$MbNz})ou6*9nq_XQo86WJ@JN~-b=Ln_8>Nz_ZS#QpRGt+bzH*-;{#x7PFqie+ z7p5e})fcDq)J2z=z~%nrFGFjbVu~0ICDHW3=HgtCW)?Z(%Cx$z!QuszcOCe&3!Al2 z`793RnB{Jj4QpQ2N#oKT>aY~aNxz_6B2&vPdJadbC4qp#H^<@o50}m>7WR?NO0$ZI z9OKTM+jxMFWX9mi7(@j)1Ji6~?HLU!KT0Y5a^-?|XH^B?R@T zn&a_U_XFAsGrNX@S~g1<=uz@~dCcZO=1??VC@PML{g}lbuN?j|_1S=dJgbT~o}}hs zP_uYZ&0+mWY1fupe(+6nn6<9-)Xluk97yX-!!lqSXq~!kL-=+4$Dy>O$sKO7M^1QY zhZGZfiNQu+?sef?E>5sqj$kHmf;kMv<>Gu)!^4!#7T009vBzq(m2aoHu#+93HBq7T z;Fs8IHvUlmxCB2hkDbm&xwFQcXUD_&sdeu|EYhFpf7v5_LCcVua9aunVe)qoGmyg# zIGlj&IrLKg=id@t7s916d&Gf(%X7^FFR9^bz-;*o1~Sa=`cKfJ0i}X+pBKN=?}!dP zg`ZMtP6xSuvHb=5HYH%ELaGxwqH{ zpY>Ic^}J!OwM!VmNM!$nUg$qN9DLtKuBvn1(x-P+tA*UHoOc727>5?^J;JFo_ac@) zU57%w^U2ME z@z^ZsB!AhyOscE8;~Ft$)NL)GcLteq4d32fw??L0QuWt_M9IJMgZ71Jm%2khx|QN+ zkm4zQ@OjyM+l=Rv(!k?%cYwnf7HWs^M+P^zo5o?7;E)V0v*zf}(;?ms0oUK)wKmZY)mSTGN4X@2=ZU!Gy73M(ftmHJHLFKQDcu`d% zeqiW{G`?}AtEP zKCnHuWzXZ_Hc>{cP@h~M$#q}kG{52%zmhATR3AbNGR~*6(%^Gs@UZ3i%7%PJ1mB^S zcdcrFDbD6lEJGZ4k6JT;eB_JbgIkkOqkz0I{q`d^kWl6a!%w4V?Y!;8%uU(-UA4Ti z{pv2+5CN^ba{ALpu1&qm`sMP@_L=-a)@-zC1*`f)uV5MU$xJj51%?S^ zoo@;kqY@4Zw0B!+hIvTT8KK*~9H@u54r>s{MX_|#z`Z$55bDJo#=hz~k)7CTbf>Gn z=!u;@JViT~(>P7UDdIOL;6kPDzOZNl16jLo5tHS4a%~T&AlicnCwZ5pZ;+WIB3tJE zv|J^!X0Kb|8njISx#zoB(Pv#!6=D}Uq(6Dg*ll##3kfDxdHdBXN*8dZOM0I{eLTO4 z=L}zF35GJX4Wee`#h=aCB+ZV0xcaZiLCH3bOFYTmEn0qf?uC#lOPC7>+nVeO1KQ@S zcZ5Z0gfk8hH03QrC@NnEKNi15bWP;FEKsGi0iUHN4L&2_auv%tIM}UFfgRyp5HWt()pn#0P9+xF2H!8zMqf`WJ*9YB zq~m+%xLtVjza4>CO4*%thB2k;Gv1Ani%8)IP6Pm^BAigXgOUHWcQDEgB??AtdsOx5 z+pXKfU4>+8ViRUJ;h()e88jRLEzSN7%O|=MovCW3@VxK@Z*xS$WLG=u_Nenb0wP@Y z6zs##uQ7oFvcSdh5?6kZ!%8l$Xuz^Rc!lv4q?e$mv(=#@x)s_VFF50vGuE_Nr{4zXB>y?7FOMC5^sBZr`mS*t_@%LYN9wl z+lsqD#V5JR63GEr9^&9*f)kFs zJ-A(>>!h~d0%9*wd+AY+&oryzurfV{QP{&-AtDs}#iq;dal?A9jE;huq2gExb3z+- zVQB@UHlVfsy1$)dF`dcZuc(GLnim09jrI9nJ6<#=03FVrkuINg2`RTPloS^^@KYD6 z1-C-Oj2OI0y9Tdx>=dNHhOYVvx!J#4EMhold-PGClLuLA~k2VDl6cPuV4lI5c(w9@7sllth~H@)0+v~XYqqC6&*fSX~S4Bii^0& z=M)D(5FoZsKxB&M$J_7lbS>$kF=@B|Z$#D|LHJQIr$aO51ta6s96Ug*Jk;|>9Yd$! zoF2W+)lFzY)J<>U$PHwbe9>BKLAeo~e%=Qy#qhvK&`)b2 z(U9#8bba`eGr9tr$SvM4`y`lLavOzPm`l<%-(R<1urb(AX0RE=R=#&QI)klkwrJ5%D5YHZ!~s zGwK?zKZeX|uO*Y|xLjO#6uzO%iXWsSE8#zLOWc! z&2L8sdT;bhUW495)_fGCcOLM-@DfGcb1xjf(ezYJxYOv<7YE$lBCrkbfBA{`I(GH- z(yHy1h=bg~fE$aIbB_3l`|p$R_p0b(+aL(~b<-Am9H@?s!T2*7{+*Vj?pCpV5&WJO z*GbW%PLj|(hbd!fQK5Y-kgDHV!-I$y6G>Y|&uo9+79v}}$s=l$>#F-_F{TjUn~-!M zBN>n)@(LkzI0Sg?f1s}uBZi`wRB}ywU7wqq-PwaS%3nitaXb{&Q=x!xvOPfiQmmkd zWpe2@y7?wbI;hF|hlqf@x+3@a4$wLdJ1PZBoRc9oRGgdM+vm*;5XBZcMZ+@4_{aPUS|`NsD4YP2JUM zZEvA&!QLB$K*%gHy~y-RVs-C zkN^usP)S1pZXjj)nugy#?&vpiE^DS|QlhiBOc?nC$9CK}Ze)ihI{p-m$pgYV^5L~B zQTU>)x*fvKCNK*9j$@Gyt@@I2LF8c7YvDJDCf%1h0zVyNg7E~R$`6JE1EQk~-c1xG zE@xT)TesWHs}ny!5_7F_AyGL9K?Q~mP?>Vs!(oWZR42kf?*iTV*h5>tnzpljZL8IR zb7}l8q%Ckfh{^e3k^3pQMk=gLu60`Ja8HdkzVbeAU*exs*ajmRVp}O}l)TqX!?G7e z{4-~g?Gq%~)IJJ7p1k*WSnL3jqECe1OU}5nirS66_-$3FzMT5t3X zg{jgP^5?%zb(vMa!S|1cOYk4W!vG2KKd{YFIbPCk3_74HL`fWJASs{fxpzY@$(}Q- zK5I4TKS~`mfiDoDOm;XycF6mi|K|+d=lh=@U?9_V)BDDaZAnEw43`Ls1677I-+uFi zG?^$Fbc*pPun65{D!fH=3Oyp$WZAY!{JhzaUtIgYCWXf@)AkTa@x4xGjp0c zs7@JB012~&;z=SMbCp8d=Ga{l0(iwx<@o(f!OwmyH-gBN6wewq7A_h)oKg)koFPft zNfdie%F63S?rGDQR(N=bPuK>G0t^ax$0P8`N_cvR8rOf(O9T7$9#5!B;#!XUpLZXu z5C(OESAmE*2+hV}!bg$4K%`cQHBk!>##tW>1RbC%am`*|5IbvoLh!BqpAi2OmdXqf zHp%|!N;d!LN_26809n^14YVJJBe7aL87U~>HZ)VK%d|rZp(~zwNH#VGuX!vfal&Vv z-c)h33DOB@xl*~m5ZZ22sVRK>8I9+)QMVtsAB>r~SMkGMZaQ;Xi|?~Xxnmx;cYwYx z^nNxRxGcq7I!sO#b%$!0vQ(OqXm6T4mTilvMlYj|*i|=MK%kT2df;bZGW@NrgeX>( zf7eBsjJv}pNuEuHPEs42>}a`ut-O9lZDNh)_CsBpeHKvPKnpcWh^bC2QtnB5a4qy) zSrZhafuAkk5{yiM|zdiecKh zuc2R;6^;@i07fmepeofAJdX*knDzBA{3tyVYu6z#z;Lsi&x_bzzLEpfXtH*NrY_G`= z^X!;eI#hV*mmjjEOlo{TxQwSdUv0P$!Qvijpv9plBI@FUU#RJ)8Vn1ZGA$ATqF&s= zvcTS>Z8pepd>k=sjPY^3fpCB@aW8$Oq%fW;R?GpYoT@ki@N#2LxgTk1dYZHNrk@lx z7=yYr0FT$I>z~I0nXpPp$t3)}D?2^<@KWH#E{irFy2`)5r{AyvWHYzn`5@h;GVj0@ zJ@1fbD9gX=vQNR7PG5i}jFE}9#!;ote)FHdW?VVe6v4dWEz(R?!HC4KeVde*DGr=F zRotamm=!I~=_{|m;mCI4#5{C3_gBXan1<>!K!8O|)&K?O_L`}=uKCJ-s&+!XTk?wi z%Bwa_&k>4}`a` zFCG!c^Cdj#Bc2z2PXBCW$G)<%9X6;oZiigwvMLXQ$0f+2bKDCKCGR*cG>+;UTQ2bj z(2r#Od&Ulv*{?U~hq`j8W&8aggxHo<6*$&cDG#k;GS?mLx0^7mda35tz zHTnFA6vB^rczV1Ai8I&XyJX?jiEcQ}n;PYCl~EUPIxF@V%#c7LW`44<>ezAiG>1ff zeOSeCd#PW2z5z+<4Y?Qc#tb&+uH++5^G@!BaaDeVN8x=3ZB{R=Z5e+zf&13+nz{l% z{{#>B^OaIK}1Xh z;}?)W)sfwuf~?Ov1!oiQ-@WVG>D#(JL4Ob-h*l`y&hBY*!EkULKFdt9+VGJ?E=r85 zl*~dE)e4&l8Fdq`I@T2BAme(u7_)}y$TNu^lWWK-M8UQ(ZuBcA(qHG3; z&7bO_w9Cp!REZ3VB`&kfYOCmrNQxu7pbLoFkf)9Jkas&36ZnTBL?~cDug+T3bw?o! z$U-GUnOTkujjaB8vxcenWsZ4UrH*vMmACDj!95aG?gE5-g<6v8X9%kXThF|rP(0eu za*9aK6%^Qu4oyr(1t4hqmPX~~L7tB(;C{DH&MWDzUG+6I(;TGeM)jR#hK~O13LRwk zRc2;#m|qsRADyxC<6XC8u+lvVXoH+-HNTQXImy0_oM&D=ngI3OP?c>&k8&P2iV%hg zq{#n%P=0$dYJ2o$clJWqpVH&Q;S5Hv`T0-)mU2aa$XL#RH`0~|_g zmmfHkP7#d=iuiU1lL&5T+egS~-01WrWiiA=({_yWBnY@x5eX}`?y?3Xdic;`1dn5T zxTwLw{;Qt1MSWowZ}r+U?8Q+R46Avz>o>^}4zhvZaa_*Jd(2A!dP8ah=_*lh!W#a~ zNUm{^sD#HbDq!m*EK}(GzVn4N2GeNpEp8Z<_tctC_id9X=Irqhb_{b^H;~}qwZI&F z3t^MPXp4BuDv9@1Kr3*u zZ|&i`IKW!_Rv5(CaTJBndmX9B{YL8HJ2}u)`_>#J_-m{T-xpj%|2|{xmnVF#+X3=* zY*5{hDkk6M{+!Ved>d}mD@q^#{3qo9ZYb-+75cj*gH%I+d=}E+qSCK>vj4p z81UxB7>Gz}5QU^Pv-AJ*EHMW3g`EwB^^}ps>1E2$#r*H_{O{u)J@@1m$?Pu=va`3n z?so1N_WbU8U+4Nb|AN$Gv|%%33+!xpvv3iSLv&=qIUrD|3^*|rn7cNTWHgpaH0mTS zbXS-J>ZVOG~>BOwxVSa1sk6ivguYJD`$YgKkB!awl#vZ1NenaIidf zIo;H>3%L>R^l(kGI`c9&1a9H-s~68yw>3t6~N-Bv<9hyv4@0XlT|13}n_wh4#^(`bgWSiUFD z?SO{pz~eEqAvU|UZ-MPN$ZoAzAm@B5l}5B&MB(X&#FQ{BiwixOTe9@pn>F;%(9zOZ zly7ELHP0wS+Ikfr4P>I383O6E%8Ps6HYh5VLs3+bL1$J`TkTm6$wnI&{gh;r(^g9_ zB1RO-zhYoFDSl^oIQ*3Sm`H4%TTjHtuLbN&=j+P%iuVlxfEi zjsZUV9XdHY8m9muB8q5Vz z(`L%J6y+JTwbc>-nW(k@1!b!V8X7{S8M4^jErN(9CY}WtZ%l(hygPSA0+WuRy2zYP z{I1rh;dEB2eq9TUxCz{Gyr5B`eQAc=V{W%c+@W5W-mHRf!`2j21`y@SR^7Oz6_2Pt zkOomwUO=FaWS0^zE_8fOUJ%bwuxpLG@_{*8@bC&b7t2Op`l< z@kNX+GMUc*Zm2{Mv|>~c3<+pti9iF4V#K8sFm1soxJDi@ z0hJgP6;T1hrbc}rAns8Ko;#S9v5&XknRCva_O>&b{J*(Da_#Ad?20`5$%Xl&Puge2 zx?l9eH%e}NIwyYKT%Sue)L;7I7JYB)tpVNP7pm4j0n6@>Y|3y<8rov)IM#WzE@P_p zpPF3p<9y7UBK}GHof5CwW07klGghQ%{IeT#5013G-@n^&IFHZTJJ6g~ zCL1d0jcUJO-+8y)#+Wl0=`qCJo^!~ia8$-;rOBE~#*_zRZ*s~5n>IEYEtin@n6TMCEC;3v*irJ77~dTlkH+Ea~ni&gW~z zEBWCpC22aJfc1md!}q~j@)~H{%|IZpVtGYMh}wWjmPAVGFG{e*)g0Ukf*24y3)BXV zL{F7d(CXNXPzVFQlu~e}UL~fsmSnqLDoUS5FIMR1VZnVc3TinGDcHznFA6zTs<73? z4WUqG_@f*^v&jR_Q>a63^$bI30RuiF&nnl+1=px4kSzi_XB+AxOARqt@H;ZXlCce# zxlDYVFRiA{;DaYx(}XclB2S^eT1Q#1;p=9y6{`}J_sm<1Th)5PG zzzBlA<6+TFhl2c=Jl_@yJ}518aXJd2YFCAVu-7TMwT$KZefT7 zs5NxjtWvoM1u)bqHBp$PBs0RBf))u;m?bp>hDT6vTw&Lr!dBTtgj5XtcKJWphk_H; zeH09+T|vQZQ8Efz6lS0!cG`T`QE*MzYzhh@C0zhrg|>NSMAtY9%Huc+TF>Ppkl@@zX1imQDFMlS23i7E;Qs+kyyrF{7O&UZxN+ z-QgiSOj1$l30gw2$s1etFkp1{tI8Eq=&i{Q(-jkZqNBkxHjo*)Mn|Eg=J}ZZ*M!@$ m8X&e#V;O~v<{(@8u;?|riGH1;*CyBcIM_}B>Hc%VBjPV`^lBFX diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 5e82d67b..fbacf711 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,6 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip networkTimeout=10000 validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME diff --git a/gradlew b/gradlew index 1aa94a42..f5feea6d 100755 --- a/gradlew +++ b/gradlew @@ -15,6 +15,8 @@ # See the License for the specific language governing permissions and # limitations under the License. # +# SPDX-License-Identifier: Apache-2.0 +# ############################################################################## # @@ -55,7 +57,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -84,7 +86,8 @@ done # shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} # Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) -APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit +APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s +' "$PWD" ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum diff --git a/gradlew.bat b/gradlew.bat index 93e3f59f..9d21a218 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -13,6 +13,8 @@ @rem See the License for the specific language governing permissions and @rem limitations under the License. @rem +@rem SPDX-License-Identifier: Apache-2.0 +@rem @if "%DEBUG%"=="" @echo off @rem ########################################################################## @@ -43,11 +45,11 @@ set JAVA_EXE=java.exe %JAVA_EXE% -version >NUL 2>&1 if %ERRORLEVEL% equ 0 goto execute -echo. -echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail @@ -57,11 +59,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe if exist "%JAVA_EXE%" goto execute -echo. -echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% -echo. -echo Please set the JAVA_HOME variable in your environment to match the -echo location of your Java installation. +echo. 1>&2 +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2 +echo. 1>&2 +echo Please set the JAVA_HOME variable in your environment to match the 1>&2 +echo location of your Java installation. 1>&2 goto fail diff --git a/settings.gradle b/settings.gradle index d94f73c6..969c7b09 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2024' + String frcYear = '2025' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0c89bbfc..70ffca24 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,7 +4,6 @@ package frc.robot; -import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import swervelib.math.Matter; @@ -26,12 +25,12 @@ public final class Constants public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. - public static final class AutonConstants - { - - public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); - public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); - } +// public static final class AutonConstants +// { +// +// public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.7, 0, 0); +// public static final PIDConstants ANGLE_PID = new PIDConstants(0.4, 0, 0.01); +// } public static final class DrivebaseConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e6fb1d1..dfbc6b84 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -112,9 +112,9 @@ private void configureBindings() driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro))); driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); driverXbox.b().whileTrue( - Commands.deferredProxy(() -> drivebase.driveToPose( + drivebase.driveToPose( new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - )); + ); driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); driverXbox.start().whileTrue(Commands.none()); driverXbox.back().whileTrue(Commands.none()); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ba3f54dc..ce8e73ed 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -7,10 +7,11 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -26,16 +27,12 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; -import frc.robot.Constants.AutonConstants; import java.io.File; import java.util.Arrays; import java.util.function.DoubleSupplier; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -56,15 +53,15 @@ public class SwerveSubsystem extends SubsystemBase /** * AprilTag field layout. */ - private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + private final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); /** * Enable vision odometry updates while driving. */ - private final boolean visionDriveTest = false; - /** - * PhotonVision class to keep an accurate odometry. - */ - private Vision vision; + private final boolean visionDriveTest = false; +// /** +// * PhotonVision class to keep an accurate odometry. +// */ +// private Vision vision; /** * Initialize {@link SwerveDrive} with the directory provided. @@ -131,7 +128,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig */ public void setupPhotonVision() { - vision = new Vision(swerveDrive::getPose, swerveDrive.field); +// vision = new Vision(swerveDrive::getPose, swerveDrive.field); } @Override @@ -141,7 +138,7 @@ public void periodic() if (visionDriveTest) { swerveDrive.updateOdometry(); - vision.updatePoseEstimation(swerveDrive); +// vision.updatePoseEstimation(swerveDrive); } } @@ -155,34 +152,55 @@ public void simulationPeriodic() */ public void setupPathPlanner() { - AutoBuilder.configureHolonomic( - this::getPose, // Robot pose supplier - this::resetOdometry, // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - AutonConstants.TRANSLATION_PID, - // Translation PID constants - AutonConstants.ANGLE_PID, - // Rotation PID constants - 4.5, - // Max module speed, in m/s - swerveDrive.swerveDriveConfiguration.getDriveBaseRadiusMeters(), - // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() - // Default path replanning config. See the API for the options here - ), - () -> { - // 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(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - }, - this // Reference to this subsystem to set requirements - ); - - //Preload PathPlanner Path finding + // Load the RobotConfig from the GUI settings. You should probably + // store this in your Constants file + RobotConfig config; + try + { + config = RobotConfig.fromGUISettings(); + + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, + // Robot pose supplier + this::resetOdometry, + // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotVelocity, + // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeeds, + // 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 PIDConstants(5.0, 0.0, 0.0), + // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) + // Rotation PID constants + ), + config, + // The robot configuration + () -> { + // 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 + ); + + } catch (Exception e) + { + // Handle exception as needed + e.printStackTrace(); + } + + //Preload PathPlanner Path finding // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE PathfindingCommand.warmupCommand().schedule(); } @@ -234,26 +252,26 @@ public Command aimAtSpeaker(double tolerance) }).until(() -> Math.abs(getSpeakerYaw().minus(getHeading()).getDegrees()) < tolerance); } - /** - * Aim the robot at the target returned by PhotonVision. - * - * @param camera {@link PhotonCamera} to communicate with. - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(PhotonCamera camera) - { - - return run(() -> { - PhotonPipelineResult result = camera.getLatestResult(); - if (result.hasTargets()) - { - drive(getTargetSpeeds(0, - 0, - Rotation2d.fromDegrees(result.getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. - } - }); - } +// /** +// * Aim the robot at the target returned by PhotonVision. +// * +// * @param camera {@link PhotonCamera} to communicate with. +// * @return A {@link Command} which will run the alignment. +// */ +// public Command aimAtTarget(PhotonCamera camera) +// { +// +// return run(() -> { +// PhotonPipelineResult result = camera.getLatestResult(); +// if (result.hasTargets()) +// { +// drive(getTargetSpeeds(0, +// 0, +// Rotation2d.fromDegrees(result.getBestTarget() +// .getYaw()))); // Not sure if this will work, more math may be required. +// } +// }); +// } /** * Get the path follower with events. @@ -284,8 +302,7 @@ public Command driveToPose(Pose2d pose) return AutoBuilder.pathfindToPose( pose, constraints, - 0.0, // Goal end velocity in meters/sec - 0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec ); } @@ -385,11 +402,9 @@ public Command centerModulesCommand() */ public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { - return Commands.deferredProxy( - () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) - .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > - distanceInMeters) - ); + return run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0))) + .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > + distanceInMeters); } /** diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index ae8f67b3..58130773 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -1,483 +1,483 @@ -package frc.robot.subsystems.swervedrive; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.Robot; -import java.awt.Desktop; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; -import java.util.function.Supplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.PhotonUtils; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import swervelib.SwerveDrive; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; -import swervelib.telemetry.SwerveDriveTelemetry; - - -/** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from - * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads - */ -public class Vision -{ - - /** - * April Tag Field Layout of the year. - */ - public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( - AprilTagFields.k2024Crescendo); - /** - * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. - */ - private final double maximumAmbiguity = 0.25; - /** - * Photon Vision Simulation - */ - public VisionSystemSim visionSim; - /** - * Count of times that the odom thinks we're more than 10meters away from the april tag. - */ - private double longDistangePoseEstimationCount = 0; - /** - * Current pose from the pose estimator using wheel odometry. - */ - private Supplier currentPose; - /** - * Field from {@link swervelib.SwerveDrive#field} - */ - private Field2d field2d; - - - /** - * Constructor for the Vision class. - * - * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} - * @param field Current field, should be {@link SwerveDrive#field} - */ - public Vision(Supplier currentPose, Field2d field) - { - this.currentPose = currentPose; - this.field2d = field; - - if (Robot.isSimulation()) - { - visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(fieldLayout); - - for (Cameras c : Cameras.values()) - { - c.addToVisionSim(visionSim); - } - - openSimCameraViews(); - } - } - - /** - * Calculates a target pose relative to an AprilTag on the field. - * - * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position - * itself correctly. - * @return The target pose of the AprilTag. - */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) - { - Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) - { - return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else - { - throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); - } - - } - - /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. - * - * @param swerveDrive {@link SwerveDrive} instance. - */ - public void updatePoseEstimation(SwerveDrive swerveDrive) - { - if (SwerveDriveTelemetry.isSimulation) - { - visionSim.update(swerveDrive.getPose()); - - } - for (Cameras camera : Cameras.values()) - { - Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) - { - var pose = poseEst.get(); - swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - getEstimationStdDevs(camera)); - } - } - - } - - /** - * Generates the estimated robot pose. Returns empty if: - *
      - *
    • No Pose Estimates could be generated
      • - *
    • The generated pose estimate was considered not accurate
      • - *
    - * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate - */ - public Optional getEstimatedGlobalPose(Cameras camera) - { - Optional poseEst = filterPose(camera.poseEstimator.update()); - // Uncomment to enable outputting of vision targets in sim. - /* - poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") - .setPose(estimatedRobotPose.estimatedPose.toPose2d())); - */ - return poseEst; - } - - /** - * The standard deviations of the estimated pose from {@link Vision#getEstimatedGlobalPose(Cameras)}, for use with - * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only be used - * when there are targets visible. - * - * @param camera Desired camera to get the standard deviation of the estimated pose. - */ - public Matrix getEstimationStdDevs(Cameras camera) - { - var poseEst = getEstimatedGlobalPose(camera); - var estStdDevs = camera.singleTagStdDevs; - var targets = getLatestResult(camera).getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) - { - var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - { - continue; - } - numTags++; - if (poseEst.isPresent()) - { - avgDist += PhotonUtils.getDistanceToPose(poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); - } - } - if (numTags == 0) - { - return estStdDevs; - } - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) - { - estStdDevs = camera.multiTagStdDevs; - } - // 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)); - } - - return estStdDevs; - } - - /** - * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than - * 10m for a short amount of time. - * - * @param pose Estimated robot pose. - * @return Could be empty if there isn't a good reading. - */ - private Optional filterPose(Optional pose) - { - if (pose.isPresent()) - { - double bestTargetAmbiguity = 1; // 1 is max ambiguity - for (PhotonTrackedTarget target : pose.get().targetsUsed) - { - double ambiguity = target.getPoseAmbiguity(); - if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) - { - bestTargetAmbiguity = ambiguity; - } - } - //ambiguity to high dont use estimate - if (bestTargetAmbiguity > maximumAmbiguity) - { - return Optional.empty(); - } - - //est pose is very far from recorded robot pose - if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) - { - longDistangePoseEstimationCount++; - - //if it calculates that were 10 meter away for more than 10 times in a row its probably right - if (longDistangePoseEstimationCount < 10) - { - return Optional.empty(); - } - } else - { - longDistangePoseEstimationCount = 0; - } - return pose; - } - return Optional.empty(); - } - - /** - * Get the latest result from a given Camera. - * - * @param camera Given camera to take the result from. - * @return Photon result from sim or a real camera. - */ - public PhotonPipelineResult getLatestResult(Cameras camera) - { - - return Robot.isReal() ? camera.camera.getLatestResult() : camera.cameraSim.getCamera().getLatestResult(); - } - - /** - * Get distance of the robot from the AprilTag pose. - * - * @param id AprilTag ID - * @return Distance - */ - public double getDistanceFromAprilTag(int id) - { - Optional tag = fieldLayout.getTagPose(id); - return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); - } - - /** - * Get tracked target from a camera of AprilTagID - * - * @param id AprilTag ID - * @param camera Camera to check. - * @return Tracked target. - */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) - { - PhotonTrackedTarget target = null; - PhotonPipelineResult result = getLatestResult(camera); - if (result.hasTargets()) - { - for (PhotonTrackedTarget i : result.getTargets()) - { - if (i.getFiducialId() == id) - { - target = i; - } - } - } - return target; - } - - /** - * Vision simulation. - * - * @return Vision Simulation - */ - public VisionSystemSim getVisionSim() - { - return visionSim; - } - - /** - * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. - */ - private void openSimCameraViews() - { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) - { -// try +//package frc.robot.subsystems.swervedrive; +// +//import edu.wpi.first.apriltag.AprilTagFieldLayout; +//import edu.wpi.first.apriltag.AprilTagFields; +//import edu.wpi.first.math.Matrix; +//import edu.wpi.first.math.VecBuilder; +//import edu.wpi.first.math.geometry.Pose2d; +//import edu.wpi.first.math.geometry.Pose3d; +//import edu.wpi.first.math.geometry.Rotation2d; +//import edu.wpi.first.math.geometry.Rotation3d; +//import edu.wpi.first.math.geometry.Transform2d; +//import edu.wpi.first.math.geometry.Transform3d; +//import edu.wpi.first.math.geometry.Translation3d; +//import edu.wpi.first.math.numbers.N1; +//import edu.wpi.first.math.numbers.N3; +//import edu.wpi.first.math.util.Units; +//import edu.wpi.first.wpilibj.smartdashboard.Field2d; +//import frc.robot.Robot; +//import java.awt.Desktop; +//import java.util.ArrayList; +//import java.util.List; +//import java.util.Optional; +//import java.util.function.Supplier; +//import org.photonvision.EstimatedRobotPose; +//import org.photonvision.PhotonCamera; +//import org.photonvision.PhotonPoseEstimator; +//import org.photonvision.PhotonPoseEstimator.PoseStrategy; +//import org.photonvision.PhotonUtils; +//import org.photonvision.simulation.PhotonCameraSim; +//import org.photonvision.simulation.SimCameraProperties; +//import org.photonvision.simulation.VisionSystemSim; +//import org.photonvision.targeting.PhotonPipelineResult; +//import org.photonvision.targeting.PhotonTrackedTarget; +//import swervelib.SwerveDrive; +//import swervelib.telemetry.Alert; +//import swervelib.telemetry.Alert.AlertType; +//import swervelib.telemetry.SwerveDriveTelemetry; +// +// +///** +// * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from +// * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads +// */ +//public class Vision +//{ +// +// /** +// * April Tag Field Layout of the year. +// */ +// public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( +// AprilTagFields.k2024Crescendo); +// /** +// * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. +// */ +// private final double maximumAmbiguity = 0.25; +// /** +// * Photon Vision Simulation +// */ +// public VisionSystemSim visionSim; +// /** +// * Count of times that the odom thinks we're more than 10meters away from the april tag. +// */ +// private double longDistangePoseEstimationCount = 0; +// /** +// * Current pose from the pose estimator using wheel odometry. +// */ +// private Supplier currentPose; +// /** +// * Field from {@link swervelib.SwerveDrive#field} +// */ +// private Field2d field2d; +// +// +// /** +// * Constructor for the Vision class. +// * +// * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} +// * @param field Current field, should be {@link SwerveDrive#field} +// */ +// public Vision(Supplier currentPose, Field2d field) +// { +// this.currentPose = currentPose; +// this.field2d = field; +// +// if (Robot.isSimulation()) +// { +// visionSim = new VisionSystemSim("Vision"); +// visionSim.addAprilTags(fieldLayout); +// +// for (Cameras c : Cameras.values()) // { -// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); -// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); -// } catch (IOException | URISyntaxException e) +// c.addToVisionSim(visionSim); +// } +// +// openSimCameraViews(); +// } +// } +// +// /** +// * Calculates a target pose relative to an AprilTag on the field. +// * +// * @param aprilTag The ID of the AprilTag. +// * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position +// * itself correctly. +// * @return The target pose of the AprilTag. +// */ +// public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) +// { +// Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); +// if (aprilTagPose3d.isPresent()) +// { +// return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); +// } else +// { +// throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); +// } +// +// } +// +// /** +// * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. +// * +// * @param swerveDrive {@link SwerveDrive} instance. +// */ +// public void updatePoseEstimation(SwerveDrive swerveDrive) +// { +// if (SwerveDriveTelemetry.isSimulation) +// { +// visionSim.update(swerveDrive.getPose()); +// +// } +// for (Cameras camera : Cameras.values()) +// { +// Optional poseEst = getEstimatedGlobalPose(camera); +// if (poseEst.isPresent()) +// { +// var pose = poseEst.get(); +// swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), +// pose.timestampSeconds, +// getEstimationStdDevs(camera)); +// } +// } +// +// } +// +// /** +// * Generates the estimated robot pose. Returns empty if: +// *
      +// *
    • No Pose Estimates could be generated
      • +// *
    • The generated pose estimate was considered not accurate
      • +// *
    +// * +// * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate +// */ +// public Optional getEstimatedGlobalPose(Cameras camera) +// { +// Optional poseEst = filterPose(camera.poseEstimator.update()); +// // Uncomment to enable outputting of vision targets in sim. +// /* +// poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") +// .setPose(estimatedRobotPose.estimatedPose.toPose2d())); +// */ +// return poseEst; +// } +// +// /** +// * The standard deviations of the estimated pose from {@link Vision#getEstimatedGlobalPose(Cameras)}, for use with +// * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only be used +// * when there are targets visible. +// * +// * @param camera Desired camera to get the standard deviation of the estimated pose. +// */ +// public Matrix getEstimationStdDevs(Cameras camera) +// { +// var poseEst = getEstimatedGlobalPose(camera); +// var estStdDevs = camera.singleTagStdDevs; +// var targets = getLatestResult(camera).getTargets(); +// int numTags = 0; +// double avgDist = 0; +// for (var tgt : targets) +// { +// var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); +// if (tagPose.isEmpty()) +// { +// continue; +// } +// numTags++; +// if (poseEst.isPresent()) +// { +// avgDist += PhotonUtils.getDistanceToPose(poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); +// } +// } +// if (numTags == 0) +// { +// return estStdDevs; +// } +// avgDist /= numTags; +// // Decrease std devs if multiple targets are visible +// if (numTags > 1) +// { +// estStdDevs = camera.multiTagStdDevs; +// } +// // 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)); +// } +// +// return estStdDevs; +// } +// +// /** +// * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than +// * 10m for a short amount of time. +// * +// * @param pose Estimated robot pose. +// * @return Could be empty if there isn't a good reading. +// */ +// private Optional filterPose(Optional pose) +// { +// if (pose.isPresent()) +// { +// double bestTargetAmbiguity = 1; // 1 is max ambiguity +// for (PhotonTrackedTarget target : pose.get().targetsUsed) +// { +// double ambiguity = target.getPoseAmbiguity(); +// if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) +// { +// bestTargetAmbiguity = ambiguity; +// } +// } +// //ambiguity to high dont use estimate +// if (bestTargetAmbiguity > maximumAmbiguity) +// { +// return Optional.empty(); +// } +// +// //est pose is very far from recorded robot pose +// if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) // { -// e.printStackTrace(); +// longDistangePoseEstimationCount++; +// +// //if it calculates that were 10 meter away for more than 10 times in a row its probably right +// if (longDistangePoseEstimationCount < 10) +// { +// return Optional.empty(); +// } +// } else +// { +// longDistangePoseEstimationCount = 0; +// } +// return pose; +// } +// return Optional.empty(); +// } +// +// /** +// * Get the latest result from a given Camera. +// * +// * @param camera Given camera to take the result from. +// * @return Photon result from sim or a real camera. +// */ +// public PhotonPipelineResult getLatestResult(Cameras camera) +// { +// +// return Robot.isReal() ? camera.camera.getLatestResult() : camera.cameraSim.getCamera().getLatestResult(); +// } +// +// /** +// * Get distance of the robot from the AprilTag pose. +// * +// * @param id AprilTag ID +// * @return Distance +// */ +// public double getDistanceFromAprilTag(int id) +// { +// Optional tag = fieldLayout.getTagPose(id); +// return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); +// } +// +// /** +// * Get tracked target from a camera of AprilTagID +// * +// * @param id AprilTag ID +// * @param camera Camera to check. +// * @return Tracked target. +// */ +// public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) +// { +// PhotonTrackedTarget target = null; +// PhotonPipelineResult result = getLatestResult(camera); +// if (result.hasTargets()) +// { +// for (PhotonTrackedTarget i : result.getTargets()) +// { +// if (i.getFiducialId() == id) +// { +// target = i; +// } // } - } - } - - /** - * Update the {@link Field2d} to include tracked targets/ - */ - public void updateVisionField() - { - - List targets = new ArrayList(); - for (Cameras c : Cameras.values()) - { - if (getLatestResult(c).hasTargets()) - { - targets.addAll(getLatestResult(c).targets); - } - } - - List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) - { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) - { - Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); - poses.add(targetPose); - } - } - - field2d.getObject("tracked targets").setPoses(poses); - } - - /** - * Camera Enum to select each camera - */ - enum Cameras - { - /** - * Left Camera - */ - LEFT_CAM("left", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), - /** - * Right Camera - */ - RIGHT_CAM("right", - new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), - new Translation3d(Units.inchesToMeters(12.056), - Units.inchesToMeters(-10.981), - Units.inchesToMeters(8.44)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), - /** - * Center Camera - */ - CENTER_CAM("center", - new Rotation3d(0, Units.degreesToRadians(18), 0), - new Translation3d(Units.inchesToMeters(-4.628), - Units.inchesToMeters(-10.687), - Units.inchesToMeters(16.129)), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); - - /** - * Latency alert to use when high latency is detected. - */ - public final Alert latencyAlert; - /** - * Camera instance for comms. - */ - public final PhotonCamera camera; - /** - * Pose estimator for camera. - */ - public final PhotonPoseEstimator poseEstimator; - public final Matrix singleTagStdDevs; - public final Matrix multiTagStdDevs; - /** - * Transform of the camera rotation and translation relative to the center of the robot - */ - private final Transform3d robotToCamTransform; - /** - * Simulated camera instance which only exists during simulations. - */ - public PhotonCameraSim cameraSim; - - /** - * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine - * estimation noise on an actual robot. - * - * @param name Name of the PhotonVision camera found in the PV UI. - * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. - */ - Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) - { - latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); - - camera = new PhotonCamera(name); - - // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html - robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); - - poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, - robotToCamTransform); - poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.singleTagStdDevs = singleTagStdDevs; - this.multiTagStdDevs = multiTagStdDevsMatrix; - - if (Robot.isSimulation()) - { - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop rate). - cameraProp.setFPS(30); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - cameraSim = new PhotonCameraSim(camera, cameraProp); - cameraSim.enableDrawWireframe(true); - } - } - - /** - * Add camera to {@link VisionSystemSim} for simulated photon vision. - * - * @param systemSim {@link VisionSystemSim} to use. - */ - public void addToVisionSim(VisionSystemSim systemSim) - { - if (Robot.isSimulation()) - { - systemSim.addCamera(cameraSim, robotToCamTransform); +// } +// return target; +// } +// +// /** +// * Vision simulation. +// * +// * @return Vision Simulation +// */ +// public VisionSystemSim getVisionSim() +// { +// return visionSim; +// } +// +// /** +// * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. +// */ +// private void openSimCameraViews() +// { +// if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) +// { +//// try +//// { +//// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); +//// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); +//// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); +//// } catch (IOException | URISyntaxException e) +//// { +//// e.printStackTrace(); +//// } +// } +// } +// +// /** +// * Update the {@link Field2d} to include tracked targets/ +// */ +// public void updateVisionField() +// { +// +// List targets = new ArrayList(); +// for (Cameras c : Cameras.values()) +// { +// if (getLatestResult(c).hasTargets()) +// { +// targets.addAll(getLatestResult(c).targets); +// } +// } +// +// List poses = new ArrayList<>(); +// for (PhotonTrackedTarget target : targets) +// { +// if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) +// { +// Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); +// poses.add(targetPose); +// } +// } +// +// field2d.getObject("tracked targets").setPoses(poses); +// } +// +// /** +// * Camera Enum to select each camera +// */ +// enum Cameras +// { +// /** +// * Left Camera +// */ +// LEFT_CAM("left", +// new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), +// new Translation3d(Units.inchesToMeters(12.056), +// Units.inchesToMeters(10.981), +// Units.inchesToMeters(8.44)), +// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), +// /** +// * Right Camera +// */ +// RIGHT_CAM("right", +// new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), +// new Translation3d(Units.inchesToMeters(12.056), +// Units.inchesToMeters(-10.981), +// Units.inchesToMeters(8.44)), +// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), +// /** +// * Center Camera +// */ +// CENTER_CAM("center", +// new Rotation3d(0, Units.degreesToRadians(18), 0), +// new Translation3d(Units.inchesToMeters(-4.628), +// Units.inchesToMeters(-10.687), +// Units.inchesToMeters(16.129)), +// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); +// +// /** +// * Latency alert to use when high latency is detected. +// */ +// public final Alert latencyAlert; +// /** +// * Camera instance for comms. +// */ +// public final PhotonCamera camera; +// /** +// * Pose estimator for camera. +// */ +// public final PhotonPoseEstimator poseEstimator; +// public final Matrix singleTagStdDevs; +// public final Matrix multiTagStdDevs; +// /** +// * Transform of the camera rotation and translation relative to the center of the robot +// */ +// private final Transform3d robotToCamTransform; +// /** +// * Simulated camera instance which only exists during simulations. +// */ +// public PhotonCameraSim cameraSim; +// +// /** +// * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine +// * estimation noise on an actual robot. +// * +// * @param name Name of the PhotonVision camera found in the PV UI. +// * @param robotToCamRotation {@link Rotation3d} of the camera. +// * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. +// * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. +// * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. +// */ +// Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, +// Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) +// { +// latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); +// +// camera = new PhotonCamera(name); +// +// // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html +// robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); +// +// poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, +// PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, +// camera, +// robotToCamTransform); +// poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); +// +// this.singleTagStdDevs = singleTagStdDevs; +// this.multiTagStdDevs = multiTagStdDevsMatrix; +// +// if (Robot.isSimulation()) +// { +// SimCameraProperties cameraProp = new SimCameraProperties(); +// // A 640 x 480 camera with a 100 degree diagonal FOV. +// cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); +// // Approximate detection noise with average and standard deviation error in pixels. +// cameraProp.setCalibError(0.25, 0.08); +// // Set the camera image capture framerate (Note: this is limited by robot loop rate). +// cameraProp.setFPS(30); +// // The average and standard deviation in milliseconds of image data latency. +// cameraProp.setAvgLatencyMs(35); +// cameraProp.setLatencyStdDevMs(5); +// +// cameraSim = new PhotonCameraSim(camera, cameraProp); // cameraSim.enableDrawWireframe(true); - } - } - } - -} +// } +// } +// +// /** +// * Add camera to {@link VisionSystemSim} for simulated photon vision. +// * +// * @param systemSim {@link VisionSystemSim} to use. +// */ +// public void addToVisionSim(VisionSystemSim systemSim) +// { +// if (Robot.isSimulation()) +// { +// systemSim.addCamera(cameraSim, robotToCamTransform); +//// cameraSim.enableDrawWireframe(true); +// } +// } +// } +// +//} diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 8b5dec10..a9d2cbf7 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -1,14 +1,18 @@ package swervelib; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; +import static edu.wpi.first.units.Units.MetersPerSecond; + 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 edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.parser.Cache; import swervelib.parser.PIDFConfig; @@ -371,9 +375,9 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, } // Cosine compensation. - double velocity = configuration.useCosineCompensator + LinearVelocity velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState) - : desiredState.speedMetersPerSecond; + : MetersPerSecond.of(desiredState.speedMetersPerSecond); if (isOpenLoop) { @@ -381,8 +385,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, driveMotor.set(percentOutput); } else { - driveMotor.setReference(velocity, driveMotorFeedforward.calculate(velocity)); - desiredState.speedMetersPerSecond = velocity; + driveMotor.setReference(velocity.magnitude(), driveMotorFeedforward.calculate(velocity).magnitude()); + desiredState.speedMetersPerSecond = velocity.baseUnitMagnitude(); } // Prevent module rotation if angle is the same as the previous angle. @@ -408,10 +412,11 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, simModule.updateStateAndPosition(desiredState); } + // TODO: Change and move to SwerveDriveTelemetry if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity; + SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity.magnitude(); } if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) @@ -429,7 +434,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, * @param desiredState Desired {@link SwerveModuleState} to use. * @return Cosine compensated velocity in meters/second. */ - private double getCosineCompensatedVelocity(SwerveModuleState desiredState) + private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState) { double cosineScalar = 1.0; // Taken from the CTRE SwerveModule class. @@ -447,7 +452,7 @@ private double getCosineCompensatedVelocity(SwerveModuleState desiredState) cosineScalar = 1; } - return desiredState.speedMetersPerSecond * (cosineScalar); + return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar); } /** @@ -629,9 +634,9 @@ public void pushOffsetsToEncoders() if (absoluteEncoder != null && angleOffset == configuration.angleOffset) { // If the absolute encoder is attached. - if (angleMotor.getMotor() instanceof CANSparkMax) + if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve) { - if (absoluteEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + if (absoluteEncoder instanceof SparkMaxEncoderSwerve) { angleMotor.setAbsoluteEncoder(absoluteEncoder); if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index bbf9fb6c..a562c304 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -1,9 +1,9 @@ package swervelib.encoders; -import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkAnalogSensor.Mode; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkAnalogSensor; +import com.revrobotics.spark.SparkMax; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -21,7 +21,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; + private Alert failureConfiguring; /** * An {@link Alert} for if the absolute encoder does not support integrated offsets. */ @@ -37,9 +37,9 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder */ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) { - if (motor.getMotor() instanceof CANSparkMax) + if (motor.getMotor() instanceof SparkMax) { - encoder = ((CANSparkMax) motor.getMotor()).getAnalog(Mode.kAbsolute); + encoder = ((SparkMax) motor.getMotor()).getAnalog(); encoder.setPositionConversionFactor(360 / maxVoltage); } else { diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index 6bd68dad..6afe7b48 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -1,9 +1,9 @@ package swervelib.encoders; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; -import com.revrobotics.SparkAbsoluteEncoder.Type; +import com.revrobotics.spark.SparkAbsoluteEncoder; +import com.revrobotics.spark.SparkMax; import java.util.function.Supplier; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -17,18 +17,18 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkMax. */ - public AbsoluteEncoder encoder; + public SparkAbsoluteEncoder encoder; /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; + private Alert failureConfiguring; /** * An {@link Alert} for if there is a failure configuring the encoder offset. */ private Alert offsetFailure; /** - * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link CANSparkMax} motor. + * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax} motor. * * @param motor Motor to create the encoder from. * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. @@ -43,9 +43,9 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) "Encoders", "Failure to set Absolute Encoder Offset", Alert.AlertType.WARNING_TRACE); - if (motor.getMotor() instanceof CANSparkMax) + if (motor.getMotor() instanceof SparkMax) { - encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle); + encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder(); configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); } else diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 400de964..aff0b5fb 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,50 +1,63 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkLowLevel.PeriodicFrame; +import com.revrobotics.spark.SparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; +import swervelib.encoders.SparkMaxAnalogEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** - * An implementation of {@link CANSparkFlex} as a {@link SwerveMotor}. + * An implementation of {@link SparkFlex} as a {@link SwerveMotor}. */ public class SparkFlexSwerve extends SwerveMotor { /** - * {@link CANSparkFlex} Instance. + * {@link SparkFlex} Instance. */ - private final CANSparkFlex motor; + private final SparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkClosedLoopController pid; + /** + * Supplier for the velocity of the motor controller. + */ + private Supplier velocity; + /** + * Supplier for the position of the motor controller. + */ + private Supplier position; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if there is an error configuring the motor. */ @@ -52,7 +65,16 @@ public class SparkFlexSwerve extends SwerveMotor /** * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ - private Alert absoluteEncoderOffsetWarning; + private Alert absoluteEncoderOffsetWarning; + /** + * Configuration object for {@link SparkFlex} motor. + */ + private SparkFlexConfig cfg = new SparkFlexConfig(); + /** + * Tracker for changes that need to be pushed. + */ + private boolean cfgUpdated = false; + /** * Initialize the swerve motor. @@ -60,7 +82,7 @@ public class SparkFlexSwerve extends SwerveMotor * @param motor The SwerveMotor as a SparkFlex object. * @param isDriveMotor Is the motor being initialized a drive motor? */ - public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) + public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor) { this.motor = motor; this.isDriveMotor = isDriveMotor; @@ -68,9 +90,9 @@ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) clearStickyFaults(); encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. + pid = motor.getClosedLoopController(); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder. + cfgUpdated = true; // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. @@ -82,18 +104,19 @@ public SparkFlexSwerve(CANSparkFlex motor, boolean isDriveMotor) "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + "absoluteEncoderOffset in the Swerve Module JSON!", Alert.AlertType.WARNING); - + velocity = encoder::getVelocity; + position = encoder::getPosition; } /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * Initialize the {@link SwerveMotor} as a {@link SparkFlex} connected to a Brushless Motor. * * @param id CAN ID of the SparkMax. * @param isDriveMotor Is the motor being initialized a drive motor? */ public SparkFlexSwerve(int id, boolean isDriveMotor) { - this(new CANSparkFlex(id, MotorType.kBrushless), isDriveMotor); + this(new SparkFlex(id, MotorType.kBrushless), isDriveMotor); } /** @@ -122,7 +145,8 @@ private void configureSparkFlex(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkFlex(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -134,7 +158,9 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; } /** @@ -145,8 +171,9 @@ public void setCurrentLimit(int currentLimit) @Override public void setLoopRampRate(double rampRate) { - configureSparkFlex(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkFlex(() -> motor.setClosedLoopRampRate(rampRate)); + cfg.closedLoopRampRate(rampRate) + .openLoopRampRate(rampRate); + cfgUpdated = true; } /** @@ -177,11 +204,7 @@ public boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkFlex(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -204,13 +227,20 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder == null) { - absoluteEncoder = null; - configureSparkFlex(() -> pid.setFeedbackDevice(this.encoder)); - } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + absoluteEncoder = null;cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfgUpdated = true; + + velocity = this.encoder::getVelocity; + position = this.encoder::getPosition; + } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + cfgUpdated = true; absoluteEncoderOffsetWarning.set(true); absoluteEncoder = encoder; - configureSparkFlex(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); + + velocity = absoluteEncoder::getVelocity; + position = absoluteEncoder::getAbsolutePosition; } return this; } @@ -223,39 +253,66 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @Override public void configureIntegratedEncoder(double positionConversionFactor) { + cfg.signals + .absoluteEncoderPositionAlwaysOn(false) + .absoluteEncoderVelocityAlwaysOn(false) + .analogPositionAlwaysOn(false) + .analogVelocityAlwaysOn(false) + .analogVoltageAlwaysOn(false) + .externalOrAltEncoderPositionAlwaysOn(false) + .externalOrAltEncoderVelocityAlwaysOn(false) + .primaryEncoderPositionAlwaysOn(false) + .primaryEncoderVelocityAlwaysOn(false) + .iAccumulationAlwaysOn(false) + .appliedOutputPeriodMs(10) + .faultsPeriodMs(20) + ; if (absoluteEncoder == null) { - configureSparkFlex(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkFlex(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); + } else { - configureSparkFlex(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } - }); - configureSparkFlex(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } - }); + // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 + // This needs to be set to 20ms or under to properly update the swerve module position for odometry + // Configuration taken from 3005, the team who helped develop the Max Swerve: + // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 + // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. + // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, + // with limited testing 19ms did not return the same value while the module was constatntly rotating. + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + cfg.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20); + + cfg.absoluteEncoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + } } + cfgUpdated = true; + } /** @@ -266,15 +323,10 @@ public void configureIntegratedEncoder(double positionConversionFactor) @Override public void configurePIDF(PIDFConfig config) { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkFlex(() -> pid.setP(config.p, pidSlot)); - configureSparkFlex(() -> pid.setI(config.i, pidSlot)); - configureSparkFlex(() -> pid.setD(config.d, pidSlot)); - configureSparkFlex(() -> pid.setFF(config.f, pidSlot)); - configureSparkFlex(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkFlex(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) + .iZone(config.iz) + .outputRange(config.output.min, config.output.max); + cfgUpdated = true; } /** @@ -286,30 +338,11 @@ public void configurePIDF(PIDFConfig config) @Override public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkFlex(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkFlex(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } + cfg.closedLoop + .positionWrappingEnabled(true) + .positionWrappingInputRange(minInput, maxInput); + cfgUpdated = true; - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) - { - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkFlex(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces } /** @@ -320,7 +353,8 @@ public void configureCANStatusFrames( @Override public void setMotorBrake(boolean isBrakeMode) { - configureSparkFlex(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); + cfgUpdated = true; } /** @@ -349,7 +383,8 @@ public void burnFlash() } catch (Exception e) { } - configureSparkFlex(() -> motor.burnFlash()); + motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + cfgUpdated = false; } /** @@ -372,27 +407,30 @@ public void set(double percentOutput) @Override public void setReference(double setpoint, double feedforward) { - boolean possibleBurnOutIssue = true; -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); int pidSlot = 0; + if(cfgUpdated) + { + burnFlash(); + Timer.delay(0.1); // Give 100ms to apply changes + } + if (isDriveMotor) { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); } else { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); if (SwerveDriveTelemetry.isSimulation) { encoder.setPosition(setpoint); @@ -482,22 +520,4 @@ public void setPosition(double position) } } - /** - * REV Slots for PID configuration. - */ - enum SparkMAX_slotIdx - { - /** - * Slot 1, used for position PID's. - */ - Position, - /** - * Slot 2, used for velocity PID's. - */ - Velocity, - /** - * Slot 3, used arbitrarily. (Documentation show simulations). - */ - Simulation - } } diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 14795716..eacdd882 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -1,57 +1,70 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAnalogSensor; -import com.revrobotics.SparkPIDController; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; +import swervelib.encoders.SparkMaxAnalogEncoderSwerve; +import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; /** - * An implementation of {@link CANSparkMax} as a {@link SwerveMotor}. + * An implementation of {@link com.revrobotics.spark.SparkMax} as a {@link SwerveMotor}. */ public class SparkMaxSwerve extends SwerveMotor { /** - * {@link CANSparkMax} Instance. + * {@link SparkMax} Instance. */ - private final CANSparkMax motor; + private final SparkMax motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkClosedLoopController pid; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; + private Supplier position; + /** + * Configuration object for {@link SparkMax} motor. + */ + private SparkMaxConfig cfg = new SparkMaxConfig(); + /** + * Tracker for changes that need to be pushed. + */ + private boolean cfgUpdated = false; + /** * Initialize the swerve motor. @@ -59,7 +72,7 @@ public class SparkMaxSwerve extends SwerveMotor * @param motor The SwerveMotor as a SparkMax object. * @param isDriveMotor Is the motor being initialized a drive motor? */ - public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) + public SparkMaxSwerve(SparkMax motor, boolean isDriveMotor) { this.motor = motor; this.isDriveMotor = isDriveMotor; @@ -67,25 +80,26 @@ public SparkMaxSwerve(CANSparkMax motor, boolean isDriveMotor) clearStickyFaults(); encoder = motor.getEncoder(); - pid = motor.getPIDController(); - pid.setFeedbackDevice( - encoder); // Configure feedback of the PID controller as the integrated encoder. + pid = motor.getClosedLoopController(); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); // Configure feedback of the PID controller as the integrated encoder. + cfgUpdated = true; velocity = encoder::getVelocity; position = encoder::getPosition; + // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. } /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor. * * @param id CAN ID of the SparkMax. * @param isDriveMotor Is the motor being initialized a drive motor? */ public SparkMaxSwerve(int id, boolean isDriveMotor) { - this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor); + this(new SparkMax(id, MotorType.kBrushless), isDriveMotor); } /** @@ -114,7 +128,8 @@ private void configureSparkMax(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -126,7 +141,9 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; + } /** @@ -137,8 +154,10 @@ public void setCurrentLimit(int currentLimit) @Override public void setLoopRampRate(double rampRate) { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + cfg.closedLoopRampRate(rampRate) + .openLoopRampRate(rampRate); + cfgUpdated = true; + } /** @@ -169,11 +188,7 @@ public boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -197,17 +212,23 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) if (encoder == null) { absoluteEncoder = null; - configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfgUpdated = true; + velocity = this.encoder::getVelocity; position = this.encoder::getPosition; - } else if (encoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor) + burnFlash(); + } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve) { + cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve + ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); + cfgUpdated = true; + DriverStation.reportWarning( "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + " absoluteEncoderOffset in the Swerve Module JSON!", false); absoluteEncoder = encoder; - configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder())); velocity = absoluteEncoder::getVelocity; position = absoluteEncoder::getAbsolutePosition; } @@ -222,10 +243,25 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @Override public void configureIntegratedEncoder(double positionConversionFactor) { + cfg.signals + .absoluteEncoderPositionAlwaysOn(false) + .absoluteEncoderVelocityAlwaysOn(false) + .analogPositionAlwaysOn(false) + .analogVelocityAlwaysOn(false) + .analogVoltageAlwaysOn(false) + .externalOrAltEncoderPositionAlwaysOn(false) + .externalOrAltEncoderVelocityAlwaysOn(false) + .primaryEncoderPositionAlwaysOn(false) + .primaryEncoderVelocityAlwaysOn(false) + .iAccumulationAlwaysOn(false) + .appliedOutputPeriodMs(10) + .faultsPeriodMs(20) + ; if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag @@ -233,13 +269,18 @@ public void configureIntegratedEncoder(double positionConversionFactor) // This value was taken from: // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches - configureSparkMax(() -> encoder.setMeasurementPeriod(10)); - configureSparkMax(() -> encoder.setAverageDepth(2)); + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 // Unused frames can be set to 65535 to decrease CAN ultilization. - configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200); + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); + } else { // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 @@ -251,36 +292,28 @@ public void configureIntegratedEncoder(double positionConversionFactor) // with limited testing 19ms did not return the same value while the module was constatntly rotating. if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { - configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50); - } - // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use - else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) + cfg.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20); + + cfg.absoluteEncoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + } else { - configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200); + cfg.signals + .analogVoltageAlwaysOn(true) + .analogPositionAlwaysOn(true) + .analogVoltagePeriodMs(20) + .analogPositionPeriodMs(20); + + cfg.analogSensor + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); } - configureSparkMax(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setPositionConversionFactor( - positionConversionFactor); - } - }); - configureSparkMax(() -> { - if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) - { - return ((AbsoluteEncoder) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } else - { - return ((SparkAnalogSensor) absoluteEncoder.getAbsoluteEncoder()).setVelocityConversionFactor( - positionConversionFactor / 60); - } - }); } + cfgUpdated = true; + } /** @@ -291,15 +324,11 @@ else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) @Override public void configurePIDF(PIDFConfig config) { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - configureSparkMax(() -> pid.setP(config.p)); - configureSparkMax(() -> pid.setI(config.i)); - configureSparkMax(() -> pid.setD(config.d)); - configureSparkMax(() -> pid.setFF(config.f)); - configureSparkMax(() -> pid.setIZone(config.iz)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max)); + cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) + .iZone(config.iz) + .outputRange(config.output.min, config.output.max); + cfgUpdated = true; + } /** @@ -311,33 +340,11 @@ public void configurePIDF(PIDFConfig config) @Override public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } + cfg.closedLoop + .positionWrappingEnabled(true) + .positionWrappingInputRange(minInput, maxInput); + cfgUpdated = true; - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - * @param CANStatus5 Duty Cycle Absolute Encoder Position, Duty Cycle Absolute Encoder Absolute Angle - * @param CANStatus6 Duty Cycle Absolute Encoder Velocity, Duty Cycle Absolute Encoder Frequency - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4, int CANStatus5, int CANStatus6) - { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, CANStatus5)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, CANStatus6)); - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces } /** @@ -348,7 +355,9 @@ public void configureCANStatusFrames( @Override public void setMotorBrake(boolean isBrakeMode) { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); + cfgUpdated = true; + } /** @@ -377,7 +386,8 @@ public void burnFlash() } catch (Exception e) { } - configureSparkMax(() -> motor.burnFlash()); + motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + cfgUpdated = false; } /** @@ -400,11 +410,14 @@ public void set(double percentOutput) @Override public void setReference(double setpoint, double feedforward) { - boolean possibleBurnOutIssue = true; -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); int pidSlot = 0; + if(cfgUpdated) + { + burnFlash(); + Timer.delay(0.1); // Give 100ms to apply changes + } + if (isDriveMotor) { configureSparkMax(() -> @@ -509,23 +522,4 @@ public void setPosition(double position) configureSparkMax(() -> encoder.setPosition(position)); } } - - /** - * REV Slots for PID configuration. - */ - enum SparkMAX_slotIdx - { - /** - * Slot 1, used for position PID's. - */ - Position, - /** - * Slot 2, used for velocity PID's. - */ - Velocity, - /** - * Slot 3, used arbitrarily. (Documentation show simulations). - */ - Simulation - } } diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json deleted file mode 100644 index e978a5f7..00000000 --- a/vendordeps/NavX.json +++ /dev/null @@ -1,40 +0,0 @@ -{ - "fileName": "NavX.json", - "name": "NavX", - "version": "2024.1.0", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "frcYear": "2024", - "mavenUrls": [ - "https://dev.studica.com/maven/release/2024/" - ], - "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-java", - "version": "2024.1.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-frc-cpp", - "version": "2024.1.0", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxraspbian", - "linuxarm32", - "linuxarm64", - "linuxx86-64", - "osxuniversal", - "windowsx86-64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib-2025.0.0-beta-3.1.json similarity index 80% rename from vendordeps/PathplannerLib.json rename to vendordeps/PathplannerLib-2025.0.0-beta-3.1.json index 6dc648db..5f4b1f18 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib-2025.0.0-beta-3.1.json @@ -1,18 +1,18 @@ { - "fileName": "PathplannerLib.json", + "fileName": "PathplannerLib-2025.0.0-beta-3.1.json", "name": "PathplannerLib", - "version": "2024.2.8", + "version": "2025.0.0-beta-3.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib-beta.json", "javaDependencies": [ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.8" + "version": "2025.0.0-beta-3.1" } ], "jniDependencies": [], @@ -20,15 +20,15 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.8", + "version": "2025.0.0-beta-3.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "linuxx86-64", "osxuniversal", + "linuxx86-64", "linuxathena", "linuxarm32", "linuxarm64" diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5-5.34.0-beta-2.json similarity index 88% rename from vendordeps/Phoenix5.json rename to vendordeps/Phoenix5-5.34.0-beta-2.json index ff7359e1..e27638b2 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5-5.34.0-beta-2.json @@ -1,13 +1,13 @@ { - "fileName": "Phoenix5.json", + "fileName": "Phoenix5-5.34.0-beta-2.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.1", - "frcYear": 2024, + "version": "5.34.0-beta-2", + "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-beta-latest.json", "requires": [ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", @@ -20,19 +20,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.1" + "version": "5.34.0-beta-2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.1" + "version": "5.34.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.34.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.1", + "version": "5.34.0-beta-2", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6-25.0.0-beta-2.json similarity index 85% rename from vendordeps/Phoenix6.json rename to vendordeps/Phoenix6-25.0.0-beta-2.json index 5eadb206..c56e61ae 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6-25.0.0-beta-2.json @@ -1,32 +1,50 @@ { - "fileName": "Phoenix6.json", + "fileName": "Phoenix6-25.0.0-beta-2.json", "name": "CTRE-Phoenix (v6)", - "version": "24.3.0", - "frcYear": 2024, + "version": "25.0.0-beta-2", + "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", "offlineFileName": "Phoenix6And5.json" + }, + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.3.0" + "version": "25.0.0-beta-2" } ], "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -38,8 +56,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "24.3.0", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -51,8 +69,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "24.3.0", + "artifactId": "tools-sim", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -64,8 +82,8 @@ }, { "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +109,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +122,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +135,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +148,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +161,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +176,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +191,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +206,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +221,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +236,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -230,25 +248,10 @@ ], "simMode": "swsim" }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonFX", - "version": "24.3.0", - "libName": "CTRE_SimTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +266,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +281,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +296,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +311,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.3.0", + "version": "25.0.0-beta-2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib-2025.0.0-beta-1.json similarity index 87% rename from vendordeps/REVLib.json rename to vendordeps/REVLib-2025.0.0-beta-1.json index 731bbbfc..691b6437 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib-2025.0.0-beta-1.json @@ -1,25 +1,25 @@ { - "fileName": "REVLib.json", + "fileName": "REVLib-2025.0.0-beta-1.json", "name": "REVLib", - "version": "2024.2.4", - "frcYear": "2024", + "version": "2025.0.0-beta-1", + "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.4" + "version": "2025.0.0-beta-1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.4", + "version": "2025.0.0-beta-1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.4", + "version": "2025.0.0-beta-1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/ReduxLib_2024.json b/vendordeps/ReduxLib_2025.json similarity index 82% rename from vendordeps/ReduxLib_2024.json rename to vendordeps/ReduxLib_2025.json index f694966d..bd164ce9 100644 --- a/vendordeps/ReduxLib_2024.json +++ b/vendordeps/ReduxLib_2025.json @@ -1,25 +1,25 @@ { - "fileName": "ReduxLib_2024.json", + "fileName": "ReduxLib_2025.json", "name": "ReduxLib", - "version": "2024.3.1", - "frcYear": 2024, + "version": "2025.0.0-beta0", + "frcYear": 2025, "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", "mavenUrls": [ "https://maven.reduxrobotics.com/" ], - "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2024.json", + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", "javaDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-java", - "version": "2024.3.1" + "version": "2025.0.0-beta0" } ], "jniDependencies": [ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-driver", - "version": "2024.3.1", + "version": "2025.0.0-beta0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.reduxrobotics.frc", "artifactId": "ReduxLib-cpp", - "version": "2024.3.1", + "version": "2025.0.0-beta0", "libName": "ReduxLib-cpp", "headerClassifier": "headers", "sourcesClassifier": "sources", diff --git a/vendordeps/Studica-2025.1.1-beta-2.json b/vendordeps/Studica-2025.1.1-beta-2.json new file mode 100644 index 00000000..b9aa624c --- /dev/null +++ b/vendordeps/Studica-2025.1.1-beta-2.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.1.1-beta-2.json", + "name": "Studica", + "version": "2025.1.1-beta-2", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-2.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.1.1-beta-2" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.1.1-beta-2" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.1.1-beta-2" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.1.1-beta-2" + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 67bf3898..3718e0ac 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2024", + "frcYear": "2025", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index 0e80a16c..00000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2024.3.1", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2024", - "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": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2024.3.1", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2024.3.1" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2024.3.1" - } - ] -} \ No newline at end of file From 3dcf429ad3e550cedc3baba1bbe59705a0904c43 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 5 Nov 2024 21:37:07 -0600 Subject: [PATCH 02/54] Added closed loop setting. --- .../swervelib/motors/SparkFlexSwerve.java | 4 +++ .../motors/SparkMaxBrushedMotorSwerve.java | 28 +++++++++++++------ .../java/swervelib/motors/SparkMaxSwerve.java | 5 ++++ 3 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index aff0b5fb..9bc6e199 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -269,6 +269,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) ; if (absoluteEncoder == null) { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfg.encoder .positionConversionFactor(positionConversionFactor) .velocityConversionFactor(positionConversionFactor / 60); @@ -302,6 +304,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) // with limited testing 19ms did not return the same value while the module was constatntly rotating. if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + cfg.signals .absoluteEncoderPositionAlwaysOn(true) .absoluteEncoderPositionPeriodMs(20); diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index f384352f..60dcf909 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -11,6 +11,10 @@ import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -18,7 +22,7 @@ import swervelib.telemetry.Alert; /** - * Brushed motor control with {@link CANSparkMax}. + * Brushed motor control with {@link SparkMax}. */ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { @@ -26,7 +30,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * SparkMAX Instance. */ - private final CANSparkMax motor; + private final SparkMax motor; /** * Absolute encoder attached to the SparkMax (if exists) @@ -35,15 +39,15 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkClosedLoopController pid; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if the motor has no encoder. */ @@ -55,7 +59,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert; + private Alert noEncoderDefinedAlert; + /** + * Configuration object for {@link SparkMax} motor. + */ + private SparkMaxConfig cfg = new SparkMaxConfig(); + /** + * Tracker for changes that need to be pushed. + */ + private boolean cfgUpdated = false; + /** * Initialize the swerve motor. @@ -66,10 +79,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. */ - public SparkMaxBrushedMotorSwerve(CANSparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution, + public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type encoderType, int countsPerRevolution, boolean useDataPortQuadEncoder) { - noEncoderAlert = new Alert("Motors", "Cannot use motor without encoder.", Alert.AlertType.ERROR_TRACE); diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index eacdd882..583a8426 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -259,6 +259,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) ; if (absoluteEncoder == null) { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); cfg.encoder .positionConversionFactor(positionConversionFactor) .velocityConversionFactor(positionConversionFactor / 60); @@ -292,6 +293,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) // with limited testing 19ms did not return the same value while the module was constatntly rotating. if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + cfg.signals .absoluteEncoderPositionAlwaysOn(true) .absoluteEncoderPositionPeriodMs(20); @@ -301,6 +304,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) .velocityConversionFactor(positionConversionFactor / 60); } else { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor); + cfg.signals .analogVoltageAlwaysOn(true) .analogPositionAlwaysOn(true) From f05182638acbcc2eaa65c546aa1b7e4f723940f0 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 00:02:33 -0600 Subject: [PATCH 03/54] Added back photonvision, working on fixing it up. Fixed up brushed sparkmax. --- .../swervedrive/SwerveSubsystem.java | 54 +- .../robot/subsystems/swervedrive/Vision.java | 972 +++++++++--------- src/main/java/swervelib/SwerveDriveTest.java | 31 +- src/main/java/swervelib/imu/NavXSwerve.java | 49 +- .../swervelib/motors/SparkFlexSwerve.java | 4 +- .../motors/SparkMaxBrushedMotorSwerve.java | 276 +++-- .../java/swervelib/motors/SparkMaxSwerve.java | 6 +- .../swervelib/parser/json/DeviceJson.java | 15 +- .../swervelib/parser/json/ModuleJson.java | 5 +- vendordeps/photonlib.json | 97 ++ 10 files changed, 849 insertions(+), 660 deletions(-) create mode 100644 vendordeps/photonlib.json diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index ce8e73ed..259a86af 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -33,6 +33,8 @@ import java.io.File; import java.util.Arrays; import java.util.function.DoubleSupplier; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -58,10 +60,10 @@ public class SwerveSubsystem extends SubsystemBase * Enable vision odometry updates while driving. */ private final boolean visionDriveTest = false; -// /** -// * PhotonVision class to keep an accurate odometry. -// */ -// private Vision vision; + /** + * PhotonVision class to keep an accurate odometry. + */ + private Vision vision; /** * Initialize {@link SwerveDrive} with the directory provided. @@ -128,7 +130,7 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig */ public void setupPhotonVision() { -// vision = new Vision(swerveDrive::getPose, swerveDrive.field); + vision = new Vision(swerveDrive::getPose, swerveDrive.field); } @Override @@ -138,7 +140,7 @@ public void periodic() if (visionDriveTest) { swerveDrive.updateOdometry(); -// vision.updatePoseEstimation(swerveDrive); + vision.updatePoseEstimation(swerveDrive); } } @@ -252,26 +254,26 @@ public Command aimAtSpeaker(double tolerance) }).until(() -> Math.abs(getSpeakerYaw().minus(getHeading()).getDegrees()) < tolerance); } -// /** -// * Aim the robot at the target returned by PhotonVision. -// * -// * @param camera {@link PhotonCamera} to communicate with. -// * @return A {@link Command} which will run the alignment. -// */ -// public Command aimAtTarget(PhotonCamera camera) -// { -// -// return run(() -> { -// PhotonPipelineResult result = camera.getLatestResult(); -// if (result.hasTargets()) -// { -// drive(getTargetSpeeds(0, -// 0, -// Rotation2d.fromDegrees(result.getBestTarget() -// .getYaw()))); // Not sure if this will work, more math may be required. -// } -// }); -// } + /** + * Aim the robot at the target returned by PhotonVision. + * + * @param camera {@link PhotonCamera} to communicate with. + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(PhotonCamera camera) + { + + return run(() -> { + PhotonPipelineResult result = camera.getLatestResult(); + if (result.hasTargets()) + { + drive(getTargetSpeeds(0, + 0, + Rotation2d.fromDegrees(result.getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } + }); + } /** * Get the path follower with events. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 58130773..d97b12b8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -1,483 +1,497 @@ -//package frc.robot.subsystems.swervedrive; -// -//import edu.wpi.first.apriltag.AprilTagFieldLayout; -//import edu.wpi.first.apriltag.AprilTagFields; -//import edu.wpi.first.math.Matrix; -//import edu.wpi.first.math.VecBuilder; -//import edu.wpi.first.math.geometry.Pose2d; -//import edu.wpi.first.math.geometry.Pose3d; -//import edu.wpi.first.math.geometry.Rotation2d; -//import edu.wpi.first.math.geometry.Rotation3d; -//import edu.wpi.first.math.geometry.Transform2d; -//import edu.wpi.first.math.geometry.Transform3d; -//import edu.wpi.first.math.geometry.Translation3d; -//import edu.wpi.first.math.numbers.N1; -//import edu.wpi.first.math.numbers.N3; -//import edu.wpi.first.math.util.Units; -//import edu.wpi.first.wpilibj.smartdashboard.Field2d; -//import frc.robot.Robot; -//import java.awt.Desktop; -//import java.util.ArrayList; -//import java.util.List; -//import java.util.Optional; -//import java.util.function.Supplier; -//import org.photonvision.EstimatedRobotPose; -//import org.photonvision.PhotonCamera; -//import org.photonvision.PhotonPoseEstimator; -//import org.photonvision.PhotonPoseEstimator.PoseStrategy; -//import org.photonvision.PhotonUtils; -//import org.photonvision.simulation.PhotonCameraSim; -//import org.photonvision.simulation.SimCameraProperties; -//import org.photonvision.simulation.VisionSystemSim; -//import org.photonvision.targeting.PhotonPipelineResult; -//import org.photonvision.targeting.PhotonTrackedTarget; -//import swervelib.SwerveDrive; -//import swervelib.telemetry.Alert; -//import swervelib.telemetry.Alert.AlertType; -//import swervelib.telemetry.SwerveDriveTelemetry; -// -// -///** -// * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from -// * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads -// */ -//public class Vision -//{ -// -// /** -// * April Tag Field Layout of the year. -// */ -// public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( -// AprilTagFields.k2024Crescendo); -// /** -// * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. -// */ -// private final double maximumAmbiguity = 0.25; -// /** -// * Photon Vision Simulation -// */ -// public VisionSystemSim visionSim; -// /** -// * Count of times that the odom thinks we're more than 10meters away from the april tag. -// */ -// private double longDistangePoseEstimationCount = 0; -// /** -// * Current pose from the pose estimator using wheel odometry. -// */ -// private Supplier currentPose; -// /** -// * Field from {@link swervelib.SwerveDrive#field} -// */ -// private Field2d field2d; -// -// -// /** -// * Constructor for the Vision class. -// * -// * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} -// * @param field Current field, should be {@link SwerveDrive#field} -// */ -// public Vision(Supplier currentPose, Field2d field) -// { -// this.currentPose = currentPose; -// this.field2d = field; -// -// if (Robot.isSimulation()) -// { -// visionSim = new VisionSystemSim("Vision"); -// visionSim.addAprilTags(fieldLayout); -// -// for (Cameras c : Cameras.values()) +package frc.robot.subsystems.swervedrive; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.robot.Robot; +import java.awt.Desktop; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.PhotonUtils; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import swervelib.SwerveDrive; +import swervelib.telemetry.Alert; +import swervelib.telemetry.Alert.AlertType; +import swervelib.telemetry.SwerveDriveTelemetry; + + +/** + * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from + * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads + */ +public class Vision +{ + + /** + * April Tag Field Layout of the year. + */ + public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( + AprilTagFields.k2024Crescendo); + /** + * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. + */ + private final double maximumAmbiguity = 0.25; + /** + * Photon Vision Simulation + */ + public VisionSystemSim visionSim; + /** + * Count of times that the odom thinks we're more than 10meters away from the april tag. + */ + private double longDistangePoseEstimationCount = 0; + /** + * Current pose from the pose estimator using wheel odometry. + */ + private Supplier currentPose; + /** + * Field from {@link swervelib.SwerveDrive#field} + */ + private Field2d field2d; + + + /** + * Constructor for the Vision class. + * + * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} + * @param field Current field, should be {@link SwerveDrive#field} + */ + public Vision(Supplier currentPose, Field2d field) + { + this.currentPose = currentPose; + this.field2d = field; + + if (Robot.isSimulation()) + { + visionSim = new VisionSystemSim("Vision"); + visionSim.addAprilTags(fieldLayout); + + for (Cameras c : Cameras.values()) + { + c.addToVisionSim(visionSim); + } + + openSimCameraViews(); + } + } + + /** + * Calculates a target pose relative to an AprilTag on the field. + * + * @param aprilTag The ID of the AprilTag. + * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position + * itself correctly. + * @return The target pose of the AprilTag. + */ + public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) + { + Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); + if (aprilTagPose3d.isPresent()) + { + return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); + } else + { + throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); + } + + } + + /** + * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. + * + * @param swerveDrive {@link SwerveDrive} instance. + */ + public void updatePoseEstimation(SwerveDrive swerveDrive) + { + if (SwerveDriveTelemetry.isSimulation) + { + visionSim.update(swerveDrive.getPose()); + + } + for (Cameras camera : Cameras.values()) + { + Optional poseEst = getEstimatedGlobalPose(camera); + if (poseEst.isPresent()) + { + var pose = poseEst.get(); + swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), + pose.timestampSeconds, + getEstimationStdDevs(camera)); + } + } + + } + + /** + * Generates the estimated robot pose. Returns empty if: + *
      + *
    • No Pose Estimates could be generated
      • + *
    • The generated pose estimate was considered not accurate
      • + *
    + * + * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate + */ + public Optional getEstimatedGlobalPose(Cameras camera) + { + Optional poseEst = filterPose(camera.poseEstimator.update()); + // Uncomment to enable outputting of vision targets in sim. + /* + poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") + .setPose(estimatedRobotPose.estimatedPose.toPose2d())); + */ + return poseEst; + } + + /** + * The standard deviations of the estimated pose from {@link Vision#getEstimatedGlobalPose(Cameras)}, for use with + * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only be used + * when there are targets visible. + * + * @param camera Desired camera to get the standard deviation of the estimated pose. + */ + public Matrix getEstimationStdDevs(Cameras camera) + { + var poseEst = getEstimatedGlobalPose(camera); + var estStdDevs = camera.singleTagStdDevs; + var targets = getLatestResult(camera).getTargets(); + int numTags = 0; + double avgDist = 0; + for (var tgt : targets) + { + var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + { + continue; + } + numTags++; + if (poseEst.isPresent()) + { + avgDist += PhotonUtils.getDistanceToPose(poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); + } + } + if (numTags == 0) + { + return estStdDevs; + } + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + { + estStdDevs = camera.multiTagStdDevs; + } + // 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)); + } + + return estStdDevs; + } + + /** + * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than + * 10m for a short amount of time. + * + * @param pose Estimated robot pose. + * @return Could be empty if there isn't a good reading. + */ + private Optional filterPose(Optional pose) + { + if (pose.isPresent()) + { + double bestTargetAmbiguity = 1; // 1 is max ambiguity + for (PhotonTrackedTarget target : pose.get().targetsUsed) + { + double ambiguity = target.getPoseAmbiguity(); + if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) + { + bestTargetAmbiguity = ambiguity; + } + } + //ambiguity to high dont use estimate + if (bestTargetAmbiguity > maximumAmbiguity) + { + return Optional.empty(); + } + + //est pose is very far from recorded robot pose + if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) + { + longDistangePoseEstimationCount++; + + //if it calculates that were 10 meter away for more than 10 times in a row its probably right + if (longDistangePoseEstimationCount < 10) + { + return Optional.empty(); + } + } else + { + longDistangePoseEstimationCount = 0; + } + return pose; + } + return Optional.empty(); + } + + /** + * Get the latest result from a given Camera. + * + * @param camera Given camera to take the result from. + * @return Photon result from sim or a real camera. + */ + public PhotonPipelineResult getLatestResult(Cameras camera) + { + if(!camera.resultsList.isEmpty()) + { + if(((double) NetworkTablesJNI.now() / 1000000) - camera.resultsList.get(0).getTimestampSeconds() > 0.015 ) + camera.resultsList = camera.camera.getAllUnreadResults(); + } + else + camera.resultsList = camera.camera.getAllUnreadResults(); + for(PhotonPipelineResult result : camera.resultsList) + { + } + return Robot.isReal() ? camera.camera.get() : camera.cameraSim.getCamera().getLatestResult(); + } + + /** + * Get distance of the robot from the AprilTag pose. + * + * @param id AprilTag ID + * @return Distance + */ + public double getDistanceFromAprilTag(int id) + { + Optional tag = fieldLayout.getTagPose(id); + return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); + } + + /** + * Get tracked target from a camera of AprilTagID + * + * @param id AprilTag ID + * @param camera Camera to check. + * @return Tracked target. + */ + public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) + { + PhotonTrackedTarget target = null; + PhotonPipelineResult result = getLatestResult(camera); + if (result.hasTargets()) + { + for (PhotonTrackedTarget i : result.getTargets()) + { + if (i.getFiducialId() == id) + { + target = i; + } + } + } + return target; + } + + /** + * Vision simulation. + * + * @return Vision Simulation + */ + public VisionSystemSim getVisionSim() + { + return visionSim; + } + + /** + * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. + */ + private void openSimCameraViews() + { + if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) + { +// try // { -// c.addToVisionSim(visionSim); -// } -// -// openSimCameraViews(); -// } -// } -// -// /** -// * Calculates a target pose relative to an AprilTag on the field. -// * -// * @param aprilTag The ID of the AprilTag. -// * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position -// * itself correctly. -// * @return The target pose of the AprilTag. -// */ -// public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) -// { -// Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); -// if (aprilTagPose3d.isPresent()) -// { -// return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); -// } else -// { -// throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); -// } -// -// } -// -// /** -// * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. -// * -// * @param swerveDrive {@link SwerveDrive} instance. -// */ -// public void updatePoseEstimation(SwerveDrive swerveDrive) -// { -// if (SwerveDriveTelemetry.isSimulation) -// { -// visionSim.update(swerveDrive.getPose()); -// -// } -// for (Cameras camera : Cameras.values()) -// { -// Optional poseEst = getEstimatedGlobalPose(camera); -// if (poseEst.isPresent()) -// { -// var pose = poseEst.get(); -// swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), -// pose.timestampSeconds, -// getEstimationStdDevs(camera)); -// } -// } -// -// } -// -// /** -// * Generates the estimated robot pose. Returns empty if: -// *
      -// *
    • No Pose Estimates could be generated
      • -// *
    • The generated pose estimate was considered not accurate
      • -// *
    -// * -// * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate -// */ -// public Optional getEstimatedGlobalPose(Cameras camera) -// { -// Optional poseEst = filterPose(camera.poseEstimator.update()); -// // Uncomment to enable outputting of vision targets in sim. -// /* -// poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") -// .setPose(estimatedRobotPose.estimatedPose.toPose2d())); -// */ -// return poseEst; -// } -// -// /** -// * The standard deviations of the estimated pose from {@link Vision#getEstimatedGlobalPose(Cameras)}, for use with -// * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only be used -// * when there are targets visible. -// * -// * @param camera Desired camera to get the standard deviation of the estimated pose. -// */ -// public Matrix getEstimationStdDevs(Cameras camera) -// { -// var poseEst = getEstimatedGlobalPose(camera); -// var estStdDevs = camera.singleTagStdDevs; -// var targets = getLatestResult(camera).getTargets(); -// int numTags = 0; -// double avgDist = 0; -// for (var tgt : targets) -// { -// var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); -// if (tagPose.isEmpty()) -// { -// continue; -// } -// numTags++; -// if (poseEst.isPresent()) -// { -// avgDist += PhotonUtils.getDistanceToPose(poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); -// } -// } -// if (numTags == 0) -// { -// return estStdDevs; -// } -// avgDist /= numTags; -// // Decrease std devs if multiple targets are visible -// if (numTags > 1) -// { -// estStdDevs = camera.multiTagStdDevs; -// } -// // 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)); -// } -// -// return estStdDevs; -// } -// -// /** -// * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than -// * 10m for a short amount of time. -// * -// * @param pose Estimated robot pose. -// * @return Could be empty if there isn't a good reading. -// */ -// private Optional filterPose(Optional pose) -// { -// if (pose.isPresent()) -// { -// double bestTargetAmbiguity = 1; // 1 is max ambiguity -// for (PhotonTrackedTarget target : pose.get().targetsUsed) -// { -// double ambiguity = target.getPoseAmbiguity(); -// if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) -// { -// bestTargetAmbiguity = ambiguity; -// } -// } -// //ambiguity to high dont use estimate -// if (bestTargetAmbiguity > maximumAmbiguity) -// { -// return Optional.empty(); -// } -// -// //est pose is very far from recorded robot pose -// if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) +// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); +// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); +// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); +// } catch (IOException | URISyntaxException e) // { -// longDistangePoseEstimationCount++; -// -// //if it calculates that were 10 meter away for more than 10 times in a row its probably right -// if (longDistangePoseEstimationCount < 10) -// { -// return Optional.empty(); -// } -// } else -// { -// longDistangePoseEstimationCount = 0; -// } -// return pose; -// } -// return Optional.empty(); -// } -// -// /** -// * Get the latest result from a given Camera. -// * -// * @param camera Given camera to take the result from. -// * @return Photon result from sim or a real camera. -// */ -// public PhotonPipelineResult getLatestResult(Cameras camera) -// { -// -// return Robot.isReal() ? camera.camera.getLatestResult() : camera.cameraSim.getCamera().getLatestResult(); -// } -// -// /** -// * Get distance of the robot from the AprilTag pose. -// * -// * @param id AprilTag ID -// * @return Distance -// */ -// public double getDistanceFromAprilTag(int id) -// { -// Optional tag = fieldLayout.getTagPose(id); -// return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); -// } -// -// /** -// * Get tracked target from a camera of AprilTagID -// * -// * @param id AprilTag ID -// * @param camera Camera to check. -// * @return Tracked target. -// */ -// public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) -// { -// PhotonTrackedTarget target = null; -// PhotonPipelineResult result = getLatestResult(camera); -// if (result.hasTargets()) -// { -// for (PhotonTrackedTarget i : result.getTargets()) -// { -// if (i.getFiducialId() == id) -// { -// target = i; -// } +// e.printStackTrace(); // } -// } -// return target; -// } -// -// /** -// * Vision simulation. -// * -// * @return Vision Simulation -// */ -// public VisionSystemSim getVisionSim() -// { -// return visionSim; -// } -// -// /** -// * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost. -// */ -// private void openSimCameraViews() -// { -// if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) -// { -//// try -//// { -//// Desktop.getDesktop().browse(new URI("http://localhost:1182/")); -//// Desktop.getDesktop().browse(new URI("http://localhost:1184/")); -//// Desktop.getDesktop().browse(new URI("http://localhost:1186/")); -//// } catch (IOException | URISyntaxException e) -//// { -//// e.printStackTrace(); -//// } -// } -// } -// -// /** -// * Update the {@link Field2d} to include tracked targets/ -// */ -// public void updateVisionField() -// { -// -// List targets = new ArrayList(); -// for (Cameras c : Cameras.values()) -// { -// if (getLatestResult(c).hasTargets()) -// { -// targets.addAll(getLatestResult(c).targets); -// } -// } -// -// List poses = new ArrayList<>(); -// for (PhotonTrackedTarget target : targets) -// { -// if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) -// { -// Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); -// poses.add(targetPose); -// } -// } -// -// field2d.getObject("tracked targets").setPoses(poses); -// } -// -// /** -// * Camera Enum to select each camera -// */ -// enum Cameras -// { -// /** -// * Left Camera -// */ -// LEFT_CAM("left", -// new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), -// new Translation3d(Units.inchesToMeters(12.056), -// Units.inchesToMeters(10.981), -// Units.inchesToMeters(8.44)), -// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), -// /** -// * Right Camera -// */ -// RIGHT_CAM("right", -// new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), -// new Translation3d(Units.inchesToMeters(12.056), -// Units.inchesToMeters(-10.981), -// Units.inchesToMeters(8.44)), -// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), -// /** -// * Center Camera -// */ -// CENTER_CAM("center", -// new Rotation3d(0, Units.degreesToRadians(18), 0), -// new Translation3d(Units.inchesToMeters(-4.628), -// Units.inchesToMeters(-10.687), -// Units.inchesToMeters(16.129)), -// VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); -// -// /** -// * Latency alert to use when high latency is detected. -// */ -// public final Alert latencyAlert; -// /** -// * Camera instance for comms. -// */ -// public final PhotonCamera camera; -// /** -// * Pose estimator for camera. -// */ -// public final PhotonPoseEstimator poseEstimator; -// public final Matrix singleTagStdDevs; -// public final Matrix multiTagStdDevs; -// /** -// * Transform of the camera rotation and translation relative to the center of the robot -// */ -// private final Transform3d robotToCamTransform; -// /** -// * Simulated camera instance which only exists during simulations. -// */ -// public PhotonCameraSim cameraSim; -// -// /** -// * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine -// * estimation noise on an actual robot. -// * -// * @param name Name of the PhotonVision camera found in the PV UI. -// * @param robotToCamRotation {@link Rotation3d} of the camera. -// * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. -// * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. -// * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. -// */ -// Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, -// Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) -// { -// latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); -// -// camera = new PhotonCamera(name); -// -// // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html -// robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); -// -// poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, -// PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, -// camera, -// robotToCamTransform); -// poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); -// -// this.singleTagStdDevs = singleTagStdDevs; -// this.multiTagStdDevs = multiTagStdDevsMatrix; -// -// if (Robot.isSimulation()) -// { -// SimCameraProperties cameraProp = new SimCameraProperties(); -// // A 640 x 480 camera with a 100 degree diagonal FOV. -// cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); -// // Approximate detection noise with average and standard deviation error in pixels. -// cameraProp.setCalibError(0.25, 0.08); -// // Set the camera image capture framerate (Note: this is limited by robot loop rate). -// cameraProp.setFPS(30); -// // The average and standard deviation in milliseconds of image data latency. -// cameraProp.setAvgLatencyMs(35); -// cameraProp.setLatencyStdDevMs(5); -// -// cameraSim = new PhotonCameraSim(camera, cameraProp); + } + } + + /** + * Update the {@link Field2d} to include tracked targets/ + */ + public void updateVisionField() + { + + List targets = new ArrayList(); + for (Cameras c : Cameras.values()) + { + if (getLatestResult(c).hasTargets()) + { + targets.addAll(getLatestResult(c).targets); + } + } + + List poses = new ArrayList<>(); + for (PhotonTrackedTarget target : targets) + { + if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) + { + Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); + poses.add(targetPose); + } + } + + field2d.getObject("tracked targets").setPoses(poses); + } + + /** + * Camera Enum to select each camera + */ + enum Cameras + { + /** + * Left Camera + */ + LEFT_CAM("left", + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)), + new Translation3d(Units.inchesToMeters(12.056), + Units.inchesToMeters(10.981), + Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + /** + * Right Camera + */ + RIGHT_CAM("right", + new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)), + new Translation3d(Units.inchesToMeters(12.056), + Units.inchesToMeters(-10.981), + Units.inchesToMeters(8.44)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + /** + * Center Camera + */ + CENTER_CAM("center", + new Rotation3d(0, Units.degreesToRadians(18), 0), + new Translation3d(Units.inchesToMeters(-4.628), + Units.inchesToMeters(-10.687), + Units.inchesToMeters(16.129)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + + /** + * Latency alert to use when high latency is detected. + */ + public final Alert latencyAlert; + /** + * Camera instance for comms. + */ + public final PhotonCamera camera; + /** + * Pose estimator for camera. + */ + public final PhotonPoseEstimator poseEstimator; + public final Matrix singleTagStdDevs; + public final Matrix multiTagStdDevs; + /** + * Transform of the camera rotation and translation relative to the center of the robot + */ + private final Transform3d robotToCamTransform; + /** + * Simulated camera instance which only exists during simulations. + */ + public PhotonCameraSim cameraSim; + /** + * Results list to be updated periodically and cached to avoid unnecessary queries. + */ + public List resultsList; + /** + * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine + * estimation noise on an actual robot. + * + * @param name Name of the PhotonVision camera found in the PV UI. + * @param robotToCamRotation {@link Rotation3d} of the camera. + * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. + * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the camera. + * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera. + */ + Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, + Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) + { + latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); + + camera = new PhotonCamera(name); + + // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html + robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); + + poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + robotToCamTransform); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + this.singleTagStdDevs = singleTagStdDevs; + this.multiTagStdDevs = multiTagStdDevsMatrix; + + if (Robot.isSimulation()) + { + SimCameraProperties cameraProp = new SimCameraProperties(); + // A 640 x 480 camera with a 100 degree diagonal FOV. + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); + // Approximate detection noise with average and standard deviation error in pixels. + cameraProp.setCalibError(0.25, 0.08); + // Set the camera image capture framerate (Note: this is limited by robot loop rate). + cameraProp.setFPS(30); + // The average and standard deviation in milliseconds of image data latency. + cameraProp.setAvgLatencyMs(35); + cameraProp.setLatencyStdDevMs(5); + + cameraSim = new PhotonCameraSim(camera, cameraProp); + cameraSim.enableDrawWireframe(true); + } + } + + /** + * Add camera to {@link VisionSystemSim} for simulated photon vision. + * + * @param systemSim {@link VisionSystemSim} to use. + */ + public void addToVisionSim(VisionSystemSim systemSim) + { + if (Robot.isSimulation()) + { + systemSim.addCamera(cameraSim, robotToCamTransform); // cameraSim.enableDrawWireframe(true); -// } -// } -// -// /** -// * Add camera to {@link VisionSystemSim} for simulated photon vision. -// * -// * @param systemSim {@link VisionSystemSim} to use. -// */ -// public void addToVisionSim(VisionSystemSim systemSim) -// { -// if (Robot.isSimulation()) -// { -// systemSim.addCamera(cameraSim, robotToCamTransform); -//// cameraSim.enableDrawWireframe(true); -// } -// } -// } -// -//} + } + } + } + +} diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index a3d71dfe..019aeb29 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -1,20 +1,27 @@ package swervelib; -import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Meter; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.units.Voltage; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.VoltageUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; @@ -37,23 +44,23 @@ public class SwerveDriveTest /** * Tracks the voltage being applied to a motor */ - private static final MutableMeasure m_appliedVoltage = mutable(Volts.of(0)); + private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts); /** * Tracks the distance travelled of a position motor */ - private static final MutableMeasure m_distance = mutable(Meters.of(0)); + private static final MutDistance m_distance = new MutDistance(0,0, Meter); /** * Tracks the velocity of a positional motor */ - private static final MutableMeasure> m_velocity = mutable(MetersPerSecond.of(0)); + private static final MutLinearVelocity m_velocity = new MutLinearVelocity(0, 9, MetersPerSecond); /** * Tracks the rotations of an angular motor */ - private static final MutableMeasure m_anglePosition = mutable(Degrees.of(0)); + private static final MutAngle m_anglePosition = new MutAngle(0,0, Degrees); /** * Tracks the velocity of an angular motor */ - private static final MutableMeasure> m_angVelocity = mutable(DegreesPerSecond.of(0)); + private static final MutAngularVelocity m_angVelocity = new MutAngularVelocity(0,0, DegreesPerSecond); /** * Set the angle of the modules to a given {@link Rotation2d} @@ -308,7 +315,7 @@ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swe SwerveDrive swerveDrive, double maxVolts) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( - (Measure voltage) -> { + (Voltage voltage) -> { SwerveDriveTest.centerModules(swerveDrive); SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts)); }, @@ -380,7 +387,7 @@ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swe SwerveDrive swerveDrive) { return new SysIdRoutine(config, new SysIdRoutine.Mechanism( - (Measure voltage) -> { + (Voltage voltage) -> { SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts)); SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0); }, diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 2d6454ef..5343f9a8 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -1,6 +1,7 @@ package swervelib.imu; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.I2C; @@ -38,17 +39,17 @@ public class NavXSwerve extends SwerveIMU * * @param port Serial Port to connect to. */ - public NavXSwerve(SerialPort.Port port) + public NavXSwerve(NavXComType port) { navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); try { + AHRS /* Communicate w/navX-MXP via the MXP SPI Bus. */ /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ imu = new AHRS(port); factoryDefault(); - SmartDashboard.putData(imu); } catch (RuntimeException ex) { navXError.setText("Error instantiating NavX: " + ex.getMessage()); @@ -56,49 +57,7 @@ public NavXSwerve(SerialPort.Port port) } } - /** - * Constructor for the NavX({@link AHRS}) swerve. - * - * @param port SPI Port to connect to. - */ - public NavXSwerve(SPI.Port port) - { - try - { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - imu = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(imu); - } catch (RuntimeException ex) - { - navXError.setText("Error instantiating NavX: " + ex.getMessage()); - navXError.set(true); - } - } - /** - * Constructor for the NavX({@link AHRS}) swerve. - * - * @param port I2C Port to connect to. - */ - public NavXSwerve(I2C.Port port) - { - try - { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - imu = new AHRS(port); - factoryDefault(); - SmartDashboard.putData(imu); - } catch (RuntimeException ex) - { - navXError.setText("Error instantiating NavX: " + ex.getMessage()); - navXError.set(true); - } - } /** * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 9bc6e199..98c484f3 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -496,7 +496,7 @@ public double getAppliedOutput() @Override public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + return velocity.get(); } /** @@ -507,7 +507,7 @@ public double getVelocity() @Override public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition(); + return position.get(); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 60dcf909..e1bae42f 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,25 +1,26 @@ package swervelib.motors; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.CANSparkBase.ControlType; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkRelativeEncoder.Type; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; +import swervelib.encoders.SparkMaxAnalogEncoderSwerve; +import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; +import swervelib.telemetry.SwerveDriveTelemetry; /** * Brushed motor control with {@link SparkMax}. @@ -31,11 +32,18 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor * SparkMAX Instance. */ private final SparkMax motor; - + /** + * Supplier for the velocity of the motor controller. + */ + private Supplier velocity; + /** + * Supplier for the position of the motor controller. + */ + private Supplier position; /** * Absolute encoder attached to the SparkMax (if exists) */ - public AbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Integrated encoder. */ @@ -69,13 +77,22 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor */ private boolean cfgUpdated = false; + /** + * Type for encoder for {@link SparkMax} + */ + public enum Type + { + kNoSensor, /** NO sensor */ + kHallSensor, /** Hall sensor attached to dataport */ + kQuadrature, /** Quad encoder attached to alt */ + } /** * Initialize the swerve motor. * * @param motor The SwerveMotor as a SparkMax object. * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device. * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. */ @@ -112,36 +129,50 @@ public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type enc clearStickyFaults(); // Get the onboard PID controller. - pid = motor.getPIDController(); + pid = motor.getClosedLoopController(); // If there is a sensor attached to the data port or encoder port set the relative encoder. if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) { - this.encoder = useDataPortQuadEncoder ? - motor.getAlternateEncoder(SparkMaxAlternateEncoder.Type.kQuadrature, countsPerRevolution) : - motor.getEncoder(encoderType, countsPerRevolution); - // Configure feedback of the PID controller as the integrated encoder. - configureSparkMax(() -> pid.setFeedbackDevice(encoder)); + if(useDataPortQuadEncoder) + { + this.encoder = motor.getAlternateEncoder(); + cfg.alternateEncoder.countsPerRevolution(countsPerRevolution); + + // Configure feedback of the PID controller as the integrated encoder. + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder); + } + else + { + this.encoder = motor.getEncoder(); + cfg.encoder.countsPerRevolution(countsPerRevolution); + + // Configure feedback of the PID controller as the integrated encoder. + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + } + cfgUpdated = true; } + velocity = encoder::getVelocity; + position = encoder::getPosition; // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented it out because it prevents feedback. } /** - * Initialize the {@link SwerveMotor} as a {@link CANSparkMax} connected to a Brushless Motor. + * Initialize the {@link SwerveMotor} as a {@link SparkMax} connected to a Brushless Motor. * * @param id CAN ID of the SparkMax. * @param isDriveMotor Is the motor being initialized a drive motor? - * @param encoderType {@link Type} of encoder to use for the {@link CANSparkMax} device. + * @param encoderType {@link Type} of encoder to use for the {@link SparkMax} device. * @param countsPerRevolution The number of encoder pulses for the {@link Type} encoder per revolution. * @param useDataPortQuadEncoder Use the encoder attached to the data port of the spark max for a quadrature encoder. */ public SparkMaxBrushedMotorSwerve(int id, boolean isDriveMotor, Type encoderType, int countsPerRevolution, boolean useDataPortQuadEncoder) { - this(new CANSparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution, + this(new SparkMax(id, MotorType.kBrushed), isDriveMotor, encoderType, countsPerRevolution, useDataPortQuadEncoder); } @@ -171,7 +202,8 @@ private void configureSparkMax(Supplier config) @Override public void setVoltageCompensation(double nominalVoltage) { - configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage)); + cfg.voltageCompensation(nominalVoltage); + cfgUpdated = true; } /** @@ -183,7 +215,8 @@ public void setVoltageCompensation(double nominalVoltage) @Override public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + cfg.smartCurrentLimit(currentLimit); + cfgUpdated = true; } /** @@ -194,8 +227,9 @@ public void setCurrentLimit(int currentLimit) @Override public void setLoopRampRate(double rampRate) { - configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate)); - configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate)); + cfg.closedLoopRampRate(rampRate) + .openLoopRampRate(rampRate); + cfgUpdated = true; } /** @@ -226,11 +260,7 @@ public boolean isAttachedAbsoluteEncoder() @Override public void factoryDefaults() { - if (!factoryDefaultOccurred) - { - configureSparkMax(motor::restoreFactoryDefaults); - factoryDefaultOccurred = true; - } + // Do nothing } /** @@ -254,11 +284,25 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) if (encoder == null) { absoluteEncoder = null; - configureSparkMax(() -> pid.setFeedbackDevice(this.encoder)); - } else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfgUpdated = true; + + velocity = this.encoder::getVelocity; + position = this.encoder::getPosition; + burnFlash(); + } else if (encoder instanceof SparkMaxAnalogEncoderSwerve || encoder instanceof SparkMaxEncoderSwerve) { - absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder(); - configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder)); + cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve + ? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder); + cfgUpdated = true; + + DriverStation.reportWarning( + "IF possible configure the encoder offset in the REV Hardware Client instead of using the" + + " absoluteEncoderOffset in the Swerve Module JSON!", + false); + absoluteEncoder = encoder; + velocity = absoluteEncoder::getVelocity; + position = absoluteEncoder::getAbsolutePosition; } if (absoluteEncoder == null && this.encoder == null) { @@ -276,19 +320,79 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) @Override public void configureIntegratedEncoder(double positionConversionFactor) { + cfg.signals + .absoluteEncoderPositionAlwaysOn(false) + .absoluteEncoderVelocityAlwaysOn(false) + .analogPositionAlwaysOn(false) + .analogVelocityAlwaysOn(false) + .analogVoltageAlwaysOn(false) + .externalOrAltEncoderPositionAlwaysOn(false) + .externalOrAltEncoderVelocityAlwaysOn(false) + .primaryEncoderPositionAlwaysOn(false) + .primaryEncoderVelocityAlwaysOn(false) + .iAccumulationAlwaysOn(false) + .appliedOutputPeriodMs(10) + .faultsPeriodMs(20); if (absoluteEncoder == null) { - configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60)); + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + cfg.encoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + // Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller + // Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement) + // Default settings of 32ms and 8 taps introduce ~100ms of measurement lag + // https://www.chiefdelphi.com/t/shooter-encoder/400211/11 + // This value was taken from: + // https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133 + // and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches + cfg.encoder + .quadratureMeasurementPeriod(10) + .quadratureAverageDepth(2); // Taken from - // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67 - configureCANStatusFrames(10, 20, 20, 500, 500); + // https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67 + // Unused frames can be set to 65535 to decrease CAN ultilization. + cfg.signals + .primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors. + .primaryEncoderPositionAlwaysOn(true) + .primaryEncoderPositionPeriodMs(20); } else { - configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor)); - configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60)); + // By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5 + // This needs to be set to 20ms or under to properly update the swerve module position for odometry + // Configuration taken from 3005, the team who helped develop the Max Swerve: + // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 + // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. + // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, + // with limited testing 19ms did not return the same value while the module was constatntly rotating. + if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) + { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder); + + cfg.signals + .absoluteEncoderPositionAlwaysOn(true) + .absoluteEncoderPositionPeriodMs(20); + + cfg.absoluteEncoder + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + } else + { + cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor); + + cfg.signals + .analogVoltageAlwaysOn(true) + .analogPositionAlwaysOn(true) + .analogVoltagePeriodMs(20) + .analogPositionPeriodMs(20); + + cfg.analogSensor + .positionConversionFactor(positionConversionFactor) + .velocityConversionFactor(positionConversionFactor / 60); + } } + cfgUpdated = true; } /** @@ -299,16 +403,10 @@ public void configureIntegratedEncoder(double positionConversionFactor) @Override public void configurePIDF(PIDFConfig config) { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); - int pidSlot = 0; - - configureSparkMax(() -> pid.setP(config.p, pidSlot)); - configureSparkMax(() -> pid.setI(config.i, pidSlot)); - configureSparkMax(() -> pid.setD(config.d, pidSlot)); - configureSparkMax(() -> pid.setFF(config.f, pidSlot)); - configureSparkMax(() -> pid.setIZone(config.iz, pidSlot)); - configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot)); + cfg.closedLoop.pidf(config.p, config.i, config.d, config.f) + .iZone(config.iz) + .outputRange(config.output.min, config.output.max); + cfgUpdated = true; } /** @@ -320,30 +418,11 @@ public void configurePIDF(PIDFConfig config) @Override public void configurePIDWrapping(double minInput, double maxInput) { - configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true)); - configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput)); - configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput)); - } + cfg.closedLoop + .positionWrappingEnabled(true) + .positionWrappingInputRange(minInput, maxInput); + cfgUpdated = true; - /** - * Set the CAN status frames. - * - * @param CANStatus0 Applied Output, Faults, Sticky Faults, Is Follower - * @param CANStatus1 Motor Velocity, Motor Temperature, Motor Voltage, Motor Current - * @param CANStatus2 Motor Position - * @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position - * @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position - */ - public void configureCANStatusFrames( - int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4) - { - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3)); - configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4)); - // TODO: Configure Status Frame 5 and 6 if necessary - // https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces } /** @@ -354,7 +433,8 @@ public void configureCANStatusFrames( @Override public void setMotorBrake(boolean isBrakeMode) { - configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast)); + cfg.idleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast); + cfgUpdated = true; } /** @@ -377,7 +457,14 @@ public void setInverted(boolean inverted) @Override public void burnFlash() { - configureSparkMax(() -> motor.burnFlash()); + try + { + Thread.sleep(200); + } catch (Exception e) + { + } + motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + cfgUpdated = false; } /** @@ -400,16 +487,35 @@ public void set(double percentOutput) @Override public void setReference(double setpoint, double feedforward) { -// int pidSlot = -// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal(); int pidSlot = 0; - configureSparkMax(() -> - pid.setReference( - setpoint, - isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, - pidSlot, - feedforward) - ); + + if(cfgUpdated) + { + burnFlash(); + Timer.delay(0.1); // Give 100ms to apply changes + } + + if (isDriveMotor) + { + configureSparkMax(() -> + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); + } else + { + configureSparkMax(() -> + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); + if (SwerveDriveTelemetry.isSimulation) + { + encoder.setPosition(setpoint); + } + } } /** @@ -466,7 +572,7 @@ public double getAppliedOutput() @Override public double getVelocity() { - return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity(); + return velocity.get(); } /** @@ -477,7 +583,7 @@ public double getVelocity() @Override public double getPosition() { - return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getPosition(); + return position.get(); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 583a8426..2ea17bcd 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -255,8 +255,8 @@ public void configureIntegratedEncoder(double positionConversionFactor) .primaryEncoderVelocityAlwaysOn(false) .iAccumulationAlwaysOn(false) .appliedOutputPeriodMs(10) - .faultsPeriodMs(20) - ; + .faultsPeriodMs(20); + if (absoluteEncoder == null) { cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); @@ -288,7 +288,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) // This needs to be set to 20ms or under to properly update the swerve module position for odometry // Configuration taken from 3005, the team who helped develop the Max Swerve: // https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244 - // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. + // Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max. // From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill, // with limited testing 19ms did not return the same value while the module was constatntly rotating. if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 7ce6fc3d..5ed1907a 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -4,7 +4,7 @@ import static swervelib.telemetry.SwerveDriveTelemetry.i2cLockupWarning; import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning; -import com.revrobotics.SparkRelativeEncoder.Type; +import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.SPI; @@ -27,6 +27,7 @@ import swervelib.imu.SwerveIMU; import swervelib.motors.SparkFlexSwerve; import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxBrushedMotorSwerve.Type; import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.motors.TalonFXSwerve; @@ -122,7 +123,7 @@ public SwerveIMU createIMU() return new CanandgyroSwerve(id); case "navx": case "navx_spi": - return new NavXSwerve(SPI.Port.kMXP); + return new NavXSwerve(NavXComType.kMXP_SPI); case "navx_i2c": DriverStation.reportWarning( "WARNING: There exists an I2C lockup issue on the roboRIO that could occur, more information here: " + @@ -130,15 +131,19 @@ public SwerveIMU createIMU() ".html#onboard-i2c-causing-system-lockups", false); i2cLockupWarning.set(true); - return new NavXSwerve(I2C.Port.kMXP); + throw new RuntimeException("Studica NavX API does not support I2C communication currently."); +// return new NavXSwerve(I2C.Port.kMXP); case "navx_usb": DriverStation.reportWarning("WARNING: There is issues when using USB camera's and the NavX like this!\n" + "https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", false); serialCommsIssueWarning.set(true); - return new NavXSwerve(Port.kUSB); + throw new RuntimeException("Studica NavX API does not support USB communication currently."); +// return new NavXSwerve(Port.kUSB); case "navx_mxp_serial": serialCommsIssueWarning.set(true); - return new NavXSwerve(Port.kMXP); + throw new RuntimeException("Studica NavX API does not support MXP Serial communication currently."); + +// return new NavXSwerve(Port.kMXP); case "pigeon": return new PigeonSwerve(id); case "pigeon2": diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 2fa83c89..3cb81ee5 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -1,8 +1,7 @@ package swervelib.parser.json; -import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; import edu.wpi.first.math.util.Units; +import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.motors.SwerveMotor; import swervelib.parser.PIDFConfig; @@ -140,7 +139,7 @@ public SwerveModuleConfiguration createModuleConfiguration( // Backwards compatibility, auto-optimization. if (conversionFactor.angle == 360 && absEncoder != null && - absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax) + absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof CANSparkMax) { angleMotor.setAbsoluteEncoder(absEncoder); } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 00000000..ceaec0ef --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,97 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-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": "edu.wpi.first.wpilibc", + "artifactId": "wpilibc-cpp", + "version": "2025.1.1-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-jni", + "version": "v2025.0.0-beta-1", + "skipInvalidPlatforms": true, + "isJar": true, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-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.0.0-beta-1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-1" + } + ] +} \ No newline at end of file From d0d015865e30906517a9cdfac9508e10e9ee996a Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 13:19:04 -0600 Subject: [PATCH 04/54] Updated PhotonVision --- .../robot/subsystems/swervedrive/Vision.java | 285 ++++++++++++------ 1 file changed, 190 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index d97b12b8..fcdd7cdd 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -1,5 +1,9 @@ package frc.robot.subsystems.swervedrive; +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Milliseconds; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -14,7 +18,6 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Robot; @@ -54,7 +57,7 @@ public class Vision /** * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}. */ - private final double maximumAmbiguity = 0.25; + private final double maximumAmbiguity = 0.25; /** * Photon Vision Simulation */ @@ -139,7 +142,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) var pose = poseEst.get(); swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, - getEstimationStdDevs(camera)); + camera.curStdDevs); } } @@ -156,64 +159,24 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) */ public Optional getEstimatedGlobalPose(Cameras camera) { - Optional poseEst = filterPose(camera.poseEstimator.update()); - // Uncomment to enable outputting of vision targets in sim. - /* - poseEst.ifPresent(estimatedRobotPose -> field2d.getObject(camera + " est pose") - .setPose(estimatedRobotPose.estimatedPose.toPose2d())); - */ - return poseEst; - } - - /** - * The standard deviations of the estimated pose from {@link Vision#getEstimatedGlobalPose(Cameras)}, for use with - * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should only be used - * when there are targets visible. - * - * @param camera Desired camera to get the standard deviation of the estimated pose. - */ - public Matrix getEstimationStdDevs(Cameras camera) - { - var poseEst = getEstimatedGlobalPose(camera); - var estStdDevs = camera.singleTagStdDevs; - var targets = getLatestResult(camera).getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) - { - var tagPose = camera.poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) - { - continue; - } - numTags++; - if (poseEst.isPresent()) - { - avgDist += PhotonUtils.getDistanceToPose(poseEst.get().estimatedPose.toPose2d(), tagPose.get().toPose2d()); - } - } - if (numTags == 0) - { - return estStdDevs; - } - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) - { - estStdDevs = camera.multiTagStdDevs; - } - // 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 + Optional poseEst = camera.getEstimatedGlobalPose(); + if (Robot.isSimulation()) { - estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + Field2d debugField = visionSim.getDebugField(); + // Uncomment to enable outputting of vision targets in sim. + poseEst.ifPresentOrElse( + est -> + debugField + .getObject("VisionEstimation") + .setPose(est.estimatedPose.toPose2d()), + () -> { + debugField.getObject("VisionEstimation").setPoses(); + }); } - - return estStdDevs; + return poseEst; } + /** * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than * 10m for a short amount of time. @@ -221,6 +184,7 @@ public Matrix getEstimationStdDevs(Cameras camera) * @param pose Estimated robot pose. * @return Could be empty if there isn't a good reading. */ + @Deprecated(since = "2024", forRemoval = true) private Optional filterPose(Optional pose) { if (pose.isPresent()) @@ -259,26 +223,6 @@ private Optional filterPose(Optional pos return Optional.empty(); } - /** - * Get the latest result from a given Camera. - * - * @param camera Given camera to take the result from. - * @return Photon result from sim or a real camera. - */ - public PhotonPipelineResult getLatestResult(Cameras camera) - { - if(!camera.resultsList.isEmpty()) - { - if(((double) NetworkTablesJNI.now() / 1000000) - camera.resultsList.get(0).getTimestampSeconds() > 0.015 ) - camera.resultsList = camera.camera.getAllUnreadResults(); - } - else - camera.resultsList = camera.camera.getAllUnreadResults(); - for(PhotonPipelineResult result : camera.resultsList) - { - } - return Robot.isReal() ? camera.camera.get() : camera.cameraSim.getCamera().getLatestResult(); - } /** * Get distance of the robot from the AprilTag pose. @@ -301,19 +245,22 @@ public double getDistanceFromAprilTag(int id) */ public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { - PhotonTrackedTarget target = null; - PhotonPipelineResult result = getLatestResult(camera); - if (result.hasTargets()) + PhotonTrackedTarget target = null; + for (PhotonPipelineResult result : camera.resultsList) { - for (PhotonTrackedTarget i : result.getTargets()) + if (result.hasTargets()) { - if (i.getFiducialId() == id) + for (PhotonTrackedTarget i : result.getTargets()) { - target = i; + if (i.getFiducialId() == id) + { + return i; + } } } } return target; + } /** @@ -354,9 +301,13 @@ public void updateVisionField() List targets = new ArrayList(); for (Cameras c : Cameras.values()) { - if (getLatestResult(c).hasTargets()) + if (!c.resultsList.isEmpty()) { - targets.addAll(getLatestResult(c).targets); + PhotonPipelineResult latest = c.resultsList.get(0); + if (latest.hasTargets()) + { + targets.addAll(latest.targets); + } } } @@ -409,29 +360,48 @@ enum Cameras /** * Latency alert to use when high latency is detected. */ - public final Alert latencyAlert; + public final Alert latencyAlert; /** * Camera instance for comms. */ - public final PhotonCamera camera; + public final PhotonCamera camera; /** * Pose estimator for camera. */ - public final PhotonPoseEstimator poseEstimator; - public final Matrix singleTagStdDevs; - public final Matrix multiTagStdDevs; + public final PhotonPoseEstimator poseEstimator; + /** + * Standard Deviation for single tag readings for pose estimation. + */ + private final Matrix singleTagStdDevs; + /** + * Standard deviation for multi-tag readings for pose estimation. + */ + private final Matrix multiTagStdDevs; + /** + * Current standard deviations used. + */ + public Matrix curStdDevs; + /** + * Estimated robot pose. + */ + public Optional estimatedRobotPose; /** * Transform of the camera rotation and translation relative to the center of the robot */ - private final Transform3d robotToCamTransform; + private final Transform3d robotToCamTransform; /** * Simulated camera instance which only exists during simulations. */ - public PhotonCameraSim cameraSim; + public PhotonCameraSim cameraSim; /** * Results list to be updated periodically and cached to avoid unnecessary queries. */ - public List resultsList; + public List resultsList; + /** + * Last read from the camera timestamp to prevent lag due to slow data fetches. + */ + private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + /** * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine * estimation noise on an actual robot. @@ -454,7 +424,6 @@ enum Cameras poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - camera, robotToCamTransform); poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); @@ -489,9 +458,135 @@ public void addToVisionSim(VisionSystemSim systemSim) if (Robot.isSimulation()) { systemSim.addCamera(cameraSim, robotToCamTransform); -// cameraSim.enableDrawWireframe(true); } } + + /** + * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp. + */ + private void updateUnreadResults() + { + double mostRecentTimestamp = resultsList.get(0).getTimestampSeconds(); + double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + double debounceTime = Milliseconds.of(15).in(Seconds); + for (PhotonPipelineResult result : resultsList) + { + mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); + } + if ((resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) && + (currentTimestamp - lastReadTimestamp) >= debounceTime) + { + resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults(); + lastReadTimestamp = currentTimestamp; + resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { + return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; + }); + if (!resultsList.isEmpty()) + { + updateEstimatedGlobalPose(); + } + } + } + + /** + * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the + * cache of results. + * + * @return Estimated pose. + */ + public Optional getEstimatedGlobalPose() + { + updateUnreadResults(); + return estimatedRobotPose; + } + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once + * per loop. + * + *

    Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link Cameras#updateEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for + * estimation. + */ + private void updateEstimatedGlobalPose() + { + Optional visionEst = Optional.empty(); + for (var change : camera.getAllUnreadResults()) + { + visionEst = poseEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + } + estimatedRobotPose = visionEst; + } + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based + * on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) + { + if (estimatedPose.isEmpty()) + { + // No pose input. Default to single-tag std devs + curStdDevs = singleTagStdDevs; + + } else + { + // Pose present. Start running Heuristic + var estStdDevs = singleTagStdDevs; + 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 = poseEstimator.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 = singleTagStdDevs; + } else + { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + { + estStdDevs = multiTagStdDevs; + } + // 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; + } + } + } + + } } From 667405e6c65bd06b54dd8c6c660a15112ceb8d02 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 13:28:28 -0600 Subject: [PATCH 05/54] Updated PhotonVision to add utility functions. --- .../swervedrive/SwerveSubsystem.java | 22 ++++--- .../robot/subsystems/swervedrive/Vision.java | 62 +++++++++++++++---- 2 files changed, 63 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 259a86af..604bef3c 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -30,10 +30,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; +import frc.robot.subsystems.swervedrive.Vision.Cameras; import java.io.File; import java.util.Arrays; +import java.util.Optional; import java.util.function.DoubleSupplier; -import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; @@ -257,20 +258,23 @@ public Command aimAtSpeaker(double tolerance) /** * Aim the robot at the target returned by PhotonVision. * - * @param camera {@link PhotonCamera} to communicate with. * @return A {@link Command} which will run the alignment. */ - public Command aimAtTarget(PhotonCamera camera) + public Command aimAtTarget(Cameras camera) { return run(() -> { - PhotonPipelineResult result = camera.getLatestResult(); - if (result.hasTargets()) + Optional resultO = camera.getBestResult(); + if (resultO.isPresent()) { - drive(getTargetSpeeds(0, - 0, - Rotation2d.fromDegrees(result.getBestTarget() - .getYaw()))); // Not sure if this will work, more math may be required. + var result = resultO.get(); + if (result.hasTargets()) + { + drive(getTargetSpeeds(0, + 0, + Rotation2d.fromDegrees(result.getBestTarget() + .getYaw()))); // Not sure if this will work, more math may be required. + } } }); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index fcdd7cdd..cd36a6ef 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -461,6 +461,56 @@ public void addToVisionSim(VisionSystemSim systemSim) } } + /** + * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most + * recent result! + * + * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result! + */ + public Optional getBestResult() + { + if (resultsList.isEmpty()) + { + return Optional.empty(); + } + + PhotonPipelineResult bestResult = resultsList.get(0); + double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); + double currentAmbiguity = 0; + for (PhotonPipelineResult result : resultsList) + { + currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); + if (currentAmbiguity < amiguity && currentAmbiguity > 0) + { + bestResult = result; + amiguity = currentAmbiguity; + } + } + return Optional.of(bestResult); + } + + /** + * Get the latest result from the current cache. + * + * @return Empty optional if nothing is found. Latest result if something is there. + */ + public Optional getLatestResult() + { + return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); + } + + /** + * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the + * cache of results. + * + * @return Estimated pose. + */ + public Optional getEstimatedGlobalPose() + { + updateUnreadResults(); + return estimatedRobotPose; + } + /** * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp. */ @@ -488,18 +538,6 @@ private void updateUnreadResults() } } - /** - * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the - * cache of results. - * - * @return Estimated pose. - */ - public Optional getEstimatedGlobalPose() - { - updateUnreadResults(); - return estimatedRobotPose; - } - /** * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once * per loop. From f2bd2df0492e2a3e9a991095887ee855cdc0494b Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 14:37:53 -0600 Subject: [PATCH 06/54] Fixed SparkMax encoders. --- .../encoders/SparkMaxAnalogEncoderSwerve.java | 27 +++++++++--- .../encoders/SparkMaxEncoderSwerve.java | 44 ++++++++++++++----- .../swervelib/motors/SparkFlexSwerve.java | 27 +++++++++++- .../motors/SparkMaxBrushedMotorSwerve.java | 27 +++++++++++- .../java/swervelib/motors/SparkMaxSwerve.java | 32 ++++++++++++-- .../swervelib/parser/json/ModuleJson.java | 3 +- 6 files changed, 136 insertions(+), 24 deletions(-) diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index a562c304..84d35c8e 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -2,9 +2,11 @@ import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAnalogSensor; -import com.revrobotics.spark.SparkAnalogSensor; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; import java.util.function.Supplier; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -26,10 +28,13 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder * An {@link Alert} for if the absolute encoder does not support integrated offsets. */ private Alert doesNotSupportIntegratedOffsets; - + /** + * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object. + */ + private final SwerveMotor sparkMax; /** - * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link CANSparkMax} motor data + * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data * port analog pin. * * @param motor Motor to create the encoder from. @@ -39,8 +44,10 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) { if (motor.getMotor() instanceof SparkMax) { + sparkMax = motor; encoder = ((SparkMax) motor.getMotor()).getAnalog(); - encoder.setPositionConversionFactor(360 / maxVoltage); + motor.setAbsoluteEncoder(this); + sparkMax.configureIntegratedEncoder(360/maxVoltage); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); @@ -99,7 +106,17 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - encoder.setInverted(inverted); + if(sparkMax instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + cfg.analogSensor.inverted(true); + ((SparkMaxSwerve) sparkMax).updateConfig(cfg); + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + cfg.analogSensor.inverted(true); + ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + } } /** diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index 6afe7b48..3e90d593 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -4,7 +4,10 @@ import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkMaxConfig; import java.util.function.Supplier; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; import swervelib.telemetry.Alert; @@ -26,6 +29,10 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder * An {@link Alert} for if there is a failure configuring the encoder offset. */ private Alert offsetFailure; + /** + * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. + */ + private SwerveMotor sparkMax; /** * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax} motor. @@ -45,9 +52,10 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) Alert.AlertType.WARNING_TRACE); if (motor.getMotor() instanceof SparkMax) { + sparkMax = motor; encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder(); - configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor)); - configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor)); + motor.setAbsoluteEncoder(this); + motor.configureIntegratedEncoder(conversionFactor); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); @@ -97,7 +105,17 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - encoder.setInverted(inverted); + if(sparkMax instanceof SparkMaxSwerve) + { + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + cfg.analogSensor.inverted(true); + ((SparkMaxSwerve) sparkMax).updateConfig(cfg); + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + cfg.analogSensor.inverted(true); + ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + } } /** @@ -131,17 +149,19 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - REVLibError error = null; - for (int i = 0; i < maximumRetries; i++) + if(sparkMax instanceof SparkMaxSwerve) { - error = encoder.setZeroOffset(offset); - if (error == REVLibError.kOk) - { - return true; - } + SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + ((SparkMaxSwerve) sparkMax).updateConfig(cfg); + return true; + } else if (sparkMax instanceof SparkMaxBrushedMotorSwerve) + { + SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig(); + cfg.absoluteEncoder.zeroOffset(offset); + ((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg); + return true; } - offsetFailure.setText("Failure to set Absolute Encoder Offset Error: " + error); - offsetFailure.set(true); return false; } diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 98c484f3..c761c665 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -1,5 +1,7 @@ package swervelib.motors; +import static edu.wpi.first.units.Units.Seconds; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -16,6 +18,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SparkMaxAnalogEncoderSwerve; @@ -132,11 +135,33 @@ private void configureSparkFlex(Supplier config) { return; } - Timer.delay(0.01); + Timer.delay(Units.Milliseconds.of(10).in(Seconds)); } failureConfiguring.set(true); } + /** + * Get the current configuration of the {@link SparkFlex} + * + * @return {@link SparkMaxConfig} + */ + public SparkFlexConfig getConfig() + { + return cfg; + } + + /** + * Update the config for the {@link SparkFlex} + * + * @param cfgGiven Given {@link SparkFlexConfig} which should have minimal modifications. + */ + public void updateConfig(SparkFlexConfig cfgGiven) + { + cfg.apply(cfgGiven); + configureSparkFlex(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); + cfgUpdated = false; + } + /** * Set the voltage compensation for the swerve module motor. * diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index e1bae42f..fb7898a4 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -1,5 +1,7 @@ package swervelib.motors; +import static edu.wpi.first.units.Units.Seconds; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; @@ -12,6 +14,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; @@ -189,11 +192,33 @@ private void configureSparkMax(Supplier config) { return; } - Timer.delay(0.01); + Timer.delay(Units.Milliseconds.of(10).in(Seconds)); } failureConfiguringAlert.set(true); } + /** + * Get the current configuration of the {@link SparkMax} + * + * @return {@link SparkMaxConfig} + */ + public SparkMaxConfig getConfig() + { + return cfg; + } + + /** + * Update the config for the {@link SparkMax} + * + * @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications. + */ + public void updateConfig(SparkMaxConfig cfgGiven) + { + cfg.apply(cfgGiven); + configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); + cfgUpdated = false; + } + /** * Set the voltage compensation for the swerve module motor. * diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 2ea17bcd..58e03683 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -1,5 +1,7 @@ package swervelib.motors; +import static edu.wpi.first.units.Units.Seconds; + import com.revrobotics.AbsoluteEncoder; import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; @@ -10,9 +12,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; @@ -59,7 +61,7 @@ public class SparkMaxSwerve extends SwerveMotor /** * Configuration object for {@link SparkMax} motor. */ - private SparkMaxConfig cfg = new SparkMaxConfig(); + private SparkMaxConfig cfg = new SparkMaxConfig(); /** * Tracker for changes that need to be pushed. */ @@ -115,11 +117,33 @@ private void configureSparkMax(Supplier config) { return; } - Timer.delay(0.01); + Timer.delay(Units.Milliseconds.of(10).in(Seconds)); } DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); } + /** + * Get the current configuration of the {@link SparkMax} + * + * @return {@link SparkMaxConfig} + */ + public SparkMaxConfig getConfig() + { + return cfg; + } + + /** + * Update the config for the {@link SparkMax} + * + * @param cfgGiven Given {@link SparkMaxConfig} which should have minimal modifications. + */ + public void updateConfig(SparkMaxConfig cfgGiven) + { + cfg.apply(cfgGiven); + configureSparkMax(() -> motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters)); + cfgUpdated = false; + } + /** * Set the voltage compensation for the swerve module motor. * @@ -417,7 +441,7 @@ public void setReference(double setpoint, double feedforward) { int pidSlot = 0; - if(cfgUpdated) + if (cfgUpdated) { burnFlash(); Timer.delay(0.1); // Give 100ms to apply changes diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 3cb81ee5..1bf1f6e8 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -1,5 +1,6 @@ package swervelib.parser.json; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.util.Units; import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -139,7 +140,7 @@ public SwerveModuleConfiguration createModuleConfiguration( // Backwards compatibility, auto-optimization. if (conversionFactor.angle == 360 && absEncoder != null && - absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof CANSparkMax) + absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax) { angleMotor.setAbsoluteEncoder(absEncoder); } From a0c7858162450a87be2a9a5999b0f6f02c885ea4 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 14:38:12 -0600 Subject: [PATCH 07/54] Fixed CTRE CANCoder and Pigeon2 --- .../swervelib/encoders/CANCoderSwerve.java | 10 +++-- .../java/swervelib/imu/Pigeon2Swerve.java | 37 ++++++++++++++----- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 24ca2013..da95e9da 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -1,5 +1,8 @@ package swervelib.encoders; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; + import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; @@ -9,6 +12,7 @@ import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import edu.wpi.first.units.measure.Angle; import swervelib.telemetry.Alert; /** @@ -139,7 +143,7 @@ public double getAbsolutePosition() readingFaulty.set(false); } - StatusSignal angle = encoder.getAbsolutePosition(); + StatusSignal angle = encoder.getAbsolutePosition(); // Taken from democat's library. // Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74 @@ -160,7 +164,7 @@ public double getAbsolutePosition() readingIgnored.set(false); } - return angle.getValue() * 360; + return angle.getValue().in(Degrees); } /** @@ -213,6 +217,6 @@ public boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - return encoder.getVelocity().getValue() * 360; + return encoder.getVelocity().getValue().in(DegreesPerSecond); } } diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 574da147..09b71b83 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -1,13 +1,18 @@ package swervelib.imu; +import static edu.wpi.first.units.Units.DegreesPerSecond; + import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.Pigeon2Configurator; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; +import java.util.function.Supplier; /** * SwerveIMU interface for the {@link Pigeon2} @@ -36,6 +41,19 @@ public class Pigeon2Swerve extends SwerveIMU */ private Pigeon2Configurator cfg; + /** + * X Acceleration supplier + */ + private Supplier> xAcc; + /** + * Y Accelleration supplier. + */ + private Supplier> yAcc; + /** + * Z Acceleration supplier. + */ + private Supplier> zAcc; + /** * Generate the SwerveIMU for {@link Pigeon2}. * @@ -46,6 +64,9 @@ public Pigeon2Swerve(int canid, String canbus) { imu = new Pigeon2(canid, canbus); this.cfg = imu.getConfigurator(); + xAcc = imu::getAccelerationX; + yAcc = imu::getAccelerationY; + zAcc = imu::getAccelerationZ; SmartDashboard.putData(imu); } @@ -123,6 +144,7 @@ public Rotation3d getRotation3d() return getRawRotation3d().minus(offset); } + /** * Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns * empty. @@ -132,24 +154,19 @@ public Rotation3d getRotation3d() @Override public Optional getAccel() { - // TODO: Switch to suppliers. - StatusSignal xAcc = imu.getAccelerationX(); - StatusSignal yAcc = imu.getAccelerationY(); - StatusSignal zAcc = imu.getAccelerationZ(); - - return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), - yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(), - zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0)); + // TODO: Implement later. + + return Optional.empty(); } /** * Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty. * - * @return {@link Double} of the rotation rate as an {@link Optional}. + * @return Rotation rate in DegreesPerSecond. */ public double getRate() { - return imu.getRate(); + return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond); } /** From 7d6ea672bd1e79fe697047b8ac55c72b2655beda Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 14:42:28 -0600 Subject: [PATCH 08/54] Updated TalonFXSwerve --- src/main/java/swervelib/motors/TalonFXSwerve.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 233a868f..0eacdf47 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -1,12 +1,17 @@ package swervelib.motors; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Voltage; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; @@ -290,7 +295,9 @@ public void setMotorBrake(boolean isBrakeMode) public void setInverted(boolean inverted) { // Timer.delay(1); - motor.setInverted(inverted); + cfg.refresh(configuration.MotorOutput); + configuration.MotorOutput.withInverted(inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive); + cfg.apply(configuration.MotorOutput); } /** @@ -357,7 +364,7 @@ public void setReference(double setpoint, double feedforward, double position) @Override public double getVoltage() { - return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(); + return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts); } /** @@ -390,7 +397,7 @@ public double getAppliedOutput() @Override public double getVelocity() { - return motor.getVelocity().getValue(); + return motor.getVelocity().getValue().magnitude(); } /** @@ -401,7 +408,7 @@ public double getVelocity() @Override public double getPosition() { - return motor.getPosition().getValue(); + return motor.getPosition().getValue().magnitude(); } /** From 00456940ac854a382bcc817dac84f459cef8fce2 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 15:12:14 -0600 Subject: [PATCH 09/54] Updated current limit to use supply instead of stator. --- .../java/swervelib/motors/TalonFXSwerve.java | 68 ++----------------- 1 file changed, 7 insertions(+), 61 deletions(-) diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 0eacdf47..904f5703 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -1,6 +1,9 @@ package swervelib.motors; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -186,63 +189,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) cfg.apply(configuration); // Taken from democat's library. // https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/Falcon500DriveControllerFactoryBuilder.java#L16 - configureCANStatusFrames(250); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - */ - public void configureCANStatusFrames(int CANStatus1) - { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - } - - /** - * Set the CAN status frames. - * - * @param CANStatus1 Applied Motor Output, Fault Information, Limit Switch Information - * @param CANStatus2 Selected Sensor Position (PID 0), Selected Sensor Velocity (PID 0), Brushed Supply Current - * Measurement, Sticky Fault Information - * @param CANStatus3 Quadrature Information - * @param CANStatus4 Analog Input, Supply Battery Voltage, Controller Temperature - * @param CANStatus8 Pulse Width Information - * @param CANStatus10 Motion Profiling/Motion Magic Information - * @param CANStatus12 Selected Sensor Position (Aux PID 1), Selected Sensor Velocity (Aux PID 1) - * @param CANStatus13 PID0 (Primary PID) Information - * @param CANStatus14 PID1 (Auxiliary PID) Information - * @param CANStatus21 Integrated Sensor Position (Talon FX), Integrated Sensor Velocity (Talon FX) - * @param CANStatusCurrent Brushless Supply Current Measurement, Brushless Stator Current Measurement - */ - public void configureCANStatusFrames( - int CANStatus1, - int CANStatus2, - int CANStatus3, - int CANStatus4, - int CANStatus8, - int CANStatus10, - int CANStatus12, - int CANStatus13, - int CANStatus14, - int CANStatus21, - int CANStatusCurrent) - { - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, CANStatus1); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, CANStatus2); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, CANStatus3); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, CANStatus4); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, CANStatus8); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, CANStatus10); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, CANStatus12); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, CANStatus13); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, CANStatus14); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_21_FeedbackIntegrated, CANStatus21); - // motor.setStatusFramePeriod(StatusFrameEnhanced.Status_Brushless_Current, - // CANStatusCurrent); - - // TODO: Configure Status Frame 2 thru 21 if necessary - // https://v5.docs.ctr-electronics.com/en/stable/ch18_CommonAPI.html#setting-status-frame-periods + // configureCANStatusFrames(250); } /** @@ -421,8 +368,7 @@ public void setPosition(double position) { if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation) { - position = position < 0 ? (position % 360) + 360 : position; - cfg.setPosition(position / 360); + cfg.setPosition(Degrees.of(position).in(Rotations)); } } @@ -448,8 +394,8 @@ public void setCurrentLimit(int currentLimit) { cfg.refresh(configuration.CurrentLimits); cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); + configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit) + .withSupplyCurrentLimitEnable(true)); } /** From 4c8ddc588ef6b0aba2407428022bda79e07cf3f5 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 15:14:07 -0600 Subject: [PATCH 10/54] Made gearRatio and diameter required properties. --- .../parser/json/modules/AngleConversionFactorsJson.java | 2 +- .../parser/json/modules/DriveConversionFactorsJson.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java index a7c8613a..0ad20b3d 100644 --- a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java @@ -13,7 +13,7 @@ public class AngleConversionFactorsJson /** * Gear ratio for the angle/steering/azimuth motor on the Swerve Module. Motor rotations to 1 wheel rotation. */ - public double gearRatio = 0; + public double gearRatio; /** * Calculated or given conversion factor. */ diff --git a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java index d792d36d..c7446af5 100644 --- a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java @@ -14,11 +14,11 @@ public class DriveConversionFactorsJson /** * Gear ratio for the drive motor rotations to turn the wheel 1 complete rotation. */ - public double gearRatio = 0; + public double gearRatio; /** * Diameter of the wheel in inches. */ - public double diameter = 0; + public double diameter; /** * Calculated conversion factor. */ From d936602a4ce162df2ad2e701c30d260d3619b69d Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 16:00:42 -0600 Subject: [PATCH 11/54] Removed old conversion factor method. --- src/main/java/swervelib/SwerveDriveTest.java | 4 +- src/main/java/swervelib/SwerveModule.java | 4 +- .../parser/SwerveModuleConfiguration.java | 7 ++- .../SwerveModulePhysicalCharacteristics.java | 11 ++-- .../swervelib/parser/json/ModuleJson.java | 55 +++++-------------- .../parser/json/PhysicalPropertiesJson.java | 29 ++++------ .../modules/AngleConversionFactorsJson.java | 6 -- .../json/modules/ConversionFactorsJson.java | 17 +++++- .../modules/DriveConversionFactorsJson.java | 6 -- 9 files changed, 53 insertions(+), 86 deletions(-) diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index 019aeb29..64d77783 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -221,7 +221,7 @@ public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo Timer.delay(1); Rotation2d startingAbsoluteEncoderPosition = Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition()); double driveEncoderPositionRotations = module.getDriveMotor().getPosition() / - module.configuration.conversionFactors.drive; + module.configuration.conversionFactors.drive.factor; if (automatic) { module.getAngleMotor().setVoltage(volts); @@ -237,7 +237,7 @@ public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo false); Timer.delay(60); } - double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive) - + double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive.factor) - driveEncoderPositionRotations; DriverStation.reportWarning(module.configuration.name + " Coupling Ratio: " + couplingRatio, false); couplingRatioSum += couplingRatio; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index a9d2cbf7..4753cae4 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -174,7 +174,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15); // Config angle motor/controller - angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle); + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); angleMotor.configurePIDWrapping(0, 180); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); @@ -187,7 +187,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat } // Config drive motor/controller - driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive); + driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor); driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); driveMotor.setInverted(moduleConfiguration.driveMotorInverted); driveMotor.setMotorBrake(true); diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index 24d06257..e88bc02d 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -4,6 +4,7 @@ import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.motors.SwerveMotor; import swervelib.parser.json.MotorConfigDouble; +import swervelib.parser.json.modules.ConversionFactorsJson; /** * Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. @@ -17,7 +18,7 @@ public class SwerveModuleConfiguration * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the * conversion factors. */ - public final MotorConfigDouble conversionFactors; + public final ConversionFactorsJson conversionFactors; /** * Angle offset in degrees for the Swerve Module. */ @@ -89,7 +90,7 @@ public class SwerveModuleConfiguration public SwerveModuleConfiguration( SwerveMotor driveMotor, SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, double xMeters, @@ -139,7 +140,7 @@ public SwerveModuleConfiguration( public SwerveModuleConfiguration( SwerveMotor driveMotor, SwerveMotor angleMotor, - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, SwerveAbsoluteEncoder absoluteEncoder, double angleOffset, double xMeters, diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 99360ce1..e0b9f741 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -1,6 +1,7 @@ package swervelib.parser; import swervelib.parser.json.MotorConfigDouble; +import swervelib.parser.json.modules.ConversionFactorsJson; /** * Configuration class which stores physical characteristics shared between every swerve module. @@ -23,13 +24,13 @@ public class SwerveModulePhysicalCharacteristics /** * The voltage to use for the smart motor voltage compensation. */ - public double optimalVoltage; + public double optimalVoltage; /** * The conversion factors for the drive and angle motors, created by * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. */ - public MotorConfigDouble conversionFactor; + public ConversionFactorsJson conversionFactor; /** * Construct the swerve module physical characteristics. @@ -49,7 +50,7 @@ public class SwerveModulePhysicalCharacteristics * overdrawing power and power loss). */ public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, double wheelGripCoefficientOfFriction, double optimalVoltage, int driveMotorCurrentLimit, @@ -64,7 +65,7 @@ public SwerveModulePhysicalCharacteristics( // Set the conversion factors to null if they are both 0. if (conversionFactors != null) { - if (conversionFactors.angle == 0 && conversionFactors.drive == 0) + if (conversionFactors.isAngleEmpty() && conversionFactors.isDriveEmpty()) { this.conversionFactor = null; } @@ -90,7 +91,7 @@ public SwerveModulePhysicalCharacteristics( * power and power loss). */ public SwerveModulePhysicalCharacteristics( - MotorConfigDouble conversionFactors, + ConversionFactorsJson conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 1bf1f6e8..571f0ec3 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -33,7 +33,7 @@ public class ModuleJson * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); +// public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** * Conversion Factors composition. Auto-calculates the conversion factors. */ @@ -81,37 +81,8 @@ public SwerveModuleConfiguration createModuleConfiguration( SwerveMotor angleMotor = angle.createMotor(false); SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); - // Setup deprecation notice. -// if (this.conversionFactor.drive != 0 && this.conversionFactor.angle != 0) -// { -// new Alert("Configuration", -// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle + -// "} \nis deprecated, please use\n" + -// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " + -// conversionFactor.angle + "} }", -// AlertType.WARNING).set(true); -// } - - // Override with composite conversion factor. - if (!conversionFactors.isAngleEmpty()) - { - conversionFactor.angle = conversionFactors.angle.calculate(); - } - if (!conversionFactors.isDriveEmpty()) - { - conversionFactor.drive = conversionFactors.drive.calculate(); - } - // Set the conversion factors to null if they are both 0. - if (this.conversionFactor != null) - { - if (this.conversionFactor.angle == 0 && this.conversionFactor.drive == 0) - { - this.conversionFactor = null; - } - } - - if (this.conversionFactor == null && physicalCharacteristics.conversionFactor == null) + if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null) { throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + @@ -120,26 +91,26 @@ public SwerveModuleConfiguration createModuleConfiguration( "OR\n" + "Set the conversion factor in physicalproperties.json OR the module JSON file." + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); - } else if (physicalCharacteristics.conversionFactor != null && this.conversionFactor == null) + } else if (physicalCharacteristics.conversionFactor.works() && !conversionFactors.works()) { - this.conversionFactor = physicalCharacteristics.conversionFactor; - } else if (physicalCharacteristics.conversionFactor != - null) // If both are defined, override 0 with the physical characterstics input. + conversionFactors = physicalCharacteristics.conversionFactor; + } else if (physicalCharacteristics.conversionFactor.works()) + // If both are defined, override 0 with the physical characterstics input. { - this.conversionFactor.angle = this.conversionFactor.angle == 0 ? physicalCharacteristics.conversionFactor.angle - : this.conversionFactor.angle; - this.conversionFactor.drive = this.conversionFactor.drive == 0 ? physicalCharacteristics.conversionFactor.drive - : this.conversionFactor.drive; + conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle + : conversionFactors.angle; + conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive + : conversionFactors.drive; } - if (this.conversionFactor.drive == 0 || this.conversionFactor.angle == 0) + if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty()) { throw new RuntimeException( "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); } // Backwards compatibility, auto-optimization. - if (conversionFactor.angle == 360 && absEncoder != null && + if (conversionFactors.angle.factor == 360 && absEncoder != null && absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax) { angleMotor.setAbsoluteEncoder(absEncoder); @@ -148,7 +119,7 @@ public SwerveModuleConfiguration createModuleConfiguration( return new SwerveModuleConfiguration( drive.createMotor(true), angleMotor, - conversionFactor, + conversionFactors, absEncoder, absoluteEncoderOffset, Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x), diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 60be9d25..d0fdceda 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -2,6 +2,8 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics; import swervelib.parser.json.modules.ConversionFactorsJson; +import swervelib.telemetry.Alert; +import swervelib.telemetry.Alert.AlertType; /** * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule. @@ -45,29 +47,20 @@ public class PhysicalPropertiesJson public SwerveModulePhysicalCharacteristics createPhysicalProperties() { // Setup deprecation notice. -// if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() && -// conversionFactors.isAngleEmpty()) -// { -// new Alert("Configuration", -// "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle + -// "} \nis deprecated, please use\n" + -// "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " + -// conversionFactor.angle + "} }", -// AlertType.ERROR_TRACE).set(true); -// } - - if (!conversionFactors.isAngleEmpty()) + if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() && + conversionFactors.isAngleEmpty()) { - conversionFactor.angle = conversionFactors.angle.calculate(); + new Alert("Configuration", + "\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle + + "} \nis deprecated, please use\n" + + "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " + + conversionFactor.angle + "} }", + AlertType.ERROR_TRACE).set(true); } - if (!conversionFactors.isDriveEmpty()) - { - conversionFactor.drive = conversionFactors.drive.calculate(); - } return new SwerveModulePhysicalCharacteristics( - conversionFactor, + conversionFactors, wheelGripCoefficientOfFriction, optimalVoltage, currentLimit.drive, diff --git a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java index 0ad20b3d..54567fe4 100644 --- a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java @@ -26,12 +26,6 @@ public class AngleConversionFactorsJson */ public double calculate() { - if (factor != 0 && gearRatio != 0) - { - new Alert("Configuration", - "The given angle conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.", - AlertType.WARNING).set(true); - } if (factor == 0) { factor = SwerveMath.calculateDegreesPerSteeringRotation(gearRatio); diff --git a/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java index 85ff25f2..546ad65a 100644 --- a/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/ConversionFactorsJson.java @@ -22,7 +22,8 @@ public class ConversionFactorsJson */ public boolean isDriveEmpty() { - return drive.factor == 0 && drive.diameter == 0 && drive.gearRatio == 0; + drive.calculate(); + return drive.factor == 0; } /** @@ -32,6 +33,18 @@ public boolean isDriveEmpty() */ public boolean isAngleEmpty() { - return angle.factor == 0 && angle.gearRatio == 0; + angle.calculate(); + return angle.factor == 0; + } + + /** + * Check if the conversion factor can be found. + * + * @return If the conversion factors can be found. + */ + public boolean works() + { + return (angle.factor != 0 && drive.factor != 0) || + ((drive.gearRatio != 0 && drive.diameter != 0)) && (angle.gearRatio != 0); } } diff --git a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java index c7446af5..3cd89065 100644 --- a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java @@ -31,12 +31,6 @@ public class DriveConversionFactorsJson */ public double calculate() { - if (factor != 0 && (diameter != 0 || gearRatio != 0)) - { - new Alert("Configuration", - "The given drive conversion factor takes precedence over the composite conversion factor, please remove 'factor' if you want to use the composite factor instead.", - AlertType.WARNING).set(true); - } if (factor == 0) { factor = SwerveMath.calculateMetersPerRotation(Units.inchesToMeters(this.diameter), this.gearRatio); From 75f690c35bc2b4d524ec964ac5ff363f6b262477 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 16:04:42 -0600 Subject: [PATCH 12/54] Removed deprecated config options. --- src/main/java/swervelib/parser/json/ModuleJson.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 571f0ec3..8029635e 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -26,14 +26,6 @@ public class ModuleJson * Angle motor device configuration. */ public DeviceJson angle; - /** - * Conversion factor for the module, if different from the one in swervedrive.json - *

    - * Conversion factor applied to the motor controllers PID loops. Can be calculated with - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. - */ -// public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** * Conversion Factors composition. Auto-calculates the conversion factors. */ From 791ee34a15a731f145db90579bc5a2212df87067 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 16:18:57 -0600 Subject: [PATCH 13/54] Updated vendordeps. --- ...-3.1.json => PathplannerLib-2025.0.0-beta-4.json} | 8 ++++---- ...5.0.0-beta-1.json => REVLib-2025.0.0-beta-2.json} | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) rename vendordeps/{PathplannerLib-2025.0.0-beta-3.1.json => PathplannerLib-2025.0.0-beta-4.json} (84%) rename vendordeps/{REVLib-2025.0.0-beta-1.json => REVLib-2025.0.0-beta-2.json} (88%) diff --git a/vendordeps/PathplannerLib-2025.0.0-beta-3.1.json b/vendordeps/PathplannerLib-2025.0.0-beta-4.json similarity index 84% rename from vendordeps/PathplannerLib-2025.0.0-beta-3.1.json rename to vendordeps/PathplannerLib-2025.0.0-beta-4.json index 5f4b1f18..de2782ee 100644 --- a/vendordeps/PathplannerLib-2025.0.0-beta-3.1.json +++ b/vendordeps/PathplannerLib-2025.0.0-beta-4.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.0.0-beta-3.1.json", + "fileName": "PathplannerLib-2025.0.0-beta-4.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-3.1", + "version": "2025.0.0-beta-4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-3.1" + "version": "2025.0.0-beta-4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-3.1", + "version": "2025.0.0-beta-4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.0-beta-1.json b/vendordeps/REVLib-2025.0.0-beta-2.json similarity index 88% rename from vendordeps/REVLib-2025.0.0-beta-1.json rename to vendordeps/REVLib-2025.0.0-beta-2.json index 691b6437..85f61514 100644 --- a/vendordeps/REVLib-2025.0.0-beta-1.json +++ b/vendordeps/REVLib-2025.0.0-beta-2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0-beta-1.json", + "fileName": "REVLib-2025.0.0-beta-2.json", "name": "REVLib", - "version": "2025.0.0-beta-1", + "version": "2025.0.0-beta-2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0-beta-1" + "version": "2025.0.0-beta-2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0-beta-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0-beta-1", + "version": "2025.0.0-beta-2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-1", + "version": "2025.0.0-beta-2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 12fcfc93f6b2d87f93fa199847056ec8f6aa44e2 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 16:56:28 -0600 Subject: [PATCH 14/54] Added initial pose support. --- .../swervedrive/SwerveSubsystem.java | 2 +- src/main/java/swervelib/SwerveDrive.java | 6 +-- .../java/swervelib/parser/SwerveParser.java | 37 ++++++++++++++----- .../parser/json/PhysicalPropertiesJson.java | 9 ++--- 4 files changed, 35 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 604bef3c..30939ad9 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -123,7 +123,7 @@ public SwerveSubsystem(File directory) */ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED); + swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED, Pose2d.kZero); } /** diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 7d83e5d0..9059d101 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -175,9 +175,10 @@ public class SwerveDrive * {@link SwerveController}. * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if * you have feet per second! + * @param startingPose Starting {@link Pose2d} on the field. */ public SwerveDrive( - SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS) + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS, Pose2d startingPose) { this.maxSpeedMPS = maxSpeedMPS; swerveDriveConfiguration = config; @@ -207,8 +208,7 @@ public SwerveDrive( kinematics, getYaw(), getModulePositions(), - new Pose2d(new Translation2d(0, 0), - Rotation2d.fromDegrees(0))); // x,y,heading in radians; Vision measurement std dev, higher=less weight + startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight zeroGyro(); setMaximumSpeed(maxSpeedMPS); diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java index 573fdd87..340f9c99 100644 --- a/src/main/java/swervelib/parser/SwerveParser.java +++ b/src/main/java/swervelib/parser/SwerveParser.java @@ -3,6 +3,7 @@ import com.fasterxml.jackson.databind.JsonNode; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import java.io.File; import java.io.IOException; import java.util.HashMap; @@ -138,7 +139,24 @@ public SwerveDrive createSwerveDrive(double maxSpeed) return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, maxSpeed, physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); + maxSpeed, Pose2d.kZero); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in + * {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @param initialPose {@link Pose2d} starting point on the field + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose) + { + return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, + maxSpeed, + physicalPropertiesJson.wheelGripCoefficientOfFriction), + maxSpeed, initialPose); } /** @@ -157,12 +175,12 @@ public SwerveDrive createSwerveDrive(double maxSpeed) */ public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; + physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion; return createSwerveDrive(SwerveMath.createDriveFeedforward(physicalPropertiesJson.optimalVoltage, maxSpeed, physicalPropertiesJson.wheelGripCoefficientOfFriction), - maxSpeed); + maxSpeed, Pose2d.kZero); } /** @@ -184,9 +202,9 @@ public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversio public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) { - physicalPropertiesJson.conversionFactor.angle = angleMotorConversionFactor; - physicalPropertiesJson.conversionFactor.drive = driveMotorConversion; - return createSwerveDrive(driveFeedforward, maxSpeed); + physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion; + return createSwerveDrive(driveFeedforward, maxSpeed, Pose2d.kZero); } /** @@ -196,9 +214,10 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do * {@link swervelib.math.SwerveMath#createDriveFeedforward(double, double, double)}. * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in * {@link swervelib.SwerveController} of the robot + * @param initialPose {@link Pose2d} initial pose. * @return {@link SwerveDrive} instance. */ - public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed) + public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, double maxSpeed, Pose2d initialPose) { SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length]; @@ -222,6 +241,6 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do return new SwerveDrive( swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed); + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed, initialPose); } } diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index d0fdceda..00e4e571 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -11,13 +11,11 @@ public class PhysicalPropertiesJson { - /** - * Conversion factor applied to the motor controllers PID loops. Can be calculated with - * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} for angle motors or - * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} for drive motors. + * DEPRECATED! Use {@link PhysicalPropertiesJson#conversionFactors} instead. */ - public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); + @Deprecated(since = "2025", forRemoval = true) + public MotorConfigDouble conversionFactor = new MotorConfigDouble(); /** * Conversion Factors composition. Auto-calculates the conversion factors. */ @@ -58,7 +56,6 @@ public SwerveModulePhysicalCharacteristics createPhysicalProperties() AlertType.ERROR_TRACE).set(true); } - return new SwerveModulePhysicalCharacteristics( conversionFactors, wheelGripCoefficientOfFriction, From 6cfc7d5302bf6f278f4571608a47873cff2ddadc Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 17:12:50 -0600 Subject: [PATCH 15/54] Added initial pose support. --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 30939ad9..a18ae08d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -91,7 +91,7 @@ public SwerveSubsystem(File directory) SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED); + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, Pose2d.kZero); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); } catch (Exception e) From e6e54b189a010ea2bb5a1028b6441609383cac08 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 20:26:26 -0600 Subject: [PATCH 16/54] Began implementing MapleSim. --- src/main/java/swervelib/SwerveDrive.java | 45 +++++++++++--- .../swervelib/motors/SparkFlexSwerve.java | 12 ++++ .../motors/SparkMaxBrushedMotorSwerve.java | 12 ++++ .../java/swervelib/motors/SparkMaxSwerve.java | 12 ++++ .../java/swervelib/motors/SwerveMotor.java | 7 +++ .../java/swervelib/motors/TalonFXSwerve.java | 12 ++++ .../java/swervelib/motors/TalonSRXSwerve.java | 12 ++++ .../parser/SwerveDriveConfiguration.java | 62 +++++++++++++++++++ .../SwerveModulePhysicalCharacteristics.java | 39 ++++++++++-- .../parser/json/PhysicalPropertiesJson.java | 19 +++++- .../simulation/SwerveModuleSimulation.java | 2 + 11 files changed, 219 insertions(+), 15 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 9059d101..eddb4ede 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -29,6 +29,9 @@ import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; import swervelib.encoders.CANCoderSwerve; import swervelib.imu.IMUVelocity; import swervelib.imu.Pigeon2Swerve; @@ -88,11 +91,15 @@ public class SwerveDrive /** * Field object. */ - public Field2d field = new Field2d(); + public Field2d field = new Field2d(); + /** + * MapleSim SwerveDrive. + */ + private SwerveDriveSimulation mapleSimDrive; /** * Swerve controller for controlling heading of the robot. */ - public SwerveController swerveController; + public SwerveController swerveController; /** * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's * correction. @@ -107,16 +114,16 @@ public class SwerveDrive * Correct for skew that scales with angular velocity in * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} */ - public boolean angularVelocityCorrection = false; + public boolean angularVelocityCorrection = false; /** * Correct for skew that scales with angular velocity in * {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto. */ - public boolean autonomousAngularVelocityCorrection = false; + public boolean autonomousAngularVelocityCorrection = false; /** * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). */ - public double angularVelocityCoefficient = 0; + public double angularVelocityCoefficient = 0; /** * Whether to correct heading when driving translationally. Set to true to enable. */ @@ -137,7 +144,7 @@ public class SwerveDrive * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. */ - private IMUVelocity imuVelocity; + private IMUVelocity imuVelocity; /** * Simulation of the swerve drive. */ @@ -175,10 +182,11 @@ public class SwerveDrive * {@link SwerveController}. * @param maxSpeedMPS Maximum speed in meters per second, remember to use {@link Units#feetToMeters(double)} if * you have feet per second! - * @param startingPose Starting {@link Pose2d} on the field. + * @param startingPose Starting {@link Pose2d} on the field. */ public SwerveDrive( - SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS, Pose2d startingPose) + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS, + Pose2d startingPose) { this.maxSpeedMPS = maxSpeedMPS; swerveDriveConfiguration = config; @@ -193,6 +201,27 @@ public SwerveDrive( { simIMU = new SwerveIMUSimulation(); imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); + SwerveModuleSimulation simModule = new SwerveModuleSimulation(config.getDriveMotorSim(), + config.getAngleMotorSim(), + config.physicalCharacteristics.driveMotorCurrentLimit, + config.physicalCharacteristics.conversionFactor.drive.gearRatio, + config.physicalCharacteristics.conversionFactor.angle.gearRatio, + config.physicalCharacteristics.driveFrictionVoltage, + config.physicalCharacteristics.angleFrictionVoltage, + config.physicalCharacteristics.wheelGripCoefficientOfFriction, + config.physicalCharacteristics.conversionFactor.drive.diameter / + 2, + config.physicalCharacteristics.steerRotationalInertia); + DriveTrainSimulationConfig simCfg = new DriveTrainSimulationConfig(config.physicalCharacteristics.robotMassKg, + Units.inchesToMeters(5), + Units.inchesToMeters(5), + config.getTracklength(), + config.getTrackwidth(), + () -> { + return simModule; + }, + config.getGyroSim()); + mapleSimDrive = new SwerveDriveSimulation(simCfg, startingPose); } else { imu = config.imu; diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index c761c665..2f60ad58 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -18,6 +18,7 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; @@ -212,6 +213,17 @@ public Object getMotor() return motor; } + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + return DCMotor.getNeoVortex(1); + } + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fb7898a4..fe19892f 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -268,6 +269,17 @@ public Object getMotor() return motor; } + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + return DCMotor.getCIM(1); + } + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 58e03683..34c086ce 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; @@ -195,6 +196,17 @@ public Object getMotor() return motor; } + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + return DCMotor.getNEO(1); + } + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index 22b104d4..38861795 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -1,5 +1,6 @@ package swervelib.motors; +import edu.wpi.first.math.system.plant.DCMotor; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -172,6 +173,12 @@ public abstract class SwerveMotor */ public abstract Object getMotor(); + /** + * Get the {@link DCMotor} of the motor class. + * @return {@link DCMotor} of this type. + */ + public abstract DCMotor getSimMotor(); + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 904f5703..d7392c24 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -14,6 +14,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Voltage; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -421,6 +422,17 @@ public Object getMotor() return motor; } + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + return DCMotor.getKrakenX60(1); + } + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 98cf6e7c..28a6359f 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.math.system.plant.DCMotor; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.parser.PIDFConfig; @@ -432,6 +433,17 @@ public Object getMotor() return motor; } + /** + * Get the {@link DCMotor} of the motor class. + * + * @return {@link DCMotor} of this type. + */ + @Override + public DCMotor getSimMotor() + { + return DCMotor.getCIM(1); + } + /** * Queries whether the absolute encoder is directly attached to the motor controller. * diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index 347b187d..2a2f31cb 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -2,8 +2,14 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import java.util.function.Supplier; +import org.ironmaple.simulation.drivesims.GyroSimulation; import swervelib.SwerveModule; +import swervelib.imu.NavXSwerve; +import swervelib.imu.Pigeon2Swerve; import swervelib.imu.SwerveIMU; +import swervelib.math.SwerveMath; /** * Swerve drive configurations used during SwerveDrive construction. @@ -96,4 +102,60 @@ public double getDriveBaseRadiusMeters() //Return Largest Radius return centerOfModules.getDistance(moduleLocationsMeters[0]); } + + /** + * Get the trackwidth of the swerve modules. + * @return Effective trackwdtih in Meters + */ + public double getTrackwidth() + { + SwerveModuleConfiguration fr = SwerveMath.getSwerveModule(modules, true, false); + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fr.moduleLocation.getDistance(fl.moduleLocation); + } + + /** + * Get the tracklength of the swerve modules. + * @return Effective tracklength in Meters + */ + public double getTracklength() + { + SwerveModuleConfiguration br = SwerveMath.getSwerveModule(modules, false, false); + SwerveModuleConfiguration bl = SwerveMath.getSwerveModule(modules, false, true); + return br.moduleLocation.getDistance(bl.moduleLocation); + } + + /** + * Get the {@link DCMotor} corresponding to the first module's configuration. + * @return {@link DCMotor} of the drive motor. + */ + public DCMotor getDriveMotorSim() + { + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fl.driveMotor.getSimMotor(); + } + + /** + * Get the {@link DCMotor} corresponding to the first module configuration. + * @return {@link DCMotor} of the angle motor. + */ + public DCMotor getAngleMotorSim() + { + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fl.angleMotor.getSimMotor(); + } + + /** + * Get the gyro simulation for the robot. + * @return {@link GyroSimulation} gyro simulation. + */ + public Supplier getGyroSim() + { + if(imu instanceof Pigeon2Swerve) + return GyroSimulation.getPigeon2(); + else if (imu instanceof NavXSwerve) + return GyroSimulation.getNav2X(); + return GyroSimulation.getGeneric(); + } + } diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index e0b9f741..28635bd8 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -1,6 +1,5 @@ package swervelib.parser; -import swervelib.parser.json.MotorConfigDouble; import swervelib.parser.json.modules.ConversionFactorsJson; /** @@ -17,20 +16,32 @@ public class SwerveModulePhysicalCharacteristics * The time it takes for the motor to go from 0 to full throttle in seconds. */ public final double driveMotorRampRate, angleMotorRampRate; + /** + * The minimum voltage to spin the module or wheel. + */ + public final double driveFrictionVoltage, angleFrictionVoltage; /** * Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ - public final double wheelGripCoefficientOfFriction; + public final double wheelGripCoefficientOfFriction; + /** + * Steer rotational inertia in (KilogramSquareMeters) kg/m_sq. + */ + public final double steerRotationalInertia; + /** + * Robot mass in Kilograms. + */ + public final double robotMassKg; /** * The voltage to use for the smart motor voltage compensation. */ - public double optimalVoltage; + public double optimalVoltage; /** * The conversion factors for the drive and angle motors, created by * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. */ - public ConversionFactorsJson conversionFactor; + public ConversionFactorsJson conversionFactor; /** * Construct the swerve module physical characteristics. @@ -48,6 +59,10 @@ public class SwerveModulePhysicalCharacteristics * over drawing power from battery) * @param angleMotorRampRate The time in seconds to go from 0 to full throttle on the motor. (Prevents * overdrawing power and power loss). + * @param angleFrictionVoltage Angle motor minimum voltage. + * @param driveFrictionVoltage Drive motor minimum voltage. + * @param steerRotationalInertia Steering rotational inertia in KilogramSquareMeters. + * @param robotMassKg Robot mass in kG. */ public SwerveModulePhysicalCharacteristics( ConversionFactorsJson conversionFactors, @@ -56,7 +71,11 @@ public SwerveModulePhysicalCharacteristics( int driveMotorCurrentLimit, int angleMotorCurrentLimit, double driveMotorRampRate, - double angleMotorRampRate) + double angleMotorRampRate, + double driveFrictionVoltage, + double angleFrictionVoltage, + double steerRotationalInertia, + double robotMassKg) { this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; this.optimalVoltage = optimalVoltage; @@ -75,6 +94,10 @@ public SwerveModulePhysicalCharacteristics( this.angleMotorCurrentLimit = angleMotorCurrentLimit; this.driveMotorRampRate = driveMotorRampRate; this.angleMotorRampRate = angleMotorRampRate; + this.driveFrictionVoltage = driveFrictionVoltage; + this.angleFrictionVoltage = angleFrictionVoltage; + this.steerRotationalInertia = steerRotationalInertia; + this.robotMassKg = robotMassKg; } /** @@ -102,6 +125,10 @@ public SwerveModulePhysicalCharacteristics( 40, 20, driveMotorRampRate, - angleMotorRampRate); + angleMotorRampRate, + 0.2, + 0.3, + 0.03, + 50); } } diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 00e4e571..3ec1f7d7 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -1,5 +1,6 @@ package swervelib.parser.json; +import edu.wpi.first.units.Units; import swervelib.parser.SwerveModulePhysicalCharacteristics; import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.telemetry.Alert; @@ -16,6 +17,18 @@ public class PhysicalPropertiesJson */ @Deprecated(since = "2025", forRemoval = true) public MotorConfigDouble conversionFactor = new MotorConfigDouble(); + /** + * Minimum voltage to spin the module or wheel. + */ + public MotorConfigDouble friction = new MotorConfigDouble(0.3, 0.2); + /** + * Steer rotational inertia in KilogramMetersSquare. + */ + public double steerRotationalInertia = 0.03; + /** + * Robot mass in lb (pounds) + */ + public double robotMass = 110.2311; /** * Conversion Factors composition. Auto-calculates the conversion factors. */ @@ -63,7 +76,11 @@ public SwerveModulePhysicalCharacteristics createPhysicalProperties() currentLimit.drive, currentLimit.angle, rampRate.drive, - rampRate.angle); + rampRate.angle, + friction.drive, + friction.angle, + steerRotationalInertia, + Units.Pounds.of(robotMass).in(Units.Kilogram)); } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 2fb06f8f..5196011a 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; /** * Class to hold simulation data for {@link swervelib.SwerveModule} @@ -51,6 +52,7 @@ public SwerveModuleSimulation() dt = 0; } + /** * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function * when simulated. From abb1317b4836a5bc17e9bbdaa4596511adf4357f Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 20:33:44 -0600 Subject: [PATCH 17/54] Added maple-sim vendordep --- vendordeps/maple-sim.json | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 vendordeps/maple-sim.json diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 00000000..da468e37 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,25 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.1.6", + "frcYear": "2025", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.1.6" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From 15d3f1861ab3b788bf6c78492d6737873541f607 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 11 Nov 2024 20:34:07 -0600 Subject: [PATCH 18/54] Formatted java. --- src/main/java/frc/robot/RobotContainer.java | 4 +- .../robot/subsystems/swervedrive/Vision.java | 8 +-- src/main/java/swervelib/SwerveDrive.java | 12 ++-- src/main/java/swervelib/SwerveDriveTest.java | 22 +++--- src/main/java/swervelib/SwerveModule.java | 14 ++-- .../encoders/SparkMaxAnalogEncoderSwerve.java | 22 +++--- .../encoders/SparkMaxEncoderSwerve.java | 11 +-- src/main/java/swervelib/imu/NavXSwerve.java | 13 ++-- .../java/swervelib/imu/Pigeon2Swerve.java | 3 +- .../math/IMULinearMovingAverageFilter.java | 2 +- .../swervelib/motors/SparkFlexSwerve.java | 53 +++++++-------- .../motors/SparkMaxBrushedMotorSwerve.java | 67 ++++++++++--------- .../java/swervelib/motors/SwerveMotor.java | 1 + .../java/swervelib/motors/TalonFXSwerve.java | 24 +++---- .../java/swervelib/motors/TalonSRXSwerve.java | 14 ++-- .../parser/SwerveDriveConfiguration.java | 12 +++- .../parser/SwerveModuleConfiguration.java | 3 +- .../SwerveModulePhysicalCharacteristics.java | 10 +-- .../java/swervelib/parser/SwerveParser.java | 4 +- .../swervelib/parser/json/DeviceJson.java | 3 - .../swervelib/parser/json/ModuleJson.java | 4 +- .../modules/AngleConversionFactorsJson.java | 4 +- .../modules/DriveConversionFactorsJson.java | 4 +- .../simulation/SwerveModuleSimulation.java | 1 - 24 files changed, 153 insertions(+), 162 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfbc6b84..c3aae0ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -113,8 +113,8 @@ private void configureBindings() driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading)); driverXbox.b().whileTrue( drivebase.driveToPose( - new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) - ); + new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) + ); driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); driverXbox.start().whileTrue(Commands.none()); driverXbox.back().whileTrue(Commands.none()); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index cd36a6ef..70842484 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -377,6 +377,10 @@ enum Cameras * Standard deviation for multi-tag readings for pose estimation. */ private final Matrix multiTagStdDevs; + /** + * Transform of the camera rotation and translation relative to the center of the robot + */ + private final Transform3d robotToCamTransform; /** * Current standard deviations used. */ @@ -385,10 +389,6 @@ enum Cameras * Estimated robot pose. */ public Optional estimatedRobotPose; - /** - * Transform of the camera rotation and translation relative to the center of the robot - */ - private final Transform3d robotToCamTransform; /** * Simulated camera instance which only exists during simulations. */ diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index eddb4ede..dad0bd1b 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -91,15 +91,11 @@ public class SwerveDrive /** * Field object. */ - public Field2d field = new Field2d(); - /** - * MapleSim SwerveDrive. - */ - private SwerveDriveSimulation mapleSimDrive; + public Field2d field = new Field2d(); /** * Swerve controller for controlling heading of the robot. */ - public SwerveController swerveController; + public SwerveController swerveController; /** * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's * correction. @@ -128,6 +124,10 @@ public class SwerveDrive * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * MapleSim SwerveDrive. + */ + private SwerveDriveSimulation mapleSimDrive; /** * Amount of seconds the duration of the timestep the speeds should be applied for. */ diff --git a/src/main/java/swervelib/SwerveDriveTest.java b/src/main/java/swervelib/SwerveDriveTest.java index 64d77783..70405a3e 100644 --- a/src/main/java/swervelib/SwerveDriveTest.java +++ b/src/main/java/swervelib/SwerveDriveTest.java @@ -9,18 +9,11 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Units; -import edu.wpi.first.units.VoltageUnit; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; @@ -44,23 +37,23 @@ public class SwerveDriveTest /** * Tracks the voltage being applied to a motor */ - private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts); + private static final MutVoltage m_appliedVoltage = new MutVoltage(0, 0, Volts); /** * Tracks the distance travelled of a position motor */ - private static final MutDistance m_distance = new MutDistance(0,0, Meter); + private static final MutDistance m_distance = new MutDistance(0, 0, Meter); /** * Tracks the velocity of a positional motor */ - private static final MutLinearVelocity m_velocity = new MutLinearVelocity(0, 9, MetersPerSecond); + private static final MutLinearVelocity m_velocity = new MutLinearVelocity(0, 9, MetersPerSecond); /** * Tracks the rotations of an angular motor */ - private static final MutAngle m_anglePosition = new MutAngle(0,0, Degrees); + private static final MutAngle m_anglePosition = new MutAngle(0, 0, Degrees); /** * Tracks the velocity of an angular motor */ - private static final MutAngularVelocity m_angVelocity = new MutAngularVelocity(0,0, DegreesPerSecond); + private static final MutAngularVelocity m_angVelocity = new MutAngularVelocity(0, 0, DegreesPerSecond); /** * Set the angle of the modules to a given {@link Rotation2d} @@ -237,8 +230,9 @@ public static double findCouplingRatio(SwerveDrive swerveDrive, double volts, bo false); Timer.delay(60); } - double couplingRatio = (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive.factor) - - driveEncoderPositionRotations; + double couplingRatio = + (module.getDriveMotor().getPosition() / module.configuration.conversionFactors.drive.factor) - + driveEncoderPositionRotations; DriverStation.reportWarning(module.configuration.name + " Coupling Ratio: " + couplingRatio, false); couplingRatioSum += couplingRatio; } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 4753cae4..d481f8b7 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -47,7 +47,7 @@ public class SwerveModule /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ - public final int moduleNumber; + public final int moduleNumber; /** * Swerve Motors. */ @@ -99,7 +99,7 @@ public class SwerveModule /** * Anti-Jitter AKA auto-centering disabled. */ - private boolean antiJitterEnabled = true; + private boolean antiJitterEnabled = true; /** * Last swerve module state applied. */ @@ -115,15 +115,15 @@ public class SwerveModule /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; /** * Encoder, Absolute encoder synchronization enabled. */ - private boolean synchronizeEncoderEnabled = false; + private boolean synchronizeEncoderEnabled = false; /** * Encoder synchronization deadband in degrees. */ - private double synchronizeEncoderDeadband = 3; + private double synchronizeEncoderDeadband = 3; /** @@ -376,8 +376,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, // Cosine compensation. LinearVelocity velocity = configuration.useCosineCompensator - ? getCosineCompensatedVelocity(desiredState) - : MetersPerSecond.of(desiredState.speedMetersPerSecond); + ? getCosineCompensatedVelocity(desiredState) + : MetersPerSecond.of(desiredState.speedMetersPerSecond); if (isOpenLoop) { diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 84d35c8e..69a017d7 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -16,26 +16,26 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder { + /** + * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object. + */ + private final SwerveMotor sparkMax; /** * The {@link SparkAnalogSensor} representing the duty cycle encoder attached to the SparkMax analog port. */ - public SparkAnalogSensor encoder; + public SparkAnalogSensor encoder; /** * An {@link Alert} for if there is a failure configuring the encoder. */ - private Alert failureConfiguring; + private Alert failureConfiguring; /** * An {@link Alert} for if the absolute encoder does not support integrated offsets. */ - private Alert doesNotSupportIntegratedOffsets; - /** - * {@link swervelib.motors.SparkMaxSwerve} or {@link swervelib.motors.SparkMaxBrushedMotorSwerve} object. - */ - private final SwerveMotor sparkMax; + private Alert doesNotSupportIntegratedOffsets; /** - * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data - * port analog pin. + * Create the {@link SparkMaxAnalogEncoderSwerve} object as a analog sensor from the {@link SparkMax} motor data port + * analog pin. * * @param motor Motor to create the encoder from. * @param maxVoltage Maximum voltage for analog input reading. @@ -47,7 +47,7 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) sparkMax = motor; encoder = ((SparkMax) motor.getMotor()).getAnalog(); motor.setAbsoluteEncoder(this); - sparkMax.configureIntegratedEncoder(360/maxVoltage); + sparkMax.configureIntegratedEncoder(360 / maxVoltage); } else { throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax"); @@ -106,7 +106,7 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - if(sparkMax instanceof SparkMaxSwerve) + if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); cfg.analogSensor.inverted(true); diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index 3e90d593..c86c4297 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -28,14 +28,15 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder /** * An {@link Alert} for if there is a failure configuring the encoder offset. */ - private Alert offsetFailure; + private Alert offsetFailure; /** * {@link SparkMaxBrushedMotorSwerve} or {@link SparkMaxSwerve} instance. */ - private SwerveMotor sparkMax; + private SwerveMotor sparkMax; /** - * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax} motor. + * Create the {@link SparkMaxEncoderSwerve} object as a duty cycle from the {@link com.revrobotics.spark.SparkMax} + * motor. * * @param motor Motor to create the encoder from. * @param conversionFactor The conversion factor to set if the output is not from 0 to 360. @@ -105,7 +106,7 @@ public void clearStickyFaults() @Override public void configure(boolean inverted) { - if(sparkMax instanceof SparkMaxSwerve) + if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); cfg.analogSensor.inverted(true); @@ -149,7 +150,7 @@ public Object getAbsoluteEncoder() @Override public boolean setAbsoluteEncoderOffset(double offset) { - if(sparkMax instanceof SparkMaxSwerve) + if (sparkMax instanceof SparkMaxSwerve) { SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig(); cfg.absoluteEncoder.zeroOffset(offset); diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index 5343f9a8..d1854e66 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -4,10 +4,6 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; import swervelib.telemetry.Alert; @@ -45,10 +41,10 @@ public NavXSwerve(NavXComType port) try { AHRS - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - imu = new AHRS(port); + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + imu = new AHRS(port); factoryDefault(); } catch (RuntimeException ex) { @@ -58,7 +54,6 @@ public NavXSwerve(NavXComType port) } - /** * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be * too slow. diff --git a/src/main/java/swervelib/imu/Pigeon2Swerve.java b/src/main/java/swervelib/imu/Pigeon2Swerve.java index 09b71b83..fc0a113c 100644 --- a/src/main/java/swervelib/imu/Pigeon2Swerve.java +++ b/src/main/java/swervelib/imu/Pigeon2Swerve.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; @@ -48,7 +47,7 @@ public class Pigeon2Swerve extends SwerveIMU /** * Y Accelleration supplier. */ - private Supplier> yAcc; + private Supplier> yAcc; /** * Z Acceleration supplier. */ diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java index 09509b75..9ac08efb 100644 --- a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java +++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java @@ -15,7 +15,7 @@ public class IMULinearMovingAverageFilter /** * Gain on each reading. */ - private final double m_inputGain; + private final double m_inputGain; /** * Construct a linear moving average fitler diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 2f60ad58..ee62d1c0 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -3,17 +3,14 @@ import static edu.wpi.first.units.Units.Seconds; import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkLowLevel.PeriodicFrame; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.REVLibError; -import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; @@ -22,7 +19,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; -import swervelib.encoders.SparkMaxAnalogEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; @@ -37,19 +33,19 @@ public class SparkFlexSwerve extends SwerveMotor /** * {@link SparkFlex} Instance. */ - private final SparkFlex motor; + private final SparkFlex motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Closed-loop PID controller. */ - public SparkClosedLoopController pid; + public SparkClosedLoopController pid; /** * Supplier for the velocity of the motor controller. */ @@ -61,23 +57,23 @@ public class SparkFlexSwerve extends SwerveMotor /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguring; + private Alert failureConfiguring; /** * An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client. */ - private Alert absoluteEncoderOffsetWarning; + private Alert absoluteEncoderOffsetWarning; /** * Configuration object for {@link SparkFlex} motor. */ - private SparkFlexConfig cfg = new SparkFlexConfig(); + private SparkFlexConfig cfg = new SparkFlexConfig(); /** * Tracker for changes that need to be pushed. */ - private boolean cfgUpdated = false; + private boolean cfgUpdated = false; /** @@ -264,7 +260,8 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) { if (encoder == null) { - absoluteEncoder = null;cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + absoluteEncoder = null; + cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); cfgUpdated = true; velocity = this.encoder::getVelocity; @@ -450,7 +447,7 @@ public void setReference(double setpoint, double feedforward) { int pidSlot = 0; - if(cfgUpdated) + if (cfgUpdated) { burnFlash(); Timer.delay(0.1); // Give 100ms to apply changes @@ -459,19 +456,19 @@ public void setReference(double setpoint, double feedforward) if (isDriveMotor) { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kVelocity, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kVelocity, + pidSlot, + feedforward)); } else { configureSparkFlex(() -> - pid.setReference( - setpoint, - ControlType.kPosition, - pidSlot, - feedforward)); + pid.setReference( + setpoint, + ControlType.kPosition, + pidSlot, + feedforward)); if (SwerveDriveTelemetry.isSimulation) { encoder.setPosition(setpoint); diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index fe19892f..ab653b25 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -35,61 +35,51 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * SparkMAX Instance. */ - private final SparkMax motor; + private final SparkMax motor; /** - * Supplier for the velocity of the motor controller. + * Absolute encoder attached to the SparkMax (if exists) */ - private Supplier velocity; + public SwerveAbsoluteEncoder absoluteEncoder; /** - * Supplier for the position of the motor controller. + * Integrated encoder. */ - private Supplier position; + public RelativeEncoder encoder; /** - * Absolute encoder attached to the SparkMax (if exists) + * Closed-loop PID controller. */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SparkClosedLoopController pid; /** - * Integrated encoder. + * Supplier for the velocity of the motor controller. */ - public RelativeEncoder encoder; + private Supplier velocity; /** - * Closed-loop PID controller. + * Supplier for the position of the motor controller. */ - public SparkClosedLoopController pid; + private Supplier position; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * An {@link Alert} for if the motor has no encoder. */ - private Alert noEncoderAlert; + private Alert noEncoderAlert; /** * An {@link Alert} for if there is an error configuring the motor. */ - private Alert failureConfiguringAlert; + private Alert failureConfiguringAlert; /** * An {@link Alert} for if the motor has no encoder defined. */ - private Alert noEncoderDefinedAlert; + private Alert noEncoderDefinedAlert; /** * Configuration object for {@link SparkMax} motor. */ - private SparkMaxConfig cfg = new SparkMaxConfig(); + private SparkMaxConfig cfg = new SparkMaxConfig(); /** * Tracker for changes that need to be pushed. */ - private boolean cfgUpdated = false; - - /** - * Type for encoder for {@link SparkMax} - */ - public enum Type - { - kNoSensor, /** NO sensor */ - kHallSensor, /** Hall sensor attached to dataport */ - kQuadrature, /** Quad encoder attached to alt */ - } + private boolean cfgUpdated = false; /** * Initialize the swerve motor. @@ -139,15 +129,14 @@ public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type enc if (isDriveMotor || (encoderType != Type.kNoSensor || useDataPortQuadEncoder)) { - if(useDataPortQuadEncoder) + if (useDataPortQuadEncoder) { this.encoder = motor.getAlternateEncoder(); cfg.alternateEncoder.countsPerRevolution(countsPerRevolution); // Configure feedback of the PID controller as the integrated encoder. cfg.closedLoop.feedbackSensor(FeedbackSensor.kAlternateOrExternalEncoder); - } - else + } else { this.encoder = motor.getEncoder(); cfg.encoder.countsPerRevolution(countsPerRevolution); @@ -526,7 +515,7 @@ public void setReference(double setpoint, double feedforward) { int pidSlot = 0; - if(cfgUpdated) + if (cfgUpdated) { burnFlash(); Timer.delay(0.1); // Give 100ms to apply changes @@ -636,4 +625,20 @@ public void setPosition(double position) configureSparkMax(() -> encoder.setPosition(position)); } } + + /** + * Type for encoder for {@link SparkMax} + */ + public enum Type + { + kNoSensor, + /** + * NO sensor + */ + kHallSensor, + /** + * Hall sensor attached to dataport + */ + kQuadrature, /** Quad encoder attached to alt */ + } } diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index 38861795..bae6d2f4 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -175,6 +175,7 @@ public abstract class SwerveMotor /** * Get the {@link DCMotor} of the motor class. + * * @return {@link DCMotor} of this type. */ public abstract DCMotor getSimMotor(); diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index d7392c24..635df8ad 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -1,9 +1,7 @@ package swervelib.motors; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -15,7 +13,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.units.measure.Voltage; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.SwerveDriveTelemetry; @@ -29,39 +26,39 @@ public class TalonFXSwerve extends SwerveMotor /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.02; + public static double STATUS_TIMEOUT_SECONDS = 0.02; /** * Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; + private final boolean factoryDefaultOccurred = false; /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private final boolean absoluteEncoder = false; /** * Motion magic angle voltage setter. */ - private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); + private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0); /** * Velocity voltage setter for controlling drive motor. */ - private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); + private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0); /** * TalonFX motor controller. */ - private final TalonFX motor; + private final TalonFX motor; /** * Conversion factor for the motor. */ - private double conversionFactor; + private double conversionFactor; /** * Current TalonFX configuration. */ - private TalonFXConfiguration configuration = new TalonFXConfiguration(); + private TalonFXConfiguration configuration = new TalonFXConfiguration(); /** * Current TalonFX Configurator. */ - private TalonFXConfigurator cfg; + private TalonFXConfigurator cfg; /** @@ -244,7 +241,8 @@ public void setInverted(boolean inverted) { // Timer.delay(1); cfg.refresh(configuration.MotorOutput); - configuration.MotorOutput.withInverted(inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive); + configuration.MotorOutput.withInverted( + inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive); cfg.apply(configuration.MotorOutput); } diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 28a6359f..28d2e356 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -22,31 +22,31 @@ public class TalonSRXSwerve extends SwerveMotor /** * Factory default already occurred. */ - private final boolean factoryDefaultOccurred = false; + private final boolean factoryDefaultOccurred = false; /** * Current TalonFX configuration. */ - private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); + private final TalonSRXConfiguration configuration = new TalonSRXConfiguration(); /** * Whether the absolute encoder is integrated. */ - private final boolean absoluteEncoder = false; + private final boolean absoluteEncoder = false; /** * TalonSRX motor controller. */ - private final WPI_TalonSRX motor; + private final WPI_TalonSRX motor; /** * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees. */ - private double positionConversionFactor = 1; + private double positionConversionFactor = 1; /** * If the TalonFX configuration has changed. */ - private boolean configChanged = true; + private boolean configChanged = true; /** * Nominal voltage default to use with feedforward. */ - private double nominalVoltage = 12.0; + private double nominalVoltage = 12.0; /** * Constructor for TalonSRX swerve motor. diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java index 2a2f31cb..9a6eebb4 100644 --- a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java @@ -105,6 +105,7 @@ public double getDriveBaseRadiusMeters() /** * Get the trackwidth of the swerve modules. + * * @return Effective trackwdtih in Meters */ public double getTrackwidth() @@ -116,6 +117,7 @@ public double getTrackwidth() /** * Get the tracklength of the swerve modules. + * * @return Effective tracklength in Meters */ public double getTracklength() @@ -127,6 +129,7 @@ public double getTracklength() /** * Get the {@link DCMotor} corresponding to the first module's configuration. + * * @return {@link DCMotor} of the drive motor. */ public DCMotor getDriveMotorSim() @@ -137,6 +140,7 @@ public DCMotor getDriveMotorSim() /** * Get the {@link DCMotor} corresponding to the first module configuration. + * * @return {@link DCMotor} of the angle motor. */ public DCMotor getAngleMotorSim() @@ -147,14 +151,18 @@ public DCMotor getAngleMotorSim() /** * Get the gyro simulation for the robot. + * * @return {@link GyroSimulation} gyro simulation. */ public Supplier getGyroSim() { - if(imu instanceof Pigeon2Swerve) + if (imu instanceof Pigeon2Swerve) + { return GyroSimulation.getPigeon2(); - else if (imu instanceof NavXSwerve) + } else if (imu instanceof NavXSwerve) + { return GyroSimulation.getNav2X(); + } return GyroSimulation.getGeneric(); } diff --git a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java index e88bc02d..8996f448 100644 --- a/src/main/java/swervelib/parser/SwerveModuleConfiguration.java +++ b/src/main/java/swervelib/parser/SwerveModuleConfiguration.java @@ -3,7 +3,6 @@ import edu.wpi.first.math.geometry.Translation2d; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.motors.SwerveMotor; -import swervelib.parser.json.MotorConfigDouble; import swervelib.parser.json.modules.ConversionFactorsJson; /** @@ -18,7 +17,7 @@ public class SwerveModuleConfiguration * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to calculate the * conversion factors. */ - public final ConversionFactorsJson conversionFactors; + public final ConversionFactorsJson conversionFactors; /** * Angle offset in degrees for the Swerve Module. */ diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 28635bd8..a626bbb8 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -23,7 +23,7 @@ public class SwerveModulePhysicalCharacteristics /** * Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ - public final double wheelGripCoefficientOfFriction; + public final double wheelGripCoefficientOfFriction; /** * Steer rotational inertia in (KilogramSquareMeters) kg/m_sq. */ @@ -31,17 +31,17 @@ public class SwerveModulePhysicalCharacteristics /** * Robot mass in Kilograms. */ - public final double robotMassKg; + public final double robotMassKg; /** * The voltage to use for the smart motor voltage compensation. */ - public double optimalVoltage; + public double optimalVoltage; /** * The conversion factors for the drive and angle motors, created by * {@link swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and * {@link swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. */ - public ConversionFactorsJson conversionFactor; + public ConversionFactorsJson conversionFactor; /** * Construct the swerve module physical characteristics. @@ -62,7 +62,7 @@ public class SwerveModulePhysicalCharacteristics * @param angleFrictionVoltage Angle motor minimum voltage. * @param driveFrictionVoltage Drive motor minimum voltage. * @param steerRotationalInertia Steering rotational inertia in KilogramSquareMeters. - * @param robotMassKg Robot mass in kG. + * @param robotMassKg Robot mass in kG. */ public SwerveModulePhysicalCharacteristics( ConversionFactorsJson conversionFactors, diff --git a/src/main/java/swervelib/parser/SwerveParser.java b/src/main/java/swervelib/parser/SwerveParser.java index 340f9c99..aa87c8e8 100644 --- a/src/main/java/swervelib/parser/SwerveParser.java +++ b/src/main/java/swervelib/parser/SwerveParser.java @@ -241,6 +241,8 @@ public SwerveDrive createSwerveDrive(SimpleMotorFeedforward driveFeedforward, do return new SwerveDrive( swerveDriveConfiguration, - controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), maxSpeed, initialPose); + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed, + initialPose); } } diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 5ed1907a..3ecd1c84 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -6,9 +6,6 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.SerialPort.Port; import swervelib.encoders.AnalogAbsoluteEncoderSwerve; import swervelib.encoders.CANCoderSwerve; import swervelib.encoders.CanAndMagSwerve; diff --git a/src/main/java/swervelib/parser/json/ModuleJson.java b/src/main/java/swervelib/parser/json/ModuleJson.java index 8029635e..f26b2e84 100644 --- a/src/main/java/swervelib/parser/json/ModuleJson.java +++ b/src/main/java/swervelib/parser/json/ModuleJson.java @@ -90,9 +90,9 @@ public SwerveModuleConfiguration createModuleConfiguration( // If both are defined, override 0 with the physical characterstics input. { conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle - : conversionFactors.angle; + : conversionFactors.angle; conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive - : conversionFactors.drive; + : conversionFactors.drive; } if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty()) diff --git a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java index 54567fe4..5a3e10f9 100644 --- a/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/AngleConversionFactorsJson.java @@ -1,8 +1,6 @@ package swervelib.parser.json.modules; import swervelib.math.SwerveMath; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; /** * Angle motor conversion factors composite JSON parse class. @@ -17,7 +15,7 @@ public class AngleConversionFactorsJson /** * Calculated or given conversion factor. */ - public double factor = 0; + public double factor = 0; /** * Calculate the drive conversion factor. diff --git a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java index 3cd89065..937b78a1 100644 --- a/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java +++ b/src/main/java/swervelib/parser/json/modules/DriveConversionFactorsJson.java @@ -2,8 +2,6 @@ import edu.wpi.first.math.util.Units; import swervelib.math.SwerveMath; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; /** * Drive motor composite JSON parse class. @@ -22,7 +20,7 @@ public class DriveConversionFactorsJson /** * Calculated conversion factor. */ - public double factor = 0; + public double factor = 0; /** * Calculate the drive conversion factor. diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 5196011a..f6f2d16e 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; /** * Class to hold simulation data for {@link swervelib.SwerveModule} From c73402fd3fb621328eeeb04886e3949976cba840 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 12 Nov 2024 11:04:27 -0600 Subject: [PATCH 19/54] Updated pathplanner. --- .../deploy/pathplanner/autos/New Auto.auto | 10 ++------- .../deploy/pathplanner/paths/New Path.path | 15 ++++++++----- src/main/deploy/pathplanner/settings.json | 21 +++++++++++++++++++ 3 files changed, 33 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 7fc8be27..268147bb 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -1,12 +1,5 @@ { - "version": 1.0, - "startingPose": { - "position": { - "x": 2.0, - "y": 7.0 - }, - "rotation": 0 - }, + "version": "2025.0", "command": { "type": "sequential", "data": { @@ -20,6 +13,7 @@ ] } }, + "resetOdom": true, "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 2012ffc1..708478f2 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -1,5 +1,5 @@ { - "version": 1.0, + "version": "2025.0", "waypoints": [ { "anchor": { @@ -46,20 +46,25 @@ ], "rotationTargets": [], "constraintZones": [], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0, - "rotateFast": false + "rotation": 0.0 }, "reversed": false, "folder": null, - "previewStartingState": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 00000000..cb761c64 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,21 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotWheelbase": 0.546, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "NEO", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2 +} \ No newline at end of file From 2feefa4beada4eba6b6da1fd8ea856e33ebfb645 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 12 Nov 2024 11:31:24 -0600 Subject: [PATCH 20/54] Added maplesim test. Updated to WPILib Alert types. --- .../robot/subsystems/swervedrive/Vision.java | 6 +- src/main/java/swervelib/SwerveDrive.java | 14 +- src/main/java/swervelib/SwerveModule.java | 7 +- .../encoders/AnalogAbsoluteEncoderSwerve.java | 7 +- .../swervelib/encoders/CANCoderSwerve.java | 11 +- .../encoders/PWMDutyCycleEncoderSwerve.java | 5 +- .../encoders/SparkMaxAnalogEncoderSwerve.java | 7 +- .../encoders/SparkMaxEncoderSwerve.java | 7 +- src/main/java/swervelib/imu/NavXSwerve.java | 5 +- .../swervelib/motors/SparkFlexSwerve.java | 7 +- .../motors/SparkMaxBrushedMotorSwerve.java | 9 +- .../parser/json/PhysicalPropertiesJson.java | 6 +- src/main/java/swervelib/telemetry/Alert.java | 237 ------------------ .../telemetry/SwerveDriveTelemetry.java | 9 +- 14 files changed, 59 insertions(+), 278 deletions(-) delete mode 100644 src/main/java/swervelib/telemetry/Alert.java diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 70842484..a6663686 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -19,6 +19,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTablesJNI; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.robot.Robot; import java.awt.Desktop; @@ -37,8 +39,6 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import swervelib.SwerveDrive; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; import swervelib.telemetry.SwerveDriveTelemetry; @@ -415,7 +415,7 @@ enum Cameras Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation, Matrix singleTagStdDevs, Matrix multiTagStdDevsMatrix) { - latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.WARNING); + latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); camera = new PhotonCamera(name); diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index dad0bd1b..9845eefc 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -18,6 +18,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -29,6 +31,7 @@ import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; @@ -42,8 +45,6 @@ import swervelib.parser.SwerveControllerConfiguration; import swervelib.parser.SwerveDriveConfiguration; import swervelib.simulation.SwerveIMUSimulation; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -87,7 +88,7 @@ public class SwerveDrive private final Alert tunerXRecommendation = new Alert("Swerve Drive", "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" + "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html", - AlertType.WARNING); + AlertType.kWarning); /** * Field object. */ @@ -201,6 +202,8 @@ public SwerveDrive( { simIMU = new SwerveIMUSimulation(); imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); + + // Setup MapleSim SwerveModuleSimulation simModule = new SwerveModuleSimulation(config.getDriveMotorSim(), config.getAngleMotorSim(), config.physicalCharacteristics.driveMotorCurrentLimit, @@ -213,7 +216,9 @@ public SwerveDrive( 2, config.physicalCharacteristics.steerRotationalInertia); DriveTrainSimulationConfig simCfg = new DriveTrainSimulationConfig(config.physicalCharacteristics.robotMassKg, + config.getTracklength() + Units.inchesToMeters(5), + config.getTrackwidth() + Units.inchesToMeters(5), config.getTracklength(), config.getTrackwidth(), @@ -222,6 +227,8 @@ public SwerveDrive( }, config.getGyroSim()); mapleSimDrive = new SwerveDriveSimulation(simCfg, startingPose); + // register the drivetrain simulation + SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); } else { imu = config.imu; @@ -1036,6 +1043,7 @@ public void updateOdometry() Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); if (SwerveDriveTelemetry.isSimulation) { + SimulatedArena.getInstance().simulationPeriodic(); simIMU.updateOdometry( kinematics, getStates(), diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index d481f8b7..df865ae8 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -7,6 +7,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; @@ -18,7 +20,6 @@ import swervelib.parser.PIDFConfig; import swervelib.parser.SwerveModuleConfiguration; import swervelib.simulation.SwerveModuleSimulation; -import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -214,11 +215,11 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat noEncoderWarning = new Alert("Motors", "There is no Absolute Encoder on module #" + moduleNumber, - Alert.AlertType.WARNING); + AlertType.kWarning); encoderOffsetWarning = new Alert("Motors", "Pushing the Absolute Encoder offset to the encoder failed on module #" + moduleNumber, - Alert.AlertType.WARNING); + AlertType.kWarning); rawAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Raw Absolute Encoder"; adjAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder"; diff --git a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java index fbd8214f..11016f65 100644 --- a/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/AnalogAbsoluteEncoderSwerve.java @@ -1,8 +1,9 @@ package swervelib.encoders; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; -import swervelib.telemetry.Alert; /** * Swerve Absolute Encoder for Thrifty Encoders and other analog encoders. @@ -39,11 +40,11 @@ public AnalogAbsoluteEncoderSwerve(AnalogInput encoder) cannotSetOffset = new Alert( "Encoders", "Cannot Set Absolute Encoder Offset of Analog Encoders Channel #" + encoder.getChannel(), - Alert.AlertType.WARNING); + AlertType.kWarning); inaccurateVelocities = new Alert( "Encoders", "The Analog Absolute encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); } /** diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index da95e9da..499bf857 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -13,7 +13,8 @@ import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.units.measure.Angle; -import swervelib.telemetry.Alert; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; /** * Swerve Absolute Encoder for CTRE CANCoders. @@ -69,21 +70,21 @@ public CANCoderSwerve(int id, String canbus) magnetFieldLessThanIdeal = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.", - Alert.AlertType.WARNING); + AlertType.kWarning); readingFaulty = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " reading was faulty.", - Alert.AlertType.WARNING); + AlertType.kWarning); readingIgnored = new Alert( "Encoders", "CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.", - Alert.AlertType.WARNING); + AlertType.kWarning); cannotSetOffset = new Alert( "Encoders", "Failure to set CANCoder " + encoder.getDeviceID() + " Absolute Encoder Offset", - Alert.AlertType.WARNING); + AlertType.kWarning); } /** diff --git a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 1b8bb6b8..52270951 100644 --- a/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -1,7 +1,8 @@ package swervelib.encoders; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DutyCycleEncoder; -import swervelib.telemetry.Alert; /** * DutyCycle encoders such as "US Digital MA3 with PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag @@ -42,7 +43,7 @@ public PWMDutyCycleEncoderSwerve(int pin) inaccurateVelocities = new Alert( "Encoders", "The PWM Duty Cycle encoder may not report accurate velocities!", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); } diff --git a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java index 69a017d7..aa88efcf 100644 --- a/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxAnalogEncoderSwerve.java @@ -4,11 +4,12 @@ import com.revrobotics.spark.SparkAnalogSensor; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import java.util.function.Supplier; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; -import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port analog pin. @@ -55,11 +56,11 @@ public SparkMaxAnalogEncoderSwerve(SwerveMotor motor, double maxVoltage) failureConfiguring = new Alert( "Encoders", "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); doesNotSupportIntegratedOffsets = new Alert( "Encoders", "SparkMax Analog Sensors do not support integrated offsets", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); } diff --git a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java index c86c4297..b01a39f1 100644 --- a/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java +++ b/src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java @@ -5,11 +5,12 @@ import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import java.util.function.Supplier; import swervelib.motors.SparkMaxBrushedMotorSwerve; import swervelib.motors.SparkMaxSwerve; import swervelib.motors.SwerveMotor; -import swervelib.telemetry.Alert; /** * SparkMax absolute encoder, attached through the data port. @@ -46,11 +47,11 @@ public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) failureConfiguring = new Alert( "Encoders", "Failure configuring SparkMax Analog Encoder", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); offsetFailure = new Alert( "Encoders", "Failure to set Absolute Encoder Offset", - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); if (motor.getMotor() instanceof SparkMax) { sparkMax = motor; diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index d1854e66..bd14e344 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -4,8 +4,9 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import java.util.Optional; -import swervelib.telemetry.Alert; /** * Communicates with the NavX({@link AHRS}) as the IMU. @@ -37,7 +38,7 @@ public class NavXSwerve extends SwerveIMU */ public NavXSwerve(NavXComType port) { - navXError = new Alert("IMU", "Error instantiating NavX.", Alert.AlertType.ERROR_TRACE); + navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError); try { AHRS diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index ee62d1c0..91c49dac 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -17,11 +17,12 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -99,11 +100,11 @@ public SparkFlexSwerve(SparkFlex motor, boolean isDriveMotor) failureConfiguring = new Alert("Motors", "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); absoluteEncoderOffsetWarning = new Alert("Motors", "IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " + "absoluteEncoderOffset in the Swerve Module JSON!", - Alert.AlertType.WARNING); + AlertType.kWarning); velocity = encoder::getVelocity; position = encoder::getPosition; } diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index ab653b25..31a1a42a 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -16,6 +16,8 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; @@ -23,7 +25,6 @@ import swervelib.encoders.SparkMaxEncoderSwerve; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; -import swervelib.telemetry.Alert; import swervelib.telemetry.SwerveDriveTelemetry; /** @@ -95,13 +96,13 @@ public SparkMaxBrushedMotorSwerve(SparkMax motor, boolean isDriveMotor, Type enc { noEncoderAlert = new Alert("Motors", "Cannot use motor without encoder.", - Alert.AlertType.ERROR_TRACE); + AlertType.kError); failureConfiguringAlert = new Alert("Motors", "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); + AlertType.kWarning); noEncoderDefinedAlert = new Alert("Motors", "An encoder MUST be defined to work with a SparkMAX", - Alert.AlertType.ERROR_TRACE); + AlertType.kError); // Drive motors **MUST** have an encoder attached. if (isDriveMotor && encoderType == Type.kNoSensor) diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 3ec1f7d7..eaab95d6 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -1,10 +1,10 @@ package swervelib.parser.json; import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import swervelib.parser.SwerveModulePhysicalCharacteristics; import swervelib.parser.json.modules.ConversionFactorsJson; -import swervelib.telemetry.Alert; -import swervelib.telemetry.Alert.AlertType; /** * {@link swervelib.parser.SwerveModulePhysicalCharacteristics} parsed data. Used to configure the SwerveModule. @@ -66,7 +66,7 @@ public SwerveModulePhysicalCharacteristics createPhysicalProperties() "} \nis deprecated, please use\n" + "'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " + conversionFactor.angle + "} }", - AlertType.ERROR_TRACE).set(true); + AlertType.kError).set(true); } return new SwerveModulePhysicalCharacteristics( diff --git a/src/main/java/swervelib/telemetry/Alert.java b/src/main/java/swervelib/telemetry/Alert.java deleted file mode 100644 index 40f9af78..00000000 --- a/src/main/java/swervelib/telemetry/Alert.java +++ /dev/null @@ -1,237 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found below. - -// MIT License -// -// Copyright (c) 2023 FRC 6328 -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -package swervelib.telemetry; - -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.List; -import java.util.Map; - -/** - * Class for managing persistent alerts to be sent over NetworkTables. - */ -public class Alert -{ - - /** - * Group of the alert. - */ - private static Map groups = new HashMap(); - /** - * Type of the Alert to raise. - */ - private final AlertType type; - /** - * Activation state of alert. - */ - private boolean active = false; - /** - * When the alert was raised. - */ - private double activeStartTime = 0.0; - /** - * Text of the alert. - */ - private String text; - - /** - * Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated, the appropriate - * entries will be added to NetworkTables. - * - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String text, AlertType type) - { - this("Alerts", text, type); - } - - /** - * Creates a new Alert. If this is the first to be instantiated in its group, the appropriate entries will be added to - * NetworkTables. - * - * @param group Group identifier, also used as NetworkTables title - * @param text Text to be displayed when the alert is active. - * @param type Alert level specifying urgency. - */ - public Alert(String group, String text, AlertType type) - { - if (!groups.containsKey(group)) - { - groups.put(group, new SendableAlerts()); - SmartDashboard.putData(group, groups.get(group)); - } - - this.text = text; - this.type = type; - groups.get(group).alerts.add(this); - } - - /** - * Sets whether the alert should currently be displayed. When activated, the alert text will also be sent to the - * console. - * - * @param active Set the alert as active and report it to the driver station. - */ - public void set(boolean active) - { - if (active && !this.active) - { - activeStartTime = Timer.getFPGATimestamp(); - printAlert(text); - } - this.active = active; - } - - /** - * Updates current alert text. - * - * @param text The text for the alert. - */ - public void setText(String text) - { - if (active && !text.equals(this.text)) - { - printAlert(text); - } - this.text = text; - } - - - /** - * Print the alert message. - * - * @param text Text to print. - */ - private void printAlert(String text) - { - switch (type) - { - case ERROR: - DriverStation.reportError(text, false); - break; - case ERROR_TRACE: - DriverStation.reportError(text, true); - break; - case WARNING: - DriverStation.reportWarning(text, false); - break; - case WARNING_TRACE: - DriverStation.reportWarning(text, true); - break; - case INFO: - System.out.println(text); - break; - } - } - - /** - * Represents an alert's level of urgency. - */ - public static enum AlertType - { - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which - * will seriously affect the robot's functionality and thus require immediate attention. - */ - ERROR, - /** - * High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type for problems which - * will seriously affect the robot's functionality and thus require immediate attention. Trace printed to driver - * station console. - */ - ERROR_TRACE, - - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems - * which could affect the robot's functionality but do not necessarily require immediate attention. - */ - WARNING, - /** - * Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this type for problems - * which could affect the robot's functionality but do not necessarily require immediate attention. Trace printed to - * driver station console. - */ - WARNING_TRACE, - /** - * Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type for problems which - * are unlikely to affect the robot's functionality, or any other alerts which do not fall under "ERROR" or - * "WARNING". - */ - INFO - } - - /** - * Sendable alert for advantage scope. - */ - private static class SendableAlerts implements Sendable - { - - /** - * Alert list for sendable. - */ - public final List alerts = new ArrayList<>(); - - /** - * Get alerts based off of type. - * - * @param type Type of alert to fetch. - * @return Active alert strings. - */ - public String[] getStrings(AlertType type) - { - List alertStrings = new ArrayList<>(); - for (Alert alert : alerts) - { - if (alert.type == type && alert.active) - { - alertStrings.add(alert.text); - } - } - // alertStrings.sort((a1, a2) -> (int) (a2.activeStartTime - a1.activeStartTime)); - return alertStrings.toArray(new String[alertStrings.size()]); - } - - @Override - public void initSendable(SendableBuilder builder) - { - builder.setSmartDashboardType("Alerts"); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null); - builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR_TRACE), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null); - builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING_TRACE), null); - builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null); - } - } -} \ No newline at end of file diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 3c90fc6f..be60dd47 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -1,8 +1,9 @@ package swervelib.telemetry; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import swervelib.telemetry.Alert.AlertType; /** * Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit) @@ -15,19 +16,19 @@ public class SwerveDriveTelemetry */ public static final Alert canIdWarning = new Alert("JSON", "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - Alert.AlertType.WARNING); + AlertType.kWarning); /** * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. */ public static final Alert i2cLockupWarning = new Alert("IMU", "I2C lockup issue detected on roboRIO. Check console for more information.", - Alert.AlertType.WARNING); + AlertType.kWarning); /** * NavX serial comm issue. */ public static final Alert serialCommsIssueWarning = new Alert("IMU", "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.", - AlertType.WARNING); + AlertType.kWarning); /** * The current telemetry verbosity level. */ From b35fbccd48729be013b5d9cbdd83d77e5e4890a2 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 12 Nov 2024 19:55:36 -0600 Subject: [PATCH 21/54] Added simulation check for Cache. Added AdvantageScope support for 2025. --- src/main/java/swervelib/SwerveDrive.java | 7 + src/main/java/swervelib/SwerveModule.java | 1 + src/main/java/swervelib/parser/Cache.java | 3 +- .../telemetry/SwerveDriveTelemetry.java | 126 ++++++++++++++---- 4 files changed, 112 insertions(+), 25 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 9845eefc..d9971e4d 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -280,6 +280,8 @@ public SwerveDrive( } SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + SwerveDriveTelemetry.desiredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; + SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; } odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); @@ -569,6 +571,7 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); + SwerveDriveTelemetry.desiredChassisSpeedsObj = velocity; } // Calculate required module states via kinematics @@ -983,6 +986,7 @@ public void lockPose() desiredState.angle.getDegrees(); SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = desiredState.speedMetersPerSecond; + SwerveDriveTelemetry.desiredStatesObj[swerveModule.moduleNumber] = desiredState; } swerveModule.setDesiredState(desiredState, false, true); @@ -1055,7 +1059,9 @@ public void updateOdometry() SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); + SwerveDriveTelemetry.measuredChassisSpeedsObj = measuredChassisSpeeds; SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); + SwerveDriveTelemetry.robotRotationObj = getOdometryHeading(); } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) @@ -1078,6 +1084,7 @@ public void updateOdometry() { SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; + SwerveDriveTelemetry.measuredStatesObj[module.moduleNumber] = moduleState; } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index df865ae8..c6082eda 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -418,6 +418,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, { SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees(); SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity.magnitude(); + SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState; } if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) diff --git a/src/main/java/swervelib/parser/Cache.java b/src/main/java/swervelib/parser/Cache.java index 830a08c0..830a2731 100644 --- a/src/main/java/swervelib/parser/Cache.java +++ b/src/main/java/swervelib/parser/Cache.java @@ -1,5 +1,6 @@ package swervelib.parser; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; import java.util.function.Supplier; @@ -95,7 +96,7 @@ public Cache updateValidityPeriod(long validityPeriod) */ public T getValue() { - if (isStale()) + if (isStale() || RobotBase.isSimulation()) { update(); } diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index be60dd47..53658fc0 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -1,5 +1,11 @@ package swervelib.telemetry; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; @@ -14,84 +20,150 @@ public class SwerveDriveTelemetry /** * An {@link Alert} for if the CAN ID is greater than 40. */ - public static final Alert canIdWarning = new Alert("JSON", - "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", - AlertType.kWarning); + public static final Alert canIdWarning = new Alert("JSON", + "CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!", + AlertType.kWarning); /** * An {@link Alert} for if there is an I2C lockup issue on the roboRIO. */ - public static final Alert i2cLockupWarning = new Alert("IMU", - "I2C lockup issue detected on roboRIO. Check console for more information.", - AlertType.kWarning); + public static final Alert i2cLockupWarning = new Alert("IMU", + "I2C lockup issue detected on roboRIO. Check console for more information.", + AlertType.kWarning); /** * NavX serial comm issue. */ - public static final Alert serialCommsIssueWarning = new Alert("IMU", - "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.", - AlertType.kWarning); + public static final Alert serialCommsIssueWarning = new Alert("IMU", + "Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.", + AlertType.kWarning); + /** + * Measured swerve module states object. + */ + public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4]; + /** + * Desired swerve module states object + */ + public static SwerveModuleState[] desiredStatesObj = new SwerveModuleState[4]; + /** + * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the + * chassis speeds properties. + */ + public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds(); + /** + * Describes the desired forward, sideways and angular velocity of the robot. + */ + public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds(); + /** + * The robot's current rotation based on odometry or gyro readings + */ + public static Rotation2d robotRotationObj = new Rotation2d(); /** * The current telemetry verbosity level. */ - public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; + public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE; /** * State of simulation of the Robot, used to optimize retrieval. */ - public static boolean isSimulation = RobotBase.isSimulation(); + public static boolean isSimulation = RobotBase.isSimulation(); /** * The number of swerve modules */ - public static int moduleCount; + public static int moduleCount; /** * The Locations of the swerve drive wheels. */ - public static double[] wheelLocations; + public static double[] wheelLocations; /** * An array of rotation and velocity values describing the measured state of each swerve module */ - public static double[] measuredStates; + public static double[] measuredStates; /** * An array of rotation and velocity values describing the desired state of each swerve module */ - public static double[] desiredStates; + public static double[] desiredStates; /** * The robot's current rotation based on odometry or gyro readings */ - public static double robotRotation = 0; + public static double robotRotation = 0; /** * The maximum achievable speed of the modules, used to adjust the size of the vectors. */ - public static double maxSpeed; + public static double maxSpeed; /** * The units of the module rotations and robot rotation */ - public static String rotationUnit = "degrees"; + public static String rotationUnit = "degrees"; /** * The distance between the left and right modules. */ - public static double sizeLeftRight; + public static double sizeLeftRight; /** * The distance between the front and back modules. */ - public static double sizeFrontBack; + public static double sizeFrontBack; /** * The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to * align with odometry data or match videos. 'up', 'right', 'down' or 'left' */ - public static String forwardDirection = "up"; + public static String forwardDirection = "up"; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static double maxAngularVelocity; + public static double maxAngularVelocity; /** * The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the * chassis speeds properties. */ - public static double[] measuredChassisSpeeds = new double[3]; + public static double[] measuredChassisSpeeds = new double[3]; /** * Describes the desired forward, sideways and angular velocity of the robot. */ - public static double[] desiredChassisSpeeds = new double[3]; + public static double[] desiredChassisSpeeds = new double[3]; + /** + * Struct publisher for AdvantageScope swerve widgets. + */ + private static StructArrayPublisher measuredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/currentStates", + SwerveModuleState.struct) + .publish(); + /** + * Struct publisher for AdvantageScope swerve widgets. + */ + private static StructArrayPublisher desiredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/desiredStates", + SwerveModuleState.struct) + .publish(); + /** + * Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static StructPublisher measuredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/measuredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static StructPublisher desiredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/measuredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Robot {@link Rotation2d} for AdvantageScope swerve widgets. + */ + private static StructPublisher robotRotationStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/robotRotation", + Rotation2d.struct) + .publish(); /** * Upload data to smartdashboard @@ -111,6 +183,12 @@ public static void updateData() SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity); SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds); SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds); + + desiredStatesStruct.set(desiredStatesObj); + measuredStatesStruct.set(measuredStatesObj); + desiredChassisSpeedsStruct.set(desiredChassisSpeedsObj); + measuredChassisSpeedsStruct.set(measuredChassisSpeedsObj); + robotRotationStruct.set(robotRotationObj); } /** From d1af1f8057f13d9927e9547f5a781c9b5b5f4c45 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 13 Nov 2024 22:52:41 +0800 Subject: [PATCH 22/54] incomplete progress on integrating maple-sim --- build.gradle | 4 + .../robot/subsystems/swervedrive/Vision.java | 2 +- src/main/java/swervelib/SwerveDrive.java | 126 +++++++++++++----- src/main/java/swervelib/SwerveModule.java | 5 + .../simulation/SwerveModuleSimulation.java | 82 +++++------- vendordeps/maple-sim.json | 4 +- 6 files changed, 144 insertions(+), 79 deletions(-) diff --git a/build.gradle b/build.gradle index ec028da5..f3ba17dd 100644 --- a/build.gradle +++ b/build.gradle @@ -100,3 +100,7 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +repositories { + mavenLocal() +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index a6663686..00ab54da 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -131,7 +131,7 @@ public void updatePoseEstimation(SwerveDrive swerveDrive) { if (SwerveDriveTelemetry.isSimulation) { - visionSim.update(swerveDrive.getPose()); + visionSim.update(swerveDrive.getSimulationDriveTrainPose().orElse(swerveDrive.getPose())); } for (Cameras camera : Cameras.values()) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d9971e4d..e4ca3f4a 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -48,6 +48,8 @@ import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; +import static edu.wpi.first.units.Units.*; + /** * Swerve Drive class representing and controlling the swerve drive. */ @@ -196,6 +198,8 @@ public SwerveDrive( kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); odometryThread = new Notifier(this::updateOdometry); + this.swerveModules = config.modules; + // Create an integrator for angle if the robot is being simulated to emulate an IMU // If the robot is real, instantiate the IMU instead. if (SwerveDriveTelemetry.isSimulation) @@ -203,41 +207,41 @@ public SwerveDrive( simIMU = new SwerveIMUSimulation(); imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); - // Setup MapleSim - SwerveModuleSimulation simModule = new SwerveModuleSimulation(config.getDriveMotorSim(), - config.getAngleMotorSim(), - config.physicalCharacteristics.driveMotorCurrentLimit, - config.physicalCharacteristics.conversionFactor.drive.gearRatio, - config.physicalCharacteristics.conversionFactor.angle.gearRatio, - config.physicalCharacteristics.driveFrictionVoltage, - config.physicalCharacteristics.angleFrictionVoltage, - config.physicalCharacteristics.wheelGripCoefficientOfFriction, - config.physicalCharacteristics.conversionFactor.drive.diameter / - 2, - config.physicalCharacteristics.steerRotationalInertia); - DriveTrainSimulationConfig simCfg = new DriveTrainSimulationConfig(config.physicalCharacteristics.robotMassKg, - config.getTracklength() + - Units.inchesToMeters(5), - config.getTrackwidth() + - Units.inchesToMeters(5), - config.getTracklength(), - config.getTrackwidth(), - () -> { - return simModule; - }, - config.getGyroSim()); - mapleSimDrive = new SwerveDriveSimulation(simCfg, startingPose); + DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() + .withBumperSize( + Meters.of(config.getTracklength()).plus(Inches.of(5)), + Meters.of(config.getTrackwidth()).plus(Inches.of(5))) + .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg)) + .withCustomModuleTranslations(config.moduleLocationsMeters) + .withGyro(config.getGyroSim()) + .withSwerveModule(() -> new SwerveModuleSimulation( + config.getDriveMotorSim(), + config.getAngleMotorSim(), + config.physicalCharacteristics.driveMotorCurrentLimit, + config.physicalCharacteristics.conversionFactor.drive.gearRatio, + config.physicalCharacteristics.conversionFactor.angle.gearRatio, + config.physicalCharacteristics.driveFrictionVoltage, + config.physicalCharacteristics.angleFrictionVoltage, + config.physicalCharacteristics.wheelGripCoefficientOfFriction, + config.physicalCharacteristics.conversionFactor.drive.diameter / 2, + config.physicalCharacteristics.steerRotationalInertia)); + + mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); + + // feed module simulation instances to modules + for (int i = 0; i < swerveModules.length; i++) + { + this.swerveModules[i].getSimModule().setMapleSimModule(mapleSimDrive.getModules()[i]); + } + // register the drivetrain simulation SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); - } else - { + } else { imu = config.imu; imu.factoryDefault(); imuReadingCache = new Cache<>(imu::getRotation3d, 5L); } - this.swerveModules = config.modules; - // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); swerveDrivePoseEstimator = new SwerveDrivePoseEstimator( @@ -693,7 +697,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) } /** - * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * Gets the measured pose (position and rotation) of the robot, as reported by odometry. * * @return The robot's pose */ @@ -707,7 +711,25 @@ public Pose2d getPose() } /** - * Gets the current field-relative velocity (x, y and omega) of the robot + * Gets the actual pose of the drivetrain during simulation + * + * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running on real robot + * */ + public Optional getSimulationDriveTrainPose() + { + if (SwerveDriveTelemetry.isSimulation) + { + odometryLock.lock(); + Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose(); + odometryLock.unlock(); + return Optional.of(simulationPose); + } + + return Optional.empty(); + } + + /** + * Gets the measured field-relative robot velocity (x, y and omega) * * @return A ChassisSpeeds object of the current field-relative velocity */ @@ -721,6 +743,24 @@ public ChassisSpeeds getFieldVelocity() kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus()); } + /** + * Gets the actual field-relative robot velocity (x, y and omega) during simulation + * + * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty optional when running on real robot + */ + public Optional getSimulationFieldVelocity() + { + if (SwerveDriveTelemetry.isSimulation) + { + odometryLock.lock(); + ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative(); + odometryLock.unlock(); + return Optional.of(simulationFieldRelativeVelocity); + } + + return Optional.empty(); + } + /** * Gets the current robot-relative velocity (x, y and omega) of the robot * @@ -731,6 +771,24 @@ public ChassisSpeeds getRobotVelocity() return kinematics.toChassisSpeeds(getStates()); } + /** + * Gets the actual robot-relative robot velocity (x, y and omega) during simulation + * + * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty optional when running on real robot + */ + public Optional getSimulationRobotVelocity() + { + if (SwerveDriveTelemetry.isSimulation) + { + odometryLock.lock(); + ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative(); + odometryLock.unlock(); + return Optional.of(simulationFieldRelativeVelocity); + } + + return Optional.empty(); + } + /** * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to @@ -744,6 +802,14 @@ public void resetOdometry(Pose2d pose) swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose); odometryLock.unlock(); kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw())); + + // teleport the robot to that pose if it's a simulation + if (SwerveDriveTelemetry.isSimulation) + { + odometryLock.lock(); + mapleSimDrive.setSimulationWorldPose(pose); + odometryLock.unlock(); + } } /** diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index c6082eda..968323f4 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -699,4 +699,9 @@ public void updateTelemetry() SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } + + public SwerveModuleSimulation getSimModule() + { + return simModule; + } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index f6f2d16e..bd8e7636 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -3,55 +3,37 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; +import org.ironmaple.simulation.motorsims.ControlRequest; + +import java.util.Optional; + +import static edu.wpi.first.units.Units.*; /** - * Class to hold simulation data for {@link swervelib.SwerveModule} + * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} */ public class SwerveModuleSimulation { + private Optional mapleSimModule = Optional.empty(); - - /** - * Main timer to simulate the passage of time. - */ - private final Timer timer; /** - * Time delta since last update + * Create simulation class */ - private double dt; - /** - * Fake motor position. - */ - private double fakePos; - /** - * The fake speed of the previous state, used to calculate {@link SwerveModuleSimulation#fakePos}. - */ - private double fakeSpeed; - /** - * Last time queried. - */ - private double lastTime; - /** - * Current simulated swerve module state. - */ - private SwerveModuleState state; + public SwerveModuleSimulation() {} /** - * Create simulation class and initialize module at 0. - */ - public SwerveModuleSimulation() + * Configures the maple-sim SwerveModuleSimulation instance + * */ + public void setMapleSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); - state = new SwerveModuleState(0, Rotation2d.fromDegrees(0)); - fakeSpeed = 0; - fakePos = 0; - dt = 0; + this.mapleSimModule = Optional.of(mapleSimModule); + mapleSimModule + .getSteerMotorConfigs() + .withPositionVoltageController( + Volts.per(Degrees).ofNative(8.0/(180.0 * 12)), Volts.per(RotationsPerSecond).ofNative(0) + ); } - /** * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function * when simulated. @@ -60,13 +42,10 @@ public SwerveModuleSimulation() */ public void updateStateAndPosition(SwerveModuleState desiredState) { - dt = timer.get() - lastTime; - lastTime = timer.get(); - - state = desiredState; - fakeSpeed = desiredState.speedMetersPerSecond; - fakePos += (fakeSpeed * dt); - + if (mapleSimModule.isPresent()) + { + mapleSimModule.get().requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); + } } /** @@ -76,8 +55,14 @@ public void updateStateAndPosition(SwerveModuleState desiredState) */ public SwerveModulePosition getPosition() { - - return new SwerveModulePosition(fakePos, state.angle); + if (mapleSimModule.isPresent()) + { + return new SwerveModulePosition( + mapleSimModule.get().getDriveWheelFinalPositionRad() * mapleSimModule.get().WHEEL_RADIUS_METERS, + mapleSimModule.get().getSteerAbsoluteFacing() + ); + } + return new SwerveModulePosition(0, new Rotation2d()); } /** @@ -87,6 +72,11 @@ public SwerveModulePosition getPosition() */ public SwerveModuleState getState() { - return state; + if (mapleSimModule.isPresent()) + { + return mapleSimModule.get().getCurrentState(); + } + + return new SwerveModuleState(); } } diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index da468e37..a3887133 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.1.6", + "version": "0.1.7prev", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.1.6" + "version": "0.1.7prev" }, { "groupId": "org.dyn4j", From a098ff6eafb85a519b6c0abfedc0a1aa0aa7ccec Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Thu, 14 Nov 2024 20:35:36 +0800 Subject: [PATCH 23/54] completed the integration of maple-sim --- src/main/java/frc/robot/RobotContainer.java | 4 ++ src/main/java/swervelib/SwerveDrive.java | 37 ++++++++++++++----- src/main/java/swervelib/SwerveModule.java | 2 +- .../simulation/SwerveIMUSimulation.java | 31 ++++------------ .../simulation/SwerveModuleSimulation.java | 11 +++--- 5 files changed, 46 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c3aae0ae..34fbdf95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,6 +96,10 @@ public RobotContainer() */ private void configureBindings() { + if (Robot.isSimulation()) + { + driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d())))); + } if (DriverStation.isTest()) { driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand()); diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index e4ca3f4a..114a56ed 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -18,10 +18,8 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; @@ -31,6 +29,8 @@ import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; + +import frc.robot.Robot; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; @@ -204,9 +204,6 @@ public SwerveDrive( // If the robot is real, instantiate the IMU instead. if (SwerveDriveTelemetry.isSimulation) { - simIMU = new SwerveIMUSimulation(); - imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); - DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() .withBumperSize( Meters.of(config.getTracklength()).plus(Inches.of(5)), @@ -223,8 +220,8 @@ public SwerveDrive( config.physicalCharacteristics.driveFrictionVoltage, config.physicalCharacteristics.angleFrictionVoltage, config.physicalCharacteristics.wheelGripCoefficientOfFriction, - config.physicalCharacteristics.conversionFactor.drive.diameter / 2, - config.physicalCharacteristics.steerRotationalInertia)); + Units.inchesToMeters(config.physicalCharacteristics.conversionFactor.drive.diameter/2), + 0.02)); mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); @@ -236,6 +233,9 @@ public SwerveDrive( // register the drivetrain simulation SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); + + simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation()); + imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); } else { imu = config.imu; imu.factoryDefault(); @@ -289,6 +289,10 @@ public SwerveDrive( } odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); + if (SwerveDriveTelemetry.isSimulation) + { + SimulatedArena.overrideSimulationTimings(0.01, 2); + } checkIfTunerXCompatible(); } @@ -342,6 +346,7 @@ private void checkIfTunerXCompatible() public void setOdometryPeriod(double period) { odometryThread.stop(); + SimulatedArena.overrideSimulationTimings(period, 2); odometryThread.startPeriodic(period); } @@ -351,6 +356,7 @@ public void setOdometryPeriod(double period) public void stopOdometryThread() { odometryThread.stop(); + SimulatedArena.overrideSimulationTimings(TimedRobot.kDefaultPeriod, 5); } /** @@ -1107,13 +1113,17 @@ public void updateOdometry() // Update odometry swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); + if (SwerveDriveTelemetry.isSimulation) + { + SimulatedArena.getInstance().simulationPeriodic(); + } + // Update angle accumulator if the robot is simulated if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); if (SwerveDriveTelemetry.isSimulation) { - SimulatedArena.getInstance().simulationPeriodic(); simIMU.updateOdometry( kinematics, getStates(), @@ -1132,7 +1142,14 @@ public void updateOdometry() if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { - field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + if (SwerveDriveTelemetry.isSimulation) + { + field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose()); + field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition()); + } else + { + field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + } } double sumVelocity = 0; diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 968323f4..7b3081e0 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -366,7 +366,7 @@ public void setAnglePIDF(PIDFConfig config) */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition())); + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); // If we are forcing the angle if (!force && antiJitterEnabled) diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java index b3f2b62d..d4c3c850 100644 --- a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import org.ironmaple.simulation.drivesims.GyroSimulation; + import java.util.Optional; /** @@ -16,27 +18,13 @@ public class SwerveIMUSimulation { - /** - * Main timer to control movement estimations. - */ - private final Timer timer; - /** - * The last time the timer was read, used to determine position changes. - */ - private double lastTime; - /** - * Heading of the robot. - */ - private double angle; + private final GyroSimulation gyroSimulation; /** * Create the swerve drive IMU simulation. */ - public SwerveIMUSimulation() - { - timer = new Timer(); - timer.start(); - lastTime = timer.get(); + public SwerveIMUSimulation(GyroSimulation gyroSimulation) { + this.gyroSimulation = gyroSimulation; } /** @@ -46,7 +34,7 @@ public SwerveIMUSimulation() */ public Rotation2d getYaw() { - return new Rotation2d(angle); + return gyroSimulation.getGyroReading(); } /** @@ -76,7 +64,7 @@ public Rotation2d getRoll() */ public Rotation3d getGyroRotation3d() { - return new Rotation3d(0, 0, angle); + return new Rotation3d(0, 0, getYaw().getRadians()); } /** @@ -104,9 +92,6 @@ public void updateOdometry( Pose2d[] modulePoses, Field2d field) { - - angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime); - lastTime = timer.get(); field.getObject("XModules").setPoses(modulePoses); } @@ -117,6 +102,6 @@ public void updateOdometry( */ public void setAngle(double angle) { - this.angle = angle; + this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle)); } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index bd8e7636..cc5bf419 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -27,11 +27,9 @@ public SwerveModuleSimulation() {} public void setMapleSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) { this.mapleSimModule = Optional.of(mapleSimModule); - mapleSimModule - .getSteerMotorConfigs() - .withPositionVoltageController( - Volts.per(Degrees).ofNative(8.0/(180.0 * 12)), Volts.per(RotationsPerSecond).ofNative(0) - ); + mapleSimModule.getDriveMotorConfigs() + .withDefaultFeedForward(Volts.zero()) + .withVelocityVoltageController(Volts.per(RPM).ofNative(12.0/1000.0)); } /** @@ -45,6 +43,9 @@ public void updateStateAndPosition(SwerveModuleState desiredState) if (mapleSimModule.isPresent()) { mapleSimModule.get().requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); + mapleSimModule.get().requestDriveControl(new ControlRequest.VelocityVoltage( + RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.get().WHEEL_RADIUS_METERS) + )); } } From 378427df82bdd82cba3ec551d998665a9455ed6a Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Fri, 15 Nov 2024 09:45:40 +0800 Subject: [PATCH 24/54] resolved all problems mentioned in the code review https://github.com/BroncBotz3481/YAGSL-Example/pull/251#pullrequestreview-2436445822 --- simgui-ds.json | 2 +- .../robot/subsystems/swervedrive/Vision.java | 12 ++++++--- src/main/java/swervelib/SwerveDrive.java | 14 +++++------ src/main/java/swervelib/SwerveModule.java | 25 ++++++++++++++----- .../simulation/SwerveModuleSimulation.java | 2 +- 5 files changed, 37 insertions(+), 18 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 179e064a..2ac2bb49 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -98,7 +98,7 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "030000004c050000e60c000000000000" }, { "guid": "030000004c050000c405000000000000" diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 00ab54da..f907751a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -129,10 +129,16 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) */ public void updatePoseEstimation(SwerveDrive swerveDrive) { - if (SwerveDriveTelemetry.isSimulation) + if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) { - visionSim.update(swerveDrive.getSimulationDriveTrainPose().orElse(swerveDrive.getPose())); - + /* + * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting. + * As a result, the odometry may not always be 100% accurate. + * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect. + * (This is why teams implement vision system to correct odometry.) + * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation. + */ + visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); } for (Cameras camera : Cameras.values()) { diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 114a56ed..675880e8 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -30,7 +30,6 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import frc.robot.Robot; import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; @@ -753,7 +752,9 @@ public ChassisSpeeds getFieldVelocity() * Gets the actual field-relative robot velocity (x, y and omega) during simulation * * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty optional when running on real robot + * @deprecated for testing version of maple-sim only */ + @Deprecated public Optional getSimulationFieldVelocity() { if (SwerveDriveTelemetry.isSimulation) @@ -781,7 +782,9 @@ public ChassisSpeeds getRobotVelocity() * Gets the actual robot-relative robot velocity (x, y and omega) during simulation * * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty optional when running on real robot + * @deprecated for testing version of maple-sim only */ + @Deprecated public Optional getSimulationRobotVelocity() { if (SwerveDriveTelemetry.isSimulation) @@ -806,16 +809,13 @@ public void resetOdometry(Pose2d pose) { odometryLock.lock(); swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose); - odometryLock.unlock(); - kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw())); - - // teleport the robot to that pose if it's a simulation if (SwerveDriveTelemetry.isSimulation) { - odometryLock.lock(); mapleSimDrive.setSimulationWorldPose(pose); - odometryLock.unlock(); } + odometryLock.unlock(); + kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw())); + } /** diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 7b3081e0..29695c31 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -171,6 +171,11 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); } + if (SwerveDriveTelemetry.isSimulation) + { + simModule = new SwerveModuleSimulation(); + } + // Setup the cache for the absolute encoder position. absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15); @@ -199,11 +204,6 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat drivePositionCache = new Cache<>(driveMotor::getPosition, 15); driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15); - if (SwerveDriveTelemetry.isSimulation) - { - simModule = new SwerveModuleSimulation(); - } - // Force a cache update on init. driveVelocityCache.update(); drivePositionCache.update(); @@ -366,7 +366,7 @@ public void setAnglePIDF(PIDFConfig config) */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition())); // If we are forcing the angle if (!force && antiJitterEnabled) @@ -525,6 +525,14 @@ public double getAbsolutePosition() */ public double getRawAbsolutePosition() { + /* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */ + if (SwerveDriveTelemetry.isSimulation) + { + Rotation2d absolutePosition = simModule.getState().angle; + /* The Rotation2d.minus() function wraps the angle to the [0, 360) range. */ + return absolutePosition.minus(new Rotation2d()).getDegrees(); + } + double angle; if (absoluteEncoder != null) { @@ -700,6 +708,11 @@ public void updateTelemetry() SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } + /** + * Obtains the {@link SwerveModuleSimulation} used in simulation. + * + * @return the module simulation, null if this method is called on a real robot + * */ public SwerveModuleSimulation getSimModule() { return simModule; diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index cc5bf419..1c6eee65 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -29,7 +29,7 @@ public void setMapleSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSim this.mapleSimModule = Optional.of(mapleSimModule); mapleSimModule.getDriveMotorConfigs() .withDefaultFeedForward(Volts.zero()) - .withVelocityVoltageController(Volts.per(RPM).ofNative(12.0/1000.0)); + .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0/3000.0), true); } /** From 81b7a782a2229cd1d27740ec69c2a33233c6abfd Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Sun, 17 Nov 2024 12:56:42 -0600 Subject: [PATCH 25/54] Updated studica vendordep --- ....1-beta-2.json => Studica-2025.1.1-beta-3.json} | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) rename vendordeps/{Studica-2025.1.1-beta-2.json => Studica-2025.1.1-beta-3.json} (87%) diff --git a/vendordeps/Studica-2025.1.1-beta-2.json b/vendordeps/Studica-2025.1.1-beta-3.json similarity index 87% rename from vendordeps/Studica-2025.1.1-beta-2.json rename to vendordeps/Studica-2025.1.1-beta-3.json index b9aa624c..2f64aaf4 100644 --- a/vendordeps/Studica-2025.1.1-beta-2.json +++ b/vendordeps/Studica-2025.1.1-beta-3.json @@ -1,13 +1,13 @@ { - "fileName": "Studica-2025.1.1-beta-2.json", + "fileName": "Studica-2025.1.1-beta-3.json", "name": "Studica", - "version": "2025.1.1-beta-2", + "version": "2025.1.1-beta-3", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2025", "mavenUrls": [ "https://dev.studica.com/maven/release/2025/" ], - "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-2.json", + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-3.json", "cppDependencies": [ { "artifactId": "Studica-cpp", @@ -24,7 +24,7 @@ "libName": "Studica", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" }, { "artifactId": "Studica-driver", @@ -41,14 +41,14 @@ "libName": "StudicaDriver", "sharedLibrary": false, "skipInvalidPlatforms": true, - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ], "javaDependencies": [ { "artifactId": "Studica-java", "groupId": "com.studica.frc", - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ], "jniDependencies": [ @@ -65,7 +65,7 @@ "osxuniversal", "windowsx86-64" ], - "version": "2025.1.1-beta-2" + "version": "2025.1.1-beta-3" } ] } \ No newline at end of file From b50ad473d25ff3a0688bb7cb6366957fce90599e Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Sun, 17 Nov 2024 15:22:57 -0600 Subject: [PATCH 26/54] Setup new types. --- src/main/java/swervelib/parser/json/DeviceJson.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/swervelib/parser/json/DeviceJson.java b/src/main/java/swervelib/parser/json/DeviceJson.java index 3ecd1c84..ee03ccd1 100644 --- a/src/main/java/swervelib/parser/json/DeviceJson.java +++ b/src/main/java/swervelib/parser/json/DeviceJson.java @@ -128,14 +128,12 @@ public SwerveIMU createIMU() ".html#onboard-i2c-causing-system-lockups", false); i2cLockupWarning.set(true); - throw new RuntimeException("Studica NavX API does not support I2C communication currently."); -// return new NavXSwerve(I2C.Port.kMXP); + return new NavXSwerve(NavXComType.kI2C); case "navx_usb": DriverStation.reportWarning("WARNING: There is issues when using USB camera's and the NavX like this!\n" + "https://pdocs.kauailabs.com/navx-mxp/guidance/selecting-an-interface/", false); serialCommsIssueWarning.set(true); - throw new RuntimeException("Studica NavX API does not support USB communication currently."); -// return new NavXSwerve(Port.kUSB); + return new NavXSwerve(NavXComType.kUSB1); case "navx_mxp_serial": serialCommsIssueWarning.set(true); throw new RuntimeException("Studica NavX API does not support MXP Serial communication currently."); From bf79692d33e662e6c012250788820bafe2656a09 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Mon, 18 Nov 2024 08:45:41 +0800 Subject: [PATCH 27/54] fixed bugs mentioned in code review removed redundant optional sim modules moved swerve state angle wrap into module simulation --- src/main/java/swervelib/SwerveDrive.java | 8 +--- src/main/java/swervelib/SwerveModule.java | 8 +++- .../simulation/SwerveModuleSimulation.java | 48 ++++++++----------- 3 files changed, 28 insertions(+), 36 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 675880e8..25730804 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -227,7 +227,7 @@ public SwerveDrive( // feed module simulation instances to modules for (int i = 0; i < swerveModules.length; i++) { - this.swerveModules[i].getSimModule().setMapleSimModule(mapleSimDrive.getModules()[i]); + this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i]); } // register the drivetrain simulation @@ -287,11 +287,7 @@ public SwerveDrive( SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; } - odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); - if (SwerveDriveTelemetry.isSimulation) - { - SimulatedArena.overrideSimulationTimings(0.01, 2); - } + setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); checkIfTunerXCompatible(); } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 29695c31..7ffcd171 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -529,8 +529,7 @@ public double getRawAbsolutePosition() if (SwerveDriveTelemetry.isSimulation) { Rotation2d absolutePosition = simModule.getState().angle; - /* The Rotation2d.minus() function wraps the angle to the [0, 360) range. */ - return absolutePosition.minus(new Rotation2d()).getDegrees(); + return absolutePosition.getDegrees(); } double angle; @@ -717,4 +716,9 @@ public SwerveModuleSimulation getSimModule() { return simModule; } + + public void configureModuleSimulation(org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) + { + this.simModule.configureSimModule(swerveModuleSimulation); + } } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 1c6eee65..8ed15872 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -14,19 +14,21 @@ */ public class SwerveModuleSimulation { - private Optional mapleSimModule = Optional.empty(); + private org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule = null; /** * Create simulation class */ - public SwerveModuleSimulation() {} + public SwerveModuleSimulation() { + + } /** - * Configures the maple-sim SwerveModuleSimulation instance - * */ - public void setMapleSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) - { - this.mapleSimModule = Optional.of(mapleSimModule); + * Configure the maple sim module + * @param mapleSimModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation + */ + public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) { + this.mapleSimModule = mapleSimModule; mapleSimModule.getDriveMotorConfigs() .withDefaultFeedForward(Volts.zero()) .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0/3000.0), true); @@ -40,13 +42,10 @@ public void setMapleSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSim */ public void updateStateAndPosition(SwerveModuleState desiredState) { - if (mapleSimModule.isPresent()) - { - mapleSimModule.get().requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); - mapleSimModule.get().requestDriveControl(new ControlRequest.VelocityVoltage( - RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.get().WHEEL_RADIUS_METERS) - )); - } + mapleSimModule.requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); + mapleSimModule.requestDriveControl(new ControlRequest.VelocityVoltage( + RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS_METERS) + )); } /** @@ -56,14 +55,10 @@ public void updateStateAndPosition(SwerveModuleState desiredState) */ public SwerveModulePosition getPosition() { - if (mapleSimModule.isPresent()) - { - return new SwerveModulePosition( - mapleSimModule.get().getDriveWheelFinalPositionRad() * mapleSimModule.get().WHEEL_RADIUS_METERS, - mapleSimModule.get().getSteerAbsoluteFacing() - ); - } - return new SwerveModulePosition(0, new Rotation2d()); + return new SwerveModulePosition( + mapleSimModule.getDriveWheelFinalPositionRad() * mapleSimModule.WHEEL_RADIUS_METERS, + mapleSimModule.getSteerAbsoluteFacing() + ); } /** @@ -73,11 +68,8 @@ public SwerveModulePosition getPosition() */ public SwerveModuleState getState() { - if (mapleSimModule.isPresent()) - { - return mapleSimModule.get().getCurrentState(); - } - - return new SwerveModuleState(); + final SwerveModuleState state = mapleSimModule.getCurrentState(); + state.angle = state.angle.minus(new Rotation2d()); + return state; } } From f010e7c2d07d53bdb314c68ae176c46894bb8875 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Mon, 18 Nov 2024 20:05:48 +0800 Subject: [PATCH 28/54] upgraded to newest version of maple-sim --- src/main/java/swervelib/SwerveDrive.java | 15 ++++++++------- .../simulation/SwerveModuleSimulation.java | 4 ++-- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 25730804..747c8256 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -213,14 +213,15 @@ public SwerveDrive( .withSwerveModule(() -> new SwerveModuleSimulation( config.getDriveMotorSim(), config.getAngleMotorSim(), - config.physicalCharacteristics.driveMotorCurrentLimit, config.physicalCharacteristics.conversionFactor.drive.gearRatio, config.physicalCharacteristics.conversionFactor.angle.gearRatio, - config.physicalCharacteristics.driveFrictionVoltage, - config.physicalCharacteristics.angleFrictionVoltage, - config.physicalCharacteristics.wheelGripCoefficientOfFriction, - Units.inchesToMeters(config.physicalCharacteristics.conversionFactor.drive.diameter/2), - 0.02)); + Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit), + Amps.of(20), + Volts.of(config.physicalCharacteristics.driveFrictionVoltage), + Volts.of(config.physicalCharacteristics.angleFrictionVoltage), + Inches.of(config.physicalCharacteristics.conversionFactor.drive.diameter/2), + KilogramSquareMeters.of(0.02), + config.physicalCharacteristics.wheelGripCoefficientOfFriction)); mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); @@ -341,7 +342,7 @@ private void checkIfTunerXCompatible() public void setOdometryPeriod(double period) { odometryThread.stop(); - SimulatedArena.overrideSimulationTimings(period, 2); + SimulatedArena.overrideSimulationTimings(Seconds.of(period), 2); odometryThread.startPeriodic(period); } diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 8ed15872..af06c3db 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -44,7 +44,7 @@ public void updateStateAndPosition(SwerveModuleState desiredState) { mapleSimModule.requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); mapleSimModule.requestDriveControl(new ControlRequest.VelocityVoltage( - RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS_METERS) + RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS.in(Meters)) )); } @@ -56,7 +56,7 @@ public void updateStateAndPosition(SwerveModuleState desiredState) public SwerveModulePosition getPosition() { return new SwerveModulePosition( - mapleSimModule.getDriveWheelFinalPositionRad() * mapleSimModule.WHEEL_RADIUS_METERS, + mapleSimModule.getDriveWheelFinalPosition().in(Radian) * mapleSimModule.WHEEL_RADIUS.in(Meters), mapleSimModule.getSteerAbsoluteFacing() ); } From 44dbc6cd79456d1165b1d8b66f052492cf2c8266 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Mon, 18 Nov 2024 20:57:49 +0800 Subject: [PATCH 29/54] fixed a small nullpoint exception --- src/main/java/swervelib/SwerveDrive.java | 2 +- .../simulation/SwerveModuleSimulation.java | 13 ++++--------- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 747c8256..589c94f7 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -352,7 +352,7 @@ public void setOdometryPeriod(double period) public void stopOdometryThread() { odometryThread.stop(); - SimulatedArena.overrideSimulationTimings(TimedRobot.kDefaultPeriod, 5); + SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5); } /** diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index af06c3db..392aeeef 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -5,8 +5,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import org.ironmaple.simulation.motorsims.ControlRequest; -import java.util.Optional; - import static edu.wpi.first.units.Units.*; /** @@ -16,13 +14,6 @@ public class SwerveModuleSimulation { private org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule = null; - /** - * Create simulation class - */ - public SwerveModuleSimulation() { - - } - /** * Configure the maple sim module * @param mapleSimModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation @@ -68,6 +59,10 @@ public SwerveModulePosition getPosition() */ public SwerveModuleState getState() { + if (mapleSimModule == null) + { + return new SwerveModuleState(); + } final SwerveModuleState state = mapleSimModule.getCurrentState(); state.angle = state.angle.minus(new Rotation2d()); return state; From cfe17e75ce02f46ced97a5a6d8df8cc1aad6effe Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 18 Nov 2024 11:44:33 -0600 Subject: [PATCH 30/54] Invalidate the cache at the beginning of odometry updates. --- src/main/java/swervelib/SwerveDrive.java | 13 +++++++++++++ src/main/java/swervelib/SwerveModule.java | 14 ++++++++++++-- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d9971e4d..e61fc7d3 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1036,6 +1036,7 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforwa public void updateOdometry() { odometryLock.lock(); + invalidateCache(); try { // Update odometry @@ -1109,6 +1110,18 @@ public void updateOdometry() odometryLock.unlock(); } + /** + * Invalidate all {@link Cache} object used by the {@link SwerveDrive} + */ + public void invalidateCache() + { + imuReadingCache.update(); + for (SwerveModule module : swerveModules) + { + module.invalidateCache(); + } + } + /** * Synchronize angle motor integrated encoders with data from absolute encoders. */ diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index c6082eda..1173064c 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -694,9 +694,19 @@ public void updateTelemetry() SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition()); } SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition()); - SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition()); - SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity()); + SmartDashboard.putNumber(rawDriveName, drivePositionCache.getValue()); + SmartDashboard.putNumber(rawDriveVelName, driveVelocityCache.getValue()); SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } + + /** + * Invalidate the {@link Cache} objects used by {@link SwerveModule}. + */ + public void invalidateCache() + { + absolutePositionCache.update(); + drivePositionCache.update(); + driveVelocityCache.update(); + } } From f02c543725d414e6832f6c405be84337fc6da8e0 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 18 Nov 2024 11:59:20 -0600 Subject: [PATCH 31/54] Update module cache validity to be 20ms to avoid mid-cycle updates. --- src/main/java/swervelib/SwerveModule.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 1173064c..e502626f 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -172,7 +172,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat } // Setup the cache for the absolute encoder position. - absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15); + absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20); // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); @@ -196,8 +196,8 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat driveMotor.burnFlash(); angleMotor.burnFlash(); - drivePositionCache = new Cache<>(driveMotor::getPosition, 15); - driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15); + drivePositionCache = new Cache<>(driveMotor::getPosition, 20); + driveVelocityCache = new Cache<>(driveMotor::getVelocity, 20); if (SwerveDriveTelemetry.isSimulation) { From 49ee2c30982fcb65322bcce8a40ee7452f961aff Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 19 Nov 2024 08:58:25 -0600 Subject: [PATCH 32/54] Added reporting. --- src/main/java/swervelib/SwerveDrive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index e61fc7d3..612d14fb 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1,5 +1,8 @@ package swervelib; +import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive; + +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -287,6 +290,9 @@ public SwerveDrive( odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); checkIfTunerXCompatible(); + + // Report creation to the FMS, uncomment in the next release. + // HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL); } /** From 7e3ca8852e57269256d3bfdf1573880f60ce113a Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 20 Nov 2024 20:59:05 +0800 Subject: [PATCH 33/54] switched to single-thread during simulation --- .../robot/subsystems/swervedrive/SwerveSubsystem.java | 5 ++++- src/main/java/swervelib/SwerveDrive.java | 11 ++++++++++- 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a18ae08d..9b168674 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -138,9 +138,12 @@ public void setupPhotonVision() public void periodic() { // When vision is enabled we must manually update odometry in SwerveDrive - if (visionDriveTest) + if (SwerveDriveTelemetry.isSimulation || visionDriveTest) { swerveDrive.updateOdometry(); + } + if (visionDriveTest) + { vision.updatePoseEstimation(swerveDrive); } } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 589c94f7..1bfdeb37 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -288,7 +288,16 @@ public SwerveDrive( SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; } - setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); + + // during simulation, call update odometry in main thread since maple-sim is not thread-safe + if (SwerveDriveTelemetry.isSimulation) + { + stopOdometryThread(); + } + // otherwise, start the odometry thread + else { + setOdometryPeriod(0.02); + } checkIfTunerXCompatible(); } From f9f4ddd02265152e3a40b129449d4cc2f9bd6b65 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Wed, 20 Nov 2024 22:33:34 +0800 Subject: [PATCH 34/54] Revert "switched to single-thread during simulation" This reverts commit 7e3ca8852e57269256d3bfdf1573880f60ce113a. --- .../robot/subsystems/swervedrive/SwerveSubsystem.java | 5 +---- src/main/java/swervelib/SwerveDrive.java | 11 +---------- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 9b168674..a18ae08d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -138,12 +138,9 @@ public void setupPhotonVision() public void periodic() { // When vision is enabled we must manually update odometry in SwerveDrive - if (SwerveDriveTelemetry.isSimulation || visionDriveTest) - { - swerveDrive.updateOdometry(); - } if (visionDriveTest) { + swerveDrive.updateOdometry(); vision.updatePoseEstimation(swerveDrive); } } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 1bfdeb37..589c94f7 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -288,16 +288,7 @@ public SwerveDrive( SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; } - - // during simulation, call update odometry in main thread since maple-sim is not thread-safe - if (SwerveDriveTelemetry.isSimulation) - { - stopOdometryThread(); - } - // otherwise, start the odometry thread - else { - setOdometryPeriod(0.02); - } + setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); checkIfTunerXCompatible(); } From 87e6343c0045db60a7cdb05b2e6902740da532de Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Thu, 21 Nov 2024 21:11:06 +0800 Subject: [PATCH 35/54] increased simulation rate to 250hz maple-sim is thread safe now, so we're free to push the simulation rate up for a smoother simulation --- src/main/java/swervelib/SwerveDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 589c94f7..8a217bc2 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -288,7 +288,7 @@ public SwerveDrive( SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; } - setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02); + setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.004 : 0.02); checkIfTunerXCompatible(); } @@ -342,7 +342,7 @@ private void checkIfTunerXCompatible() public void setOdometryPeriod(double period) { odometryThread.stop(); - SimulatedArena.overrideSimulationTimings(Seconds.of(period), 2); + SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1); odometryThread.startPeriodic(period); } From 7c939da78fb445f0d4260c9e2d0c219a2feb48be Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Thu, 21 Nov 2024 21:13:14 +0800 Subject: [PATCH 36/54] upgraded to maple-sim 0.2.0 beta1 and it's all ready! --- build.gradle | 4 ---- vendordeps/maple-sim.json | 4 ++-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index f3ba17dd..e3a654c2 100644 --- a/build.gradle +++ b/build.gradle @@ -99,8 +99,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} - -repositories { - mavenLocal() } \ No newline at end of file diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index a3887133..ca0cf908 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.1.7prev", + "version": "0.2.0", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.1.7prev" + "version": "0.2.0" }, { "groupId": "org.dyn4j", From 5f350cfc55945bc606074ddd14c36382d2ee8f59 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Fri, 22 Nov 2024 08:54:24 +0800 Subject: [PATCH 37/54] fixed issues from the code review --- simgui-ds.json | 2 +- src/main/java/swervelib/simulation/SwerveModuleSimulation.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 2ac2bb49..179e064a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -98,7 +98,7 @@ ], "robotJoysticks": [ { - "guid": "030000004c050000e60c000000000000" + "guid": "Keyboard0" }, { "guid": "030000004c050000c405000000000000" diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 392aeeef..91570102 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -63,7 +63,7 @@ public SwerveModuleState getState() { return new SwerveModuleState(); } - final SwerveModuleState state = mapleSimModule.getCurrentState(); + SwerveModuleState state = mapleSimModule.getCurrentState(); state.angle = state.angle.minus(new Rotation2d()); return state; } From d76a8a24ed75aa2de61c8e710a6a9b2f3ee76d5e Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Fri, 22 Nov 2024 09:20:14 +0800 Subject: [PATCH 38/54] almost forgot, the license --- IronMaple-License.md | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 IronMaple-License.md diff --git a/IronMaple-License.md b/IronMaple-License.md new file mode 100644 index 00000000..3e1a5b7c --- /dev/null +++ b/IronMaple-License.md @@ -0,0 +1,15 @@ +

    + team logo + project logo +

    + +#### This project uses [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) for simulation. +If you encounter any bugs related to drivetrain physics simulation, please [submit an issue to maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/issues/new/choose). + +## MIT License, Copyright (c) 2024 Team5516 "Iron Maple" + +**Permission is hereby granted, free of charge, to any person obtaining a copyof this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:** + +> #### The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +*THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.* \ No newline at end of file From 8e1b88a75871de42eeceb0a6305dd43ef98a238a Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Sun, 24 Nov 2024 23:58:18 -0600 Subject: [PATCH 39/54] Update SwerveModule.java Fixed wrapping error, might cause unnecessary movements which need to be investigated --- src/main/java/swervelib/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index e502626f..30d7e87e 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -177,7 +177,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(0, 180); + angleMotor.configurePIDWrapping(0, 360); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); angleMotor.setMotorBrake(false); From 077be2c6f42cebb573e3f3bdabb136c2088ede07 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Mon, 25 Nov 2024 08:37:14 -0600 Subject: [PATCH 40/54] Update SwerveModule.java --- src/main/java/swervelib/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 30d7e87e..39f5679f 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -177,7 +177,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(0, 360); + angleMotor.configurePIDWrapping(-180, 180); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); angleMotor.setMotorBrake(false); From 1e0e19a4d47a51beb7fb17380f9fd413f6d4b622 Mon Sep 17 00:00:00 2001 From: Curt Date: Wed, 27 Nov 2024 17:00:57 -0500 Subject: [PATCH 41/54] AHRS imu variable is lost after constructor scope ends --- src/main/java/swervelib/imu/NavXSwerve.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index bd14e344..f5b0c19a 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -41,7 +41,6 @@ public NavXSwerve(NavXComType port) navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError); try { - AHRS /* Communicate w/navX-MXP via the MXP SPI Bus. */ /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ From 0eb6e9afa19818fcda06129df7898638bfb695ea Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Wed, 27 Nov 2024 23:39:52 -0600 Subject: [PATCH 42/54] Update SwerveModule.java --- src/main/java/swervelib/SwerveModule.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 39f5679f..30d7e87e 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -177,7 +177,7 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat // Config angle motor/controller angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); angleMotor.configurePIDF(moduleConfiguration.anglePIDF); - angleMotor.configurePIDWrapping(-180, 180); + angleMotor.configurePIDWrapping(0, 360); angleMotor.setInverted(moduleConfiguration.angleMotorInverted); angleMotor.setMotorBrake(false); From baed8ea0ef691870898be864962852cf06b8f302 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 2 Dec 2024 12:35:23 -0600 Subject: [PATCH 43/54] Fixed NavX offset while inverted. --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 2 ++ src/main/java/swervelib/imu/NavXSwerve.java | 1 + 2 files changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a18ae08d..d44c1fd1 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -312,6 +312,8 @@ public Command driveToPose(Pose2d pose) ); } + // TODO: Make a drive command with PathPlanner setpoint generator, derived off of 254's setpoint generator + /** * Command to drive the robot using translative values and heading as a setpoint. * diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index f5b0c19a..a7e73ce6 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -91,6 +91,7 @@ public void setOffset(Rotation3d offset) public void setInverted(boolean invertIMU) { invertedIMU = invertIMU; + setOffset(getRawRotation3d()); } /** From 9d8b7efa201cf93899d75619511000ac71ff7e45 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 2 Dec 2024 13:44:13 -0600 Subject: [PATCH 44/54] Fixed Spark inversion deprecation, moved away from Deprecated ChassisSpeeds. Updated REVLib. --- build.gradle | 2 +- simgui.json | 5 + .../swervedrive/SwerveSubsystem.java | 11 +- src/main/java/swervelib/SwerveDrive.java | 128 ++++++++++-------- .../swervelib/motors/SparkFlexSwerve.java | 6 +- .../motors/SparkMaxBrushedMotorSwerve.java | 6 +- .../java/swervelib/motors/SparkMaxSwerve.java | 6 +- ...EVLib-2025.0.0-beta-2.json => REVLib.json} | 12 +- 8 files changed, 91 insertions(+), 85 deletions(-) rename vendordeps/{REVLib-2025.0.0-beta-2.json => REVLib.json} (88%) diff --git a/build.gradle b/build.gradle index e3a654c2..7d42139a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2" } java { diff --git a/simgui.json b/simgui.json index 10b3d8cd..73511efd 100644 --- a/simgui.json +++ b/simgui.json @@ -60,10 +60,15 @@ "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", "/LiveWindow/Ungrouped/Pigeon 2 [13]": "Gyro", "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/SmartDashboard/Encoders": "Alerts", "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/IMU": "Alerts", + "/SmartDashboard/JSON": "Alerts", + "/SmartDashboard/Motors": "Alerts", "/SmartDashboard/Pigeon 2 (v6) [13]": "Gyro", "/SmartDashboard/Pigeon 2 [13]": "Gyro", "/SmartDashboard/SendableChooser[0]": "String Chooser", + "/SmartDashboard/Swerve Drive": "Alerts", "/SmartDashboard/VisionSystemSim-Vision/Sim Field": "Field2d", "/SmartDashboard/navX-Sensor[1]": "Gyro", "/SmartDashboard/navX-Sensor[4]": "Gyro" diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index d44c1fd1..a80c4f3a 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -246,12 +246,11 @@ public Command aimAtSpeaker(double tolerance) SwerveController controller = swerveDrive.getSwerveController(); return run( () -> { - drive(ChassisSpeeds.fromFieldRelativeSpeeds(0, - 0, - controller.headingCalculate(getHeading().getRadians(), - getSpeakerYaw().getRadians()), - getHeading()) - ); + ChassisSpeeds speeds = new ChassisSpeeds(0, 0, + controller.headingCalculate(getHeading().getRadians(), + getSpeakerYaw().getRadians())); + speeds.toRobotRelativeSpeeds(getHeading()); + drive(speeds); }).until(() -> Math.abs(getSpeakerYaw().minus(getHeading()).getDegrees()) < tolerance); } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 82bd1fa9..d6f3fa7a 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1,8 +1,13 @@ package swervelib; -import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -21,8 +26,11 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; @@ -32,7 +40,6 @@ import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; - import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; @@ -50,8 +57,6 @@ import swervelib.telemetry.SwerveDriveTelemetry; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; -import static edu.wpi.first.units.Units.*; - /** * Swerve Drive class representing and controlling the swerve drive. */ @@ -207,24 +212,28 @@ public SwerveDrive( if (SwerveDriveTelemetry.isSimulation) { DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() - .withBumperSize( - Meters.of(config.getTracklength()).plus(Inches.of(5)), - Meters.of(config.getTrackwidth()).plus(Inches.of(5))) - .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg)) - .withCustomModuleTranslations(config.moduleLocationsMeters) - .withGyro(config.getGyroSim()) - .withSwerveModule(() -> new SwerveModuleSimulation( - config.getDriveMotorSim(), - config.getAngleMotorSim(), - config.physicalCharacteristics.conversionFactor.drive.gearRatio, - config.physicalCharacteristics.conversionFactor.angle.gearRatio, - Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit), - Amps.of(20), - Volts.of(config.physicalCharacteristics.driveFrictionVoltage), - Volts.of(config.physicalCharacteristics.angleFrictionVoltage), - Inches.of(config.physicalCharacteristics.conversionFactor.drive.diameter/2), - KilogramSquareMeters.of(0.02), - config.physicalCharacteristics.wheelGripCoefficientOfFriction)); + .withBumperSize( + Meters.of(config.getTracklength()) + .plus(Inches.of(5)), + Meters.of(config.getTrackwidth()) + .plus(Inches.of(5))) + .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg)) + .withCustomModuleTranslations(config.moduleLocationsMeters) + .withGyro(config.getGyroSim()) + .withSwerveModule(() -> new SwerveModuleSimulation( + config.getDriveMotorSim(), + config.getAngleMotorSim(), + config.physicalCharacteristics.conversionFactor.drive.gearRatio, + config.physicalCharacteristics.conversionFactor.angle.gearRatio, + Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit), + Amps.of(20), + Volts.of(config.physicalCharacteristics.driveFrictionVoltage), + Volts.of(config.physicalCharacteristics.angleFrictionVoltage), + Inches.of( + config.physicalCharacteristics.conversionFactor.drive.diameter / + 2), + KilogramSquareMeters.of(0.02), + config.physicalCharacteristics.wheelGripCoefficientOfFriction)); mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); @@ -239,7 +248,8 @@ public SwerveDrive( simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation()); imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); - } else { + } else + { imu = config.imu; imu.factoryDefault(); imuReadingCache = new Cache<>(imu::getRotation3d, 5L); @@ -431,9 +441,8 @@ public void setHeadingCorrection(boolean state, double deadband) public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity) { - ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()) - .plus(robotOrientedVelocity); - drive(TotalVelocties); + fieldOrientedVelocity.toRobotRelativeSpeeds(getOdometryHeading()); + drive(fieldOrientedVelocity.plus(robotOrientedVelocity)); } /** @@ -443,8 +452,8 @@ public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVeloci */ public void driveFieldOriented(ChassisSpeeds velocity) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); - drive(fieldOrientedVelocity); + velocity.toRobotRelativeSpeeds(getOdometryHeading()); + drive(velocity); } /** @@ -455,8 +464,8 @@ public void driveFieldOriented(ChassisSpeeds velocity) */ public void driveFieldOriented(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) { - ChassisSpeeds fieldOrientedVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); - drive(fieldOrientedVelocity, centerOfRotationMeters); + velocity.toRobotRelativeSpeeds(getOdometryHeading()); + drive(velocity, centerOfRotationMeters); } /** @@ -504,12 +513,11 @@ public void drive( { // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getOdometryHeading()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - + ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + if (fieldRelative) + { + velocity.toRobotRelativeSpeeds(getOdometryHeading()); + } drive(velocity, isOpenLoop, centerOfRotationMeters); } @@ -533,12 +541,10 @@ public void drive( { // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if // necessary. - ChassisSpeeds velocity = - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds( - translation.getX(), translation.getY(), rotation, getOdometryHeading()) - : new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + if(fieldRelative) + velocity.toRobotRelativeSpeeds(getOdometryHeading()); drive(velocity, isOpenLoop, new Translation2d()); } @@ -721,8 +727,9 @@ public Pose2d getPose() /** * Gets the actual pose of the drivetrain during simulation * - * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running on real robot - * */ + * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running + * on real robot + */ public Optional getSimulationDriveTrainPose() { if (SwerveDriveTelemetry.isSimulation) @@ -747,14 +754,16 @@ public ChassisSpeeds getFieldVelocity() // but not the reverse. However, because this transform is a simple rotation, negating the // angle // given as the robot angle reverses the direction of rotation, and the conversion is reversed. - return ChassisSpeeds.fromFieldRelativeSpeeds( - kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus()); + ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates()); + robotRelativeSpeeds.toFieldRelativeSpeeds(getOdometryHeading().unaryMinus()); + return robotRelativeSpeeds; } /** * Gets the actual field-relative robot velocity (x, y and omega) during simulation * - * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty optional when running on real robot + * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty + * optional when running on real robot * @deprecated for testing version of maple-sim only */ @Deprecated @@ -784,7 +793,8 @@ public ChassisSpeeds getRobotVelocity() /** * Gets the actual robot-relative robot velocity (x, y and omega) during simulation * - * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty optional when running on real robot + * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty + * optional when running on real robot * @deprecated for testing version of maple-sim only */ @Deprecated @@ -817,7 +827,9 @@ public void resetOdometry(Pose2d pose) mapleSimDrive.setSimulationWorldPose(pose); } odometryLock.unlock(); - kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw())); + ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(); + robotRelativeSpeeds.toFieldRelativeSpeeds(getYaw()); + kinematics.toSwerveModuleStates(robotRelativeSpeeds); } @@ -1403,7 +1415,7 @@ public void setCosineCompensator(boolean enabled) * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop * * @param enable Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following. + * {@link ChassisSpeeds#discretize(double)} with the following. * @param dtSeconds The duration of the timestep the speeds should be applied for. */ public void setChassisDiscretization(boolean enable, double dtSeconds) @@ -1420,9 +1432,9 @@ public void setChassisDiscretization(boolean enable, double dtSeconds) * and/or auto * * @param useInTeleop Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. + * {@link ChassisSpeeds#discretize(double)} with the following in teleop. * @param useInAuto Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. + * {@link ChassisSpeeds#discretize(double)} with the following in auto. * @param dtSeconds The duration of the timestep the speeds should be applied for. */ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) @@ -1471,12 +1483,8 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient); if (angularVelocity.getRadians() != 0.0) { - velocity = ChassisSpeeds.fromRobotRelativeSpeeds( - velocity.vxMetersPerSecond, - velocity.vyMetersPerSecond, - velocity.omegaRadiansPerSecond, - getOdometryHeading()); - velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); + velocity.toFieldRelativeSpeeds(getOdometryHeading()); + velocity.toRobotRelativeSpeeds(getOdometryHeading().plus(angularVelocity)); } return velocity; } @@ -1502,7 +1510,7 @@ private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesC // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 if (uesChassisDiscretize) { - velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); + velocity.discretize(discretizationdtSeconds); } return velocity; diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 91c49dac..7b58f04a 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -404,10 +404,8 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkFlex(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 31a1a42a..1ce69a82 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -472,10 +472,8 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkMax(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 34c086ce..68067a6c 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -409,10 +409,8 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - configureSparkMax(() -> { - motor.setInverted(inverted); - return motor.getLastError(); - }); + cfg.inverted(inverted); + cfgUpdated = true; } /** diff --git a/vendordeps/REVLib-2025.0.0-beta-2.json b/vendordeps/REVLib.json similarity index 88% rename from vendordeps/REVLib-2025.0.0-beta-2.json rename to vendordeps/REVLib.json index 85f61514..70cdac98 100644 --- a/vendordeps/REVLib-2025.0.0-beta-2.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0-beta-2.json", + "fileName": "REVLib.json", "name": "REVLib", - "version": "2025.0.0-beta-2", + "version": "2025.0.0-beta-3", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0-beta-2" + "version": "2025.0.0-beta-3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-2", + "version": "2025.0.0-beta-3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0-beta-2", + "version": "2025.0.0-beta-3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0-beta-2", + "version": "2025.0.0-beta-3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From f882c9d94b0d54f037374fbe5b1b29be423da47f Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 2 Dec 2024 13:52:41 -0600 Subject: [PATCH 45/54] Added FMS Reporting. --- src/main/java/swervelib/SwerveDrive.java | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index d6f3fa7a..93c341c9 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1,5 +1,7 @@ package swervelib; +import static edu.wpi.first.hal.FRCNetComm.tInstances.kRobotDriveSwerve_YAGSL; +import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; @@ -8,6 +10,7 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import edu.wpi.first.hal.HAL; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -305,8 +308,7 @@ public SwerveDrive( checkIfTunerXCompatible(); - // Report creation to the FMS, uncomment in the next release. - // HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL); + HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL); } /** @@ -543,8 +545,10 @@ public void drive( // necessary. ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); - if(fieldRelative) + if (fieldRelative) + { velocity.toRobotRelativeSpeeds(getOdometryHeading()); + } drive(velocity, isOpenLoop, new Translation2d()); } @@ -1414,8 +1418,8 @@ public void setCosineCompensator(boolean enabled) /** * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop * - * @param enable Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(double)} with the following. + * @param enable Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with + * the following. * @param dtSeconds The duration of the timestep the speeds should be applied for. */ public void setChassisDiscretization(boolean enable, double dtSeconds) @@ -1431,10 +1435,10 @@ public void setChassisDiscretization(boolean enable, double dtSeconds) * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop * and/or auto * - * @param useInTeleop Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(double)} with the following in teleop. - * @param useInAuto Enable chassis velocity correction, which will use - * {@link ChassisSpeeds#discretize(double)} with the following in auto. + * @param useInTeleop Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with + * the following in teleop. + * @param useInAuto Enable chassis velocity correction, which will use {@link ChassisSpeeds#discretize(double)} with + * the following in auto. * @param dtSeconds The duration of the timestep the speeds should be applied for. */ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) From 415de87b9a6f19f5fcad5d09a8197df8dc9b4564 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Mon, 2 Dec 2024 14:23:35 -0600 Subject: [PATCH 46/54] Updated telemetry to remove redundancy. --- src/main/java/swervelib/SwerveDrive.java | 27 +++---------------- src/main/java/swervelib/SwerveModule.java | 4 +-- .../telemetry/SwerveDriveTelemetry.java | 24 +++++++++++++++++ 3 files changed, 28 insertions(+), 27 deletions(-) diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 93c341c9..fb1dd9bb 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -590,9 +590,6 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { - SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(velocity.omegaRadiansPerSecond); SwerveDriveTelemetry.desiredChassisSpeedsObj = velocity; } @@ -707,9 +704,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); - SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); + SwerveDriveTelemetry.desiredChassisSpeedsObj = chassisSpeeds; setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false); } @@ -1073,10 +1068,6 @@ public void lockPose() new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { - SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = - desiredState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(swerveModule.moduleNumber * 2) + 1] = - desiredState.speedMetersPerSecond; SwerveDriveTelemetry.desiredStatesObj[swerveModule.moduleNumber] = desiredState; } swerveModule.setDesiredState(desiredState, false, true); @@ -1141,22 +1132,12 @@ public void updateOdometry() // Update angle accumulator if the robot is simulated if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { - Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); if (SwerveDriveTelemetry.isSimulation) { - simIMU.updateOdometry( - kinematics, - getStates(), - modulePoses, - field); + field.getObject("XModules").setPoses(getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition())); } - ChassisSpeeds measuredChassisSpeeds = getRobotVelocity(); - SwerveDriveTelemetry.measuredChassisSpeeds[1] = measuredChassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[0] = measuredChassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeeds.omegaRadiansPerSecond); - SwerveDriveTelemetry.measuredChassisSpeedsObj = measuredChassisSpeeds; - SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); + SwerveDriveTelemetry.measuredChassisSpeedsObj = getRobotVelocity(); SwerveDriveTelemetry.robotRotationObj = getOdometryHeading(); } @@ -1185,8 +1166,6 @@ public void updateOdometry() } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { - SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); - SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; SwerveDriveTelemetry.measuredStatesObj[module.moduleNumber] = moduleState; } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index b9504dee..f0c260aa 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -392,7 +392,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, } else { driveMotor.setReference(velocity.magnitude(), driveMotorFeedforward.calculate(velocity).magnitude()); - desiredState.speedMetersPerSecond = velocity.baseUnitMagnitude(); + desiredState.speedMetersPerSecond = velocity.magnitude(); } // Prevent module rotation if angle is the same as the previous angle. @@ -421,8 +421,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, // TODO: Change and move to SwerveDriveTelemetry if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { - SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees(); - SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity.magnitude(); SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState; } diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 53658fc0..005b70dd 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -170,6 +170,30 @@ public class SwerveDriveTelemetry */ public static void updateData() { + measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond; + measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond; + measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond); + + desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond; + desiredChassisSpeeds[1] = desiredChassisSpeedsObj.vyMetersPerSecond; + desiredChassisSpeeds[2] = Math.toDegrees(desiredChassisSpeedsObj.omegaRadiansPerSecond); + + robotRotation = robotRotationObj.getDegrees(); + + for (int i = 0; i < measuredStatesObj.length; i++) + { + SwerveModuleState state = measuredStatesObj[i]; + measuredStates[i * 2] = state.angle.getDegrees(); + measuredStates[i * 2 + 1] = state.speedMetersPerSecond; + } + + for (int i = 0; i < desiredStatesObj.length; i++) + { + SwerveModuleState state = desiredStatesObj[i]; + desiredStates[i * 2] = state.angle.getDegrees(); + desiredStates[i * 2 + 1] = state.speedMetersPerSecond; + } + SmartDashboard.putNumber("swerve/moduleCount", moduleCount); SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations); SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates); From 8b875032666e5ab8685e034eea4d87da53b732f2 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 09:06:50 -0600 Subject: [PATCH 47/54] Maplesim is still broken. --- src/main/deploy/pathplanner/settings.json | 15 +++- .../swervelib/encoders/CANCoderSwerve.java | 4 +- ...0-beta-4.json => PathplannerLib-beta.json} | 8 +- ...json => Phoenix5-frc2025-beta-latest.json} | 48 ++++++++---- ...json => Phoenix6-frc2025-beta-latest.json} | 77 +++++++++++-------- vendordeps/maple-sim.json | 7 +- vendordeps/photonlib.json | 38 ++------- 7 files changed, 110 insertions(+), 87 deletions(-) rename vendordeps/{PathplannerLib-2025.0.0-beta-4.json => PathplannerLib-beta.json} (85%) rename vendordeps/{Phoenix5-5.34.0-beta-2.json => Phoenix5-frc2025-beta-latest.json} (74%) rename vendordeps/{Phoenix6-25.0.0-beta-2.json => Phoenix6-frc2025-beta-latest.json} (85%) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index cb761c64..2ca3c4c0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,14 +8,25 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, "robotMass": 74.088, "robotMOI": 6.883, - "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "NEO", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] } \ No newline at end of file diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java index 499bf857..0477b240 100644 --- a/src/main/java/swervelib/encoders/CANCoderSwerve.java +++ b/src/main/java/swervelib/encoders/CANCoderSwerve.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; @@ -9,7 +10,6 @@ import com.ctre.phoenix6.configs.CANcoderConfigurator; import com.ctre.phoenix6.configs.MagnetSensorConfigs; import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.units.measure.Angle; @@ -117,7 +117,7 @@ public void configure(boolean inverted) MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs(); cfg.refresh(magnetSensorConfiguration); cfg.apply(magnetSensorConfiguration - .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1) + .withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive)); } diff --git a/vendordeps/PathplannerLib-2025.0.0-beta-4.json b/vendordeps/PathplannerLib-beta.json similarity index 85% rename from vendordeps/PathplannerLib-2025.0.0-beta-4.json rename to vendordeps/PathplannerLib-beta.json index de2782ee..e79fe1ae 100644 --- a/vendordeps/PathplannerLib-2025.0.0-beta-4.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.0.0-beta-4.json", + "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-4", + "version": "2025.0.0-beta-5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-4" + "version": "2025.0.0-beta-5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-4", + "version": "2025.0.0-beta-5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5-5.34.0-beta-2.json b/vendordeps/Phoenix5-frc2025-beta-latest.json similarity index 74% rename from vendordeps/Phoenix5-5.34.0-beta-2.json rename to vendordeps/Phoenix5-frc2025-beta-latest.json index e27638b2..65dfcff9 100644 --- a/vendordeps/Phoenix5-5.34.0-beta-2.json +++ b/vendordeps/Phoenix5-frc2025-beta-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix5-5.34.0-beta-2.json", + "fileName": "Phoenix5-frc2025-beta-latest.json", "name": "CTRE-Phoenix (v5)", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "frcYear": "2025", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -12,32 +12,45 @@ { "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", - "offlineFileName": "Phoenix6.json", - "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + "offlineFileName": "Phoenix6-frc2025-beta-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-beta-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.34.0-beta-2" + "version": "5.34.0-beta-3" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.34.0-beta-2" + "version": "5.34.0-beta-3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -45,12 +58,13 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -60,7 +74,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -68,6 +82,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -75,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -83,6 +98,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -90,7 +106,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -98,6 +114,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -105,7 +122,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -113,6 +130,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -120,7 +138,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -128,6 +146,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,7 +154,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.34.0-beta-2", + "version": "5.34.0-beta-3", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -143,6 +162,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/Phoenix6-25.0.0-beta-2.json b/vendordeps/Phoenix6-frc2025-beta-latest.json similarity index 85% rename from vendordeps/Phoenix6-25.0.0-beta-2.json rename to vendordeps/Phoenix6-frc2025-beta-latest.json index c56e61ae..ff0b8c9e 100644 --- a/vendordeps/Phoenix6-25.0.0-beta-2.json +++ b/vendordeps/Phoenix6-frc2025-beta-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.0.0-beta-2.json", + "fileName": "Phoenix6-frc2025-beta-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -9,11 +9,6 @@ ], "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json", "conflictsWith": [ - { - "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", - "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", - "offlineFileName": "Phoenix6And5.json" - }, { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", @@ -24,19 +19,20 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-2" + "version": "25.0.0-beta-3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -44,12 +40,13 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -57,12 +54,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -70,12 +68,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -83,12 +82,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -96,12 +96,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -109,12 +110,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -122,12 +124,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -135,12 +138,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -148,12 +152,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -161,12 +166,13 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -176,7 +182,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -184,6 +190,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -191,7 +198,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -199,6 +206,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "linuxathena" ], "simMode": "hwsim" @@ -206,7 +214,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,6 +222,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -221,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -229,6 +238,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -236,7 +246,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,6 +254,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -251,7 +262,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,6 +270,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -266,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,6 +286,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -281,7 +294,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -289,6 +302,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -296,7 +310,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,6 +318,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -311,7 +326,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -319,6 +334,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" @@ -326,7 +342,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-2", + "version": "25.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -334,6 +350,7 @@ "binaryPlatforms": [ "windowsx86-64", "linuxx86-64", + "linuxarm64", "osxuniversal" ], "simMode": "swsim" diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index ca0cf908..c0b82958 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,18 +1,19 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.2.0", + "version": "0.2.1", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ - "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases" + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" ], "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", "javaDependencies": [ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.2.0" + "version": "0.2.1" }, { "groupId": "org.dyn4j", diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index ceaec0ef..ad3a530c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.0.0-beta-1", + "version": "v2025.0.0-beta-5", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -10,23 +10,10 @@ ], "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", "jniDependencies": [ - { - "groupId": "edu.wpi.first.wpilibc", - "artifactId": "wpilibc-cpp", - "version": "2025.1.1-beta-1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-1", + "version": "v2025.0.0-beta-5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -35,26 +22,13 @@ "linuxx86-64", "osxuniversal" ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-jni", - "version": "v2025.0.0-beta-1", - "skipInvalidPlatforms": true, - "isJar": true, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] } ], "cppDependencies": [ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.0.0-beta-1", + "version": "v2025.0.0-beta-5", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -69,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-beta-1", + "version": "v2025.0.0-beta-5", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -86,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.0.0-beta-1" + "version": "v2025.0.0-beta-5" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.0.0-beta-1" + "version": "v2025.0.0-beta-5" } ] } \ No newline at end of file From 933246a08c2e13a28df9d4f34cd8520903391f96 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 10:33:22 -0600 Subject: [PATCH 48/54] Fixed broken simulation. --- .../swervelib/telemetry/SwerveDriveTelemetry.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java index 005b70dd..e4d98380 100644 --- a/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/src/main/java/swervelib/telemetry/SwerveDriveTelemetry.java @@ -183,15 +183,21 @@ public static void updateData() for (int i = 0; i < measuredStatesObj.length; i++) { SwerveModuleState state = measuredStatesObj[i]; - measuredStates[i * 2] = state.angle.getDegrees(); - measuredStates[i * 2 + 1] = state.speedMetersPerSecond; + if (state != null) + { + measuredStates[i * 2] = state.angle.getDegrees(); + measuredStates[i * 2 + 1] = state.speedMetersPerSecond; + } } for (int i = 0; i < desiredStatesObj.length; i++) { SwerveModuleState state = desiredStatesObj[i]; - desiredStates[i * 2] = state.angle.getDegrees(); - desiredStates[i * 2 + 1] = state.speedMetersPerSecond; + if (state != null) + { + desiredStates[i * 2] = state.angle.getDegrees(); + desiredStates[i * 2 + 1] = state.speedMetersPerSecond; + } } SmartDashboard.putNumber("swerve/moduleCount", moduleCount); From 25e46b7fedef05c227814f0a1c47adb45e37fd7c Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 10:44:57 -0600 Subject: [PATCH 49/54] Default to within the field, bc physics. --- simgui.json | 14 ++++++++++++++ .../subsystems/swervedrive/SwerveSubsystem.java | 13 +++++++++++-- src/main/java/swervelib/SwerveDrive.java | 7 ++----- 3 files changed, 27 insertions(+), 7 deletions(-) diff --git a/simgui.json b/simgui.json index 73511efd..18af23dd 100644 --- a/simgui.json +++ b/simgui.json @@ -80,6 +80,20 @@ } }, "/SmartDashboard/Field": { + "OdometryPose": { + "arrowColor": [ + 255.0, + 255.0, + 0.0, + 255.0 + ], + "color": [ + 224.99996948242188, + 0.0, + 255.0, + 255.0 + ] + }, "XModules": { "arrowColor": [ 0.09611687064170837, diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index a80c4f3a..f6b5ec33 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -4,6 +4,8 @@ package frc.robot.subsystems.swervedrive; +import static edu.wpi.first.units.Units.Meter; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.commands.PathfindingCommand; @@ -91,7 +93,10 @@ public SwerveSubsystem(File directory) SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; try { - swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, Pose2d.kZero); + swerveDrive = new SwerveParser(directory).createSwerveDrive(Constants.MAX_SPEED, + new Pose2d(new Translation2d(Meter.of(1), + Meter.of(4)), + Rotation2d.fromDegrees(0))); // Alternative method if you don't want to supply the conversion factor via JSON files. // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, angleConversionFactor, driveConversionFactor); } catch (Exception e) @@ -123,7 +128,11 @@ public SwerveSubsystem(File directory) */ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive(driveCfg, controllerCfg, Constants.MAX_SPEED, Pose2d.kZero); + swerveDrive = new SwerveDrive(driveCfg, + controllerCfg, + Constants.MAX_SPEED, + new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), + Rotation2d.fromDegrees(0))); } /** diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index fb1dd9bb..1693575d 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -1132,11 +1132,6 @@ public void updateOdometry() // Update angle accumulator if the robot is simulated if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { - if (SwerveDriveTelemetry.isSimulation) - { - field.getObject("XModules").setPoses(getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition())); - } - SwerveDriveTelemetry.measuredChassisSpeedsObj = getRobotVelocity(); SwerveDriveTelemetry.robotRotationObj = getOdometryHeading(); } @@ -1147,6 +1142,8 @@ public void updateOdometry() { field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose()); field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition()); + field.getObject("XModules").setPoses(getSwerveModulePoses(mapleSimDrive.getSimulatedDriveTrainPose())); + } else { field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); From a038cd44108730139d11c2d3e8137b47c08e8a1e Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 10:46:52 -0600 Subject: [PATCH 50/54] Added javadocs. --- src/main/java/swervelib/SwerveModule.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index f0c260aa..56b67022 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -730,6 +730,10 @@ public SwerveModuleSimulation getSimModule() return simModule; } + /** + * Configure the {@link SwerveModule#simModule} with the MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} + * @param swerveModuleSimulation + */ public void configureModuleSimulation(org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) { this.simModule.configureSimModule(swerveModuleSimulation); From 9292cd74ecfedcd87f781e11ba649b9974b84418 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 16:12:42 -0600 Subject: [PATCH 51/54] Added javadocs, implemented the beginning of the SwerveSetpointGenerator. --- src/main/java/frc/robot/RobotContainer.java | 5 ++ .../swervedrive/SwerveSubsystem.java | 62 +++++++++++++++++++ src/main/java/swervelib/SwerveDrive.java | 49 +++++++++++++-- src/main/java/swervelib/SwerveModule.java | 40 +++++++++--- 4 files changed, 141 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34fbdf95..ccc30ab6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,6 +78,11 @@ public class RobotContainer () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), () -> driverXbox.getRawAxis(2)); + Command driveSetpointGenSim = drivebase.simDriveCommand( + () -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> driverXbox.getRawAxis(2)); + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index f6b5ec33..953c1450 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -14,6 +14,9 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.swerve.SwerveSetpoint; +import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -29,14 +32,19 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants; import frc.robot.subsystems.swervedrive.Vision.Cameras; import java.io.File; +import java.io.IOException; import java.util.Arrays; import java.util.Optional; +import java.util.concurrent.atomic.AtomicReference; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.json.simple.parser.ParseException; import org.photonvision.targeting.PhotonPipelineResult; import swervelib.SwerveController; import swervelib.SwerveDrive; @@ -322,6 +330,60 @@ public Command driveToPose(Pose2d pose) // TODO: Make a drive command with PathPlanner setpoint generator, derived off of 254's setpoint generator + /** + * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner. + * + * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to achieve. + * @return {@link Command} to run. + * @throws IOException If the PathPlanner GUI settings is invalid + * @throws ParseException If PathPlanner GUI settings is nonexistent. + */ + private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) + throws IOException, ParseException + { + SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(RobotConfig.fromGUISettings(), + swerveDrive.getMaximumAngularVelocity()); + AtomicReference prevSetpoint + = new AtomicReference<>(new SwerveSetpoint(swerveDrive.getRobotVelocity(), + swerveDrive.getStates(), + DriveFeedforwards.zeros(swerveDrive.getModules().length))); + double start = Timer.getFPGATimestamp(); + return run(() -> { + SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint(prevSetpoint.get(), + robotRelativeChassisSpeed.get(), + start - Timer.getFPGATimestamp()); + swerveDrive.drive(newSetpoint.robotRelativeSpeeds(), + newSetpoint.moduleStates(), + newSetpoint.feedforwards().torqueCurrentsAmps()); + prevSetpoint.set(newSetpoint); + + + }); + } + + public Command driveWithSetpointGenerator(DoubleSupplier translationX, DoubleSupplier translationY, + DoubleSupplier rotationX) + { + SwerveController swerveController = swerveDrive.getSwerveController(); + try + { + return driveWithSetpointGenerator(() -> { + Translation2d scaledInputs = SwerveMath.scaleTranslation(new Translation2d(translationX.getAsDouble(), + translationY.getAsDouble()), 0.8); + + return new ChassisSpeeds(scaledInputs.getX(), + scaledInputs.getY(), + rotationX.getAsDouble() * swerveDrive.getMaximumAngularVelocity()); + }); + } catch (Exception e) + { + DriverStation.reportError(e.toString(), true); + } + return Commands.none(); + + } + + /** * Command to drive the robot using translative values and heading as a setpoint. * diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 1693575d..74ed27f6 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -31,6 +31,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -584,10 +585,6 @@ public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d cent } // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO) - { - SmartDashboard.putString("RobotVelocity", velocity.toString()); - } if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { SwerveDriveTelemetry.desiredChassisSpeedsObj = velocity; @@ -692,6 +689,27 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo } } + /** + * Drive the robot using the {@link SwerveModuleState[]}, it is recommended to have + * {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this. + * + * @param robotRelativeVelocity Robot relative {@link ChassisSpeeds} + * @param states Corresponding {@link SwerveModuleState[]} to use (not checked against the + * {@param robotRelativeVelocity}). + * @param feedforwardAmp Feedforward in amperage. (Ignored for now) + */ + public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, double[] feedforwardAmp) + { + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity; + } + for (SwerveModule module : swerveModules) + { + module.setDesiredState(states[module.moduleNumber], false, 0); //feedforwardAmp[module.moduleNumber]); + } + } + /** * Set chassis speeds with closed-loop velocity control. * @@ -1126,7 +1144,13 @@ public void updateOdometry() if (SwerveDriveTelemetry.isSimulation) { - SimulatedArena.getInstance().simulationPeriodic(); + try + { + SimulatedArena.getInstance().simulationPeriodic(); + } catch (Exception e) + { + DriverStation.reportError("MapleSim error", false); + } } // Update angle accumulator if the robot is simulated @@ -1496,4 +1520,19 @@ private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesC return velocity; } + /** + * Convert a {@link ChassisSpeeds} to {@link SwerveModuleState[]} for use elsewhere. + * + * @param velocity {@link ChassisSpeeds} velocity to use. + * @param optimize Perform chassis velocity correction or angular velocity correction. + * @return {@link SwerveModuleState[]} for use elsewhere. + */ + public SwerveModuleState[] toServeModuleStates(ChassisSpeeds velocity, boolean optimize) + { + if (optimize) + { + velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection); + } + return kinematics.toSwerveModuleStates(velocity); + } } diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 56b67022..b54e6b67 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -364,13 +364,14 @@ public void setAnglePIDF(PIDFConfig config) * Set the desired state of the swerve module.
    WARNING: If you are not using one of the functions from * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} * - * @param desiredState Desired swerve module state. - * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. - * @param force Disables optimizations that prevent movement in the angle motor and forces the desired state - * onto the swerve module. + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param force Disables optimizations that prevent movement in the angle motor and forces the + * desired state onto the swerve module. */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { + desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition())); // If we are forcing the angle @@ -384,6 +385,23 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, LinearVelocity velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState) : MetersPerSecond.of(desiredState.speedMetersPerSecond); + desiredState.speedMetersPerSecond = velocity.magnitude(); + + + setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(velocity).magnitude()); + } + + /** + * Set the desired state of the swerve module.
    WARNING: If you are not using one of the functions from + * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} + * + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param driveFeedforwardVoltage Drive motor controller feedforward as a voltage. + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, + double driveFeedforwardVoltage) + { if (isOpenLoop) { @@ -391,8 +409,7 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, driveMotor.set(percentOutput); } else { - driveMotor.setReference(velocity.magnitude(), driveMotorFeedforward.calculate(velocity).magnitude()); - desiredState.speedMetersPerSecond = velocity.magnitude(); + driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage); } // Prevent module rotation if angle is the same as the previous angle. @@ -724,17 +741,20 @@ public void invalidateCache() * Obtains the {@link SwerveModuleSimulation} used in simulation. * * @return the module simulation, null if this method is called on a real robot - * */ + */ public SwerveModuleSimulation getSimModule() { return simModule; } /** - * Configure the {@link SwerveModule#simModule} with the MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} - * @param swerveModuleSimulation + * Configure the {@link SwerveModule#simModule} with the MapleSim + * {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} + * + * @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to configure with. */ - public void configureModuleSimulation(org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) + public void configureModuleSimulation( + org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) { this.simModule.configureSimModule(swerveModuleSimulation); } From 6aa9ba1bcf04c3109fe414987f63fe62e969be14 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 16:13:33 -0600 Subject: [PATCH 52/54] Reformatted the code. --- src/main/java/swervelib/SwerveModule.java | 12 +++++----- src/main/java/swervelib/imu/NavXSwerve.java | 8 +++---- .../simulation/SwerveIMUSimulation.java | 9 ++++---- .../simulation/SwerveModuleSimulation.java | 23 ++++++++++++------- 4 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index b54e6b67..6b11317c 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -364,10 +364,10 @@ public void setAnglePIDF(PIDFConfig config) * Set the desired state of the swerve module.
    WARNING: If you are not using one of the functions from * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} * - * @param desiredState Desired swerve module state. - * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. - * @param force Disables optimizations that prevent movement in the angle motor and forces the - * desired state onto the swerve module. + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param force Disables optimizations that prevent movement in the angle motor and forces the desired state + * onto the swerve module. */ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { @@ -387,7 +387,6 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, : MetersPerSecond.of(desiredState.speedMetersPerSecond); desiredState.speedMetersPerSecond = velocity.magnitude(); - setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(velocity).magnitude()); } @@ -751,7 +750,8 @@ public SwerveModuleSimulation getSimModule() * Configure the {@link SwerveModule#simModule} with the MapleSim * {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} * - * @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to configure with. + * @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to + * configure with. */ public void configureModuleSimulation( org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation) diff --git a/src/main/java/swervelib/imu/NavXSwerve.java b/src/main/java/swervelib/imu/NavXSwerve.java index a7e73ce6..dd2797d6 100644 --- a/src/main/java/swervelib/imu/NavXSwerve.java +++ b/src/main/java/swervelib/imu/NavXSwerve.java @@ -41,10 +41,10 @@ public NavXSwerve(NavXComType port) navXError = new Alert("IMU", "Error instantiating NavX.", AlertType.kError); try { - /* Communicate w/navX-MXP via the MXP SPI Bus. */ - /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ - /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ - imu = new AHRS(port); + /* Communicate w/navX-MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */ + imu = new AHRS(port); factoryDefault(); } catch (RuntimeException ex) { diff --git a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java index d4c3c850..f9e3b995 100644 --- a/src/main/java/swervelib/simulation/SwerveIMUSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveIMUSimulation.java @@ -6,11 +6,9 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import org.ironmaple.simulation.drivesims.GyroSimulation; - import java.util.Optional; +import org.ironmaple.simulation.drivesims.GyroSimulation; /** * Simulation for {@link swervelib.SwerveDrive} IMU. @@ -23,8 +21,9 @@ public class SwerveIMUSimulation /** * Create the swerve drive IMU simulation. */ - public SwerveIMUSimulation(GyroSimulation gyroSimulation) { - this.gyroSimulation = gyroSimulation; + public SwerveIMUSimulation(GyroSimulation gyroSimulation) + { + this.gyroSimulation = gyroSimulation; } /** diff --git a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java index 91570102..97b85ba2 100644 --- a/src/main/java/swervelib/simulation/SwerveModuleSimulation.java +++ b/src/main/java/swervelib/simulation/SwerveModuleSimulation.java @@ -1,28 +1,35 @@ package swervelib.simulation; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radian; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import org.ironmaple.simulation.motorsims.ControlRequest; -import static edu.wpi.first.units.Units.*; - /** * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} */ public class SwerveModuleSimulation { + private org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule = null; /** * Configure the maple sim module + * * @param mapleSimModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation */ - public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) { + public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation mapleSimModule) + { this.mapleSimModule = mapleSimModule; mapleSimModule.getDriveMotorConfigs() - .withDefaultFeedForward(Volts.zero()) - .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0/3000.0), true); + .withDefaultFeedForward(Volts.zero()) + .withVelocityVoltageController(Volts.per(RPM).ofNative(7.0 / 3000.0), true); } /** @@ -35,7 +42,7 @@ public void updateStateAndPosition(SwerveModuleState desiredState) { mapleSimModule.requestSteerControl(new ControlRequest.PositionVoltage(desiredState.angle.getMeasure())); mapleSimModule.requestDriveControl(new ControlRequest.VelocityVoltage( - RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS.in(Meters)) + RadiansPerSecond.of(desiredState.speedMetersPerSecond / mapleSimModule.WHEEL_RADIUS.in(Meters)) )); } @@ -47,8 +54,8 @@ public void updateStateAndPosition(SwerveModuleState desiredState) public SwerveModulePosition getPosition() { return new SwerveModulePosition( - mapleSimModule.getDriveWheelFinalPosition().in(Radian) * mapleSimModule.WHEEL_RADIUS.in(Meters), - mapleSimModule.getSteerAbsoluteFacing() + mapleSimModule.getDriveWheelFinalPosition().in(Radian) * mapleSimModule.WHEEL_RADIUS.in(Meters), + mapleSimModule.getSteerAbsoluteFacing() ); } From 3619c0796190bc03a61d4096eac542f64107fff0 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Tue, 3 Dec 2024 16:16:32 -0600 Subject: [PATCH 53/54] Added TODO. --- .../java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 953c1450..e3c90245 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -328,8 +328,6 @@ public Command driveToPose(Pose2d pose) ); } - // TODO: Make a drive command with PathPlanner setpoint generator, derived off of 254's setpoint generator - /** * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner. * @@ -355,6 +353,7 @@ private Command driveWithSetpointGenerator(Supplier robotRelative swerveDrive.drive(newSetpoint.robotRelativeSpeeds(), newSetpoint.moduleStates(), newSetpoint.feedforwards().torqueCurrentsAmps()); + // TODO: Convert the amp feedforward to usable generic feedforward. Currently it is ignored. prevSetpoint.set(newSetpoint); From a92da81edd174f0ccd6eef75a8fa78fe12d3a9e3 Mon Sep 17 00:00:00 2001 From: Etaash Mathamsetty Date: Tue, 3 Dec 2024 17:43:14 -0500 Subject: [PATCH 54/54] Implement support for kA --- src/main/java/swervelib/SwerveModule.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 6b11317c..e989cdab 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -372,7 +372,7 @@ public void setAnglePIDF(PIDFConfig config) public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) { - desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition())); + desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition())); // If we are forcing the angle if (!force && antiJitterEnabled) @@ -382,12 +382,13 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, } // Cosine compensation. - LinearVelocity velocity = configuration.useCosineCompensator + LinearVelocity nextVelocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState) : MetersPerSecond.of(desiredState.speedMetersPerSecond); - desiredState.speedMetersPerSecond = velocity.magnitude(); + LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond); + desiredState.speedMetersPerSecond = nextVelocity.magnitude(); - setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(velocity).magnitude()); + setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(curVelocity, nextVelocity).magnitude()); } /**