From 47f0425c75011cded7ca0f4f865c7c7562ff145d Mon Sep 17 00:00:00 2001 From: Benjamin Hall Date: Thu, 31 Oct 2024 17:11:04 -0400 Subject: [PATCH] Add C++ SwerveWithChoreo --- cpp/SwerveWithChoreo/.gitignore | 184 ++++ .../.wpilib/wpilib_preferences.json | 6 + cpp/SwerveWithChoreo/WPILib-License.md | 24 + cpp/SwerveWithChoreo/build.gradle | 99 ++ .../gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43583 bytes .../gradle/wrapper/gradle-wrapper.properties | 7 + cpp/SwerveWithChoreo/gradlew | 252 +++++ cpp/SwerveWithChoreo/gradlew.bat | 94 ++ cpp/SwerveWithChoreo/settings.gradle | 30 + .../src/main/cpp/AutoRoutines.cpp | 24 + cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp | 74 ++ .../src/main/cpp/RobotContainer.cpp | 63 ++ .../src/main/cpp/Telemetry.cpp | 43 + .../subsystems/CommandSwerveDrivetrain.cpp | 67 ++ .../src/main/deploy/choreo/SimplePath.traj | 67 ++ .../src/main/deploy/choreo/Tests.chor | 74 ++ .../src/main/deploy/example.txt | 3 + .../src/main/include/AutoRoutines.h | 17 + .../src/main/include/LimelightHelpers.h | 877 ++++++++++++++++++ cpp/SwerveWithChoreo/src/main/include/Robot.h | 37 + .../src/main/include/RobotContainer.h | 53 ++ .../src/main/include/Telemetry.h | 75 ++ .../main/include/generated/TunerConstants.h | 168 ++++ .../subsystems/CommandSwerveDrivetrain.h | 242 +++++ cpp/SwerveWithChoreo/src/test/cpp/main.cpp | 10 + cpp/SwerveWithChoreo/tuner-project.json | 1 + .../vendordeps/ChoreoLib2025Beta.json | 44 + cpp/SwerveWithChoreo/vendordeps/Phoenix6.json | 342 +++++++ .../vendordeps/WPILibNewCommands.json | 38 + .../src/main/java/frc/robot/AutoRoutines.java | 12 +- .../src/main/java/frc/robot/Robot.java | 2 +- .../main/java/frc/robot/RobotContainer.java | 4 +- 32 files changed, 3023 insertions(+), 10 deletions(-) create mode 100644 cpp/SwerveWithChoreo/.gitignore create mode 100644 cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json create mode 100644 cpp/SwerveWithChoreo/WPILib-License.md create mode 100644 cpp/SwerveWithChoreo/build.gradle create mode 100644 cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar create mode 100644 cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties create mode 100644 cpp/SwerveWithChoreo/gradlew create mode 100644 cpp/SwerveWithChoreo/gradlew.bat create mode 100644 cpp/SwerveWithChoreo/settings.gradle create mode 100644 cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp create mode 100644 cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp create mode 100644 cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp create mode 100644 cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp create mode 100644 cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp create mode 100644 cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj create mode 100644 cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor create mode 100644 cpp/SwerveWithChoreo/src/main/deploy/example.txt create mode 100644 cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/Robot.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/RobotContainer.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/Telemetry.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h create mode 100644 cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h create mode 100644 cpp/SwerveWithChoreo/src/test/cpp/main.cpp create mode 100644 cpp/SwerveWithChoreo/tuner-project.json create mode 100644 cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json create mode 100644 cpp/SwerveWithChoreo/vendordeps/Phoenix6.json create mode 100644 cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json diff --git a/cpp/SwerveWithChoreo/.gitignore b/cpp/SwerveWithChoreo/.gitignore new file mode 100644 index 00000000..375359c2 --- /dev/null +++ b/cpp/SwerveWithChoreo/.gitignore @@ -0,0 +1,184 @@ +# This gitignore has been specially created by the WPILib team. +# If you remove items from this file, intellisense might break. + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +# DO NOT REMOVE .classpath and .project +.classpath +.project +.settings/ +bin/ + +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + +# Simulation GUI and other tools window save file +networktables.json +simgui.json +*-window.json + +# Simulation data log directory +logs/ + +# Folder that has CTRE Phoenix Sim device config storage +ctre_sim/ + +# clangd +/.cache +compile_commands.json diff --git a/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json b/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..4b0bb1c1 --- /dev/null +++ b/cpp/SwerveWithChoreo/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2025beta", + "teamNumber": 7762 +} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/WPILib-License.md b/cpp/SwerveWithChoreo/WPILib-License.md new file mode 100644 index 00000000..645e5425 --- /dev/null +++ b/cpp/SwerveWithChoreo/WPILib-License.md @@ -0,0 +1,24 @@ +Copyright (c) 2009-2024 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/cpp/SwerveWithChoreo/build.gradle b/cpp/SwerveWithChoreo/build.gradle new file mode 100644 index 00000000..99861ab2 --- /dev/null +++ b/cpp/SwerveWithChoreo/build.gradle @@ -0,0 +1,99 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1" +} + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project DeployUtils. +deploy { + targets { + roborio(getTargetTypeClass('RoboRIO')) { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = project.frc.getTeamNumber() + debug = project.frc.getDebugOrDefault(false) + + artifacts { + // First part is artifact name, 2nd is artifact type + // getTargetTypeClass is a shortcut to get the class type using a string + + frcCpp(getArtifactTypeClass('FRCNativeArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcCpp + +// Set this to true to enable desktop support. +def includeDesktopSupport = true + +// Set to true to run simulation in debug mode +wpi.cpp.debugSimulation = false + +// Default enable simgui +wpi.sim.addGui().defaultEnabled = true +// Enable DS but not by default +wpi.sim.addDriverstation() + +model { + components { + frcUserProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.roborio + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + } + } + + // Set deploy task to deploy this component + deployArtifact.component = it + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + // Enable simulation for this component + wpi.sim.enable(it) + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + // Enable run tasks for this component + wpi.cpp.enableExternalTasks(it) + + wpi.cpp.vendor.cpp(it) + wpi.cpp.deps.wpilib(it) + wpi.cpp.deps.googleTest(it) + } + } +} diff --git a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar b/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000000000000000000000000000000000000..a4b76b9530d66f5e68d973ea569d8e19de379189 GIT binary patch literal 43583 zcma&N1CXTcmMvW9vTb(Rwr$&4wr$(C?dmSu>@vG-+vuvg^_??!{yS%8zW-#zn-LkA z5&1^$^{lnmUON?}LBF8_K|(?T0Ra(xUH{($5eN!MR#ZihR#HxkUPe+_R8Cn`RRs(P z_^*#_XlXmGv7!4;*Y%p4nw?{bNp@UZHv1?Um8r6)Fei3p@ClJn0ECfg1hkeuUU@Or zDaPa;U3fE=3L}DooL;8f;P0ipPt0Z~9P0)lbStMS)ag54=uL9ia-Lm3nh|@(Y?B`; zx_#arJIpXH!U{fbCbI^17}6Ri*H<>OLR%c|^mh8+)*h~K8Z!9)DPf zR2h?lbDZQ`p9P;&DQ4F0sur@TMa!Y}S8irn(%d-gi0*WxxCSk*A?3lGh=gcYN?FGl z7D=Js!i~0=u3rox^eO3i@$0=n{K1lPNU zwmfjRVmLOCRfe=seV&P*1Iq=^i`502keY8Uy-WNPwVNNtJFx?IwAyRPZo2Wo1+S(xF37LJZ~%i)kpFQ3Fw=mXfd@>%+)RpYQLnr}B~~zoof(JVm^^&f zxKV^+3D3$A1G;qh4gPVjhrC8e(VYUHv#dy^)(RoUFM?o%W-EHxufuWf(l*@-l+7vt z=l`qmR56K~F|v<^Pd*p~1_y^P0P^aPC##d8+HqX4IR1gu+7w#~TBFphJxF)T$2WEa zxa?H&6=Qe7d(#tha?_1uQys2KtHQ{)Qco)qwGjrdNL7thd^G5i8Os)CHqc>iOidS} z%nFEDdm=GXBw=yXe1W-ShHHFb?Cc70+$W~z_+}nAoHFYI1MV1wZegw*0y^tC*s%3h zhD3tN8b=Gv&rj}!SUM6|ajSPp*58KR7MPpI{oAJCtY~JECm)*m_x>AZEu>DFgUcby z1Qaw8lU4jZpQ_$;*7RME+gq1KySGG#Wql>aL~k9tLrSO()LWn*q&YxHEuzmwd1?aAtI zBJ>P=&$=l1efe1CDU;`Fd+_;&wI07?V0aAIgc(!{a z0Jg6Y=inXc3^n!U0Atk`iCFIQooHqcWhO(qrieUOW8X(x?(RD}iYDLMjSwffH2~tB z)oDgNBLB^AJBM1M^c5HdRx6fBfka`(LD-qrlh5jqH~);#nw|iyp)()xVYak3;Ybik z0j`(+69aK*B>)e_p%=wu8XC&9e{AO4c~O1U`5X9}?0mrd*m$_EUek{R?DNSh(=br# z#Q61gBzEpmy`$pA*6!87 zSDD+=@fTY7<4A?GLqpA?Pb2z$pbCc4B4zL{BeZ?F-8`s$?>*lXXtn*NC61>|*w7J* z$?!iB{6R-0=KFmyp1nnEmLsA-H0a6l+1uaH^g%c(p{iT&YFrbQ$&PRb8Up#X3@Zsk zD^^&LK~111%cqlP%!_gFNa^dTYT?rhkGl}5=fL{a`UViaXWI$k-UcHJwmaH1s=S$4 z%4)PdWJX;hh5UoK?6aWoyLxX&NhNRqKam7tcOkLh{%j3K^4Mgx1@i|Pi&}<^5>hs5 zm8?uOS>%)NzT(%PjVPGa?X%`N2TQCKbeH2l;cTnHiHppPSJ<7y-yEIiC!P*ikl&!B z%+?>VttCOQM@ShFguHVjxX^?mHX^hSaO_;pnyh^v9EumqSZTi+#f&_Vaija0Q-e*| z7ulQj6Fs*bbmsWp{`auM04gGwsYYdNNZcg|ph0OgD>7O}Asn7^Z=eI>`$2*v78;sj-}oMoEj&@)9+ycEOo92xSyY344^ z11Hb8^kdOvbf^GNAK++bYioknrpdN>+u8R?JxG=!2Kd9r=YWCOJYXYuM0cOq^FhEd zBg2puKy__7VT3-r*dG4c62Wgxi52EMCQ`bKgf*#*ou(D4-ZN$+mg&7$u!! z-^+Z%;-3IDwqZ|K=ah85OLwkO zKxNBh+4QHh)u9D?MFtpbl)us}9+V!D%w9jfAMYEb>%$A;u)rrI zuBudh;5PN}_6J_}l55P3l_)&RMlH{m!)ai-i$g)&*M`eN$XQMw{v^r@-125^RRCF0 z^2>|DxhQw(mtNEI2Kj(;KblC7x=JlK$@78`O~>V!`|1Lm-^JR$-5pUANAnb(5}B}JGjBsliK4& zk6y(;$e&h)lh2)L=bvZKbvh@>vLlreBdH8No2>$#%_Wp1U0N7Ank!6$dFSi#xzh|( zRi{Uw%-4W!{IXZ)fWx@XX6;&(m_F%c6~X8hx=BN1&q}*( zoaNjWabE{oUPb!Bt$eyd#$5j9rItB-h*5JiNi(v^e|XKAj*8(k<5-2$&ZBR5fF|JA z9&m4fbzNQnAU}r8ab>fFV%J0z5awe#UZ|bz?Ur)U9bCIKWEzi2%A+5CLqh?}K4JHi z4vtM;+uPsVz{Lfr;78W78gC;z*yTch~4YkLr&m-7%-xc ztw6Mh2d>_iO*$Rd8(-Cr1_V8EO1f*^@wRoSozS) zy1UoC@pruAaC8Z_7~_w4Q6n*&B0AjOmMWa;sIav&gu z|J5&|{=a@vR!~k-OjKEgPFCzcJ>#A1uL&7xTDn;{XBdeM}V=l3B8fE1--DHjSaxoSjNKEM9|U9#m2<3>n{Iuo`r3UZp;>GkT2YBNAh|b z^jTq-hJp(ebZh#Lk8hVBP%qXwv-@vbvoREX$TqRGTgEi$%_F9tZES@z8Bx}$#5eeG zk^UsLBH{bc2VBW)*EdS({yw=?qmevwi?BL6*=12k9zM5gJv1>y#ML4!)iiPzVaH9% zgSImetD@dam~e>{LvVh!phhzpW+iFvWpGT#CVE5TQ40n%F|p(sP5mXxna+Ev7PDwA zamaV4m*^~*xV+&p;W749xhb_X=$|LD;FHuB&JL5?*Y2-oIT(wYY2;73<^#46S~Gx| z^cez%V7x$81}UWqS13Gz80379Rj;6~WdiXWOSsdmzY39L;Hg3MH43o*y8ibNBBH`(av4|u;YPq%{R;IuYow<+GEsf@R?=@tT@!}?#>zIIn0CoyV!hq3mw zHj>OOjfJM3F{RG#6ujzo?y32m^tgSXf@v=J$ELdJ+=5j|=F-~hP$G&}tDZsZE?5rX ztGj`!S>)CFmdkccxM9eGIcGnS2AfK#gXwj%esuIBNJQP1WV~b~+D7PJTmWGTSDrR` zEAu4B8l>NPuhsk5a`rReSya2nfV1EK01+G!x8aBdTs3Io$u5!6n6KX%uv@DxAp3F@{4UYg4SWJtQ-W~0MDb|j-$lwVn znAm*Pl!?Ps&3wO=R115RWKb*JKoexo*)uhhHBncEDMSVa_PyA>k{Zm2(wMQ(5NM3# z)jkza|GoWEQo4^s*wE(gHz?Xsg4`}HUAcs42cM1-qq_=+=!Gk^y710j=66(cSWqUe zklbm8+zB_syQv5A2rj!Vbw8;|$@C!vfNmNV!yJIWDQ>{+2x zKjuFX`~~HKG~^6h5FntRpnnHt=D&rq0>IJ9#F0eM)Y-)GpRjiN7gkA8wvnG#K=q{q z9dBn8_~wm4J<3J_vl|9H{7q6u2A!cW{bp#r*-f{gOV^e=8S{nc1DxMHFwuM$;aVI^ zz6A*}m8N-&x8;aunp1w7_vtB*pa+OYBw=TMc6QK=mbA-|Cf* zvyh8D4LRJImooUaSb7t*fVfih<97Gf@VE0|z>NcBwBQze);Rh!k3K_sfunToZY;f2 z^HmC4KjHRVg+eKYj;PRN^|E0>Gj_zagfRbrki68I^#~6-HaHg3BUW%+clM1xQEdPYt_g<2K+z!$>*$9nQ>; zf9Bei{?zY^-e{q_*|W#2rJG`2fy@{%6u0i_VEWTq$*(ZN37|8lFFFt)nCG({r!q#9 z5VK_kkSJ3?zOH)OezMT{!YkCuSSn!K#-Rhl$uUM(bq*jY? zi1xbMVthJ`E>d>(f3)~fozjg^@eheMF6<)I`oeJYx4*+M&%c9VArn(OM-wp%M<-`x z7sLP1&3^%Nld9Dhm@$3f2}87!quhI@nwd@3~fZl_3LYW-B?Ia>ui`ELg z&Qfe!7m6ze=mZ`Ia9$z|ARSw|IdMpooY4YiPN8K z4B(ts3p%2i(Td=tgEHX z0UQ_>URBtG+-?0E;E7Ld^dyZ;jjw0}XZ(}-QzC6+NN=40oDb2^v!L1g9xRvE#@IBR zO!b-2N7wVfLV;mhEaXQ9XAU+>=XVA6f&T4Z-@AX!leJ8obP^P^wP0aICND?~w&NykJ#54x3_@r7IDMdRNy4Hh;h*!u(Ol(#0bJdwEo$5437-UBjQ+j=Ic>Q2z` zJNDf0yO6@mr6y1#n3)s(W|$iE_i8r@Gd@!DWDqZ7J&~gAm1#~maIGJ1sls^gxL9LLG_NhU!pTGty!TbhzQnu)I*S^54U6Yu%ZeCg`R>Q zhBv$n5j0v%O_j{QYWG!R9W?5_b&67KB$t}&e2LdMvd(PxN6Ir!H4>PNlerpBL>Zvyy!yw z-SOo8caEpDt(}|gKPBd$qND5#a5nju^O>V&;f890?yEOfkSG^HQVmEbM3Ugzu+UtH zC(INPDdraBN?P%kE;*Ae%Wto&sgw(crfZ#Qy(<4nk;S|hD3j{IQRI6Yq|f^basLY; z-HB&Je%Gg}Jt@={_C{L$!RM;$$|iD6vu#3w?v?*;&()uB|I-XqEKqZPS!reW9JkLewLb!70T7n`i!gNtb1%vN- zySZj{8-1>6E%H&=V}LM#xmt`J3XQoaD|@XygXjdZ1+P77-=;=eYpoEQ01B@L*a(uW zrZeZz?HJsw_4g0vhUgkg@VF8<-X$B8pOqCuWAl28uB|@r`19DTUQQsb^pfqB6QtiT z*`_UZ`fT}vtUY#%sq2{rchyfu*pCg;uec2$-$N_xgjZcoumE5vSI{+s@iLWoz^Mf; zuI8kDP{!XY6OP~q5}%1&L}CtfH^N<3o4L@J@zg1-mt{9L`s^z$Vgb|mr{@WiwAqKg zp#t-lhrU>F8o0s1q_9y`gQNf~Vb!F%70f}$>i7o4ho$`uciNf=xgJ>&!gSt0g;M>*x4-`U)ysFW&Vs^Vk6m%?iuWU+o&m(2Jm26Y(3%TL; zA7T)BP{WS!&xmxNw%J=$MPfn(9*^*TV;$JwRy8Zl*yUZi8jWYF>==j~&S|Xinsb%c z2?B+kpet*muEW7@AzjBA^wAJBY8i|#C{WtO_or&Nj2{=6JTTX05}|H>N2B|Wf!*3_ z7hW*j6p3TvpghEc6-wufFiY!%-GvOx*bZrhZu+7?iSrZL5q9}igiF^*R3%DE4aCHZ zqu>xS8LkW+Auv%z-<1Xs92u23R$nk@Pk}MU5!gT|c7vGlEA%G^2th&Q*zfg%-D^=f z&J_}jskj|Q;73NP4<4k*Y%pXPU2Thoqr+5uH1yEYM|VtBPW6lXaetokD0u z9qVek6Q&wk)tFbQ8(^HGf3Wp16gKmr>G;#G(HRBx?F`9AIRboK+;OfHaLJ(P>IP0w zyTbTkx_THEOs%Q&aPrxbZrJlio+hCC_HK<4%f3ZoSAyG7Dn`=X=&h@m*|UYO-4Hq0 z-Bq&+Ie!S##4A6OGoC~>ZW`Y5J)*ouaFl_e9GA*VSL!O_@xGiBw!AF}1{tB)z(w%c zS1Hmrb9OC8>0a_$BzeiN?rkPLc9%&;1CZW*4}CDDNr2gcl_3z+WC15&H1Zc2{o~i) z)LLW=WQ{?ricmC`G1GfJ0Yp4Dy~Ba;j6ZV4r{8xRs`13{dD!xXmr^Aga|C=iSmor% z8hi|pTXH)5Yf&v~exp3o+sY4B^^b*eYkkCYl*T{*=-0HniSA_1F53eCb{x~1k3*`W zr~};p1A`k{1DV9=UPnLDgz{aJH=-LQo<5%+Em!DNN252xwIf*wF_zS^!(XSm(9eoj z=*dXG&n0>)_)N5oc6v!>-bd(2ragD8O=M|wGW z!xJQS<)u70m&6OmrF0WSsr@I%T*c#Qo#Ha4d3COcX+9}hM5!7JIGF>7<~C(Ear^Sn zm^ZFkV6~Ula6+8S?oOROOA6$C&q&dp`>oR-2Ym3(HT@O7Sd5c~+kjrmM)YmgPH*tL zX+znN>`tv;5eOfX?h{AuX^LK~V#gPCu=)Tigtq9&?7Xh$qN|%A$?V*v=&-2F$zTUv z`C#WyIrChS5|Kgm_GeudCFf;)!WH7FI60j^0o#65o6`w*S7R@)88n$1nrgU(oU0M9 zx+EuMkC>(4j1;m6NoGqEkpJYJ?vc|B zOlwT3t&UgL!pX_P*6g36`ZXQ; z9~Cv}ANFnJGp(;ZhS(@FT;3e)0)Kp;h^x;$*xZn*k0U6-&FwI=uOGaODdrsp-!K$Ac32^c{+FhI-HkYd5v=`PGsg%6I`4d9Jy)uW0y%) zm&j^9WBAp*P8#kGJUhB!L?a%h$hJgQrx!6KCB_TRo%9{t0J7KW8!o1B!NC)VGLM5! zpZy5Jc{`r{1e(jd%jsG7k%I+m#CGS*BPA65ZVW~fLYw0dA-H_}O zrkGFL&P1PG9p2(%QiEWm6x;U-U&I#;Em$nx-_I^wtgw3xUPVVu zqSuKnx&dIT-XT+T10p;yjo1Y)z(x1fb8Dzfn8e yu?e%!_ptzGB|8GrCfu%p?(_ zQccdaaVK$5bz;*rnyK{_SQYM>;aES6Qs^lj9lEs6_J+%nIiuQC*fN;z8md>r_~Mfl zU%p5Dt_YT>gQqfr@`cR!$NWr~+`CZb%dn;WtzrAOI>P_JtsB76PYe*<%H(y>qx-`Kq!X_; z<{RpAqYhE=L1r*M)gNF3B8r(<%8mo*SR2hu zccLRZwGARt)Hlo1euqTyM>^!HK*!Q2P;4UYrysje@;(<|$&%vQekbn|0Ruu_Io(w4#%p6ld2Yp7tlA`Y$cciThP zKzNGIMPXX%&Ud0uQh!uQZz|FB`4KGD?3!ND?wQt6!n*f4EmCoJUh&b?;B{|lxs#F- z31~HQ`SF4x$&v00@(P+j1pAaj5!s`)b2RDBp*PB=2IB>oBF!*6vwr7Dp%zpAx*dPr zb@Zjq^XjN?O4QcZ*O+8>)|HlrR>oD*?WQl5ri3R#2?*W6iJ>>kH%KnnME&TT@ZzrHS$Q%LC?n|e>V+D+8D zYc4)QddFz7I8#}y#Wj6>4P%34dZH~OUDb?uP%-E zwjXM(?Sg~1!|wI(RVuxbu)-rH+O=igSho_pDCw(c6b=P zKk4ATlB?bj9+HHlh<_!&z0rx13K3ZrAR8W)!@Y}o`?a*JJsD+twZIv`W)@Y?Amu_u zz``@-e2X}27$i(2=9rvIu5uTUOVhzwu%mNazS|lZb&PT;XE2|B&W1>=B58#*!~D&) zfVmJGg8UdP*fx(>Cj^?yS^zH#o-$Q-*$SnK(ZVFkw+er=>N^7!)FtP3y~Xxnu^nzY zikgB>Nj0%;WOltWIob|}%lo?_C7<``a5hEkx&1ku$|)i>Rh6@3h*`slY=9U}(Ql_< zaNG*J8vb&@zpdhAvv`?{=zDedJ23TD&Zg__snRAH4eh~^oawdYi6A3w8<Ozh@Kw)#bdktM^GVb zrG08?0bG?|NG+w^&JvD*7LAbjED{_Zkc`3H!My>0u5Q}m!+6VokMLXxl`Mkd=g&Xx z-a>m*#G3SLlhbKB!)tnzfWOBV;u;ftU}S!NdD5+YtOjLg?X}dl>7m^gOpihrf1;PY zvll&>dIuUGs{Qnd- zwIR3oIrct8Va^Tm0t#(bJD7c$Z7DO9*7NnRZorrSm`b`cxz>OIC;jSE3DO8`hX955ui`s%||YQtt2 z5DNA&pG-V+4oI2s*x^>-$6J?p=I>C|9wZF8z;VjR??Icg?1w2v5Me+FgAeGGa8(3S z4vg*$>zC-WIVZtJ7}o9{D-7d>zCe|z#<9>CFve-OPAYsneTb^JH!Enaza#j}^mXy1 z+ULn^10+rWLF6j2>Ya@@Kq?26>AqK{A_| zQKb*~F1>sE*=d?A?W7N2j?L09_7n+HGi{VY;MoTGr_)G9)ot$p!-UY5zZ2Xtbm=t z@dpPSGwgH=QtIcEulQNI>S-#ifbnO5EWkI;$A|pxJd885oM+ zGZ0_0gDvG8q2xebj+fbCHYfAXuZStH2j~|d^sBAzo46(K8n59+T6rzBwK)^rfPT+B zyIFw)9YC-V^rhtK`!3jrhmW-sTmM+tPH+;nwjL#-SjQPUZ53L@A>y*rt(#M(qsiB2 zx6B)dI}6Wlsw%bJ8h|(lhkJVogQZA&n{?Vgs6gNSXzuZpEyu*xySy8ro07QZ7Vk1!3tJphN_5V7qOiyK8p z#@jcDD8nmtYi1^l8ml;AF<#IPK?!pqf9D4moYk>d99Im}Jtwj6c#+A;f)CQ*f-hZ< z=p_T86jog%!p)D&5g9taSwYi&eP z#JuEK%+NULWus;0w32-SYFku#i}d~+{Pkho&^{;RxzP&0!RCm3-9K6`>KZpnzS6?L z^H^V*s!8<>x8bomvD%rh>Zp3>Db%kyin;qtl+jAv8Oo~1g~mqGAC&Qi_wy|xEt2iz zWAJEfTV%cl2Cs<1L&DLRVVH05EDq`pH7Oh7sR`NNkL%wi}8n>IXcO40hp+J+sC!W?!krJf!GJNE8uj zg-y~Ns-<~D?yqbzVRB}G>0A^f0!^N7l=$m0OdZuqAOQqLc zX?AEGr1Ht+inZ-Qiwnl@Z0qukd__a!C*CKuGdy5#nD7VUBM^6OCpxCa2A(X;e0&V4 zM&WR8+wErQ7UIc6LY~Q9x%Sn*Tn>>P`^t&idaOEnOd(Ufw#>NoR^1QdhJ8s`h^|R_ zXX`c5*O~Xdvh%q;7L!_!ohf$NfEBmCde|#uVZvEo>OfEq%+Ns7&_f$OR9xsihRpBb z+cjk8LyDm@U{YN>+r46?nn{7Gh(;WhFw6GAxtcKD+YWV?uge>;+q#Xx4!GpRkVZYu zzsF}1)7$?%s9g9CH=Zs+B%M_)+~*j3L0&Q9u7!|+T`^O{xE6qvAP?XWv9_MrZKdo& z%IyU)$Q95AB4!#hT!_dA>4e@zjOBD*Y=XjtMm)V|+IXzjuM;(l+8aA5#Kaz_$rR6! zj>#&^DidYD$nUY(D$mH`9eb|dtV0b{S>H6FBfq>t5`;OxA4Nn{J(+XihF(stSche7$es&~N$epi&PDM_N`As;*9D^L==2Q7Z2zD+CiU(|+-kL*VG+&9!Yb3LgPy?A zm7Z&^qRG_JIxK7-FBzZI3Q<;{`DIxtc48k> zc|0dmX;Z=W$+)qE)~`yn6MdoJ4co;%!`ddy+FV538Y)j(vg}5*k(WK)KWZ3WaOG!8 z!syGn=s{H$odtpqFrT#JGM*utN7B((abXnpDM6w56nhw}OY}0TiTG1#f*VFZr+^-g zbP10`$LPq_;PvrA1XXlyx2uM^mrjTzX}w{yuLo-cOClE8MMk47T25G8M!9Z5ypOSV zAJUBGEg5L2fY)ZGJb^E34R2zJ?}Vf>{~gB!8=5Z) z9y$>5c)=;o0HeHHSuE4U)#vG&KF|I%-cF6f$~pdYJWk_dD}iOA>iA$O$+4%@>JU08 zS`ep)$XLPJ+n0_i@PkF#ri6T8?ZeAot$6JIYHm&P6EB=BiaNY|aA$W0I+nz*zkz_z zkEru!tj!QUffq%)8y0y`T&`fuus-1p>=^hnBiBqD^hXrPs`PY9tU3m0np~rISY09> z`P3s=-kt_cYcxWd{de@}TwSqg*xVhp;E9zCsnXo6z z?f&Sv^U7n4`xr=mXle94HzOdN!2kB~4=%)u&N!+2;z6UYKUDqi-s6AZ!haB;@&B`? z_TRX0%@suz^TRdCb?!vNJYPY8L_}&07uySH9%W^Tc&1pia6y1q#?*Drf}GjGbPjBS zbOPcUY#*$3sL2x4v_i*Y=N7E$mR}J%|GUI(>WEr+28+V z%v5{#e!UF*6~G&%;l*q*$V?&r$Pp^sE^i-0$+RH3ERUUdQ0>rAq2(2QAbG}$y{de( z>{qD~GGuOk559Y@%$?N^1ApVL_a704>8OD%8Y%8B;FCt%AoPu8*D1 zLB5X>b}Syz81pn;xnB}%0FnwazlWfUV)Z-~rZg6~b z6!9J$EcE&sEbzcy?CI~=boWA&eeIa%z(7SE^qgVLz??1Vbc1*aRvc%Mri)AJaAG!p z$X!_9Ds;Zz)f+;%s&dRcJt2==P{^j3bf0M=nJd&xwUGlUFn?H=2W(*2I2Gdu zv!gYCwM10aeus)`RIZSrCK=&oKaO_Ry~D1B5!y0R=%!i2*KfXGYX&gNv_u+n9wiR5 z*e$Zjju&ODRW3phN925%S(jL+bCHv6rZtc?!*`1TyYXT6%Ju=|X;6D@lq$8T zW{Y|e39ioPez(pBH%k)HzFITXHvnD6hw^lIoUMA;qAJ^CU?top1fo@s7xT13Fvn1H z6JWa-6+FJF#x>~+A;D~;VDs26>^oH0EI`IYT2iagy23?nyJ==i{g4%HrAf1-*v zK1)~@&(KkwR7TL}L(A@C_S0G;-GMDy=MJn2$FP5s<%wC)4jC5PXoxrQBFZ_k0P{{s@sz+gX`-!=T8rcB(=7vW}^K6oLWMmp(rwDh}b zwaGGd>yEy6fHv%jM$yJXo5oMAQ>c9j`**}F?MCry;T@47@r?&sKHgVe$MCqk#Z_3S z1GZI~nOEN*P~+UaFGnj{{Jo@16`(qVNtbU>O0Hf57-P>x8Jikp=`s8xWs^dAJ9lCQ z)GFm+=OV%AMVqVATtN@|vp61VVAHRn87}%PC^RAzJ%JngmZTasWBAWsoAqBU+8L8u z4A&Pe?fmTm0?mK-BL9t+{y7o(7jm+RpOhL9KnY#E&qu^}B6=K_dB}*VlSEiC9fn)+V=J;OnN)Ta5v66ic1rG+dGAJ1 z1%Zb_+!$=tQ~lxQrzv3x#CPb?CekEkA}0MYSgx$Jdd}q8+R=ma$|&1a#)TQ=l$1tQ z=tL9&_^vJ)Pk}EDO-va`UCT1m#Uty1{v^A3P~83_#v^ozH}6*9mIjIr;t3Uv%@VeW zGL6(CwCUp)Jq%G0bIG%?{_*Y#5IHf*5M@wPo6A{$Um++Co$wLC=J1aoG93&T7Ho}P z=mGEPP7GbvoG!uD$k(H3A$Z))+i{Hy?QHdk>3xSBXR0j!11O^mEe9RHmw!pvzv?Ua~2_l2Yh~_!s1qS`|0~0)YsbHSz8!mG)WiJE| z2f($6TQtt6L_f~ApQYQKSb=`053LgrQq7G@98#igV>y#i==-nEjQ!XNu9 z~;mE+gtj4IDDNQJ~JVk5Ux6&LCSFL!y=>79kE9=V}J7tD==Ga+IW zX)r7>VZ9dY=V&}DR))xUoV!u(Z|%3ciQi_2jl}3=$Agc(`RPb z8kEBpvY>1FGQ9W$n>Cq=DIpski};nE)`p3IUw1Oz0|wxll^)4dq3;CCY@RyJgFgc# zKouFh!`?Xuo{IMz^xi-h=StCis_M7yq$u) z?XHvw*HP0VgR+KR6wI)jEMX|ssqYvSf*_3W8zVTQzD?3>H!#>InzpSO)@SC8q*ii- z%%h}_#0{4JG;Jm`4zg};BPTGkYamx$Xo#O~lBirRY)q=5M45n{GCfV7h9qwyu1NxOMoP4)jjZMxmT|IQQh0U7C$EbnMN<3)Kk?fFHYq$d|ICu>KbY_hO zTZM+uKHe(cIZfEqyzyYSUBZa8;Fcut-GN!HSA9ius`ltNebF46ZX_BbZNU}}ZOm{M2&nANL9@0qvih15(|`S~z}m&h!u4x~(%MAO$jHRWNfuxWF#B)E&g3ghSQ9|> z(MFaLQj)NE0lowyjvg8z0#m6FIuKE9lDO~Glg}nSb7`~^&#(Lw{}GVOS>U)m8bF}x zVjbXljBm34Cs-yM6TVusr+3kYFjr28STT3g056y3cH5Tmge~ASxBj z%|yb>$eF;WgrcOZf569sDZOVwoo%8>XO>XQOX1OyN9I-SQgrm;U;+#3OI(zrWyow3 zk==|{lt2xrQ%FIXOTejR>;wv(Pb8u8}BUpx?yd(Abh6? zsoO3VYWkeLnF43&@*#MQ9-i-d0t*xN-UEyNKeyNMHw|A(k(_6QKO=nKMCxD(W(Yop zsRQ)QeL4X3Lxp^L%wzi2-WVSsf61dqliPUM7srDB?Wm6Lzn0&{*}|IsKQW;02(Y&| zaTKv|`U(pSzuvR6Rduu$wzK_W-Y-7>7s?G$)U}&uK;<>vU}^^ns@Z!p+9?St1s)dG zK%y6xkPyyS1$~&6v{kl?Md6gwM|>mt6Upm>oa8RLD^8T{0?HC!Z>;(Bob7el(DV6x zi`I)$&E&ngwFS@bi4^xFLAn`=fzTC;aimE^!cMI2n@Vo%Ae-ne`RF((&5y6xsjjAZ zVguVoQ?Z9uk$2ON;ersE%PU*xGO@T*;j1BO5#TuZKEf(mB7|g7pcEA=nYJ{s3vlbg zd4-DUlD{*6o%Gc^N!Nptgay>j6E5;3psI+C3Q!1ZIbeCubW%w4pq9)MSDyB{HLm|k zxv-{$$A*pS@csolri$Ge<4VZ}e~78JOL-EVyrbxKra^d{?|NnPp86!q>t<&IP07?Z z^>~IK^k#OEKgRH+LjllZXk7iA>2cfH6+(e&9ku5poo~6y{GC5>(bRK7hwjiurqAiZ zg*DmtgY}v83IjE&AbiWgMyFbaRUPZ{lYiz$U^&Zt2YjG<%m((&_JUbZcfJ22(>bi5 z!J?<7AySj0JZ&<-qXX;mcV!f~>G=sB0KnjWca4}vrtunD^1TrpfeS^4dvFr!65knK zZh`d;*VOkPs4*-9kL>$GP0`(M!j~B;#x?Ba~&s6CopvO86oM?-? zOw#dIRc;6A6T?B`Qp%^<U5 z19x(ywSH$_N+Io!6;e?`tWaM$`=Db!gzx|lQ${DG!zb1Zl&|{kX0y6xvO1o z220r<-oaS^^R2pEyY;=Qllqpmue|5yI~D|iI!IGt@iod{Opz@*ml^w2bNs)p`M(Io z|E;;m*Xpjd9l)4G#KaWfV(t8YUn@A;nK^#xgv=LtnArX|vWQVuw3}B${h+frU2>9^ z!l6)!Uo4`5k`<<;E(ido7M6lKTgWezNLq>U*=uz&s=cc$1%>VrAeOoUtA|T6gO4>UNqsdK=NF*8|~*sl&wI=x9-EGiq*aqV!(VVXA57 zw9*o6Ir8Lj1npUXvlevtn(_+^X5rzdR>#(}4YcB9O50q97%rW2me5_L=%ffYPUSRc z!vv?Kv>dH994Qi>U(a<0KF6NH5b16enCp+mw^Hb3Xs1^tThFpz!3QuN#}KBbww`(h z7GO)1olDqy6?T$()R7y%NYx*B0k_2IBiZ14&8|JPFxeMF{vW>HF-Vi3+ZOI=+qP}n zw(+!WcTd~4ZJX1!ZM&y!+uyt=&i!+~d(V%GjH;-NsEEv6nS1TERt|RHh!0>W4+4pp z1-*EzAM~i`+1f(VEHI8So`S`akPfPTfq*`l{Fz`hS%k#JS0cjT2mS0#QLGf=J?1`he3W*;m4)ce8*WFq1sdP=~$5RlH1EdWm|~dCvKOi4*I_96{^95p#B<(n!d?B z=o`0{t+&OMwKcxiBECznJcfH!fL(z3OvmxP#oWd48|mMjpE||zdiTBdWelj8&Qosv zZFp@&UgXuvJw5y=q6*28AtxZzo-UUpkRW%ne+Ylf!V-0+uQXBW=5S1o#6LXNtY5!I z%Rkz#(S8Pjz*P7bqB6L|M#Er{|QLae-Y{KA>`^} z@lPjeX>90X|34S-7}ZVXe{wEei1<{*e8T-Nbj8JmD4iwcE+Hg_zhkPVm#=@b$;)h6 z<<6y`nPa`f3I6`!28d@kdM{uJOgM%`EvlQ5B2bL)Sl=|y@YB3KeOzz=9cUW3clPAU z^sYc}xf9{4Oj?L5MOlYxR{+>w=vJjvbyO5}ptT(o6dR|ygO$)nVCvNGnq(6;bHlBd zl?w-|plD8spjDF03g5ip;W3Z z><0{BCq!Dw;h5~#1BuQilq*TwEu)qy50@+BE4bX28+7erX{BD4H)N+7U`AVEuREE8 z;X?~fyhF-x_sRfHIj~6f(+^@H)D=ngP;mwJjxhQUbUdzk8f94Ab%59-eRIq?ZKrwD z(BFI=)xrUlgu(b|hAysqK<}8bslmNNeD=#JW*}^~Nrswn^xw*nL@Tx!49bfJecV&KC2G4q5a!NSv)06A_5N3Y?veAz;Gv+@U3R% z)~UA8-0LvVE{}8LVDOHzp~2twReqf}ODIyXMM6=W>kL|OHcx9P%+aJGYi_Om)b!xe zF40Vntn0+VP>o<$AtP&JANjXBn7$}C@{+@3I@cqlwR2MdwGhVPxlTIcRVu@Ho-wO` z_~Or~IMG)A_`6-p)KPS@cT9mu9RGA>dVh5wY$NM9-^c@N=hcNaw4ITjm;iWSP^ZX| z)_XpaI61<+La+U&&%2a z0za$)-wZP@mwSELo#3!PGTt$uy0C(nTT@9NX*r3Ctw6J~7A(m#8fE)0RBd`TdKfAT zCf@$MAxjP`O(u9s@c0Fd@|}UQ6qp)O5Q5DPCeE6mSIh|Rj{$cAVIWsA=xPKVKxdhg zLzPZ`3CS+KIO;T}0Ip!fAUaNU>++ZJZRk@I(h<)RsJUhZ&Ru9*!4Ptn;gX^~4E8W^TSR&~3BAZc#HquXn)OW|TJ`CTahk+{qe`5+ixON^zA9IFd8)kc%*!AiLu z>`SFoZ5bW-%7}xZ>gpJcx_hpF$2l+533{gW{a7ce^B9sIdmLrI0)4yivZ^(Vh@-1q zFT!NQK$Iz^xu%|EOK=n>ug;(7J4OnS$;yWmq>A;hsD_0oAbLYhW^1Vdt9>;(JIYjf zdb+&f&D4@4AS?!*XpH>8egQvSVX`36jMd>$+RgI|pEg))^djhGSo&#lhS~9%NuWfX zDDH;3T*GzRT@5=7ibO>N-6_XPBYxno@mD_3I#rDD?iADxX`! zh*v8^i*JEMzyN#bGEBz7;UYXki*Xr(9xXax(_1qVW=Ml)kSuvK$coq2A(5ZGhs_pF z$*w}FbN6+QDseuB9=fdp_MTs)nQf!2SlROQ!gBJBCXD&@-VurqHj0wm@LWX-TDmS= z71M__vAok|@!qgi#H&H%Vg-((ZfxPAL8AI{x|VV!9)ZE}_l>iWk8UPTGHs*?u7RfP z5MC&=c6X;XlUzrz5q?(!eO@~* zoh2I*%J7dF!!_!vXoSIn5o|wj1#_>K*&CIn{qSaRc&iFVxt*^20ngCL;QonIS>I5^ zMw8HXm>W0PGd*}Ko)f|~dDd%;Wu_RWI_d;&2g6R3S63Uzjd7dn%Svu-OKpx*o|N>F zZg=-~qLb~VRLpv`k zWSdfHh@?dp=s_X`{yxOlxE$4iuyS;Z-x!*E6eqmEm*j2bE@=ZI0YZ5%Yj29!5+J$4h{s($nakA`xgbO8w zi=*r}PWz#lTL_DSAu1?f%-2OjD}NHXp4pXOsCW;DS@BC3h-q4_l`<))8WgzkdXg3! zs1WMt32kS2E#L0p_|x+x**TFV=gn`m9BWlzF{b%6j-odf4{7a4y4Uaef@YaeuPhU8 zHBvRqN^;$Jizy+ z=zW{E5<>2gp$pH{M@S*!sJVQU)b*J5*bX4h>5VJve#Q6ga}cQ&iL#=(u+KroWrxa%8&~p{WEUF0il=db;-$=A;&9M{Rq`ouZ5m%BHT6%st%saGsD6)fQgLN}x@d3q>FC;=f%O3Cyg=Ke@Gh`XW za@RajqOE9UB6eE=zhG%|dYS)IW)&y&Id2n7r)6p_)vlRP7NJL(x4UbhlcFXWT8?K=%s7;z?Vjts?y2+r|uk8Wt(DM*73^W%pAkZa1Jd zNoE)8FvQA>Z`eR5Z@Ig6kS5?0h;`Y&OL2D&xnnAUzQz{YSdh0k zB3exx%A2TyI)M*EM6htrxSlep!Kk(P(VP`$p0G~f$smld6W1r_Z+o?=IB@^weq>5VYsYZZR@` z&XJFxd5{|KPZmVOSxc@^%71C@;z}}WhbF9p!%yLj3j%YOlPL5s>7I3vj25 z@xmf=*z%Wb4;Va6SDk9cv|r*lhZ`(y_*M@>q;wrn)oQx%B(2A$9(74>;$zmQ!4fN; z>XurIk-7@wZys<+7XL@0Fhe-f%*=(weaQEdR9Eh6>Kl-EcI({qoZqyzziGwpg-GM#251sK_ z=3|kitS!j%;fpc@oWn65SEL73^N&t>Ix37xgs= zYG%eQDJc|rqHFia0!_sm7`@lvcv)gfy(+KXA@E{3t1DaZ$DijWAcA)E0@X?2ziJ{v z&KOYZ|DdkM{}t+@{@*6ge}m%xfjIxi%qh`=^2Rwz@w0cCvZ&Tc#UmCDbVwABrON^x zEBK43FO@weA8s7zggCOWhMvGGE`baZ62cC)VHyy!5Zbt%ieH+XN|OLbAFPZWyC6)p z4P3%8sq9HdS3=ih^0OOlqTPbKuzQ?lBEI{w^ReUO{V?@`ARsL|S*%yOS=Z%sF)>-y z(LAQdhgAcuF6LQjRYfdbD1g4o%tV4EiK&ElLB&^VZHbrV1K>tHTO{#XTo>)2UMm`2 z^t4s;vnMQgf-njU-RVBRw0P0-m#d-u`(kq7NL&2T)TjI_@iKuPAK-@oH(J8?%(e!0Ir$yG32@CGUPn5w4)+9@8c&pGx z+K3GKESI4*`tYlmMHt@br;jBWTei&(a=iYslc^c#RU3Q&sYp zSG){)V<(g7+8W!Wxeb5zJb4XE{I|&Y4UrFWr%LHkdQ;~XU zgy^dH-Z3lmY+0G~?DrC_S4@=>0oM8Isw%g(id10gWkoz2Q%7W$bFk@mIzTCcIB(K8 zc<5h&ZzCdT=9n-D>&a8vl+=ZF*`uTvQviG_bLde*k>{^)&0o*b05x$MO3gVLUx`xZ z43j+>!u?XV)Yp@MmG%Y`+COH2?nQcMrQ%k~6#O%PeD_WvFO~Kct za4XoCM_X!c5vhRkIdV=xUB3xI2NNStK*8_Zl!cFjOvp-AY=D;5{uXj}GV{LK1~IE2 z|KffUiBaStRr;10R~K2VVtf{TzM7FaPm;Y(zQjILn+tIPSrJh&EMf6evaBKIvi42-WYU9Vhj~3< zZSM-B;E`g_o8_XTM9IzEL=9Lb^SPhe(f(-`Yh=X6O7+6ALXnTcUFpI>ekl6v)ZQeNCg2 z^H|{SKXHU*%nBQ@I3It0m^h+6tvI@FS=MYS$ZpBaG7j#V@P2ZuYySbp@hA# ze(kc;P4i_-_UDP?%<6>%tTRih6VBgScKU^BV6Aoeg6Uh(W^#J^V$Xo^4#Ekp ztqQVK^g9gKMTHvV7nb64UU7p~!B?>Y0oFH5T7#BSW#YfSB@5PtE~#SCCg3p^o=NkMk$<8- z6PT*yIKGrvne7+y3}_!AC8NNeI?iTY(&nakN>>U-zT0wzZf-RuyZk^X9H-DT_*wk= z;&0}6LsGtfVa1q)CEUPlx#(ED@-?H<1_FrHU#z5^P3lEB|qsxEyn%FOpjx z3S?~gvoXy~L(Q{Jh6*i~=f%9kM1>RGjBzQh_SaIDfSU_9!<>*Pm>l)cJD@wlyxpBV z4Fmhc2q=R_wHCEK69<*wG%}mgD1=FHi4h!98B-*vMu4ZGW~%IrYSLGU{^TuseqVgV zLP<%wirIL`VLyJv9XG_p8w@Q4HzNt-o;U@Au{7%Ji;53!7V8Rv0^Lu^Vf*sL>R(;c zQG_ZuFl)Mh-xEIkGu}?_(HwkB2jS;HdPLSxVU&Jxy9*XRG~^HY(f0g8Q}iqnVmgjI zfd=``2&8GsycjR?M%(zMjn;tn9agcq;&rR!Hp z$B*gzHsQ~aXw8c|a(L^LW(|`yGc!qOnV(ZjU_Q-4z1&0;jG&vAKuNG=F|H?@m5^N@ zq{E!1n;)kNTJ>|Hb2ODt-7U~-MOIFo%9I)_@7fnX+eMMNh>)V$IXesJpBn|uo8f~#aOFytCT zf9&%MCLf8mp4kwHTcojWmM3LU=#|{3L>E}SKwOd?%{HogCZ_Z1BSA}P#O(%H$;z7XyJ^sjGX;j5 zrzp>|Ud;*&VAU3x#f{CKwY7Vc{%TKKqmB@oTHA9;>?!nvMA;8+Jh=cambHz#J18x~ zs!dF>$*AnsQ{{82r5Aw&^7eRCdvcgyxH?*DV5(I$qXh^zS>us*I66_MbL8y4d3ULj z{S(ipo+T3Ag!+5`NU2sc+@*m{_X|&p#O-SAqF&g_n7ObB82~$p%fXA5GLHMC+#qqL zdt`sJC&6C2)=juQ_!NeD>U8lDVpAOkW*khf7MCcs$A(wiIl#B9HM%~GtQ^}yBPjT@ z+E=|A!Z?A(rwzZ;T}o6pOVqHzTr*i;Wrc%&36kc@jXq~+w8kVrs;%=IFdACoLAcCAmhFNpbP8;s`zG|HC2Gv?I~w4ITy=g$`0qMQdkijLSOtX6xW%Z9Nw<;M- zMN`c7=$QxN00DiSjbVt9Mi6-pjv*j(_8PyV-il8Q-&TwBwH1gz1uoxs6~uU}PrgWB zIAE_I-a1EqlIaGQNbcp@iI8W1sm9fBBNOk(k&iLBe%MCo#?xI$%ZmGA?=)M9D=0t7 zc)Q0LnI)kCy{`jCGy9lYX%mUsDWwsY`;jE(;Us@gmWPqjmXL+Hu#^;k%eT>{nMtzj zsV`Iy6leTA8-PndszF;N^X@CJrTw5IIm!GPeu)H2#FQitR{1p;MasQVAG3*+=9FYK zw*k!HT(YQorfQj+1*mCV458(T5=fH`um$gS38hw(OqVMyunQ;rW5aPbF##A3fGH6h z@W)i9Uff?qz`YbK4c}JzQpuxuE3pcQO)%xBRZp{zJ^-*|oryTxJ-rR+MXJ)!f=+pp z10H|DdGd2exhi+hftcYbM0_}C0ZI-2vh+$fU1acsB-YXid7O|=9L!3e@$H*6?G*Zp z%qFB(sgl=FcC=E4CYGp4CN>=M8#5r!RU!u+FJVlH6=gI5xHVD&k;Ta*M28BsxfMV~ zLz+@6TxnfLhF@5=yQo^1&S}cmTN@m!7*c6z;}~*!hNBjuE>NLVl2EwN!F+)0$R1S! zR|lF%n!9fkZ@gPW|x|B={V6x3`=jS*$Pu0+5OWf?wnIy>Y1MbbGSncpKO0qE(qO=ts z!~@&!N`10S593pVQu4FzpOh!tvg}p%zCU(aV5=~K#bKi zHdJ1>tQSrhW%KOky;iW+O_n;`l9~omqM%sdxdLtI`TrJzN6BQz+7xOl*rM>xVI2~# z)7FJ^Dc{DC<%~VS?@WXzuOG$YPLC;>#vUJ^MmtbSL`_yXtNKa$Hk+l-c!aC7gn(Cg ze?YPYZ(2Jw{SF6MiO5(%_pTo7j@&DHNW`|lD`~{iH+_eSTS&OC*2WTT*a`?|9w1dh zh1nh@$a}T#WE5$7Od~NvSEU)T(W$p$s5fe^GpG+7fdJ9=enRT9$wEk+ZaB>G3$KQO zgq?-rZZnIv!p#>Ty~}c*Lb_jxJg$eGM*XwHUwuQ|o^}b3^T6Bxx{!?va8aC@-xK*H ztJBFvFfsSWu89%@b^l3-B~O!CXs)I6Y}y#0C0U0R0WG zybjroj$io0j}3%P7zADXOwHwafT#uu*zfM!oD$6aJx7+WL%t-@6^rD_a_M?S^>c;z zMK580bZXo1f*L$CuMeM4Mp!;P@}b~$cd(s5*q~FP+NHSq;nw3fbWyH)i2)-;gQl{S zZO!T}A}fC}vUdskGSq&{`oxt~0i?0xhr6I47_tBc`fqaSrMOzR4>0H^;A zF)hX1nfHs)%Zb-(YGX;=#2R6C{BG;k=?FfP?9{_uFLri~-~AJ;jw({4MU7e*d)?P@ zXX*GkNY9ItFjhwgAIWq7Y!ksbMzfqpG)IrqKx9q{zu%Mdl+{Dis#p9q`02pr1LG8R z@As?eG!>IoROgS!@J*to<27coFc1zpkh?w=)h9CbYe%^Q!Ui46Y*HO0mr% zEff-*$ndMNw}H2a5@BsGj5oFfd!T(F&0$<{GO!Qdd?McKkorh=5{EIjDTHU`So>8V zBA-fqVLb2;u7UhDV1xMI?y>fe3~4urv3%PX)lDw+HYa;HFkaLqi4c~VtCm&Ca+9C~ zge+67hp#R9`+Euq59WhHX&7~RlXn=--m8$iZ~~1C8cv^2(qO#X0?vl91gzUKBeR1J z^p4!!&7)3#@@X&2aF2-)1Ffcc^F8r|RtdL2X%HgN&XU-KH2SLCbpw?J5xJ*!F-ypZ zMG%AJ!Pr&}`LW?E!K~=(NJxuSVTRCGJ$2a*Ao=uUDSys!OFYu!Vs2IT;xQ6EubLIl z+?+nMGeQQhh~??0!s4iQ#gm3!BpMpnY?04kK375e((Uc7B3RMj;wE?BCoQGu=UlZt!EZ1Q*auI)dj3Jj{Ujgt zW5hd~-HWBLI_3HuO) zNrb^XzPsTIb=*a69wAAA3J6AAZZ1VsYbIG}a`=d6?PjM)3EPaDpW2YP$|GrBX{q*! z$KBHNif)OKMBCFP5>!1d=DK>8u+Upm-{hj5o|Wn$vh1&K!lVfDB&47lw$tJ?d5|=B z^(_9=(1T3Fte)z^>|3**n}mIX;mMN5v2F#l(q*CvU{Ga`@VMp#%rQkDBy7kYbmb-q z<5!4iuB#Q_lLZ8}h|hPODI^U6`gzLJre9u3k3c#%86IKI*^H-@I48Bi*@avYm4v!n0+v zWu{M{&F8#p9cx+gF0yTB_<2QUrjMPo9*7^-uP#~gGW~y3nfPAoV%amgr>PSyVAd@l)}8#X zR5zV6t*uKJZL}?NYvPVK6J0v4iVpwiN|>+t3aYiZSp;m0!(1`bHO}TEtWR1tY%BPB z(W!0DmXbZAsT$iC13p4f>u*ZAy@JoLAkJhzFf1#4;#1deO8#8d&89}en&z!W&A3++^1(;>0SB1*54d@y&9Pn;^IAf3GiXbfT`_>{R+Xv; zQvgL>+0#8-laO!j#-WB~(I>l0NCMt_;@Gp_f0#^c)t?&#Xh1-7RR0@zPyBz!U#0Av zT?}n({(p?p7!4S2ZBw)#KdCG)uPnZe+U|0{BW!m)9 zi_9$F?m<`2!`JNFv+w8MK_K)qJ^aO@7-Ig>cM4-r0bi=>?B_2mFNJ}aE3<+QCzRr*NA!QjHw# z`1OsvcoD0?%jq{*7b!l|L1+Tw0TTAM4XMq7*ntc-Ived>Sj_ZtS|uVdpfg1_I9knY z2{GM_j5sDC7(W&}#s{jqbybqJWyn?{PW*&cQIU|*v8YGOKKlGl@?c#TCnmnAkAzV- zmK={|1G90zz=YUvC}+fMqts0d4vgA%t6Jhjv?d;(Z}(Ep8fTZfHA9``fdUHkA+z3+ zhh{ohP%Bj?T~{i0sYCQ}uC#5BwN`skI7`|c%kqkyWIQ;!ysvA8H`b-t()n6>GJj6xlYDu~8qX{AFo$Cm3d|XFL=4uvc?Keb zzb0ZmMoXca6Mob>JqkNuoP>B2Z>D`Q(TvrG6m`j}-1rGP!g|qoL=$FVQYxJQjFn33lODt3Wb1j8VR zlR++vIT6^DtYxAv_hxupbLLN3e0%A%a+hWTKDV3!Fjr^cWJ{scsAdfhpI)`Bms^M6 zQG$waKgFr=c|p9Piug=fcJvZ1ThMnNhQvBAg-8~b1?6wL*WyqXhtj^g(Ke}mEfZVM zJuLNTUVh#WsE*a6uqiz`b#9ZYg3+2%=C(6AvZGc=u&<6??!slB1a9K)=VL zY9EL^mfyKnD zSJyYBc_>G;5RRnrNgzJz#Rkn3S1`mZgO`(r5;Hw6MveN(URf_XS-r58Cn80K)ArH4 z#Rrd~LG1W&@ttw85cjp8xV&>$b%nSXH_*W}7Ch2pg$$c0BdEo-HWRTZcxngIBJad> z;C>b{jIXjb_9Jis?NZJsdm^EG}e*pR&DAy0EaSGi3XWTa(>C%tz1n$u?5Fb z1qtl?;_yjYo)(gB^iQq?=jusF%kywm?CJP~zEHi0NbZ);$(H$w(Hy@{i>$wcVRD_X|w-~(0Z9BJyh zhNh;+eQ9BEIs;tPz%jSVnfCP!3L&9YtEP;svoj_bNzeGSQIAjd zBss@A;)R^WAu-37RQrM%{DfBNRx>v!G31Z}8-El9IOJlb_MSoMu2}GDYycNaf>uny z+8xykD-7ONCM!APry_Lw6-yT>5!tR}W;W`C)1>pxSs5o1z#j7%m=&=7O4hz+Lsqm` z*>{+xsabZPr&X=}G@obTb{nPTkccJX8w3CG7X+1+t{JcMabv~UNv+G?txRqXib~c^Mo}`q{$`;EBNJ;#F*{gvS12kV?AZ%O0SFB$^ zn+}!HbmEj}w{Vq(G)OGAzH}R~kS^;(-s&=ectz8vN!_)Yl$$U@HNTI-pV`LSj7Opu zTZ5zZ)-S_{GcEQPIQXLQ#oMS`HPu{`SQiAZ)m1at*Hy%3xma|>o`h%E%8BEbi9p0r zVjcsh<{NBKQ4eKlXU|}@XJ#@uQw*$4BxKn6#W~I4T<^f99~(=}a`&3(ur8R9t+|AQ zWkQx7l}wa48-jO@ft2h+7qn%SJtL%~890FG0s5g*kNbL3I&@brh&f6)TlM`K^(bhr zJWM6N6x3flOw$@|C@kPi7yP&SP?bzP-E|HSXQXG>7gk|R9BTj`e=4de9C6+H7H7n# z#GJeVs1mtHhLDmVO?LkYRQc`DVOJ_vdl8VUihO-j#t=0T3%Fc1f9F73ufJz*adn*p zc%&vi(4NqHu^R>sAT_0EDjVR8bc%wTz#$;%NU-kbDyL_dg0%TFafZwZ?5KZpcuaO54Z9hX zD$u>q!-9`U6-D`E#`W~fIfiIF5_m6{fvM)b1NG3xf4Auw;Go~Fu7cth#DlUn{@~yu z=B;RT*dp?bO}o%4x7k9v{r=Y@^YQ^UUm(Qmliw8brO^=NP+UOohLYiaEB3^DB56&V zK?4jV61B|1Uj_5fBKW;8LdwOFZKWp)g{B%7g1~DgO&N& z#lisxf?R~Z@?3E$Mms$$JK8oe@X`5m98V*aV6Ua}8Xs2#A!{x?IP|N(%nxsH?^c{& z@vY&R1QmQs83BW28qAmJfS7MYi=h(YK??@EhjL-t*5W!p z^gYX!Q6-vBqcv~ruw@oMaU&qp0Fb(dbVzm5xJN%0o_^@fWq$oa3X?9s%+b)x4w-q5Koe(@j6Ez7V@~NRFvd zfBH~)U5!ix3isg`6be__wBJp=1@yfsCMw1C@y+9WYD9_C%{Q~7^0AF2KFryfLlUP# zwrtJEcH)jm48!6tUcxiurAMaiD04C&tPe6DI0#aoqz#Bt0_7_*X*TsF7u*zv(iEfA z;$@?XVu~oX#1YXtceQL{dSneL&*nDug^OW$DSLF0M1Im|sSX8R26&)<0Fbh^*l6!5wfSu8MpMoh=2l z^^0Sr$UpZp*9oqa23fcCfm7`ya2<4wzJ`Axt7e4jJrRFVf?nY~2&tRL* zd;6_njcz01c>$IvN=?K}9ie%Z(BO@JG2J}fT#BJQ+f5LFSgup7i!xWRKw6)iITjZU z%l6hPZia>R!`aZjwCp}I zg)%20;}f+&@t;(%5;RHL>K_&7MH^S+7<|(SZH!u zznW|jz$uA`P9@ZWtJgv$EFp>)K&Gt+4C6#*khZQXS*S~6N%JDT$r`aJDs9|uXWdbg zBwho$phWx}x!qy8&}6y5Vr$G{yGSE*r$^r{}pw zVTZKvikRZ`J_IJrjc=X1uw?estdwm&bEahku&D04HD+0Bm~q#YGS6gp!KLf$A{%Qd z&&yX@Hp>~(wU{|(#U&Bf92+1i&Q*-S+=y=3pSZy$#8Uc$#7oiJUuO{cE6=tsPhwPe| zxQpK>`Dbka`V)$}e6_OXKLB%i76~4N*zA?X+PrhH<&)}prET;kel24kW%+9))G^JI zsq7L{P}^#QsZViX%KgxBvEugr>ZmFqe^oAg?{EI=&_O#e)F3V#rc z8$4}0Zr19qd3tE4#$3_f=Bbx9oV6VO!d3(R===i-7p=Vj`520w0D3W6lQfY48}!D* z&)lZMG;~er2qBoI2gsX+Ts-hnpS~NYRDtPd^FPzn!^&yxRy#CSz(b&E*tL|jIkq|l zf%>)7Dtu>jCf`-7R#*GhGn4FkYf;B$+9IxmqH|lf6$4irg{0ept__%)V*R_OK=T06 zyT_m-o@Kp6U{l5h>W1hGq*X#8*y@<;vsOFqEjTQXFEotR+{3}ODDnj;o0@!bB5x=N z394FojuGOtVKBlVRLtHp%EJv_G5q=AgF)SKyRN5=cGBjDWv4LDn$IL`*=~J7u&Dy5 zrMc83y+w^F&{?X(KOOAl-sWZDb{9X9#jrQtmrEXD?;h-}SYT7yM(X_6qksM=K_a;Z z3u0qT0TtaNvDER_8x*rxXw&C^|h{P1qxK|@pS7vdlZ#P z7PdB7MmC2}%sdzAxt>;WM1s0??`1983O4nFK|hVAbHcZ3x{PzytQLkCVk7hA!Lo` zEJH?4qw|}WH{dc4z%aB=0XqsFW?^p=X}4xnCJXK%c#ItOSjdSO`UXJyuc8bh^Cf}8 z@Ht|vXd^6{Fgai8*tmyRGmD_s_nv~r^Fy7j`Bu`6=G)5H$i7Q7lvQnmea&TGvJp9a|qOrUymZ$6G|Ly z#zOCg++$3iB$!6!>215A4!iryregKuUT344X)jQb3|9qY>c0LO{6Vby05n~VFzd?q zgGZv&FGlkiH*`fTurp>B8v&nSxNz)=5IF$=@rgND4d`!AaaX;_lK~)-U8la_Wa8i?NJC@BURO*sUW)E9oyv3RG^YGfN%BmxzjlT)bp*$<| zX3tt?EAy<&K+bhIuMs-g#=d1}N_?isY)6Ay$mDOKRh z4v1asEGWoAp=srraLW^h&_Uw|6O+r;wns=uwYm=JN4Q!quD8SQRSeEcGh|Eb5Jg8m zOT}u;N|x@aq)=&;wufCc^#)5U^VcZw;d_wwaoh9$p@Xrc{DD6GZUqZ ziC6OT^zSq@-lhbgR8B+e;7_Giv;DK5gn^$bs<6~SUadiosfewWDJu`XsBfOd1|p=q zE>m=zF}!lObA%ePey~gqU8S6h-^J2Y?>7)L2+%8kV}Gp=h`Xm_}rlm)SyUS=`=S7msKu zC|T!gPiI1rWGb1z$Md?0YJQ;%>uPLOXf1Z>N~`~JHJ!^@D5kSXQ4ugnFZ>^`zH8CAiZmp z6Ms|#2gcGsQ{{u7+Nb9sA?U>(0e$5V1|WVwY`Kn)rsnnZ4=1u=7u!4WexZD^IQ1Jk zfF#NLe>W$3m&C^ULjdw+5|)-BSHwpegdyt9NYC{3@QtMfd8GrIWDu`gd0nv-3LpGCh@wgBaG z176tikL!_NXM+Bv#7q^cyn9$XSeZR6#!B4JE@GVH zoobHZN_*RF#@_SVYKkQ_igme-Y5U}cV(hkR#k1c{bQNMji zU7aE`?dHyx=1`kOYZo_8U7?3-7vHOp`Qe%Z*i+FX!s?6huNp0iCEW-Z7E&jRWmUW_ z67j>)Ew!yq)hhG4o?^z}HWH-e=es#xJUhDRc4B51M4~E-l5VZ!&zQq`gWe`?}#b~7w1LH4Xa-UCT5LXkXQWheBa2YJYbyQ zl1pXR%b(KCXMO0OsXgl0P0Og<{(@&z1aokU-Pq`eQq*JYgt8xdFQ6S z6Z3IFSua8W&M#`~*L#r>Jfd6*BzJ?JFdBR#bDv$_0N!_5vnmo@!>vULcDm`MFU823 zpG9pqjqz^FE5zMDoGqhs5OMmC{Y3iVcl>F}5Rs24Y5B^mYQ;1T&ks@pIApHOdrzXF z-SdX}Hf{X;TaSxG_T$0~#RhqKISGKNK47}0*x&nRIPtmdwxc&QT3$8&!3fWu1eZ_P zJveQj^hJL#Sn!*4k`3}(d(aasl&7G0j0-*_2xtAnoX1@9+h zO#c>YQg60Z;o{Bi=3i7S`Ic+ZE>K{(u|#)9y}q*j8uKQ1^>+(BI}m%1v3$=4ojGBc zm+o1*!T&b}-lVvZqIUBc8V}QyFEgm#oyIuC{8WqUNV{Toz`oxhYpP!_p2oHHh5P@iB*NVo~2=GQm+8Yrkm2Xjc_VyHg1c0>+o~@>*Qzo zHVBJS>$$}$_4EniTI;b1WShX<5-p#TPB&!;lP!lBVBbLOOxh6FuYloD%m;n{r|;MU3!q4AVkua~fieeWu2 zQAQ$ue(IklX6+V;F1vCu-&V?I3d42FgWgsb_e^29ol}HYft?{SLf>DrmOp9o!t>I^ zY7fBCk+E8n_|apgM|-;^=#B?6RnFKlN`oR)`e$+;D=yO-(U^jV;rft^G_zl`n7qnM zL z*-Y4Phq+ZI1$j$F-f;`CD#|`-T~OM5Q>x}a>B~Gb3-+9i>Lfr|Ca6S^8g*{*?_5!x zH_N!SoRP=gX1?)q%>QTY!r77e2j9W(I!uAz{T`NdNmPBBUzi2{`XMB^zJGGwFWeA9 z{fk33#*9SO0)DjROug+(M)I-pKA!CX;IY(#gE!UxXVsa)X!UftIN98{pt#4MJHOhY zM$_l}-TJlxY?LS6Nuz1T<44m<4i^8k@D$zuCPrkmz@sdv+{ciyFJG2Zwy&%c7;atIeTdh!a(R^QXnu1Oq1b42*OQFWnyQ zWeQrdvP|w_idy53Wa<{QH^lFmEd+VlJkyiC>6B#s)F;w-{c;aKIm;Kp50HnA-o3lY z9B~F$gJ@yYE#g#X&3ADx&tO+P_@mnQTz9gv30_sTsaGXkfNYXY{$(>*PEN3QL>I!k zp)KibPhrfX3%Z$H6SY`rXGYS~143wZrG2;=FLj50+VM6soI~up_>fU(2Wl@{BRsMi zO%sL3x?2l1cXTF)k&moNsHfQrQ+wu(gBt{sk#CU=UhrvJIncy@tJX5klLjgMn>~h= zg|FR&;@eh|C7`>s_9c~0-{IAPV){l|Ts`i=)AW;d9&KPc3fMeoTS%8@V~D8*h;&(^>yjT84MM}=%#LS7shLAuuj(0VAYoozhWjq z4LEr?wUe2^WGwdTIgWBkDUJa>YP@5d9^Rs$kCXmMRxuF*YMVrn?0NFyPl}>`&dqZb z<5eqR=ZG3>n2{6v6BvJ`YBZeeTtB88TAY(x0a58EWyuf>+^|x8Qa6wA|1Nb_p|nA zWWa}|z8a)--Wj`LqyFk_a3gN2>5{Rl_wbW?#by7&i*^hRknK%jwIH6=dQ8*-_{*x0j^DUfMX0`|K@6C<|1cgZ~D(e5vBFFm;HTZF(!vT8=T$K+|F)x3kqzBV4-=p1V(lzi(s7jdu0>LD#N=$Lk#3HkG!a zIF<7>%B7sRNzJ66KrFV76J<2bdYhxll0y2^_rdG=I%AgW4~)1Nvz=$1UkE^J%BxLo z+lUci`UcU062os*=`-j4IfSQA{w@y|3}Vk?i;&SSdh8n+$iHA#%ERL{;EpXl6u&8@ zzg}?hkEOUOJt?ZL=pWZFJ19mI1@P=$U5*Im1e_8Z${JsM>Ov?nh8Z zP5QvI!{Jy@&BP48%P2{Jr_VgzW;P@7)M9n|lDT|Ep#}7C$&ud&6>C^5ZiwKIg2McPU(4jhM!BD@@L(Gd*Nu$ji(ljZ<{FIeW_1Mmf;76{LU z-ywN~=uNN)Xi6$<12A9y)K%X|(W0p|&>>4OXB?IiYr||WKDOJPxiSe01NSV-h24^L z_>m$;|C+q!Mj**-qQ$L-*++en(g|hw;M!^%_h-iDjFHLo-n3JpB;p?+o2;`*jpvJU zLY^lt)Un4joij^^)O(CKs@7E%*!w>!HA4Q?0}oBJ7Nr8NQ7QmY^4~jvf0-`%waOLn zdNjAPaC0_7c|RVhw)+71NWjRi!y>C+Bl;Z`NiL^zn2*0kmj5gyhCLCxts*cWCdRI| zjsd=sT5BVJc^$GxP~YF$-U{-?kW6r@^vHXB%{CqYzU@1>dzf#3SYedJG-Rm6^RB7s zGM5PR(yKPKR)>?~vpUIeTP7A1sc8-knnJk*9)3t^e%izbdm>Y=W{$wm(cy1RB-19i za#828DMBY+ps#7Y8^6t)=Ea@%Nkt)O6JCx|ybC;Ap}Z@Zw~*}3P>MZLPb4Enxz9Wf zssobT^(R@KuShj8>@!1M7tm|2%-pYYDxz-5`rCbaTCG5{;Uxm z*g=+H1X8{NUvFGzz~wXa%Eo};I;~`37*WrRU&K0dPSB$yk(Z*@K&+mFal^?c zurbqB-+|Kb5|sznT;?Pj!+kgFY1#Dr;_%A(GIQC{3ct|{*Bji%FNa6c-thbpBkA;U zURV!Dr&X{0J}iht#-Qp2=xzuh(fM>zRoiGrYl5ttw2#r34gC41CCOC31m~^UPTK@s z6;A@)7O7_%C)>bnAXerYuAHdE93>j2N}H${zEc6&SbZ|-fiG*-qtGuy-qDelH(|u$ zorf8_T6Zqe#Ub!+e3oSyrskt_HyW_^5lrWt#30l)tHk|j$@YyEkXUOV;6B51L;M@=NIWZXU;GrAa(LGxO%|im%7F<-6N;en0Cr zLH>l*y?pMwt`1*cH~LdBPFY_l;~`N!Clyfr;7w<^X;&(ZiVdF1S5e(+Q%60zgh)s4 zn2yj$+mE=miVERP(g8}G4<85^-5f@qxh2ec?n+$A_`?qN=iyT1?U@t?V6DM~BIlBB z>u~eXm-aE>R0sQy!-I4xtCNi!!qh?R1!kKf6BoH2GG{L4%PAz0{Sh6xpuyI%*~u)s z%rLuFl)uQUCBQAtMyN;%)zFMx4loh7uTfKeB2Xif`lN?2gq6NhWhfz0u5WP9J>=V2 zo{mLtSy&BA!mSzs&CrKWq^y40JF5a&GSXIi2= z{EYb59J4}VwikL4P=>+mc6{($FNE@e=VUwG+KV21;<@lrN`mnz5jYGASyvz7BOG_6(p^eTxD-4O#lROgon;R35=|nj#eHIfJBYPWG>H>`dHKCDZ3`R{-?HO0mE~(5_WYcFmp8sU?wr*UkAQiNDGc6T zA%}GOLXlOWqL?WwfHO8MB#8M8*~Y*gz;1rWWoVSXP&IbKxbQ8+s%4Jnt?kDsq7btI zCDr0PZ)b;B%!lu&CT#RJzm{l{2fq|BcY85`w~3LSK<><@(2EdzFLt9Y_`;WXL6x`0 zDoQ?=?I@Hbr;*VVll1Gmd8*%tiXggMK81a+T(5Gx6;eNb8=uYn z5BG-0g>pP21NPn>$ntBh>`*})Fl|38oC^9Qz>~MAazH%3Q~Qb!ALMf$srexgPZ2@&c~+hxRi1;}+)-06)!#Mq<6GhP z-Q?qmgo${aFBApb5p}$1OJKTClfi8%PpnczyVKkoHw7Ml9e7ikrF0d~UB}i3vizos zXW4DN$SiEV9{faLt5bHy2a>33K%7Td-n5C*N;f&ZqAg#2hIqEb(y<&f4u5BWJ>2^4 z414GosL=Aom#m&=x_v<0-fp1r%oVJ{T-(xnomNJ(Dryv zh?vj+%=II_nV+@NR+(!fZZVM&(W6{6%9cm+o+Z6}KqzLw{(>E86uA1`_K$HqINlb1 zKelh3-jr2I9V?ych`{hta9wQ2c9=MM`2cC{m6^MhlL2{DLv7C^j z$xXBCnDl_;l|bPGMX@*tV)B!c|4oZyftUlP*?$YU9C_eAsuVHJ58?)zpbr30P*C`T z7y#ao`uE-SOG(Pi+`$=e^mle~)pRrdwL5)N;o{gpW21of(QE#U6w%*C~`v-z0QqBML!!5EeYA5IQB0 z^l01c;L6E(iytN!LhL}wfwP7W9PNAkb+)Cst?qg#$n;z41O4&v+8-zPs+XNb-q zIeeBCh#ivnFLUCwfS;p{LC0O7tm+Sf9Jn)~b%uwP{%69;QC)Ok0t%*a5M+=;y8j=v z#!*pp$9@!x;UMIs4~hP#pnfVc!%-D<+wsG@R2+J&%73lK|2G!EQC)O05TCV=&3g)C!lT=czLpZ@Sa%TYuoE?v8T8`V;e$#Zf2_Nj6nvBgh1)2 GZ~q4|mN%#X literal 0 HcmV?d00001 diff --git a/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties b/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..fbacf711 --- /dev/null +++ b/cpp/SwerveWithChoreo/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.10.2-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/cpp/SwerveWithChoreo/gradlew b/cpp/SwerveWithChoreo/gradlew new file mode 100644 index 00000000..f5feea6d --- /dev/null +++ b/cpp/SwerveWithChoreo/gradlew @@ -0,0 +1,252 @@ +#!/bin/sh + +# +# Copyright © 2015-2021 the original authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +# + +############################################################################## +# +# Gradle start up script for POSIX generated by Gradle. +# +# Important for running: +# +# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is +# noncompliant, but you have some other compliant shell such as ksh or +# bash, then to run this script, type that shell name before the whole +# command line, like: +# +# ksh Gradle +# +# Busybox and similar reduced shells will NOT work, because this script +# requires all of these POSIX shell features: +# * functions; +# * expansions «$var», «${var}», «${var:-default}», «${var+SET}», +# «${var#prefix}», «${var%suffix}», and «$( cmd )»; +# * compound commands having a testable exit status, especially «case»; +# * various built-in commands including «command», «set», and «ulimit». +# +# Important for patching: +# +# (2) This script targets any POSIX shell, so it avoids extensions provided +# by Bash, Ksh, etc; in particular arrays are avoided. +# +# The "traditional" practice of packing multiple parameters into a +# space-separated string is a well documented source of bugs and security +# problems, so this is (mostly) avoided, by progressively accumulating +# options in "$@", and eventually passing that to Java. +# +# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS, +# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly; +# see the in-line comments for details. +# +# There are tweaks for specific operating systems such as AIX, CygWin, +# Darwin, MinGW, and NonStop. +# +# (3) This script is generated from the Groovy template +# 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/. +# +############################################################################## + +# Attempt to set APP_HOME + +# Resolve links: $0 may be a link +app_path=$0 + +# Need this for daisy-chained symlinks. +while + APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path + [ -h "$app_path" ] +do + ls=$( ls -ld "$app_path" ) + link=${ls#*' -> '} + case $link in #( + /*) app_path=$link ;; #( + *) app_path=$APP_HOME$link ;; + esac +done + +# This is normally unused +# 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 -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 + +warn () { + echo "$*" +} >&2 + +die () { + echo + echo "$*" + echo + exit 1 +} >&2 + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "$( uname )" in #( + CYGWIN* ) cygwin=true ;; #( + Darwin* ) darwin=true ;; #( + MSYS* | MINGW* ) msys=true ;; #( + NONSTOP* ) nonstop=true ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD=$JAVA_HOME/jre/sh/java + else + JAVACMD=$JAVA_HOME/bin/java + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD=java + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +fi + +# Increase the maximum file descriptors if we can. +if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then + case $MAX_FD in #( + max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + MAX_FD=$( ulimit -H -n ) || + warn "Could not query maximum file descriptor limit" + esac + case $MAX_FD in #( + '' | soft) :;; #( + *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 + ulimit -n "$MAX_FD" || + warn "Could not set maximum file descriptor limit to $MAX_FD" + esac +fi + +# Collect all arguments for the java command, stacking in reverse order: +# * args from the command line +# * the main class name +# * -classpath +# * -D...appname settings +# * --module-path (only if needed) +# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables. + +# For Cygwin or MSYS, switch paths to Windows format before running java +if "$cygwin" || "$msys" ; then + APP_HOME=$( cygpath --path --mixed "$APP_HOME" ) + CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" ) + + JAVACMD=$( cygpath --unix "$JAVACMD" ) + + # Now convert the arguments - kludge to limit ourselves to /bin/sh + for arg do + if + case $arg in #( + -*) false ;; # don't mess with options #( + /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath + [ -e "$t" ] ;; #( + *) false ;; + esac + then + arg=$( cygpath --path --ignore --mixed "$arg" ) + fi + # Roll the args list around exactly as many times as the number of + # args, so each arg winds up back in the position where it started, but + # possibly modified. + # + # NB: a `for` loop captures its iteration list before it begins, so + # changing the positional parameters here affects neither the number of + # iterations, nor the values presented in `arg`. + shift # remove old arg + set -- "$@" "$arg" # push replacement arg + done +fi + + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. + +set -- \ + "-Dorg.gradle.appname=$APP_BASE_NAME" \ + -classpath "$CLASSPATH" \ + org.gradle.wrapper.GradleWrapperMain \ + "$@" + +# Stop when "xargs" is not available. +if ! command -v xargs >/dev/null 2>&1 +then + die "xargs is not available" +fi + +# Use "xargs" to parse quoted args. +# +# With -n1 it outputs one arg per line, with the quotes and backslashes removed. +# +# In Bash we could simply go: +# +# readarray ARGS < <( xargs -n1 <<<"$var" ) && +# set -- "${ARGS[@]}" "$@" +# +# but POSIX shell has neither arrays nor command substitution, so instead we +# post-process each arg (as a line of input to sed) to backslash-escape any +# character that might be a shell metacharacter, then use eval to reverse +# that process (while maintaining the separation between arguments), and wrap +# the whole thing up as a single "set" statement. +# +# This will of course break if any of these variables contains a newline or +# an unmatched quote. +# + +eval "set -- $( + printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" | + xargs -n1 | + sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' | + tr '\n' ' ' + )" '"$@"' + +exec "$JAVACMD" "$@" diff --git a/cpp/SwerveWithChoreo/gradlew.bat b/cpp/SwerveWithChoreo/gradlew.bat new file mode 100644 index 00000000..9d21a218 --- /dev/null +++ b/cpp/SwerveWithChoreo/gradlew.bat @@ -0,0 +1,94 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@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 ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Resolve any "." and ".." in APP_HOME to make it shorter. +for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if %ERRORLEVEL% equ 0 goto execute + +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 + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto execute + +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 + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %* + +:end +@rem End local scope for the variables with windows NT shell +if %ERRORLEVEL% equ 0 goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +set EXIT_CODE=%ERRORLEVEL% +if %EXIT_CODE% equ 0 set EXIT_CODE=1 +if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE% +exit /b %EXIT_CODE% + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/cpp/SwerveWithChoreo/settings.gradle b/cpp/SwerveWithChoreo/settings.gradle new file mode 100644 index 00000000..969c7b09 --- /dev/null +++ b/cpp/SwerveWithChoreo/settings.gradle @@ -0,0 +1,30 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2025' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp b/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp new file mode 100644 index 00000000..3aea8cdb --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/cpp/AutoRoutines.cpp @@ -0,0 +1,24 @@ +#include "AutoRoutines.h" + +frc2::CommandPtr AutoRoutines::SimplePathAuto(choreo::AutoFactory &factory) +{ + auto routine = std::make_shared>( + factory.NewLoop("SimplePath Auto") + ); + auto simplePath = std::make_shared>( + factory.Trajectory("SimplePath", *routine) + ); + + routine->Enabled().OnTrue( + m_drivetrain.RunOnce([=] { + auto const pose = simplePath->GetInitialPose(); + if (pose) { + m_drivetrain.ResetPose(*pose); + } else { + routine->Kill(); + } + }) + .AndThen(simplePath->Cmd()) + ); + return routine->Cmd(); +} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp b/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp new file mode 100644 index 00000000..84d22a06 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/cpp/Robot.cpp @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "Robot.h" +#include "LimelightHelpers.h" + +#include + +Robot::Robot() {} + +void Robot::RobotPeriodic() { + frc2::CommandScheduler::GetInstance().Run(); + + /* update auto routine selection */ + m_container.autoChooser.Update(); + + /* + * This example of adding Limelight is very simple and may not be sufficient for on-field use. + * Users typically need to provide a standard deviation that scales with the distance to target + * and changes with number of tags available. + * + * This example is sufficient to show that vision integration is possible, though exact implementation + * of how to use vision should be tuned per-robot and to the team's specification. + */ + if (kUseLimelight) { + auto llMeasurement = LimelightHelpers::getBotPoseEstimate_wpiBlue("limelight"); + if (llMeasurement) { + m_container.drivetrain.AddVisionMeasurement(llMeasurement->pose, utils::FPGAToCurrentTime(llMeasurement->timestampSeconds)); + } + } +} + +void Robot::DisabledInit() {} + +void Robot::DisabledPeriodic() {} + +void Robot::DisabledExit() {} + +void Robot::AutonomousInit() { + m_autonomousCommand = m_container.GetAutonomousCommand(); + + if (m_autonomousCommand) { + m_autonomousCommand->Schedule(); + } +} + +void Robot::AutonomousPeriodic() {} + +void Robot::AutonomousExit() {} + +void Robot::TeleopInit() { + if (m_autonomousCommand) { + m_autonomousCommand->Cancel(); + } +} + +void Robot::TeleopPeriodic() {} + +void Robot::TeleopExit() {} + +void Robot::TestInit() { + frc2::CommandScheduler::GetInstance().CancelAll(); +} + +void Robot::TestPeriodic() {} + +void Robot::TestExit() {} + +#ifndef RUNNING_FRC_TESTS +int main() { + return frc::StartRobot(); +} +#endif diff --git a/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp b/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp new file mode 100644 index 00000000..ba581485 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/cpp/RobotContainer.cpp @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "RobotContainer.h" + +#include +#include + +RobotContainer::RobotContainer() : + autoRoutines{drivetrain}, + autoFactory{ + [this] { return drivetrain.GetState().Pose; }, + [this](auto &&pose, auto &&sample) { drivetrain.FollowPath(pose, sample); }, + [] { + auto const alliance = frc::DriverStation::GetAlliance().value_or(frc::DriverStation::Alliance::kBlue); + return alliance == frc::DriverStation::Alliance::kRed; + }, + {&drivetrain}, + std::nullopt + }, + autoChooser{autoFactory, ""} +{ + autoChooser.AddAutoRoutine("SimplePath", [this] { return autoRoutines.SimplePathAuto(autoFactory); }); + + ConfigureBindings(); +} + +void RobotContainer::ConfigureBindings() +{ + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + drivetrain.SetDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.ApplyRequest([this] { + return drive.WithVelocityX(-joystick.GetLeftY() * MaxSpeed) // Drive forward with negative Y (forward) + .WithVelocityY(-joystick.GetLeftX() * MaxSpeed) // Drive left with negative X (left) + .WithRotationalRate(-joystick.GetRightX() * MaxAngularRate); // Drive counterclockwise with negative X (left) + }) + ); + + joystick.A().WhileTrue(drivetrain.ApplyRequest([this] { return brake; })); + joystick.B().WhileTrue(drivetrain.ApplyRequest([this] { + return point.WithModuleDirection(frc::Rotation2d{-joystick.GetLeftY(), -joystick.GetLeftX()}); + })); + + // Run SysId routines when holding back/start and X/Y. + // Note that each routine should be run exactly once in a single log. + (joystick.Back() && joystick.Y()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kForward)); + (joystick.Back() && joystick.X()).WhileTrue(drivetrain.SysIdDynamic(frc2::sysid::Direction::kReverse)); + (joystick.Start() && joystick.Y()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kForward)); + (joystick.Start() && joystick.X()).WhileTrue(drivetrain.SysIdQuasistatic(frc2::sysid::Direction::kReverse)); + + // reset the field-centric heading on left bumper press + joystick.LeftBumper().OnTrue(drivetrain.RunOnce([this] { drivetrain.SeedFieldCentric(); })); + + drivetrain.RegisterTelemetry([this](auto const &state) { logger.Telemeterize(state); }); +} + +frc2::CommandPtr RobotContainer::GetAutonomousCommand() +{ + return autoChooser.GetSelectedAutoRoutine(); +} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp b/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp new file mode 100644 index 00000000..749b62f8 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/cpp/Telemetry.cpp @@ -0,0 +1,43 @@ +#include "Telemetry.h" +#include + +using namespace ctre::phoenix6; + +void Telemetry::Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state) +{ + /* Telemeterize the pose */ + frc::Pose2d const pose = state.Pose; + fieldTypePub.Set("Field2d"); + fieldPub.Set(std::array{ + pose.X().value(), + pose.Y().value(), + pose.Rotation().Degrees().value() + }); + + /* Telemeterize the robot's general speeds */ + units::second_t const currentTime = utils::GetCurrentTime(); + units::second_t const diffTime = currentTime - lastTime; + lastTime = currentTime; + + frc::Translation2d const distanceDiff = (pose - m_lastPose).Translation(); + m_lastPose = pose; + + frc::Translation2d const velocities = distanceDiff / diffTime.value(); + + speed.Set(velocities.Norm().value()); + velocityX.Set(velocities.X().value()); + velocityY.Set(velocities.Y().value()); + odomFreq.Set((1.0 / state.OdometryPeriod).value()); + + /* Telemeterize the module's states */ + for (size_t i = 0; i < m_moduleSpeeds.size(); ++i) { + m_moduleSpeeds[i]->SetAngle(state.ModuleStates[i].angle.Degrees()); + m_moduleDirections[i]->SetAngle(state.ModuleStates[i].angle.Degrees()); + m_moduleSpeeds[i]->SetLength(state.ModuleStates[i].speed / (2 * MaxSpeed)); + + frc::SmartDashboard::PutData("Module " + std::to_string(i), &m_moduleMechanisms[i]); + } + + SignalLogger::WriteDoubleArray("odometry", {pose.X().value(), pose.Y().value(), pose.Rotation().Degrees().value()}); + SignalLogger::WriteDouble("odom period", state.OdometryPeriod.value(), "seconds"); +} diff --git a/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp b/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp new file mode 100644 index 00000000..5b50b251 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/cpp/subsystems/CommandSwerveDrivetrain.cpp @@ -0,0 +1,67 @@ +#include "subsystems/CommandSwerveDrivetrain.h" + +using namespace subsystems; + +void CommandSwerveDrivetrain::FollowPath(frc::Pose2d const &pose, choreo::SwerveSample const &sample) +{ + m_pathThetaController.EnableContinuousInput( + units::radian_t{-0.5_tr}.value(), + units::radian_t{0.5_tr}.value() + ); + + auto targetSpeeds = sample.GetChassisSpeeds(); + targetSpeeds.vx += m_pathXController.Calculate( + pose.X().value(), sample.x.value() + ) * 1_mps; + targetSpeeds.vy += m_pathYController.Calculate( + pose.Y().value(), sample.y.value() + ) * 1_mps; + targetSpeeds.omega += m_pathThetaController.Calculate( + pose.Rotation().Radians().value(), sample.heading.value() + ) * 1_rad_per_s; + + std::vector moduleForcesX(sample.moduleForcesX.begin(), sample.moduleForcesX.end()); + std::vector moduleForcesY(sample.moduleForcesY.begin(), sample.moduleForcesY.end()); + + SetControl( + m_applyFieldSpeeds.WithSpeeds(targetSpeeds) + .WithWheelForceFeedforwardsX(std::move(moduleForcesX)) + .WithWheelForceFeedforwardsY(std::move(moduleForcesY)) + ); +} + +void CommandSwerveDrivetrain::Periodic() +{ + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || frc::DriverStation::IsDisabled()) { + auto const allianceColor = frc::DriverStation::GetAlliance(); + if (allianceColor) { + SetOperatorPerspectiveForward( + *allianceColor == frc::DriverStation::Alliance::kRed + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); + m_hasAppliedOperatorPerspective = true; + } + } +} + +void CommandSwerveDrivetrain::StartSimThread() +{ + m_lastSimTime = utils::GetCurrentTime(); + m_simNotifier = std::make_unique([this] { + units::second_t const currentTime = utils::GetCurrentTime(); + auto const deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + UpdateSimState(deltaTime, frc::RobotController::GetBatteryVoltage()); + }); + m_simNotifier->StartPeriodic(kSimLoopPeriod); +} diff --git a/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj b/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj new file mode 100644 index 00000000..b81e28af --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/deploy/choreo/SimplePath.traj @@ -0,0 +1,67 @@ +{ + "name":"SimplePath", + "version":"v2025.0.0", + "snapshot":{ + "waypoints":[ + {"x":4.062, "y":5.039, "heading":0.0, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":5.293, "y":4.495, "heading":1.5707963267948966, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":6.152, "y":5.296, "heading":0.0, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}, + {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":0.0}}, "enabled":true}], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.062 m", "val":4.062}, "y":{"exp":"5.039 m", "val":5.039}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":15, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"5.293 m", "val":5.293}, "y":{"exp":"4.495 m", "val":4.495}, "heading":{"exp":"90 deg", "val":1.5707963267948966}, "intervals":14, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.152 m", "val":6.152}, "y":{"exp":"5.296 m", "val":5.296}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}, + {"from":1, "to":1, "data":{"type":"MaxAngularVelocity", "props":{"max":{"exp":"0 rad / s", "val":0.0}}}, "enabled":true}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "waypoints":[0.0,0.5697684177504896,1.0926420849095964], + "samples":[ + {"t":0.0, "x":4.062, "y":5.039, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":29.226458822189535, "ay":-12.424796441743036, "alpha":25.02487346105948, "fx":[308.66375142931423,118.2281283217828,256.3850513508591,310.9904731878708], "fy":[32.17344906648464,-287.6695818306081,-176.35209560720307,9.16376351299228]}, + {"t":0.03798456118336597, "x":4.08308436031912, "y":5.030036584806144, "heading":0.0, "vx":1.1101542133045843, "vy":-0.4719504406322561, "omega":0.9505588370876052, "ax":29.021758238876433, "ay":-12.07119821332368, "alpha":25.78921823332505, "fx":[306.2166540936873,114.45097851791286,256.04716744650557,310.58880752731835], "fy":[42.657858451708805,-288.738405766401,-176.3894323043004,11.81472414489855]}, + {"t":0.07596912236673194, "x":4.146189767523652, "y":5.003401429740383, "heading":0.03610656030574334, "vx":2.212532964778042, "vy":-0.9304696077227876, "omega":1.9301509749425176, "ax":-5.315878660499058, "ay":5.733966718633051, "alpha":57.24869949145691, "fx":[-206.30006834011385,-257.3487409635288,190.7307809589154,92.07487832611284], "fy":[198.6755951461055,-105.55381254440157,-161.88633874149653,263.8308226452342]}, + {"t":0.11395368355009793, "x":4.226396914961343, "y":4.972194510675685, "heading":0.10942249810658088, "vx":2.0106116465549655, "vy":-0.7126673980754862, "omega":4.104717703443895, "ax":-14.85421527076752, "ay":1.7900763501026262, "alpha":35.169356623635565, "fx":[-221.64642597565933,-217.8085179657918,-19.047496376066427,-46.829462869304706], "fy":[103.27062466367552,-95.72339988244455,-139.34234880126178,192.6924970793308]}, + {"t":0.15193824473346387, "x":4.292053085467321, "y":4.946415537435109, "heading":0.265338398853491, "vx":1.4463807977716068, "vy":-0.6446721334321163, "omega":5.440610281894, "ax":1.87865415324475, "ay":-12.069381361863892, "alpha":-17.57272954129737, "fx":[84.37697252110135,70.1020588457297,-66.58460191226365,-23.983690221020176], "fy":[-137.69418022294582,-36.91127952292192,-71.99502473592722,-163.99296274533037]}, + {"t":0.18992280591682983, "x":4.3483485117377745, "y":4.913220935362567, "heading":0.4719975929809436, "vx":1.5177406513979177, "vy":-1.103122288217213, "omega":4.773117861473843, "ax":6.729116420714396, "ay":-9.426907219822809, "alpha":-22.954990510598343, "fx":[147.43351150590618,110.99549997339378,-54.43224924954307,24.92392766607512], "fy":[-105.79432002739296,22.954753871071947,-63.820368974565866,-174.03805393982847]}, + {"t":0.2279073671001958, "x":4.41085369942441, "y":4.864518621707944, "heading":0.6533023804255137, "vx":1.7733431857905364, "vy":-1.4611992222784869, "omega":3.901182619960436, "ax":6.217212503653662, "ay":-5.363273704941779, "alpha":-17.017852030853437, "fx":[125.1957022037703,70.8271420047267,-27.14760101229933,42.63076837824471], "fy":[-54.043516257935,34.310332066852396,-41.500362262106535,-121.22195585555252]}, + {"t":0.2658919282835618, "x":4.482698542849427, "y":4.8051464726928375, "heading":0.8014870903408847, "vx":2.0095012745255563, "vy":-1.6649208204669856, "omega":3.2547669782850157, "ax":7.452297109521514, "ay":-0.7638505668761703, "alpha":-16.002906071348082, "fx":[130.73340422281083,66.00340850569457,-10.17432983130126,66.96040019169689], "fy":[-5.13399005852738,69.58721024287266,-8.93518948065377,-81.50378987533193]}, + {"t":0.30387648946692775, "x":4.564404754284277, "y":4.741354133854086, "heading":0.925117985765151, "vx":2.292573510038798, "vy":-1.6939353490594422, "omega":2.6469036135062356, "ax":7.1884593920664095, "ay":3.818034260590328, "alpha":-11.910038807587478, "fx":[111.42368834322436,53.46840279853804,7.055691780048093,72.59949200040163], "fy":[38.599253433555575,87.31169861138011,27.53575831360324,-23.559369683361584]}, + {"t":0.3418610506502937, "x":4.656673004291847, "y":4.6797651241928735, "heading":1.0256594580188512, "vx":2.565623985630886, "vy":-1.5489089930878617, "omega":2.194506015723166, "ax":2.723181317765863, "ay":5.779809499044349, "alpha":1.3149162094791331, "fx":[17.237452910502054,24.76903049316307,29.049576865635597,21.58500982058484], "fy":[47.90557286513214,43.397061253469744,50.43156616299465,54.891611379906394]}, + {"t":0.37984561183365967, "x":4.756091645161041, "y":4.625100128054678, "heading":1.1090168060403525, "vx":2.6690628330089625, "vy":-1.3293654655432117, "omega":2.244452530933126, "ax":-1.6577212706357212, "ay":7.714780667545829, "alpha":14.673305524362782, "fx":[-81.57053672741239,8.297740564223327,50.85551212702628,-33.97744495986733], "fy":[46.588403532014816,2.458568899281396,89.5164898000497,123.88896129532614]}, + {"t":0.4178301730170256, "x":4.856278923232433, "y":4.580170310685905, "heading":1.1942713505247422, "vx":2.606095017979533, "vy":-1.0363229072605682, "omega":2.8018116023855058, "ax":-5.0005666993769635, "ay":9.36496492674863, "alpha":23.194455968434497, "fx":[-147.17469622055927,-2.2379332041393987,57.49367627088672,-78.19746438469859], "fy":[41.90057052998772,-20.495938115090336,133.76600311948602,163.4201121724257]}, + {"t":0.4558147342003916, "x":4.95166282284726, "y":4.547562051411459, "heading":1.3006969347598192, "vx":2.4161506862355457, "vy":-0.6805988240204084, "omega":3.682842834233394, "ax":6.8738129624669035, "ay":-3.904023420766954, "alpha":-29.79573890174241, "fx":[174.06517618077348,-0.9561061006684474,-59.05323057541544,119.78734393896669], "fy":[27.23921591658561,99.80438837463473,-119.21491896314672,-140.64132802516286]}, + {"t":0.4937992953837575, "x":5.0483981075004785, "y":4.518893388757126, "heading":1.4405881037254784, "vx":2.677249455271384, "vy":-0.8288914405078245, "omega":2.5510647669165616, "ax":16.88750155862789, "ay":7.461571076732664, "alpha":-44.07592406956584, "fx":[243.3503743643859,41.43767722148993,4.0201436014902105,285.69494396438773], "fy":[169.89802561155176,286.9921470883066,-143.50308199455745,-59.54871255890434]}, + {"t":0.5317838565671236, "x":5.162275123899805, "y":4.492791188809997, "heading":1.5374891794471497, "vx":3.318713791459273, "vy":-0.5454669374196388, "omega":0.8768601323827456, "ax":6.466457031376719, "ay":31.782237542095473, "alpha":-23.08464557876047, "fx":[131.34583520578482,-38.73569427885679,-101.76576644307411,229.1407932935459], "fy":[280.1132982326035,306.79025429889623,288.9853074972303,205.32467376792448]}, + {"t":0.5697684177504896, "x":5.293, "y":4.495, "heading":1.5707963267948966, "vx":3.564339324207209, "vy":0.6617674090423575, "omega":0.0, "ax":-17.46995262929559, "ay":29.018547812206343, "alpha":-17.231876555711285, "fx":[-39.50542738838971,-184.48359090699591,-273.6713547280688,-96.65741824478924], "fy":[306.4819959225835,248.4794912616831,142.4475943346916,289.7853091883159]}, + {"t":0.6071165368332829, "x":5.413937108308784, "y":4.539954492988536, "heading":1.5707963267948966, "vx":2.911869453037518, "vy":1.7455555883423737, "omega":-0.6435781776227005, "ax":-23.364641119392896, "ay":5.7807117477879935, "alpha":-39.67090658706738, "fx":[-67.34423076259405,-252.6730740798265,-297.2001208453638,-177.6342947780813], "fy":[289.77837510305454,168.01639761931054,-53.64736080012576,-207.49090627478944]}, + {"t":0.6444646559160763, "x":5.506394496738847, "y":4.609179416346846, "heading":1.5467598923779569, "vx":2.039244054183701, "vy":1.9614542990820625, "omega":-2.125211920958866, "ax":-17.08062391895474, "ay":-6.113696994733446, "alpha":-43.98571665319571, "fx":[-45.13948058631435,-279.97389696639857,-232.99854159165108,-22.961132191438647], "fy":[187.3372315018108,55.16288998573377,-173.33039167205808,-277.154203013213]}, + {"t":0.6818127749988696, "x":5.570643699095066, "y":4.678172102141704, "heading":1.467387224477813, "vx":1.4013148780501703, "vy":1.7331192156866413, "omega":-3.767995704464427, "ax":-13.783674765700583, "ay":-2.060787922443541, "alpha":-33.13955160972574, "fx":[-13.955054195494917,-206.469783817391,-209.47327070653645,-39.014619101826874], "fy":[120.73464042758,88.84614026474486,-101.22862305402826,-178.45898347393725]}, + {"t":0.719160894081663, "x":5.613366874127446, "y":4.741463567005436, "heading":1.3266596722040216, "vx":0.8865205515022906, "vy":1.6561526629548375, "omega":-5.005695624334843, "ax":2.672856932809343, "ay":9.54645245402934, "alpha":16.0222098923521, "fx":[-47.38196837882718,70.11939865428032,83.23389464940595,-15.042261613065044], "fy":[49.59058204687828,20.06847309949008,115.03442906944572,140.0713653128472]}, + {"t":0.7565090131644563, "x":5.648340909265066, "y":4.8099758412220845, "heading":1.139706355934146, "vx":0.9863467305201242, "vy":2.01269470602615, "omega":-4.4072962213057645, "ax":12.859139992620332, "ay":3.667765887073824, "alpha":26.60201862217061, "fx":[-4.295504320648231,164.00355143520014,201.0275711835912,76.72496560793975], "fy":[-16.39144275639421,-87.29775417519376,66.37748920237186,162.08700432843872]}, + {"t":0.7938571322472496, "x":5.694147595862764, "y":4.88770425308631, "heading":0.9751021318276732, "vx":1.4666114222668192, "vy":2.1496788631443913, "omega":-3.4137608619622495, "ax":9.05159801140143, "ay":-2.8712116541580657, "alpha":9.840448242313665, "fx":[33.74996676276294,84.39228031979921,117.89061174702113,71.89732574133126], "fy":[-36.34012022303755,-69.3187976732754,-14.902156768284591,22.88409724100924]}, + {"t":0.831205251330043, "x":5.755235729474016, "y":4.965988214530906, "heading":0.8476045846349277, "vx":1.8046715826862156, "vy":2.042444508372992, "omega":-3.046238629180254, "ax":3.140941449362886, "ay":-8.186298171812115, "alpha":-10.876582378244004, "fx":[77.7135813106321,26.038748456400604,-25.48154243504908,28.582243371597357], "fy":[-66.39100380634368,-20.537552718511357,-76.11454639420802,-115.45007627685624]}, + {"t":0.8685533704128363, "x":5.8248274399933635, "y":5.036560215270374, "heading":0.7338333015576982, "vx":1.9219798379691024, "vy":1.7367016694048991, "omega":-3.4524585230567233, "ax":-2.1136936679481573, "ay":-12.199823559824065, "alpha":-26.5652580412972, "fx":[113.85459795703362,-18.463577787989276,-145.61248556710063,-21.685183624338517], "fy":[-125.93798736716592,20.908405277057756,-106.13010292994028,-203.87133113613396]}, + {"t":0.9059014894956297, "x":5.8951355952321265, "y":5.09291409889317, "heading":0.6048904695101708, "vx":1.843037355154028, "vy":1.2810612063035216, "omega":-4.444620943848226, "ax":-4.975894252152143, "ay":-12.842141108806985, "alpha":-28.569652725571643, "fx":[98.33440398761331,-32.790105159095255,-176.94929691499476,-57.87207691625341], "fy":[-156.51287788540583,25.786971604579417,-93.35174350373175,-212.8046418218059]}, + {"t":0.943249608578423, "x":5.960499181185772, "y":5.131802689647749, "heading":0.4388922372214496, "vx":1.6571970640812623, "vy":0.801431390893762, "omega":-5.511643735996928, "ax":1.5837604531226042, "ay":16.193366722649536, "alpha":43.88839575402028, "fx":[-149.68061371021807,-121.31934856991572,246.5150473068684,78.36353928157708], "fy":[232.0159727719713,-66.98157990187129,112.687353810509,273.16732256982107]}, + {"t":0.9805977276612164, "x":6.023496953952147, "y":5.173028562544035, "heading":0.23304271062750417, "vx":1.7163475380831044, "vy":1.4062231796026206, "omega":-3.8724947050230143, "ax":0.4044973447465699, "ay":8.038246855365111, "alpha":60.94919355877796, "fx":[-190.75104638704465,-208.20122535075024,289.0879228400815,123.6251170923862], "fy":[226.59281633314,-191.0430903735248,-33.154933301743746,271.0617654748866]}, + {"t":1.0179458467440097, "x":6.087881419224354, "y":5.231154556233942, "heading":0.08841231723681776, "vx":1.731454753083373, "vy":1.706436580373686, "omega":-1.5961569659895516, "ax":-23.57280881417639, "ay":-22.046919504329445, "alpha":22.09119026985219, "fx":[-306.5951390537582,-209.05062942645768,-23.56351817578218,-262.7241796624389], "fy":[-50.58280431138501,-230.15481580587175,-309.79414240855715,-159.49182266178744]}, + {"t":1.055293965826803, "x":6.136107354188334, "y":5.279510327269729, "heading":0.028798856796209797, "vx":0.8510546823755913, "vy":0.88302560531723, "omega":-0.7710925611104655, "ax":-22.787082811023808, "ay":-23.643107792382715, "alpha":20.646088211326145, "fx":[-306.95613371172647,-214.45640732387,-35.299148510694714,-218.49182777660005], "fy":[-50.46330441881009,-225.4797127562022,-309.064962685133,-219.3170174682804]}, + {"t":1.0926420849095964, "x":6.152, "y":5.296, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}], + "splits":[0] + }, + "events":[] +} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor b/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor new file mode 100644 index 00000000..f424b0c1 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/deploy/choreo/Tests.chor @@ -0,0 +1,74 @@ +{ + "name":"Tests", + "version":"v2025.0.0", + "type":"Swerve", + "variables":{ + "expressions":{}, + "poses":{} + }, + "config":{ + "frontLeft":{ + "x":{ + "exp":"10.5 in", + "val":0.2667 + }, + "y":{ + "exp":"10.5 in", + "val":0.2667 + } + }, + "backLeft":{ + "x":{ + "exp":"-10.5 in", + "val":-0.2667 + }, + "y":{ + "exp":"10.5 in", + "val":0.2667 + } + }, + "mass":{ + "exp":"75 lbs", + "val":34.01942775 + }, + "inertia":{ + "exp":"6.883 kg m ^ 2", + "val":6.883 + }, + "gearing":{ + "exp":"7.363636364", + "val":7.363636364 + }, + "radius":{ + "exp":"2.167 in", + "val":0.055041799999999995 + }, + "vmax":{ + "exp":"485.89966375522135 rad / s", + "val":485.8996637552213 + }, + "tmax":{ + "exp":"2.327950310559006 N m", + "val":2.327950310559006 + }, + "bumper":{ + "front":{ + "exp":"16 in", + "val":0.4064 + }, + "side":{ + "exp":"16 in", + "val":0.4064 + }, + "back":{ + "exp":"16 in", + "val":0.4064 + } + }, + "differentialTrackWidth":{ + "exp":"22 in", + "val":0.5588 + } + }, + "generationFeatures":[] +} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/deploy/example.txt b/cpp/SwerveWithChoreo/src/main/deploy/example.txt new file mode 100644 index 00000000..bb82515d --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h b/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h new file mode 100644 index 00000000..ba9336ba --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/AutoRoutines.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include "subsystems/CommandSwerveDrivetrain.h" + +class AutoRoutines { +private: + subsystems::CommandSwerveDrivetrain &m_drivetrain; + +public: + AutoRoutines(subsystems::CommandSwerveDrivetrain &drivetrain) : + m_drivetrain{drivetrain} + {} + + frc2::CommandPtr SimplePathAuto(choreo::AutoFactory &factory); +}; diff --git a/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h b/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h new file mode 100644 index 00000000..35a53040 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/LimelightHelpers.h @@ -0,0 +1,877 @@ +#pragma once + +/// +//https://github.com/LimelightVision/limelightlib-wpicpp +/// + +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableInstance.h" +#include "networktables/NetworkTableEntry.h" +#include "networktables/NetworkTableValue.h" +#include +#include "wpi/json.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +//#include +// #include +// #include +// #include +#include +#include + +namespace LimelightHelpers +{ + inline std::string sanitizeName(const std::string &name) + { + if (name == "") + { + return "limelight"; + } + return name; + } + + inline frc::Pose3d toPose3D(const std::vector& inData) + { + if(inData.size() < 6) + { + return frc::Pose3d(); + } + return frc::Pose3d( + frc::Translation3d(units::length::meter_t(inData[0]), units::length::meter_t(inData[1]), units::length::meter_t(inData[2])), + frc::Rotation3d(units::angle::degree_t(inData[3]), units::angle::degree_t(inData[4]), + units::angle::degree_t(inData[5]))); + } + + inline frc::Pose2d toPose2D(const std::vector& inData) + { + if(inData.size() < 6) + { + return frc::Pose2d(); + } + return frc::Pose2d( + frc::Translation2d(units::length::meter_t(inData[0]), units::length::meter_t(inData[1])), + frc::Rotation2d(units::angle::degree_t(inData[5]))); + } + + inline std::shared_ptr getLimelightNTTable(const std::string &tableName) + { + return nt::NetworkTableInstance::GetDefault().GetTable(sanitizeName(tableName)); + } + + inline nt::NetworkTableEntry getLimelightNTTableEntry(const std::string &tableName, const std::string &entryName) + { + return getLimelightNTTable(tableName)->GetEntry(entryName); + } + + inline double getLimelightNTDouble(const std::string &tableName, const std::string &entryName) + { + return getLimelightNTTableEntry(tableName, entryName).GetDouble(0.0); + } + + inline std::vector getLimelightNTDoubleArray(const std::string &tableName, const std::string &entryName) + { + return getLimelightNTTableEntry(tableName, entryName).GetDoubleArray(std::span{}); + } + + inline std::string getLimelightNTString(const std::string &tableName, const std::string &entryName) + { + return getLimelightNTTableEntry(tableName, entryName).GetString(""); + } + + inline void setLimelightNTDouble(const std::string &tableName, const std::string entryName, double val) + { + getLimelightNTTableEntry(tableName, entryName).SetDouble(val); + } + + inline void setLimelightNTDoubleArray(const std::string &tableName, const std::string &entryName, const std::span &vals) + { + getLimelightNTTableEntry(tableName, entryName).SetDoubleArray(vals); + } + + inline double getTX(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "tx"); + } + + inline double getTV(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "tv"); + } + + inline double getTY(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "ty"); + } + + inline double getTA(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "ta"); + } + + inline double getLatency_Pipeline(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "tl"); + } + + inline double getLatency_Capture(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "cl"); + } + + inline std::string getJSONDump(const std::string &limelightName = "") + { + return getLimelightNTString(limelightName, "json"); + } + + inline std::vector getBotpose(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + inline std::vector getBotpose_wpiRed(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + inline std::vector getBotpose_wpiBlue(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + + + inline std::vector getBotpose_TargetSpace(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + inline std::vector getCameraPose_TargetSpace(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + inline std::vector getCameraPose_RobotSpace(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + } + + inline std::vector getTargetPose_CameraSpace(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + inline std::vector getTargetPose_RobotSpace(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + inline std::vector getTargetColor(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + inline double getFiducialID(const std::string &limelightName = "") + { + return getLimelightNTDouble(limelightName, "tid"); + } + + inline std::string getNeuralClassID(const std::string &limelightName = "") + { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + inline void setPipelineIndex(const std::string &limelightName, int index) + { + setLimelightNTDouble(limelightName, "pipeline", index); + } + + inline void setPriorityTagID(const std::string &limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + inline void setLEDMode_PipelineControl(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + inline void setLEDMode_ForceOff(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + inline void setLEDMode_ForceBlink(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + inline void setLEDMode_ForceOn(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + inline void setStreamMode_Standard(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "stream", 0); + } + + inline void setStreamMode_PiPMain(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "stream", 1); + } + + inline void setStreamMode_PiPSecondary(const std::string &limelightName = "") + { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + inline void setCropWindow(const std::string &limelightName, double cropXMin, + double cropXMax, double cropYMin, double cropYMax) + { + double cropWindow[4]{cropXMin, cropXMax, cropYMin, cropYMax}; + setLimelightNTDoubleArray(limelightName, "crop", cropWindow); + } + + /** + * Sets the robot orientation for mt2. + */ + inline void SetRobotOrientation(const std::string& limelightName, + double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) + { + std::vector entries = {yaw, yawRate, pitch, pitchRate, roll, rollRate}; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + inline void SetFiducialDownscaling(const std::string& limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + inline void SetFiducialIDFiltersOverride(const std::string& limelightName, const std::vector& validIDs) + { + std::vector validIDsDouble(validIDs.begin(), validIDs.end()); + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + ///// + ///// + + /** + * Sets the camera pose in robotspace. The UI camera pose must be set to zeros + */ + inline void setCameraPose_RobotSpace(const std::string &limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double entries[6] ={forward, side, up, roll, pitch, yaw}; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + inline void setPythonScriptData(const std::string &limelightName, const std::vector &outgoingPythonData) + { + setLimelightNTDoubleArray(limelightName, "llrobot", std::span{outgoingPythonData.begin(), outgoingPythonData.size()}); + } + + inline std::vector getPythonScriptData(const std::string &limelightName = "") + { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + + inline double extractArrayEntry(const std::vector& inData, int position) { + if (inData.size() < static_cast(position + 1)) { + return 0.0; + } + return inData[position]; + } + + class RawFiducial + { + public: + int id{0}; + double txnc{0.0}; + double tync{0.0}; + double ta{0.0}; + double distToCamera{0.0}; + double distToRobot{0.0}; + double ambiguity{0.0}; + + RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) + : id(id), txnc(txnc), tync(tync), ta(ta), distToCamera(distToCamera), distToRobot(distToRobot), ambiguity(ambiguity) {} + }; + + inline std::vector getRawFiducials(const std::string& limelightName) + { + nt::NetworkTableEntry entry = LimelightHelpers::getLimelightNTTableEntry(limelightName, "rawfiducials"); + std::vector rawFiducialArray = entry.GetDoubleArray({}); + int valsPerEntry = 7; + if (rawFiducialArray.size() % valsPerEntry != 0) { + return {}; + } + + int numFiducials = rawFiducialArray.size() / valsPerEntry; + std::vector rawFiducials; + + for (int i = 0; i < numFiducials; ++i) { + int baseIndex = i * valsPerEntry; + int id = static_cast(extractArrayEntry(rawFiducialArray, baseIndex)); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials.emplace_back(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + + class RawDetection + { + public: + int classId{-1}; + double txnc{0.0}; + double tync{9.0}; // It seems like you intentionally set this to 9.0, so I kept it as is. + double ta{0.0}; + double corner0_X{0.0}; + double corner0_Y{0.0}; + double corner1_X{0.0}; + double corner1_Y{0.0}; + double corner2_X{0.0}; + double corner2_Y{0.0}; + double corner3_X{0.0}; + double corner3_Y{0.0}; + + RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y) + : classId(classId), txnc(txnc), tync(tync), ta(ta), + corner0_X(corner0_X), corner0_Y(corner0_Y), + corner1_X(corner1_X), corner1_Y(corner1_Y), + corner2_X(corner2_X), corner2_Y(corner2_Y), + corner3_X(corner3_X), corner3_Y(corner3_Y) {} + }; + + inline std::vector getRawDetections(const std::string& limelightName) + { + nt::NetworkTableEntry entry = LimelightHelpers::getLimelightNTTableEntry(limelightName, "rawdetections"); + std::vector rawDetectionArray = entry.GetDoubleArray({}); + int valsPerEntry = 11; + + if (rawDetectionArray.size() % valsPerEntry != 0) { + return {}; + } + + int numDetections = rawDetectionArray.size() / valsPerEntry; + std::vector rawDetections; + + for (int i = 0; i < numDetections; ++i) { + int baseIndex = i * valsPerEntry; + int classId = static_cast(extractArrayEntry(rawDetectionArray, baseIndex)); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections.emplace_back(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + class PoseEstimate + { + public: + frc::Pose2d pose; + units::time::second_t timestampSeconds{0.0}; + double latency{0.0}; + int tagCount{0}; + double tagSpan{0.0}; + double avgTagDist{0.0}; + double avgTagArea{0.0}; + std::vector rawFiducials; + + PoseEstimate() = default; + + PoseEstimate(const frc::Pose2d& pose, units::time::second_t timestampSeconds, + double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea, + const std::vector& rawFiducials) + : pose(pose), timestampSeconds(timestampSeconds), + latency(latency), tagCount(tagCount), tagSpan(tagSpan), + avgTagDist(avgTagDist), avgTagArea(avgTagArea), rawFiducials(rawFiducials) + { + } + }; + + inline std::optional getBotPoseEstimate(const std::string& limelightName, const std::string& entryName) { + nt::NetworkTableEntry poseEntry = getLimelightNTTableEntry(limelightName, entryName); + std::vector poseArray = poseEntry.GetDoubleArray(std::span{}); + frc::Pose2d pose = toPose2D(poseArray); + + if (poseArray.size() == 0) { + // Handle the case where no data is available + return std::nullopt; // or some default PoseEstimate + } + + double latency = extractArrayEntry(poseArray, 6); + int tagCount = static_cast(extractArrayEntry(poseArray, 7)); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // getLastChange: microseconds; latency: milliseconds + units::time::second_t timestamp = units::time::second_t((poseEntry.GetLastChange() / 1000000.0) - (latency / 1000.0)); + + std::vector rawFiducials; + constexpr int valsPerFiducial = 7; + size_t expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.size() == expectedTotalVals) + { + for (int i = 0; i < tagCount; i++) + { + int baseIndex = 11 + (i * valsPerFiducial); + int id = static_cast(extractArrayEntry(poseArray, baseIndex)); + double txnc = extractArrayEntry(poseArray, baseIndex + 1); + double tync = extractArrayEntry(poseArray, baseIndex + 2); + double ta = extractArrayEntry(poseArray, baseIndex + 3); + double distToCamera = extractArrayEntry(poseArray, baseIndex + 4); + double distToRobot = extractArrayEntry(poseArray, baseIndex + 5); + double ambiguity = extractArrayEntry(poseArray, baseIndex + 6); + rawFiducials.emplace_back(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + inline std::optional getBotPoseEstimate_wpiBlue(const std::string &limelightName = "") { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + inline std::optional getBotPoseEstimate_wpiRed(const std::string &limelightName = "") { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + inline std::optional getBotPoseEstimate_wpiBlue_MegaTag2(const std::string &limelightName = "") { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + inline std::optional getBotPoseEstimate_wpiRed_MegaTag2(const std::string &limelightName = "") { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + inline const double INVALID_TARGET = 0.0; + class SingleTargetingResultClass + { + public: + SingleTargetingResultClass(){}; + ~SingleTargetingResultClass(){}; + double m_TargetXPixels{INVALID_TARGET}; + double m_TargetYPixels{INVALID_TARGET}; + + double m_TargetXNormalized{INVALID_TARGET}; + double m_TargetYNormalized{INVALID_TARGET}; + + double m_TargetXNormalizedCrosshairAdjusted{INVALID_TARGET}; + double m_TargetYNormalizedCrosshairAdjusted{INVALID_TARGET}; + + double m_TargetXDegreesCrosshairAdjusted{INVALID_TARGET}; + double m_TargetYDegreesCrosshairAdjusted{INVALID_TARGET}; + + double m_TargetAreaPixels{INVALID_TARGET}; + double m_TargetAreaNormalized{INVALID_TARGET}; + double m_TargetAreaNormalizedPercentage{INVALID_TARGET}; + + // not included in json// + double m_timeStamp{-1.0}; + double m_latency{0}; + double m_pipelineIndex{-1.0}; + std::vector> m_TargetCorners; + + std::vector m_CAMERATransform6DTARGETSPACE; + std::vector m_TargetTransform6DCAMERASPACE; + std::vector m_TargetTransform6DROBOTSPACE; + std::vector m_ROBOTTransform6DTARGETSPACE; + std::vector m_ROBOTTransform6DFIELDSPACE; + std::vector m_CAMERATransform6DROBOTSPACE; + + }; + + class RetroreflectiveResultClass : public SingleTargetingResultClass + { + public: + RetroreflectiveResultClass() {} + ~RetroreflectiveResultClass() {} + }; + + class FiducialResultClass : public SingleTargetingResultClass + { + public: + FiducialResultClass() {} + ~FiducialResultClass() {} + int m_fiducialID{0}; + std::string m_family{""}; + }; + + class DetectionResultClass : public SingleTargetingResultClass + { + public: + DetectionResultClass() {} + ~DetectionResultClass() {} + + int m_classID{-1}; + std::string m_className{""}; + double m_confidence{0}; + }; + + class ClassificationResultClass : public SingleTargetingResultClass + { + public: + ClassificationResultClass() {} + ~ClassificationResultClass() {} + + int m_classID{-1}; + std::string m_className{""}; + double m_confidence{0}; + }; + + class VisionResultsClass + { + public: + VisionResultsClass() {} + ~VisionResultsClass() {} + std::vector RetroResults; + std::vector FiducialResults; + std::vector DetectionResults; + std::vector ClassificationResults; + double m_timeStamp{-1.0}; + double m_latencyPipeline{0}; + double m_latencyCapture{0}; + double m_latencyJSON{0}; + double m_pipelineIndex{-1.0}; + int valid{0}; + std::vector botPose{6,0.0}; + std::vector botPose_wpired{6,0.0}; + std::vector botPose_wpiblue{6,0.0}; + void Clear() + { + RetroResults.clear(); + FiducialResults.clear(); + DetectionResults.clear(); + ClassificationResults.clear(); + botPose.clear(); + botPose_wpired.clear(); + botPose_wpiblue.clear(); + m_timeStamp = -1.0; + m_latencyPipeline = 0; + + m_pipelineIndex = -1.0; + } + }; + + class LimelightResultsClass + { + public: + LimelightResultsClass() {} + ~LimelightResultsClass() {} + VisionResultsClass targetingResults; + }; + + namespace internal + { + inline const std::string _key_timestamp{"ts"}; + inline const std::string _key_latency_pipeline{"tl"}; + inline const std::string _key_latency_capture{"cl"}; + + inline const std::string _key_pipelineIndex{"pID"}; + inline const std::string _key_TargetXDegrees{"txdr"}; + inline const std::string _key_TargetYDegrees{"tydr"}; + inline const std::string _key_TargetXNormalized{"txnr"}; + inline const std::string _key_TargetYNormalized{"tynr"}; + inline const std::string _key_TargetXPixels{"txp"}; + inline const std::string _key_TargetYPixels{"typ"}; + + inline const std::string _key_TargetXDegreesCrosshair{"tx"}; + inline const std::string _key_TargetYDegreesCrosshair{"ty"}; + inline const std::string _key_TargetXNormalizedCrosshair{"txn"}; + inline const std::string _key_TargetYNormalizedCrosshair{"tyn"}; + inline const std::string _key_TargetAreaNormalized{"ta"}; + inline const std::string _key_TargetAreaPixels{"tap"}; + inline const std::string _key_className{"class"}; + inline const std::string _key_classID{"classID"}; + inline const std::string _key_confidence{"conf"}; + inline const std::string _key_fiducialID{"fID"}; + inline const std::string _key_corners{"pts"}; + inline const std::string _key_transformCAMERAPOSE_TARGETSPACE{"t6c_ts"}; + inline const std::string _key_transformCAMERAPOSE_ROBOTSPACE{"t6c_rs"}; + + inline const std::string _key_transformTARGETPOSE_CAMERASPACE{"t6t_cs"}; + inline const std::string _key_transformROBOTPOSE_TARGETSPACE{"t6r_ts"}; + inline const std::string _key_transformTARGETPOSE_ROBOTSPACE{"t6t_rs"}; + + inline const std::string _key_botpose{"botpose"}; + inline const std::string _key_botpose_wpiblue{"botpose_wpiblue"}; + inline const std::string _key_botpose_wpired{"botpose_wpired"}; + + inline const std::string _key_transformROBOTPOSE_FIELDSPACE{"t6r_fs"}; + inline const std::string _key_skew{"skew"}; + inline const std::string _key_ffamily{"fam"}; + inline const std::string _key_colorRGB{"cRGB"}; + inline const std::string _key_colorHSV{"cHSV"}; + } + + // inline void PhoneHome() + // { + // static int sockfd = -1; + // static struct sockaddr_in servaddr, cliaddr; + + // if (sockfd == -1) { + // sockfd = socket(AF_INET, SOCK_DGRAM, 0); + // if (sockfd < 0) { + // std::cerr << "Socket creation failed" << std::endl; + // return; + // } + + // memset(&servaddr, 0, sizeof(servaddr)); + // servaddr.sin_family = AF_INET; + // servaddr.sin_addr.s_addr = inet_addr("255.255.255.255"); + // servaddr.sin_port = htons(5809); + + // // Set socket for broadcast + // int broadcast = 1; + // if (setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)) < 0) { + // std::cerr << "Error in setting Broadcast option" << std::endl; + // close(sockfd); + // sockfd = -1; + // return; + // } + + // // Set socket to non-blocking + // if (fcntl(sockfd, F_SETFL, O_NONBLOCK) < 0) { + // std::cerr << "Error setting socket to non-blocking" << std::endl; + // close(sockfd); + // sockfd = -1; + // return; + // } + + // const char *msg = "LLPhoneHome"; + // sendto(sockfd, msg, strlen(msg), 0, (const struct sockaddr *) &servaddr, sizeof(servaddr)); + // } + + // char receiveData[1024]; + // socklen_t len = sizeof(cliaddr); + + // ssize_t n = recvfrom(sockfd, (char *)receiveData, 1024, 0, (struct sockaddr *) &cliaddr, &len); + // if (n > 0) { + // receiveData[n] = '\0'; // Null-terminate the received string + // std::string received(receiveData, n); + // std::cout << "Received response: " << received << std::endl; + // } else if (n < 0 && errno != EWOULDBLOCK && errno != EAGAIN) { + // std::cerr << "Error receiving data" << std::endl; + // close(sockfd); + // sockfd = -1; + // } + // } + + inline void SetupPortForwarding(const std::string& limelightName) + { + auto& portForwarder = wpi::PortForwarder::GetInstance(); + portForwarder.Add(5800, sanitizeName(limelightName), 5800); + portForwarder.Add(5801, sanitizeName(limelightName), 5801); + portForwarder.Add(5802, sanitizeName(limelightName), 5802); + portForwarder.Add(5803, sanitizeName(limelightName), 5803); + portForwarder.Add(5804, sanitizeName(limelightName), 5804); + portForwarder.Add(5805, sanitizeName(limelightName), 5805); + portForwarder.Add(5806, sanitizeName(limelightName), 5806); + portForwarder.Add(5807, sanitizeName(limelightName), 5807); + portForwarder.Add(5808, sanitizeName(limelightName), 5808); + portForwarder.Add(5809, sanitizeName(limelightName), 5809); + } + + template + T SafeJSONAccess(const wpi::json& jsonData, const KeyType& key, const T& defaultValue) + { + try + { + return jsonData.at(key).template get(); + } + catch (wpi::json::exception&) + { + return defaultValue; + } + catch (...) + { + return defaultValue; + } + } + inline void from_json(const wpi::json &j, RetroreflectiveResultClass &t) + { + std::vector defaultValueVector(6, 0.0); + t.m_CAMERATransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_TARGETSPACE, defaultValueVector); + t.m_CAMERATransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_ROBOTSPACE, defaultValueVector); + + t.m_TargetTransform6DCAMERASPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_CAMERASPACE, defaultValueVector); + t.m_TargetTransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_ROBOTSPACE, defaultValueVector); + t.m_ROBOTTransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_TARGETSPACE, defaultValueVector); + t.m_ROBOTTransform6DFIELDSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_FIELDSPACE, defaultValueVector); + + t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); + t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); + t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); + t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); + t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); + t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); + } + + inline void from_json(const wpi::json &j, FiducialResultClass &t) + { + std::vector defaultValueVector(6, 0.0); + t.m_family = SafeJSONAccess(j, internal::_key_ffamily, ""); + t.m_fiducialID = SafeJSONAccess(j, internal::_key_fiducialID, 0.0); + t.m_CAMERATransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_TARGETSPACE, defaultValueVector); + t.m_CAMERATransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformCAMERAPOSE_ROBOTSPACE, defaultValueVector); + t.m_TargetTransform6DCAMERASPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_CAMERASPACE, defaultValueVector); + t.m_TargetTransform6DROBOTSPACE = SafeJSONAccess>(j, internal::_key_transformTARGETPOSE_ROBOTSPACE, defaultValueVector); + t.m_ROBOTTransform6DTARGETSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_TARGETSPACE, defaultValueVector); + t.m_ROBOTTransform6DFIELDSPACE = SafeJSONAccess>(j, internal::_key_transformROBOTPOSE_FIELDSPACE, defaultValueVector); + + t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); + t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); + t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); + t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); + t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); + t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); + } + + inline void from_json(const wpi::json &j, DetectionResultClass &t) + { + t.m_confidence = SafeJSONAccess(j, internal::_key_confidence, 0.0); + t.m_classID = SafeJSONAccess(j, internal::_key_classID, 0.0); + t.m_className = SafeJSONAccess(j, internal::_key_className, ""); + t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); + t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); + t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); + t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); + t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); + t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); + } + + inline void from_json(const wpi::json &j, ClassificationResultClass &t) + { + t.m_confidence = SafeJSONAccess(j, internal::_key_confidence, 0.0); + t.m_classID = SafeJSONAccess(j, internal::_key_classID, 0.0); + t.m_className = SafeJSONAccess(j, internal::_key_className, ""); + t.m_TargetXPixels = SafeJSONAccess(j, internal::_key_TargetXPixels, 0.0); + t.m_TargetYPixels = SafeJSONAccess(j, internal::_key_TargetYPixels, 0.0); + t.m_TargetXDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetXDegreesCrosshair, 0.0); + t.m_TargetYDegreesCrosshairAdjusted = SafeJSONAccess(j, internal::_key_TargetYDegreesCrosshair, 0.0); + t.m_TargetAreaNormalized = SafeJSONAccess(j, internal::_key_TargetAreaNormalized, 0.0); + t.m_TargetCorners = SafeJSONAccess>>(j, internal::_key_corners, std::vector>{}); + } + + inline void from_json(const wpi::json &j, VisionResultsClass &t) + { + t.m_timeStamp = SafeJSONAccess(j, internal::_key_timestamp, 0.0); + t.m_latencyPipeline = SafeJSONAccess(j, internal::_key_latency_pipeline, 0.0); + t.m_latencyCapture = SafeJSONAccess(j, internal::_key_latency_capture, 0.0); + t.m_pipelineIndex = SafeJSONAccess(j, internal::_key_pipelineIndex, 0.0); + t.valid = SafeJSONAccess(j, "v", 0.0); + + std::vector defaultVector{}; + t.botPose = SafeJSONAccess>(j, internal::_key_botpose, defaultVector); + t.botPose_wpired = SafeJSONAccess>(j, internal::_key_botpose_wpired, defaultVector); + t.botPose_wpiblue = SafeJSONAccess>(j, internal::_key_botpose_wpiblue, defaultVector); + + t.RetroResults = SafeJSONAccess>(j, "Retro", std::vector< RetroreflectiveResultClass>{}); + t.FiducialResults = SafeJSONAccess>(j, "Fiducial", std::vector< FiducialResultClass>{}); + t.DetectionResults = SafeJSONAccess>(j, "Detector", std::vector< DetectionResultClass>{}); + t.ClassificationResults = SafeJSONAccess>(j, "Detector", std::vector< ClassificationResultClass>{}); + } + + inline void from_json(const wpi::json &j, LimelightResultsClass &t) + { + t.targetingResults = SafeJSONAccess(j, "Results", LimelightHelpers::VisionResultsClass{}); + } + + inline LimelightResultsClass getLatestResults(const std::string &limelightName = "", bool profile = false) + { + auto start = std::chrono::high_resolution_clock::now(); + std::string jsonString = getJSONDump(limelightName); + wpi::json data; + try + { + data = wpi::json::parse(jsonString); + } + catch(const std::exception&) + { + return LimelightResultsClass(); + } + + auto end = std::chrono::high_resolution_clock::now(); + double nanos = std::chrono::duration_cast(end - start).count(); + double millis = (nanos * 0.000001); + try + { + LimelightResultsClass out = data.get(); + out.targetingResults.m_latencyJSON = millis; + if (profile) + { + std::cout << "lljson: " << millis << std::endl; + } + return out; + } + catch(...) + { + return LimelightResultsClass(); + } + } +} diff --git a/cpp/SwerveWithChoreo/src/main/include/Robot.h b/cpp/SwerveWithChoreo/src/main/include/Robot.h new file mode 100644 index 00000000..dd003efc --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/Robot.h @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include "RobotContainer.h" + +class Robot : public frc::TimedRobot { + public: + Robot(); + void RobotPeriodic() override; + void DisabledInit() override; + void DisabledPeriodic() override; + void DisabledExit() override; + void AutonomousInit() override; + void AutonomousPeriodic() override; + void AutonomousExit() override; + void TeleopInit() override; + void TeleopPeriodic() override; + void TeleopExit() override; + void TestInit() override; + void TestPeriodic() override; + void TestExit() override; + + private: + std::optional m_autonomousCommand; + + RobotContainer m_container; + + static constexpr bool kUseLimelight = false; +}; diff --git a/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h b/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h new file mode 100644 index 00000000..c2ca7b4d --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/RobotContainer.h @@ -0,0 +1,53 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +#include "generated/TunerConstants.h" +#include "AutoRoutines.h" +#include "Telemetry.h" + +class RobotContainer { +private: + units::meters_per_second_t MaxSpeed = TunerConstants::kSpeedAt12Volts; // kSpeedAt12Volts desired top speed + units::radians_per_second_t MaxAngularRate = 0.75_tps; // 3/4 of a rotation per second max angular velocity + + frc2::CommandXboxController joystick{0}; + + /* Note: This must be constructed before the drivetrain, otherwise we need to + * define a destructor to un-register the telemetry from the drivetrain */ + Telemetry logger{MaxSpeed}; + +public: + subsystems::CommandSwerveDrivetrain drivetrain{TunerConstants::CreateDrivetrain()}; + +private: + /* Setting up bindings for necessary control of the swerve drive platform */ + swerve::requests::FieldCentric drive = swerve::requests::FieldCentric{} + .WithDeadband(MaxSpeed * 0.1).WithRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage); // Use open-loop control for drive motors + swerve::requests::SwerveDriveBrake brake{}; + swerve::requests::PointWheelsAt point{}; + swerve::requests::RobotCentric forwardStraight = swerve::requests::RobotCentric{} + .WithDriveRequestType(swerve::SwerveModule::DriveRequestType::OpenLoopVoltage); + + /* Path follower */ + AutoRoutines autoRoutines; + choreo::AutoFactory autoFactory; + +public: + choreo::AutoChooser autoChooser; + + RobotContainer(); + + frc2::CommandPtr GetAutonomousCommand(); + +private: + void ConfigureBindings(); +}; diff --git a/cpp/SwerveWithChoreo/src/main/include/Telemetry.h b/cpp/SwerveWithChoreo/src/main/include/Telemetry.h new file mode 100644 index 00000000..f6a9f808 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/Telemetry.h @@ -0,0 +1,75 @@ +#pragma once + +#include "ctre/phoenix6/SignalLogger.hpp" +#include +#include +#include +#include +#include +#include + +#include "subsystems/CommandSwerveDrivetrain.h" + +class Telemetry { +private: + units::meters_per_second_t MaxSpeed; + + /* What to publish over networktables for telemetry */ + nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault(); + + /* Robot pose for field positioning */ + std::shared_ptr table = inst.GetTable("Pose"); + nt::DoubleArrayPublisher fieldPub = table->GetDoubleArrayTopic("robotPose").Publish(); + nt::StringPublisher fieldTypePub = table->GetStringTopic(".type").Publish(); + + /* Robot speeds for general checking */ + std::shared_ptr driveStats = inst.GetTable("Drive"); + nt::DoublePublisher velocityX = driveStats->GetDoubleTopic("Velocity X").Publish(); + nt::DoublePublisher velocityY = driveStats->GetDoubleTopic("Velocity Y").Publish(); + nt::DoublePublisher speed = driveStats->GetDoubleTopic("Speed").Publish(); + nt::DoublePublisher odomFreq = driveStats->GetDoubleTopic("Odometry Frequency").Publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + frc::Pose2d m_lastPose{}; + units::second_t lastTime = ctre::phoenix6::utils::GetCurrentTime(); + + /* Mechanisms to represent the swerve module states */ + std::array m_moduleMechanisms{ + frc::Mechanism2d{1, 1}, + frc::Mechanism2d{1, 1}, + frc::Mechanism2d{1, 1}, + frc::Mechanism2d{1, 1}, + }; + /* A direction and length changing ligament for speed representation */ + std::array m_moduleSpeeds{ + m_moduleMechanisms[0].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), + m_moduleMechanisms[1].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), + m_moduleMechanisms[2].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), + m_moduleMechanisms[3].GetRoot("RootSpeed", 0.5, 0.5)->Append("Speed", 0.5, 0_deg), + }; + /* A direction changing and length constant ligament for module direction */ + std::array m_moduleDirections{ + m_moduleMechanisms[0].GetRoot("RootDirection", 0.5, 0.5) + ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), + m_moduleMechanisms[1].GetRoot("RootDirection", 0.5, 0.5) + ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), + m_moduleMechanisms[2].GetRoot("RootDirection", 0.5, 0.5) + ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), + m_moduleMechanisms[3].GetRoot("RootDirection", 0.5, 0.5) + ->Append("Direction", 0.1, 0_deg, 0, frc::Color8Bit{frc::Color::kWhite}), + }; + +public: + /** + * Construct a telemetry object with the specified max speed of the robot. + * + * \param maxSpeed Maximum speed + */ + Telemetry(units::meters_per_second_t maxSpeed) : MaxSpeed{maxSpeed} + { + ctre::phoenix6::SignalLogger::Start(); + } + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + void Telemeterize(subsystems::CommandSwerveDrivetrain::SwerveDriveState const &state); +}; diff --git a/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h b/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h new file mode 100644 index 00000000..26d32b82 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/generated/TunerConstants.h @@ -0,0 +1,168 @@ +#pragma once + +#include "subsystems/CommandSwerveDrivetrain.h" + +using namespace ctre::phoenix6; + +// Generated by the Tuner X Swerve Project Generator +// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +class TunerConstants { + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants::SteerMotorClosedLoopOutput + static constexpr configs::Slot0Configs steerGains = configs::Slot0Configs{} + .WithKP(100).WithKI(0).WithKD(2.0) + .WithKS(0.2).WithKV(1.5).WithKA(0); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants::DriveMotorClosedLoopOutput + static constexpr configs::Slot0Configs driveGains = configs::Slot0Configs{} + .WithKP(0.1).WithKI(0).WithKD(0) + .WithKS(0).WithKV(0.12); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + static constexpr swerve::ClosedLoopOutputType kSteerClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + static constexpr swerve::ClosedLoopOutputType kDriveClosedLoopOutput = swerve::ClosedLoopOutputType::Voltage; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder + static constexpr swerve::SteerFeedbackType kSteerFeedbackType = swerve::SteerFeedbackType::FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + static constexpr units::ampere_t kSlipCurrent = 120_A; + + // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. + // Some configs will be overwritten; check the `With*InitialConfigs()` API documentation. + static constexpr configs::TalonFXConfiguration driveInitialConfigs{}; + static constexpr configs::TalonFXConfiguration steerInitialConfigs = configs::TalonFXConfiguration{} + .WithCurrentLimits( + configs::CurrentLimitsConfigs{} + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .WithStatorCurrentLimit(60_A) + .WithStatorCurrentLimitEnable(true) + ); + static constexpr configs::CANcoderConfiguration cancoderInitialConfigs{}; + // Configs for the Pigeon 2; leave this nullopt to skip applying Pigeon 2 configs + static constexpr std::optional pigeonConfigs = std::nullopt; + +public: + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + static constexpr units::meters_per_second_t kSpeedAt12Volts = 4.70_mps; + +private: + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + static constexpr units::scalar_t kCoupleRatio = 3.5; + + static constexpr units::scalar_t kDriveGearRatio = 7.363636364; + static constexpr units::scalar_t kSteerGearRatio = 12.8; + static constexpr units::inch_t kWheelRadius = 2.167_in; + + static constexpr bool kInvertLeftSide = false; + static constexpr bool kInvertRightSide = true; + + static constexpr std::string_view kCANBusName = "rio"; + static inline const CANBus kCANBus{kCANBusName, "./logs/example.hoot"}; + static constexpr int kPigeonId = 1; + + + // These are only used for simulation + static constexpr units::kilogram_square_meter_t kSteerInertia = 0.00001_kg_sq_m; + static constexpr units::kilogram_square_meter_t kDriveInertia = 0.001_kg_sq_m; + // Simulated voltage necessary to overcome friction + static constexpr units::volt_t kSteerFrictionVoltage = 0.25_V; + static constexpr units::volt_t kDriveFrictionVoltage = 0.25_V; + +public: + static constexpr swerve::SwerveDrivetrainConstants DrivetrainConstants = swerve::SwerveDrivetrainConstants{} + .WithCANBusName(kCANBusName) + .WithPigeon2Id(kPigeonId) + .WithPigeon2Configs(pigeonConfigs); + +private: + static constexpr swerve::SwerveModuleConstantsFactory ConstantCreator = swerve::SwerveModuleConstantsFactory{} + .WithDriveMotorGearRatio(kDriveGearRatio) + .WithSteerMotorGearRatio(kSteerGearRatio) + .WithCouplingGearRatio(kCoupleRatio) + .WithWheelRadius(kWheelRadius) + .WithSteerMotorGains(steerGains) + .WithDriveMotorGains(driveGains) + .WithSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .WithDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .WithSlipCurrent(kSlipCurrent) + .WithSpeedAt12Volts(kSpeedAt12Volts) + .WithFeedbackSource(kSteerFeedbackType) + .WithDriveMotorInitialConfigs(driveInitialConfigs) + .WithSteerMotorInitialConfigs(steerInitialConfigs) + .WithCANcoderInitialConfigs(cancoderInitialConfigs) + .WithSteerInertia(kSteerInertia) + .WithDriveInertia(kDriveInertia) + .WithSteerFrictionVoltage(kSteerFrictionVoltage) + .WithDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + static constexpr int kFrontLeftDriveMotorId = 5; + static constexpr int kFrontLeftSteerMotorId = 4; + static constexpr int kFrontLeftEncoderId = 2; + static constexpr units::turn_t kFrontLeftEncoderOffset = -0.83544921875_tr; + static constexpr bool kFrontLeftSteerMotorInverted = true; + + static constexpr units::inch_t kFrontLeftXPos = 10.5_in; + static constexpr units::inch_t kFrontLeftYPos = 10.5_in; + + // Front Right + static constexpr int kFrontRightDriveMotorId = 7; + static constexpr int kFrontRightSteerMotorId = 6; + static constexpr int kFrontRightEncoderId = 3; + static constexpr units::turn_t kFrontRightEncoderOffset = -0.15234375_tr; + static constexpr bool kFrontRightSteerMotorInverted = true; + + static constexpr units::inch_t kFrontRightXPos = 10.5_in; + static constexpr units::inch_t kFrontRightYPos = -10.5_in; + + // Back Left + static constexpr int kBackLeftDriveMotorId = 1; + static constexpr int kBackLeftSteerMotorId = 0; + static constexpr int kBackLeftEncoderId = 0; + static constexpr units::turn_t kBackLeftEncoderOffset = -0.4794921875_tr; + static constexpr bool kBackLeftSteerMotorInverted = true; + + static constexpr units::inch_t kBackLeftXPos = -10.5_in; + static constexpr units::inch_t kBackLeftYPos = 10.5_in; + + // Back Right + static constexpr int kBackRightDriveMotorId = 3; + static constexpr int kBackRightSteerMotorId = 2; + static constexpr int kBackRightEncoderId = 1; + static constexpr units::turn_t kBackRightEncoderOffset = -0.84130859375_tr; + static constexpr bool kBackRightSteerMotorInverted = true; + + static constexpr units::inch_t kBackRightXPos = -10.5_in; + static constexpr units::inch_t kBackRightYPos = -10.5_in; + +public: + static constexpr swerve::SwerveModuleConstants FrontLeft = ConstantCreator.CreateModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted); + static constexpr swerve::SwerveModuleConstants FrontRight = ConstantCreator.CreateModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted); + static constexpr swerve::SwerveModuleConstants BackLeft = ConstantCreator.CreateModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted); + static constexpr swerve::SwerveModuleConstants BackRight = ConstantCreator.CreateModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program. + */ + static subsystems::CommandSwerveDrivetrain CreateDrivetrain() + { + return {DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight}; + } +}; diff --git a/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h b/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h new file mode 100644 index 00000000..62b65dfb --- /dev/null +++ b/cpp/SwerveWithChoreo/src/main/include/subsystems/CommandSwerveDrivetrain.h @@ -0,0 +1,242 @@ +#pragma once + +#include "ctre/phoenix6/swerve/SwerveDrivetrain.hpp" +#include "ctre/phoenix6/SignalLogger.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace ctre::phoenix6; + +namespace subsystems { + +/** + * \brief Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. + */ +class CommandSwerveDrivetrain : public frc2::SubsystemBase, public swerve::SwerveDrivetrain { + static constexpr units::second_t kSimLoopPeriod = 5_ms; + std::unique_ptr m_simNotifier; + units::second_t m_lastSimTime; + + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ + static constexpr frc::Rotation2d kBlueAlliancePerspectiveRotation{0_deg}; + /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ + static constexpr frc::Rotation2d kRedAlliancePerspectiveRotation{180_deg}; + /* Keep track if we've ever applied the operator perspective before or not */ + bool m_hasAppliedOperatorPerspective = false; + + /* Swerve request to apply during path following */ + swerve::requests::ApplyFieldSpeeds m_applyFieldSpeeds; + frc::PIDController m_pathXController{10, 0, 0}; + frc::PIDController m_pathYController{10, 0, 0}; + frc::PIDController m_pathThetaController{7, 0, 0}; + + /* Swerve requests to apply during SysId characterization */ + swerve::requests::SysIdSwerveTranslation m_translationCharacterization; + swerve::requests::SysIdSwerveSteerGains m_steerCharacterization; + swerve::requests::SysIdSwerveRotation m_rotationCharacterization; + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + frc2::sysid::SysIdRoutine m_sysIdRoutineTranslation{ + frc2::sysid::Config{ + std::nullopt, // Use default ramp rate (1 V/s) + 4_V, // Reduce dynamic step voltage to 4 V to prevent brownout + std::nullopt, // Use default timeout (10 s) + // Log state with SignalLogger class + [](frc::sysid::State state) + { + SignalLogger::WriteString("SysIdTranslation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); + } + }, + frc2::sysid::Mechanism{ + [this](units::volt_t output) { SetControl(m_translationCharacterization.WithVolts(output)); }, + {}, + this + } + }; + + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + frc2::sysid::SysIdRoutine m_sysIdRoutineSteer{ + frc2::sysid::Config{ + std::nullopt, // Use default ramp rate (1 V/s) + 7_V, // Use dynamic voltage of 7 V + std::nullopt, // Use default timeout (10 s) + // Log state with SignalLogger class + [](frc::sysid::State state) + { + SignalLogger::WriteString("SysIdSteer_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); + } + }, + frc2::sysid::Mechanism{ + [this](units::volt_t output) { SetControl(m_steerCharacterization.WithVolts(output)); }, + {}, + this + } + }; + + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of swerve::requests::SysIdSwerveRotation for info on importing the log to SysId. + */ + frc2::sysid::SysIdRoutine m_sysIdRoutineRotation{ + frc2::sysid::Config{ + /* This is in radians per second², but SysId only supports "volts per second" */ + units::constants::detail::PI_VAL / 6 * (1_V / 1_s), + /* This is in radians per second, but SysId only supports "volts" */ + units::constants::detail::PI_VAL * 1_V, + std::nullopt, // Use default timeout (10 s) + // Log state with SignalLogger class + [](frc::sysid::State state) + { + SignalLogger::WriteString("SysIdRotation_State", frc::sysid::SysIdRoutineLog::StateEnumToString(state)); + } + }, + frc2::sysid::Mechanism{ + [this](units::volt_t output) + { + /* output is actually radians per second, but SysId only supports "volts" */ + SetControl(m_rotationCharacterization.WithRotationalRate(output * (1_rad_per_s / 1_V))); + /* also log the requested output for SysId */ + SignalLogger::WriteValue("Rotational_Rate", output * (1_rad_per_s / 1_V)); + }, + {}, + this + } + }; + + /* The SysId routine to test */ + frc2::sysid::SysIdRoutine *m_sysIdRoutineToApply = &m_sysIdRoutineTranslation; + +public: + /** + * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * This constructs the underlying hardware devices, so user should not construct + * the devices themselves. If they need the devices, they can access them + * through getters in the classes. + * + * \param drivetrainConstants Drivetrain-wide constants for the swerve drive + * \param modules Constants for each specific module + */ + template ... ModuleConstants> + CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, ModuleConstants const &... modules) : + SwerveDrivetrain{driveTrainConstants, modules...} + { + if (utils::IsSimulation()) { + StartSimThread(); + } + } + + /** + * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * This constructs the underlying hardware devices, so user should not construct + * the devices themselves. If they need the devices, they can access them + * through getters in the classes. + * + * \param driveTrainConstants Drivetrain-wide constants for the swerve drive + * \param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * \param modules Constants for each specific module + */ + template ... ModuleConstants> + CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency, + ModuleConstants const &... modules) : + SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency, modules...} + { + if (utils::IsSimulation()) { + StartSimThread(); + } + } + + /** + * \brief Constructs a CTRE SwerveDrivetrain using the specified constants. + * + * This constructs the underlying hardware devices, so user should not construct + * the devices themselves. If they need the devices, they can access them + * through getters in the classes. + * + * \param driveTrainConstants Drivetrain-wide constants for the swerve drive + * \param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * \param odometryStandardDeviation The standard deviation for odometry calculation + * \param visionStandardDeviation The standard deviation for vision calculation + * \param modules Constants for each specific module + */ + template ... ModuleConstants> + CommandSwerveDrivetrain(swerve::SwerveDrivetrainConstants const &driveTrainConstants, units::hertz_t odometryUpdateFrequency, + std::array const &odometryStandardDeviation, std::array const &visionStandardDeviation, + ModuleConstants const &... modules) : + SwerveDrivetrain{driveTrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules...} + { + if (utils::IsSimulation()) { + StartSimThread(); + } + } + + /** + * \brief Returns a command that applies the specified control request to this swerve drivetrain. + * + * \param request Function returning the request to apply + * \returns Command to run + */ + template + requires std::derived_from< + std::invoke_result_t, + swerve::requests::SwerveRequest> + frc2::CommandPtr ApplyRequest(RequestSupplier request) + { + return Run([this, request=std::move(request)] { + return SetControl(request()); + }); + } + + /** + * \brief Follows the given field-centric path sample with PID. + * + * \param pose Current pose of the robot + * \param sample Sample along the path to follow + */ + void FollowPath(frc::Pose2d const &pose, choreo::SwerveSample const &sample); + + /** + * \brief Runs the SysId Quasistatic test in the given direction for the routine + * specified by m_sysIdRoutineToApply. + * + * \param direction Direction of the SysId Quasistatic test + * \returns Command to run + */ + frc2::CommandPtr SysIdQuasistatic(frc2::sysid::Direction direction) + { + return m_sysIdRoutineToApply->Quasistatic(direction); + } + + /** + * \brief Runs the SysId Dynamic test in the given direction for the routine + * specified by m_sysIdRoutineToApply. + * + * \param direction Direction of the SysId Dynamic test + * \returns Command to run + */ + frc2::CommandPtr SysIdDynamic(frc2::sysid::Direction direction) + { + return m_sysIdRoutineToApply->Dynamic(direction); + } + + void Periodic() override; + +private: + void StartSimThread(); +}; + +} diff --git a/cpp/SwerveWithChoreo/src/test/cpp/main.cpp b/cpp/SwerveWithChoreo/src/test/cpp/main.cpp new file mode 100644 index 00000000..b8b23d23 --- /dev/null +++ b/cpp/SwerveWithChoreo/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/cpp/SwerveWithChoreo/tuner-project.json b/cpp/SwerveWithChoreo/tuner-project.json new file mode 100644 index 00000000..2e525562 --- /dev/null +++ b/cpp/SwerveWithChoreo/tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Left","ModuleId":0,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Front Right","ModuleId":1,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Left","ModuleId":2,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false},{"ValidatedSteerId":-1,"ValidatedDriveId":-1,"ValidatedEncoderId":-1,"IsEncoderOffsetConfigured":false,"IsDeviceSelectionCompleted":false,"IsConfigurationCompleted":false,"ModuleName":"Back Right","ModuleId":3,"Encoder":null,"SteerMotor":null,"DriveMotor":null,"IsSteerInverted":false,"EncoderOffset":0.0,"DriveMotorSelectionState":0,"SteerMotorSelectionState":0,"SteerEncoderSelectionState":0,"IsModuleValidationComplete":false}],"SwerveOptions":{"IsValidConfiguration":false,"Gyro":{"Id":1,"Name":"Pigeon 2 (Device ID 1)","Model":"Pigeon 2","CANbus":"","CANbusFriendly":""},"IsValidGyroCANbus":false,"VerticalTrackSizeInches":0.0,"HorizontalTrackSizeInches":0.0,"WheelRadiusInches":0.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":5,"SwerveModuleConfiguration":{"_CouplingRatio":0.0,"_CustomName":null,"SteerRatio":15.42857142857143,"Flipped":0,"DriveRatio":0.0,"CouplingRatio":0.0,"CustomName":null},"HasVerifiedSteer":false,"HasVerifiedDrive":false}} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json b/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json new file mode 100644 index 00000000..1f6b2b3f --- /dev/null +++ b/cpp/SwerveWithChoreo/vendordeps/ChoreoLib2025Beta.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2025Beta.json", + "name": "ChoreoLib", + "version": "2025.0.0-beta-6", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://SleipnirGroup.github.io/ChoreoLib/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.0-beta-6" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.0-beta-6", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json b/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json new file mode 100644 index 00000000..8a423302 --- /dev/null +++ b/cpp/SwerveWithChoreo/vendordeps/Phoenix6.json @@ -0,0 +1,342 @@ +{ + "fileName": "Phoenix6-frc2025-beta-latest.json", + "name": "CTRE-Phoenix (v6)", + "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-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": "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": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.0.0-beta-2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.0.0-beta-2", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.0.0-beta-2", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.0.0-beta-2", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.0.0-beta-2", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json b/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json new file mode 100644 index 00000000..3718e0ac --- /dev/null +++ b/cpp/SwerveWithChoreo/vendordeps/WPILibNewCommands.json @@ -0,0 +1,38 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "1.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2025", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } + ] +} diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/AutoRoutines.java b/java/SwerveWithChoreo/src/main/java/frc/robot/AutoRoutines.java index 2c85cd8b..3c64cf8d 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/AutoRoutines.java +++ b/java/SwerveWithChoreo/src/main/java/frc/robot/AutoRoutines.java @@ -3,7 +3,6 @@ import choreo.auto.AutoFactory; import choreo.auto.AutoLoop; import choreo.auto.AutoTrajectory; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -20,13 +19,12 @@ public Command simplePathAuto(AutoFactory factory) { routine.enabled().onTrue( m_drivetrain.runOnce(() -> - m_drivetrain.resetPose( - simplePath.getInitialPose().orElseGet(() -> { - routine.kill(); - return new Pose2d(); - }) + simplePath.getInitialPose().ifPresentOrElse( + pose -> m_drivetrain.resetPose(pose), + routine::kill ) - ).andThen(simplePath.cmd()) + ) + .andThen(simplePath.cmd()) ); return routine.cmd(); } diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java b/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java index d9cc4a44..191040e9 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java +++ b/java/SwerveWithChoreo/src/main/java/frc/robot/Robot.java @@ -28,7 +28,7 @@ public void robotPeriodic() { /* update auto routine selection */ m_robotContainer.autoChooser.update(); - /** + /* * This example of adding Limelight is very simple and may not be sufficient for on-field use. * Users typically need to provide a standard deviation that scales with the distance to target * and changes with number of tags available. diff --git a/java/SwerveWithChoreo/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithChoreo/src/main/java/frc/robot/RobotContainer.java index 5f6e72ff..18f96d6f 100644 --- a/java/SwerveWithChoreo/src/main/java/frc/robot/RobotContainer.java +++ b/java/SwerveWithChoreo/src/main/java/frc/robot/RobotContainer.java @@ -49,8 +49,6 @@ public class RobotContainer { public final AutoChooser autoChooser; public RobotContainer() { - configureBindings(); - autoFactory = Choreo.createAutoFactory( drivetrain, () -> drivetrain.getState().Pose, @@ -61,6 +59,8 @@ public RobotContainer() { autoChooser = new AutoChooser(autoFactory, ""); autoChooser.addAutoRoutine("SimplePath", autoRoutines::simplePathAuto); + + configureBindings(); } private void configureBindings() {