From 5f0dd51925fc8cca59b357616266d35459ea0983 Mon Sep 17 00:00:00 2001 From: Dhairya Trivedi <140292400+Dhairya-exe@users.noreply.github.com> Date: Mon, 24 Nov 2025 19:02:52 -0700 Subject: [PATCH 1/2] auto following works now with the json --- 2026-Robot/.vscode/launch.json | 21 + 2026-Robot/.vscode/settings.json | 60 + 2026-Robot/.wpilib/wpilib_preferences.json | 6 + 2026-Robot/WPILib-License.md | 24 + 2026-Robot/build.gradle | 106 + 2026-Robot/gradle/wrapper/gradle-wrapper.jar | Bin 0 -> 43583 bytes .../gradle/wrapper/gradle-wrapper.properties | 7 + 2026-Robot/gradlew | 252 + 2026-Robot/gradlew.bat | 94 + 2026-Robot/settings.gradle | 30 + .../main/deploy/2025-reefscape-welded.json | 404 ++ 2026-Robot/src/main/deploy/sample_path.json | 92 + .../src/main/java/frc/robot/Constants.java | 4316 ++++++++++++++++ 2026-Robot/src/main/java/frc/robot/Main.java | 25 + 2026-Robot/src/main/java/frc/robot/OI.java | 300 ++ 2026-Robot/src/main/java/frc/robot/Robot.java | 98 + .../main/java/frc/robot/RobotContainer.java | 50 + .../java/frc/robot/commands/AutoRunner.java | 316 ++ .../main/java/frc/robot/commands/Autos.java | 16 + .../java/frc/robot/commands/CommandBlock.java | 29 + .../java/frc/robot/commands/FollowPath.java | 101 + .../robot/commands/PurePursuitFollowPath.java | 243 + .../frc/robot/commands/SetRobotState.java | 50 + .../commands/SetRobotStateComplicated.java | 47 + .../SetRobotStateComplicatedContinuous.java | 48 + .../frc/robot/commands/SetRobotStateOnce.java | 52 + .../robot/commands/SetRobotStateSimple.java | 47 + .../commands/SetRobotStateSimpleOnce.java | 46 + .../main/java/frc/robot/commands/Trigger.java | 37 + .../java/frc/robot/commands/ZeroPigeon.java | 41 + .../commands/conditionals/Conditional.java | 17 + .../robot/commands/conditionals/LeftPath.java | 38 + .../commands/conditionals/StartMainPath.java | 29 + .../commands/conditionals/StartRightPath.java | 28 + .../main/java/frc/robot/subsystems/Drive.java | 4459 +++++++++++++++++ .../frc/robot/subsystems/Peripherals.java | 496 ++ .../frc/robot/subsystems/Superstructure.java | 111 + .../frc/robot/subsystems/SwerveModule.java | 534 ++ .../main/java/frc/robot/tools/PathLoader.java | 52 + .../java/frc/robot/tools/TriggerButton.java | 10 + .../frc/robot/tools/controlloops/PID.java | 233 + .../main/java/frc/robot/tools/math/PID.java | 100 + .../java/frc/robot/tools/math/Vector.java | 138 + .../robot/tools/wrappers/AutoFollower.java | 27 + 2026-Robot/vendordeps/AdvantageKit.json | 35 + .../vendordeps/Phoenix6-frc2025-latest.json | 479 ++ 2026-Robot/vendordeps/WPILibNewCommands.json | 38 + 2026-Robot/vendordeps/photonlib.json | 71 + 48 files changed, 13853 insertions(+) create mode 100644 2026-Robot/.vscode/launch.json create mode 100644 2026-Robot/.vscode/settings.json create mode 100644 2026-Robot/.wpilib/wpilib_preferences.json create mode 100644 2026-Robot/WPILib-License.md create mode 100644 2026-Robot/build.gradle create mode 100644 2026-Robot/gradle/wrapper/gradle-wrapper.jar create mode 100644 2026-Robot/gradle/wrapper/gradle-wrapper.properties create mode 100644 2026-Robot/gradlew create mode 100644 2026-Robot/gradlew.bat create mode 100644 2026-Robot/settings.gradle create mode 100644 2026-Robot/src/main/deploy/2025-reefscape-welded.json create mode 100644 2026-Robot/src/main/deploy/sample_path.json create mode 100644 2026-Robot/src/main/java/frc/robot/Constants.java create mode 100644 2026-Robot/src/main/java/frc/robot/Main.java create mode 100644 2026-Robot/src/main/java/frc/robot/OI.java create mode 100644 2026-Robot/src/main/java/frc/robot/Robot.java create mode 100644 2026-Robot/src/main/java/frc/robot/RobotContainer.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/Autos.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/FollowPath.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/Trigger.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java create mode 100644 2026-Robot/src/main/java/frc/robot/subsystems/Drive.java create mode 100644 2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java create mode 100644 2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java create mode 100644 2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/PathLoader.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/math/PID.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/math/Vector.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java create mode 100644 2026-Robot/vendordeps/AdvantageKit.json create mode 100644 2026-Robot/vendordeps/Phoenix6-frc2025-latest.json create mode 100644 2026-Robot/vendordeps/WPILibNewCommands.json create mode 100644 2026-Robot/vendordeps/photonlib.json diff --git a/2026-Robot/.vscode/launch.json b/2026-Robot/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/2026-Robot/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/2026-Robot/.vscode/settings.json b/2026-Robot/.vscode/settings.json new file mode 100644 index 0000000..612cdd0 --- /dev/null +++ b/2026-Robot/.vscode/settings.json @@ -0,0 +1,60 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + }, + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*", + ] +} diff --git a/2026-Robot/.wpilib/wpilib_preferences.json b/2026-Robot/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..96ea8d5 --- /dev/null +++ b/2026-Robot/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2025", + "teamNumber": 4499 +} \ No newline at end of file diff --git a/2026-Robot/WPILib-License.md b/2026-Robot/WPILib-License.md new file mode 100644 index 0000000..645e542 --- /dev/null +++ b/2026-Robot/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/2026-Robot/build.gradle b/2026-Robot/build.gradle new file mode 100644 index 0000000..417f7e9 --- /dev/null +++ b/2026-Robot/build.gradle @@ -0,0 +1,106 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2025.2.1" +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// 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 + + frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + } + + // Static files artifact + frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { + files = project.fileTree('src/main/deploy') + directory = '/home/lvuser/deploy' + deleteOldFiles = false // Change to true to delete files on roboRIO that no + // longer exist in deploy directory of this project + } + } + } + } +} + +def deployArtifact = deploy.targets.roborio.artifacts.frcJava + +// Set to true to use debug for JNI. +wpi.java.debugJni = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 5. +dependencies { + annotationProcessor wpi.java.deps.wpilibAnnotations() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + + implementation 'org.json:json:20240303' + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) + roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) + + roborioRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.roborio) + roborioRelease wpi.java.vendor.jniRelease(wpi.platforms.roborio) + + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() + + nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() + + testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' + testRuntimeOnly 'org.junit.platform:junit-platform-launcher' +} + +test { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} + +// Simulation configuration (e.g. environment variables). +wpi.sim.addGui().defaultEnabled = true +wpi.sim.addDriverstation() + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) + duplicatesStrategy = DuplicatesStrategy.INCLUDE +} + +// Configure jar and deploy tasks +deployArtifact.jarTask = jar +wpi.java.configureExecutableTasks(jar) +wpi.java.configureTestTasks(test) + +// Configure string concat to always inline compile +tasks.withType(JavaCompile) { + options.compilerArgs.add '-XDstringConcat=inline' +} diff --git a/2026-Robot/gradle/wrapper/gradle-wrapper.jar b/2026-Robot/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/2026-Robot/gradle/wrapper/gradle-wrapper.properties b/2026-Robot/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..34bd9ce --- /dev/null +++ b/2026-Robot/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,7 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip +networkTimeout=10000 +validateDistributionUrl=true +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/2026-Robot/gradlew b/2026-Robot/gradlew new file mode 100644 index 0000000..f5feea6 --- /dev/null +++ b/2026-Robot/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/2026-Robot/gradlew.bat b/2026-Robot/gradlew.bat new file mode 100644 index 0000000..9d21a21 --- /dev/null +++ b/2026-Robot/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/2026-Robot/settings.gradle b/2026-Robot/settings.gradle new file mode 100644 index 0000000..c493958 --- /dev/null +++ b/2026-Robot/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/2026-Robot/src/main/deploy/2025-reefscape-welded.json b/2026-Robot/src/main/deploy/2025-reefscape-welded.json new file mode 100644 index 0000000..eb395c0 --- /dev/null +++ b/2026-Robot/src/main/deploy/2025-reefscape-welded.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.697198, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.697198, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.560809999999998, + "y": 8.05561, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.851154, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.851154, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 5.9875419999999995, + "y": -0.0038099999999999996, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.052 + } +} diff --git a/2026-Robot/src/main/deploy/sample_path.json b/2026-Robot/src/main/deploy/sample_path.json new file mode 100644 index 0000000..6e1c0db --- /dev/null +++ b/2026-Robot/src/main/deploy/sample_path.json @@ -0,0 +1,92 @@ +{ + "pathName": "Auto", + "sampleRate": 1.0, + "pathVersion": "0.0.1", + "commands": [ + { + "index": 0, + "type": "CommandBlock", + "commands": [ + "PurePursuitFollowPath" + ], + "arguments": { + "points": [ + { + "index": 0, + "x": 0, + "y": 0, + "angle": 0, + "time": 0.0 + }, + { + "index": 1, + "x": 1, + "y": 0, + "angle": 0, + "time": 1.0 + } + ] + } + }, + { + "index": 1, + "type": "Switch", + "condition": "should_turn_left", + "conditionArguments": { + "value": 41 + }, + "onTrue": [ + { + "type": "CommandBlock", + "commands": [ + "FollowPath" + ], + "arguments": { + "points": [ + { + "index": 0, + "x": 1, + "y": 0, + "angle": 0, + "time": 1.0 + }, + { + "index": 1, + "x": 1, + "y": 1, + "angle": 0, + "time": 2.0 + } + ] + } + } + ], + "onFalse": [ + { + "type": "CommandBlock", + "commands": [ + "FollowPath" + ], + "arguments": { + "points": [ + { + "index": 0, + "x": 1, + "y": 0, + "angle": 0, + "time": 1.0 + }, + { + "index": 1, + "x": 1, + "y": -1, + "angle": 0, + "time": 2.0 + } + ] + } + } + ] + } + ] +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Constants.java b/2026-Robot/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..adafbec --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/Constants.java @@ -0,0 +1,4316 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import java.io.File; +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public final class Constants { + public static final class Autonomous { + public static final int STAGNATE_BOOST = 25; + public static final int STAGNATE_THRESHOLD = 8; // Number of cycles of stagnation before ending path + // lookahead distance is a function: + // LOOKAHEAD = AUTONOMOUS_LOOKAHEAD_DISTANCE * velocity + MIN_LOOKAHEAD_DISTANCE + // their constants + public static final double AUTONOMOUS_LOOKAHEAD_DISTANCE = 0.04; // Lookahead at 1m/s scaled by wanted + // velocity + public static final double FULL_SEND_LOOKAHEAD = 0.60; + public static final double MIN_LOOKAHEAD_DISTANCE = 0.05; // Lookahead distance at 0m/s + // Path follower will end if within this radius of the final point + public static final double AUTONOMOUS_END_ACCURACY = 0.40; + public static final double ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY = 0.05; + // When calculating the point distance, will divide x and y by this constant + public static final double AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS = 1.0; + // When calculating the point distance, will divide theta by this constant + public static final double AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS = 4 * Math.PI; + // Feed Forward Multiplier + public static final double FEED_FORWARD_MULTIPLIER = 0.8044; + public static final double ACCURATE_FOLLOWER_FEED_FORWARD_MULTIPLIER = 1; + public static String[] paths; + + static { + ArrayList autoPaths = new ArrayList<>(); + File[] dir = Filesystem.getDeployDirectory().listFiles(); + for (File file : dir) { + if (file.getName().contains(".polarauto")) { + autoPaths.add(file.getName()); + } + } + paths = new String[autoPaths.size()]; + for (int i = 0; i < autoPaths.size(); i++) { + paths[i] = autoPaths.get(i); + } + } + + public static int getSelectedPathIndex() { + String path = OI.getSelectedPath(); + if (path.equals("None")) { + return -1; + } + for (int i = 0; i < paths.length; i++) { + if (path.equals(paths[i])) { + return i; + } + } + return -1; + } + + } + + public static void periodic() { + int index = Autonomous.getSelectedPathIndex(); + if (index == -1 || index > Constants.Autonomous.paths.length) { + Logger.recordOutput("Selected Auto", "Do Nothing"); + } else { + Logger.recordOutput("Selected Auto", Autonomous.paths[index]); + } + } + + public static void init() { + + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint1); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint2); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint3); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint4); + // // Only have these 4 now + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint5); + // // The rest are 0, 0 + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint6); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint7); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint8); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint9); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint10); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint11); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint12); + + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint1); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint2); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint3); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint4); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint5); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint6); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint7); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint8); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint9); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint10); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint11); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint12); + + // for (int i = 0; i < Constants.Vision.redSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.redSideReefTags[i][0], + // Constants.Vision.redSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.redSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.redSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // } + // for (int i = 0; i < Constants.Vision.blueSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.blueSideReefTags[i][0], + // Constants.Vision.blueSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.blueSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.blueSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // } + + // Logger.recordOutput("red side scoring", + // Constants.Physical.redCoralScoringPositions.toString()); + // Logger.recordOutput("blue side scoring", + // Constants.Physical.blueCoralScoringPositions.toString()); + System.out.println("blue algae front positions: " + + Constants.Reef.algaeBlueFrontPlacingPositions.toString()); + System.out.println("red algae front positions: " + + Constants.Reef.algaeRedFrontPlacingPositions.toString()); + System.out.println("blue algae back positions: " + + Constants.Reef.algaeBlueBackPlacingPositions.toString()); + System.out.println( + "red algae back positions: " + Constants.Reef.algaeRedBackPlacingPositions.toString()); + + System.out.println("blue positions: " + Constants.Reef.blueFrontPlacingPositions.toString()); + System.out.println("red positions: " + Constants.Reef.redFrontPlacingPositions.toString()); + System.out.println("blue back positions: " + Constants.Reef.blueBackPlacingPositions.toString()); + System.out.println("red back positions: " + Constants.Reef.redBackPlacingPositions.toString()); + + System.out.println("l4 blue positions: " + Constants.Reef.l4BlueFrontPlacingPositions.toString()); + System.out.println("l4 red positions: " + Constants.Reef.l4RedFrontPlacingPositions.toString()); + System.out.println("l4 blue back positions: " + Constants.Reef.l4BlueBackPlacingPositions.toString()); + System.out.println("l4 red back positions: " + Constants.Reef.l4RedBackPlacingPositions.toString()); + + System.out.println("l3 blue positions: " + Constants.Reef.l3BlueFrontPlacingPositions.toString()); + System.out.println("l3 red positions: " + Constants.Reef.l3RedFrontPlacingPositions.toString()); + System.out.println("l3 blue back positions: " + Constants.Reef.l3BlueBackPlacingPositions.toString()); + System.out.println("l3 red back positions: " + Constants.Reef.l3RedBackPlacingPositions.toString()); + + System.out.println("L1 Blue Corners: " + Constants.Reef.l1BlueCornerPoints.toString()); + System.out.println("L1 Red Corners: " + Constants.Reef.l1RedCornerPoints.toString()); + + System.out.println("L1 Blue Drive: " + Constants.Reef.l1BlueDrivePoints.toString()); + System.out.println("L1 Red Drive: " + Constants.Reef.l1RedDrivePoints.toString()); + + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + Logger.recordOutput("L1 Blue Corners " + i + " ", Constants.Reef.l1BlueDrivePoints.get(i)); + } + + Logger.recordOutput("feeder Positions", new Pose2d[] { Constants.Reef.RED_LEFT_FEEDER_LEFT, + Constants.Reef.RED_RIGHT_FEEDER_RIGHT, Constants.Reef.RED_RIGHT_FEEDER_LEFT, + Constants.Reef.RED_LEFT_FEEDER_RIGHT, }); + } + + public static class Reef { + public static final double PERFECT_BRANCH_OFFSET_L23 = inchesToMeters(1.625); + public static final double PERFECT_BRANCH_OFFSET_L4 = inchesToMeters(1.125); + + // positive is from face of reef towards center of reef + // negative means futher from reef + // public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.875); + // public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.25); + // public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(-0.5940); + // public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.375); + // public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.7157); + // public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.75); + + // public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.125); + // public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + // public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + + // public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.5); + // public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.875); + // public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.25); + // public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.25); + // public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + + // // // right when facing the reef side is positive + // // // negative makes robot go more to the left + // public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); // + // TODO: make red and blue + // // seperate + // public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + + public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + // right when facing the reef side is positive + // negative makes robot go more to the left + public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final Translation2d centerBlue = new Translation2d(inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final Translation2d centerRed = new Translation2d( + Constants.Physical.FIELD_LENGTH - inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final double faceToZoneLine = inchesToMeters(12); // Side of the reef to the inside of the + // reef + // zone line + + public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in + // clockwise + // order + // public static final List> + // blueBranchPositions = new + // ArrayList<>(); // Starting at the right + // // branch facing the driver + // // station in clockwise + // public static final List> + // redBranchPositions = new + // ArrayList<>(); // Starting at the right + + public static final double RED_LEFT_FEEDER_X = 16.28; + public static final double RED_LEFT_FEEDER_Y = 0.92; + public static final double RED_LEFT_FEEDER_THETA = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_X_TELEOP = 16.544; + public static final double RED_LEFT_FEEDER_Y_TELEOP = 0.890; + public static final double RED_LEFT_FEEDER_THETA_TELEOP = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_LEFT_X = 16.873; + public static final double RED_LEFT_FEEDER_LEFT_Y = 1.209; + public static final double RED_LEFT_FEEDER_LEFT_THETA = Math.toRadians(126.0); + public static final double RED_LEFT_FEEDER_RIGHT_X = 15.952; + public static final double RED_LEFT_FEEDER_RIGHT_Y = 0.564; + public static final double RED_LEFT_FEEDER_RIGHT_THETA = Math.toRadians(126.0); + + public static final Pose2d RED_LEFT_FEEDER_LEFT = new Pose2d(RED_LEFT_FEEDER_LEFT_X, + RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(RED_LEFT_FEEDER_LEFT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_LEFT = new Pose2d( + RED_LEFT_FEEDER_LEFT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(-54.0))); + public static final Pose2d RED_LEFT_FEEDER_RIGHT = new Pose2d(RED_LEFT_FEEDER_RIGHT_X, + RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(RED_LEFT_FEEDER_RIGHT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_RIGHT = new Pose2d( + RED_LEFT_FEEDER_RIGHT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER = new Pose2d(RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(RED_LEFT_FEEDER_THETA)); + public static final Pose2d RED_RIGHT_FEEDER = new Pose2d( + RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER_TELEOP = new Pose2d(RED_LEFT_FEEDER_X_TELEOP, + RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(RED_LEFT_FEEDER_THETA_TELEOP)); + public static final Pose2d RED_RIGHT_FEEDER_TELEOP = new Pose2d( + RED_LEFT_FEEDER_X_TELEOP, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(-54.0))); + + public static final double PROCESSOR_Y_OFFSET_M = inchesToMeters(50.0); + public static final double PROCESSOR_MORE_Y_OFFSET_M = inchesToMeters(25.0); + public static final double NET_X_OFFSET_M = inchesToMeters(53.0); + public static final double NET_X_OFFSET_MORE = inchesToMeters(34.0); + + public static final Translation2d processorBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedFrontPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueBackPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedBackPlacingRotation = new Rotation2d(degreesToRadians(270)); + + public static final Translation2d processorMoreBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorMoreRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedFrontPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueBackPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedBackPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Pose2d processorBlueFrontPlacingPosition = new Pose2d( + processorBlueFrontPlacingTranslation, + processorBlueFrontPlacingRotation); + public static final Pose2d processorRedFrontPlacingPosition = new Pose2d( + processorRedFrontPlacingTranslation, + processorRedFrontPlacingRotation); + public static final Pose2d processorBlueBackPlacingPosition = new Pose2d( + processorBlueBackPlacingTranslation, + processorBlueBackPlacingRotation); + public static final Pose2d processorRedBackPlacingPosition = new Pose2d( + processorRedBackPlacingTranslation, + processorRedBackPlacingRotation); + + public static final Pose2d processorMoreBlueFrontPlacingPosition = new Pose2d( + processorMoreBlueFrontPlacingTranslation, + processorMoreBlueFrontPlacingRotation); + public static final Pose2d processorMoreRedFrontPlacingPosition = new Pose2d( + processorMoreRedFrontPlacingTranslation, + processorMoreRedFrontPlacingRotation); + public static final Pose2d processorMoreBlueBackPlacingPosition = new Pose2d( + processorMoreBlueBackPlacingTranslation, + processorMoreBlueBackPlacingRotation); + public static final Pose2d processorMoreRedBackPlacingPosition = new Pose2d( + processorMoreRedBackPlacingTranslation, + processorMoreRedBackPlacingRotation); + + public static final double netBlueXM = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_M; + public static final double netRedXM = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_M; + public static final double netBlueXMore = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_MORE; + public static final double netRedXMore = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_MORE; + + public static final double netBlueFrontThetaR = degreesToRadians(0.0); + public static final double netRedFrontThetaR = degreesToRadians(180.0); + public static final double netBlueBackThetaR = degreesToRadians(180.0); + public static final double netRedBackThetaR = degreesToRadians(0.0); + + public static final List blueL1FrontPlacingPositions = new ArrayList<>(); + public static final List redL1FrontPlacingPositions = new ArrayList<>(); + public static final List blueL1BackPlacingPositions = new ArrayList<>(); + public static final List redL1BackPlacingPositions = new ArrayList<>(); + + public static final List blueL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List redL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueL1BackPlacingPositionsMore = new ArrayList<>(); + public static final List redL1BackPlacingPositionsMore = new ArrayList<>(); + + public static final List blueFrontPlacingPositions = new ArrayList<>(); + public static final List redFrontPlacingPositions = new ArrayList<>(); + public static final List blueBackPlacingPositions = new ArrayList<>(); + public static final List redBackPlacingPositions = new ArrayList<>(); + + public static final List blueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List redFrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueBackPlacingPositionsMore = new ArrayList<>(); + public static final List redBackPlacingPositionsMore = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMoreMore = new ArrayList<>(); + + public static final List l4BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l4RedFrontPlacingPositions = new ArrayList<>(); + public static final List l4BlueBackPlacingPositions = new ArrayList<>(); + public static final List l4RedBackPlacingPositions = new ArrayList<>(); + + public static final List l3BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l3RedFrontPlacingPositions = new ArrayList<>(); + public static final List l3BlueBackPlacingPositions = new ArrayList<>(); + public static final List l3RedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositions = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositions = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositions = new ArrayList<>(); + public static final List algaeRedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMore = new ArrayList<>(); + + public static final List l1BlueCornerPoints = new ArrayList<>(); + public static final List l1RedCornerPoints = new ArrayList<>(); + public static final List l1BlueDrivePoints = new ArrayList<>(); + public static final List l1RedDrivePoints = new ArrayList<>(); + + static { + calculateReefPoints(); + } + + // Angle to face red side: + // 180=AB + // 120=CD + // 60=EF + // 0=GH + // -60=IJ + // -120=KL + + public static void calculateReefPoints() { + blueL1FrontPlacingPositions.clear(); + redL1FrontPlacingPositions.clear(); + blueL1BackPlacingPositions.clear(); + redL1BackPlacingPositions.clear(); + blueL1FrontPlacingPositionsMore.clear(); + redL1FrontPlacingPositionsMore.clear(); + blueL1BackPlacingPositionsMore.clear(); + redL1BackPlacingPositionsMore.clear(); + blueFrontPlacingPositions.clear(); + redFrontPlacingPositions.clear(); + blueBackPlacingPositions.clear(); + redBackPlacingPositions.clear(); + blueFrontPlacingPositionsMore.clear(); + redFrontPlacingPositionsMore.clear(); + blueBackPlacingPositionsMore.clear(); + redBackPlacingPositionsMore.clear(); + l4BlueFrontPlacingPositions.clear(); + l4RedFrontPlacingPositions.clear(); + l4BlueBackPlacingPositions.clear(); + l4RedBackPlacingPositions.clear(); + l3BlueFrontPlacingPositions.clear(); + l3RedFrontPlacingPositions.clear(); + l3BlueBackPlacingPositions.clear(); + l3RedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositions.clear(); + algaeRedFrontPlacingPositions.clear(); + algaeBlueBackPlacingPositions.clear(); + algaeRedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositionsMore.clear(); + algaeRedFrontPlacingPositionsMore.clear(); + algaeBlueBackPlacingPositionsMore.clear(); + algaeRedBackPlacingPositionsMore.clear(); + algaeBlueFrontPlacingPositionsMoreMore.clear(); + algaeRedFrontPlacingPositionsMoreMore.clear(); + algaeBlueBackPlacingPositionsMoreMore.clear(); + algaeRedBackPlacingPositionsMoreMore.clear(); + l1BlueCornerPoints.clear(); + l1RedCornerPoints.clear(); + l1BlueDrivePoints.clear(); + l1RedDrivePoints.clear(); + centerFaces[0] = new Pose2d( + inchesToMeters(144.003), + inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = new Pose2d( + inchesToMeters(160.373), + inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = new Pose2d( + inchesToMeters(193.116), + inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = new Pose2d( + inchesToMeters(209.489), + inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = new Pose2d( + inchesToMeters(193.118), + inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = new Pose2d( + inchesToMeters(160.375), + inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + for (int face = 0; face < 6; face++) { + Pose2d l1FrontRight = new Pose2d(); + Pose2d l1FrontLeft = new Pose2d(); + Pose2d l1BackRight = new Pose2d(); + Pose2d l1BackLeft = new Pose2d(); + Pose2d l1FrontRightMore = new Pose2d(); + Pose2d l1FrontLeftMore = new Pose2d(); + Pose2d l1BackRightMore = new Pose2d(); + Pose2d l1BackLeftMore = new Pose2d(); + Pose2d l2FrontRight = new Pose2d(); + Pose2d l2FrontLeft = new Pose2d(); + Pose2d frontRightMore = new Pose2d(); + Pose2d frontLeftMore = new Pose2d(); + Pose2d backRightMore = new Pose2d(); + Pose2d backLeftMore = new Pose2d(); + Pose2d l2BackRight = new Pose2d(); + Pose2d l2BackLeft = new Pose2d(); + Pose2d l3FrontRight = new Pose2d(); + Pose2d l3FrontLeft = new Pose2d(); + Pose2d l3BackRight = new Pose2d(); + Pose2d l3BackLeft = new Pose2d(); + Pose2d l4FrontRight = new Pose2d(); + Pose2d l4FrontLeft = new Pose2d(); + Pose2d l4BackRight = new Pose2d(); + Pose2d l4BackLeft = new Pose2d(); + Pose2d algaeFront = new Pose2d(); + Pose2d algaeBack = new Pose2d(); + Pose2d algaeFrontMore = new Pose2d(); + Pose2d algaeBackMore = new Pose2d(); + Pose2d algaeFrontMoreMore = new Pose2d(); + Pose2d algaeBackMoreMore = new Pose2d(); + Pose2d l1Corner = new Pose2d(); + Pose2d l1Drive = new Pose2d(); + Pose2d poseDirection = new Pose2d(centerBlue, + Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = inchesToMeters(30.738); + double adjustY = inchesToMeters(6.469); + double adjustXL1 = inchesToMeters(30.738); + double adjustYL1 = inchesToMeters(6.469); + double adjustXMore = inchesToMeters(41.67); + double adjustYMore = inchesToMeters(6.469); + double adjustAlgaeX = inchesToMeters(45.738); + double adjustAlgaeY = inchesToMeters(0.0); + double adjustAlgaeMoreX = inchesToMeters(16.738); + double adjustAlgaeMoreY = inchesToMeters(0.0); + double adjustAlgaeMoreMoreX = inchesToMeters(56.738); + double adjustAlgaeMoreMoreY = inchesToMeters(0.0); + double adjustL1CornerX = inchesToMeters(9.0); + double adjustL1CornerY = inchesToMeters(18.0); + double adjustL1DriveX = inchesToMeters(38.0); + double adjustL1DriveY = inchesToMeters(25.0); + + l1FrontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + algaeFront = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBack = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Corner = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Drive = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + frontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + // l2FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + + // Angle to face red side: + // 180=AB + // 120=KL + // 60=IJ + // 0=GH + // -60=EF + // -120=CD + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + // l2FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + frontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + blueFrontPlacingPositions.add(l2FrontRight); + blueFrontPlacingPositions.add(l2FrontLeft); + blueBackPlacingPositions.add(l2BackRight); + blueBackPlacingPositions.add(l2BackLeft); + blueFrontPlacingPositionsMore.add(frontRightMore); + blueFrontPlacingPositionsMore.add(frontLeftMore); + blueBackPlacingPositionsMore.add(backRightMore); + blueBackPlacingPositionsMore.add(backLeftMore); + l4BlueFrontPlacingPositions.add(l4FrontRight); + l4BlueFrontPlacingPositions.add(l4FrontLeft); + l4BlueBackPlacingPositions.add(l4BackRight); + l4BlueBackPlacingPositions.add(l4BackLeft); + l3BlueFrontPlacingPositions.add(l3FrontRight); + l3BlueFrontPlacingPositions.add(l3FrontLeft); + l3BlueBackPlacingPositions.add(l3BackRight); + l3BlueBackPlacingPositions.add(l3BackLeft); + algaeBlueFrontPlacingPositions.add(algaeFront); + algaeBlueBackPlacingPositions.add(algaeBack); + algaeBlueFrontPlacingPositionsMore.add(algaeFrontMore); + algaeBlueBackPlacingPositionsMore.add(algaeBackMore); + algaeBlueFrontPlacingPositionsMoreMore.add(algaeFrontMoreMore); + algaeBlueBackPlacingPositionsMoreMore.add(algaeBackMoreMore); + l1BlueCornerPoints.add(l1Corner); + l1BlueDrivePoints.add(l1Drive); + blueL1FrontPlacingPositions.add(l1FrontLeft); + blueL1FrontPlacingPositions.add(l1FrontRight); + blueL1BackPlacingPositions.add(l1BackLeft); + blueL1BackPlacingPositions.add(l1BackRight); + blueL1FrontPlacingPositionsMore.add(l1FrontLeftMore); + blueL1FrontPlacingPositionsMore.add(l1FrontRightMore); + blueL1BackPlacingPositionsMore.add(l1BackLeftMore); + blueL1BackPlacingPositionsMore.add(l1BackRightMore); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l1BlueCornerPoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedCornerPoints.add(redPose); + } + + for (Pose2d bluePose : l1BlueDrivePoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedDrivePoints.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : l4BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l4BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedBackPlacingPositions.add(redPose); + } + } + } + + // Physical constants (e.g. field and robot dimensions) + public static final class Physical { + public static final double FIELD_WIDTH = 8.052; + public static final double FIELD_LENGTH = 17.548; + public static final double WHEEL_DIAMETER = inchesToMeters(4); + public static final double WHEEL_CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER; + public static final double WHEEL_ROTATION_PER_METER = 1 / WHEEL_CIRCUMFERENCE; + public static final double WHEEL_TO_FRAME_DISTANCE = inchesToMeters(2.5); + public static final double TOP_SPEED = feetToMeters(30.0); + public static final double MAX_ACCELERATION = feetToMeters(30.0); // TODO: actually tune the top speed + // and max acceleration. Add a max + // deceleration if needed. + + public static final double ROBOT_LENGTH = inchesToMeters(32); + public static final double ROBOT_WIDTH = inchesToMeters(26); + public static final double MODULE_OFFSET = inchesToMeters(2.625); + public static final double ROBOT_RADIUS = Math.hypot(ROBOT_LENGTH / 2 - WHEEL_TO_FRAME_DISTANCE, + ROBOT_WIDTH / 2 - WHEEL_TO_FRAME_DISTANCE); + public static double INTAKE_X_OFFSET_FRONT = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_BACK = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_BACK = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_FRONT_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_FRONT_ALGAE = inchesToMeters(3.8); + public static double INTAKE_X_OFFSET_BACK_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_BACK_ALGAE = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_FRONT_MORE = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_BACK_MORE = inchesToMeters(-0.0); + public static double L2_INTAKE_X_OFFSET_FRONT = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L2_INTAKE_X_OFFSET_BACK = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L3_INTAKE_X_OFFSET_FRONT = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L3_INTAKE_X_OFFSET_BACK = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L4_INTAKE_X_OFFSET_FRONT = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_FRONT = inchesToMeters(-1.0); + public static double L4_INTAKE_X_OFFSET_BACK = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_BACK = inchesToMeters(-1.0); + + public static final double GRAVITY_ACCEL_MS2 = 9.806; + } + + public static final class Arm { + + public static final double L4_Score = -20.0; + public static final double L4_Place = 45.0; + public static final double L3_Score = -10.0; + public static final double L3_Place = 47.0; + public static final double L2_Score = -10.0; + public static final double L2_Place = 47.0; + public static final double L1_Place = -25.0; + public static final double HANDOFF = -91.0; + public static final double DEFAULT = -90.0; + public static final double HORIZONTAL = 5.0; + public static final double VERTICAL = 95.0; + public static final double NET = 135.0; + public static final double IDLE = 0.0; + } + + public static final class Elevator { + public static final double AUTO_L1 = inchesToMeters(18.0); + public static final double AUTO_L2 = inchesToMeters(8.0); + public static final double AUTO_L3 = inchesToMeters(24.25); + public static final double AUTO_L4 = inchesToMeters(50.0); + public static final double AUTO_SCORE_L2_HIGH = inchesToMeters(14.0); + public static final double AUTO_SCORE_L2 = inchesToMeters(5.0); + public static final double AUTO_SCORE_L3 = inchesToMeters(20.0); + public static final double AUTO_SCORE_L4 = inchesToMeters(48.0); + public static final double HANDOFF_HIGH = inchesToMeters(11.0); + public static final double HANDOFF_LOW = inchesToMeters(3.0); + public static final double ALGAE_HIGH = inchesToMeters(33); + public static final double ALGAE_LOW = inchesToMeters(14.5); + public static final double PROCESSOR = inchesToMeters(0.0); + public static final double NET = inchesToMeters(55.0); + } + + // Subsystem setpoint constants + public static final class SetPoints { + public static class IntakeSetpoints { + public static final double INTAKE_ACCELERATION = 500.0; + public static final double INTAKE_CRUISE_VELOCITY = 400.0; + public static final double INTAKE_MOTION_PROFILE_SCALAR = 1.0; + public static final double INTAKE_DOWN = -18.2; // rotations + public static final double INTAKE_UP = 0; // rotations + public static final double INTAKE_ROLLER_MAX_SPEED = 1.0; // percent + public static final double INTAKE_ROLLER_HOLDING_SPEED = 0.1; // percent + public static final double INTAKE_ROLLER_TORQUE = 80.0; // amps + public static final double INTAKE_HOLDING_TORQUE = 60.0; // amps + } + + public static final double ELEVATOR_BOTTOM_POSITION_M = 0.0; + public static final double ELEVATOR_MID_POSITION_M = inchesToMeters(26.0); // L2 after placement + public static final double ELEVATOR_TOP_POSITION_M = inchesToMeters(43.0); + public static final double ELEVATOR_L1_POSITION_M = inchesToMeters(6.6); + public static final double ELEVATOR_L2_POSITION_M = inchesToMeters(15); + public static final double ELEVATOR_AUTO_L2_POSITION_M = inchesToMeters(18.5); + public static final double ELEVATOR_AUTO_L2_POSITION_SCORE_M = inchesToMeters(16); + public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(34.25); + // public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_SCORE_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_L3_POSITION_M = inchesToMeters(28); + public static final double ELEVATOR_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_ALGAE_POSITION_M = inchesToMeters(8.0); + public static final double ELEVATOR_GROUND_CORAL_POSITION_M = inchesToMeters(5.4); + public static final double ELEVATOR_GROUND_ALGAE_POSITION_M = inchesToMeters(5.0); + public static final double ELEVATOR_FEEDER_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_OVER_POSITION_M = inchesToMeters(20); + public static final double ELEVATOR_NET_POSITION_M = inchesToMeters(65); + public static final double ELEVATOR_L2_ALGAE_POSITION_M = inchesToMeters(15.7); + public static final double ELEVATOR_L3_ALGAE_POSITION_M = inchesToMeters(37.6741); + public static final double ELEVATOR_PROCESSOR_POSITION_M = inchesToMeters(6.5); + public static final double ELEVATOR_LOLLIPOP_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_PRE_HANDOFF_POSITION_M = inchesToMeters(39.0); + public static final double ELEVATOR_HANDOFF_POSITION_M = inchesToMeters(35.0); + + public enum ElevatorPosition { + kDOWN(ELEVATOR_BOTTOM_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_BOTTOM_POSITION_M)), + kMID(ELEVATOR_MID_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_MID_POSITION_M)), + kUP(ELEVATOR_TOP_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_TOP_POSITION_M)), + kL1(ELEVATOR_L1_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L1_POSITION_M)), + kL2(ELEVATOR_L2_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L2_POSITION_M)), + kAUTOL2(ELEVATOR_AUTO_L2_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_M)), + kAUTOL2SCORE(ELEVATOR_AUTO_L2_POSITION_SCORE_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_SCORE_M)), + kAUTOL3(ELEVATOR_AUTO_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L3_POSITION_M)), + kAUTOL3SCORE(ELEVATOR_AUTO_SCORE_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_SCORE_L3_POSITION_M)), + kAUTOL4(ELEVATOR_AUTO_L4_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L4_POSITION_M)), + kL3(ELEVATOR_L3_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L3_POSITION_M)), + kL4(ELEVATOR_L4_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L4_POSITION_M)), + kALGAE(ELEVATOR_ALGAE_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_ALGAE_POSITION_M)), + kFEEDER(ELEVATOR_FEEDER_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_FEEDER_POSITION_M)), + kGROUNDCORAL(ELEVATOR_GROUND_CORAL_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_CORAL_POSITION_M)), + kGROUNDALGAE(ELEVATOR_GROUND_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_ALGAE_POSITION_M)), + kNET(ELEVATOR_NET_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_NET_POSITION_M)), + kPROCESSOR(ELEVATOR_PROCESSOR_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PROCESSOR_POSITION_M)), + kL2ALGAE(ELEVATOR_L2_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L2_ALGAE_POSITION_M)), + kL3ALGAE(ELEVATOR_L3_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L3_ALGAE_POSITION_M)), + kOVER(ELEVATOR_OVER_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_OVER_POSITION_M)), + kLOLLIPOP(ELEVATOR_LOLLIPOP_POSITION_M, Ratios.elevatorMetersToRotations( + ELEVATOR_LOLLIPOP_POSITION_M)), + kHANDOFF(ELEVATOR_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_HANDOFF_POSITION_M)), + kPREHANDOFF(ELEVATOR_PRE_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PRE_HANDOFF_POSITION_M)); + + public final double meters; + public final double rotations; + + private ElevatorPosition(double meters, double rotations) { + this.meters = meters; + this.rotations = rotations; + } + } + + public static final double PIVOT_L1_POSITION_D = 65.67; + public static final double PIVOT_L23_POSITION_D = 52.5; + // public static final double PIVOT_AUTO_L23_POSITION_D = 45.0; + public static final double PIVOT_AUTO_L2_POSITION_D = 62.0; + public static final double PIVOT_AUTO_L3_POSITION_D = 62.0; + // public static final double PIVOT_AUTO_L3_POSITION_D = 30.0; + public static final double PIVOT_AUTO_L4_POSITION_D = 0.0; + public static final double PIVOT_AUTO_L4_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D = 70.0; + public static final double PIVOT_AUTO_L3_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L2_SCORE_POSITION_D = 100.0; + public static final double PIVOT_L4_POSITION_D = 60.0; + public static final double PIVOT_UPRIGHT_POSITION_D = 45.0; + public static final double PIVOT_GROUND_ALGAE_POSITION_D = 82.5; + public static final double PIVOT_GROUND_CORAL_POSITION_FRONT_D = 111.0; + public static final double PIVOT_GROUND_CORAL_POSITION_BACK_D = -111.0; + public static final double PIVOT_GROUND_CORAL_PREP_BACK_D = -90; + // public static final double PIVOT_DEFAULT_POSITION_D = 30.0; + public static final double PIVOT_DEFAULT_POSITION_D = 0.0; + public static final double PIVOT_DEFAULT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_PREP_POSITION_D = 30.0; + public static final double PIVOT_FEEDER_POSITION_D = 21.0; + public static final double PIVOT_NET_POSITION_D = 15.0; + public static final double PIVOT_PROCESSOR_POSITION_D = 76.0; + public static final double PIVOT_REEF_ALGAE_POSITION_D = 80.0; + public static final double PIVOT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_LOLLIPOP_POSITION_D = -98.0; + public static final double PIVOT_HANDOFF_POSITION_D = 145.0; + + public enum PivotPosition { + kL1(PIVOT_L1_POSITION_D, Constants.degreesToRotations(PIVOT_L1_POSITION_D)), + kL23(PIVOT_L23_POSITION_D, Constants.degreesToRotations(PIVOT_L23_POSITION_D)), + kAUTOL2(PIVOT_AUTO_L2_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L2_POSITION_D)), + kAUTOL3(PIVOT_AUTO_L3_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L3_POSITION_D)), + kAUTOL4(PIVOT_AUTO_L4_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L4_POSITION_D)), + kL4(PIVOT_L4_POSITION_D, Constants.degreesToRotations(PIVOT_L4_POSITION_D)), + kUP(PIVOT_UPRIGHT_POSITION_D, Constants.degreesToRotations(PIVOT_UPRIGHT_POSITION_D)), + kGROUNDALGAE(PIVOT_GROUND_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_GROUND_ALGAE_POSITION_D)), + kGROUNDCORALFRONT(PIVOT_GROUND_CORAL_POSITION_FRONT_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_FRONT_D)), + kGROUNDCORALBACK(PIVOT_GROUND_CORAL_POSITION_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_BACK_D)), + kGROUNDCORALPREPBACK(PIVOT_GROUND_CORAL_PREP_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_PREP_BACK_D)), + kNET(PIVOT_NET_POSITION_D, + Constants.degreesToRotations(PIVOT_NET_POSITION_D)), + kPROCESSOR(PIVOT_PROCESSOR_POSITION_D, + Constants.degreesToRotations(PIVOT_PROCESSOR_POSITION_D)), + kAUTOL2SCORE(PIVOT_AUTO_L2_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L2_SCORE_POSITION_D)), + kAUTOL3SCORE(PIVOT_AUTO_L3_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L3_SCORE_POSITION_D)), + kAUTOL4SCORE(PIVOT_AUTO_L4_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_POSITION_D)), + kAUTOL4SCORESLOW(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D)), + kDEFAULT(PIVOT_DEFAULT_POSITION_D, Constants.degreesToRotations(PIVOT_DEFAULT_POSITION_D)), + kDEFAULTCLIMB(PIVOT_DEFAULT_CLIMB_POSITION_D, + Constants.degreesToRotations(PIVOT_DEFAULT_CLIMB_POSITION_D)), + kFEEDER(PIVOT_FEEDER_POSITION_D, Constants.degreesToRotations(PIVOT_FEEDER_POSITION_D)), + kREEFALGAE(PIVOT_REEF_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_REEF_ALGAE_POSITION_D)), + kPREP(PIVOT_PREP_POSITION_D, Constants.degreesToRotations(PIVOT_PREP_POSITION_D)), + kCLIMB(PIVOT_CLIMB_POSITION_D, Constants.degreesToRotations(PIVOT_CLIMB_POSITION_D)), + kLOLLIPOP(PIVOT_LOLLIPOP_POSITION_D, Constants.degreesToRotations(PIVOT_LOLLIPOP_POSITION_D)), + kHANDOFF(PIVOT_HANDOFF_POSITION_D, Constants.degreesToRotations(PIVOT_HANDOFF_POSITION_D)); + + public final double degrees; + public final double rotations; + + private PivotPosition(double degrees, double rotations) { + this.degrees = degrees; + this.rotations = rotations; + } + } + } + + // Vision constants (e.g. camera offsets) + public static final class Vision { + // Poses of all 16 AprilTags, {x, y, z, yaw, pitch}, in meters and radians + public static final double[][] TAG_POSES = { + { inchesToMeters(657.37), inchesToMeters(25.8), inchesToMeters(58.5), + Math.toRadians(126), + Math.toRadians(0) }, + { inchesToMeters(657.37), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(234), + Math.toRadians(0) }, + { inchesToMeters(455.15), inchesToMeters(317.15), inchesToMeters(51.25), + Math.toRadians(270), + Math.toRadians(0) }, + { inchesToMeters(365.2), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(365.2), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(530.49), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) }, + { inchesToMeters(546.87), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(530.49), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(481.39), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(25.8), inchesToMeters(58.5), Math.toRadians(54), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(306), + Math.toRadians(0) }, + { inchesToMeters(325.68), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(325.68), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(235.73), inchesToMeters(-0.15), inchesToMeters(51.25), + Math.toRadians(90), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(144), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(209.49), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) } + }; + + public static final double[][] redSideReefTags = { + TAG_POSES[6 - 1], TAG_POSES[7 - 1], TAG_POSES[8 - 1], TAG_POSES[9 - 1], + TAG_POSES[10 - 1], + TAG_POSES[11 - 1] }; + + public static final double[][] blueSideReefTags = { + TAG_POSES[6 + 11 - 1], TAG_POSES[7 + 11 - 1], TAG_POSES[8 + 11 - 1], + TAG_POSES[9 + 11 - 1], + TAG_POSES[10 + 11 - 1], TAG_POSES[11 + + 11 - 1] }; + + // Poses of cameras relative to robot, {x, y, z, rx, ry, rz}, in meters and + // radians + public static final double[] FRONT_CAMERA_POSE = { Constants.inchesToMeters(1.75), + Constants.inchesToMeters(11.625), + Constants.inchesToMeters(33.5), 0, -33.5, 0 }; + + // Standard deviation adjustments + public static final double STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE = 3; + public static final double TAG_STANDARD_DEVIATION_DISTANCE = 2; // meters + public static final double TAG_STANDARD_DEVIATION_FLATNESS = 5; + // public static final double XY_STD_DEV_SCALAR = 0.1; + + // Standard deviation regressions + /** + * Calculates the standard deviation scalar based on the distance from the tag. + * + * @param dist The distance from the tag. + * @return The standard deviation scalar. + */ + public static double getTagDistStdDevScalar(double dist) { + // double a = TAG_STANDARD_DEVIATION_FLATNESS; + // double b = 1 - a * Math.pow(TAG_STANDARD_DEVIATION_DISTANCE, 2); + // Logger.recordOutput("std devs", + // -0.000277778 * Math.pow(dist, 3) + 0.00988095 * Math.pow(dist, 2) + + // 0.00444444 * dist + 0.0371429); + // return Math.max(1, a * Math.pow(dist, 2) + b); + return 0.0000520833 * Math.pow(dist, 4) + 0.000394571 * Math.pow(dist, 3) + + 0.000440341 * Math.pow(dist, 2) + + 0.0554117 * dist + 0.0298674; + } + + /** + * Calculates the standard deviation scalar based on the number of detected + * tags. + * + * @param numTags The number of detected tags. + * @return The standard deviation scalar. + */ + public static double getNumTagStdDevScalar(int numTags) { + if (numTags == 0) { + return 99999; + } else if (numTags == 1) { + return 1; + } else if (numTags == 2) { + return 0.6; + } else { + return 0.75; + } + } + + /** + * Calculates the standard deviation of the x-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the x-coordinate. + */ + public static double getTagStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.005533021491867763 * (xOffset * xOffset + yOffset * yOffset) + - 0.010807566510145635) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation of the y-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the y-coordinate. + */ + public static double getTagStdDevY(double xOffset, double yOffset) { + return Math.max(0, 0.0055 * (xOffset * xOffset + yOffset * yOffset) - 0.01941597810542626) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the x-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the x-coordinate. + */ + public static double getTriStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.004544133588821881 * (xOffset * xOffset + yOffset * yOffset) + - 0.01955724864971872) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the y-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the y-coordinate. + */ + public static double getTriStdDevY(double xOffset, double yOffset) { + return Math.max(0, + 0.002615358015002413 * (xOffset * xOffset + yOffset * yOffset) + - 0.008955462032388808) + * STANDARD_DEVIATION_SCALAR; + } + + public static double distBetweenPose(Pose3d pose1, Pose3d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static double distBetweenPose2d(Pose2d pose1, Pose2d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static final double DISTANCE_OFFSET = 7.0; + public static final double CAMERA_ANGLE_OFFSET = 0.0; + // pitch, distance + public static final double[][] CORAL_LOOKUP_TABLE = { + { -17.72 + CAMERA_ANGLE_OFFSET, 24.5 + DISTANCE_OFFSET }, + { -12.22 + CAMERA_ANGLE_OFFSET, 28.5 + DISTANCE_OFFSET }, + { -8.02 + CAMERA_ANGLE_OFFSET, 33.0 + DISTANCE_OFFSET }, + { -5.69 + CAMERA_ANGLE_OFFSET, 38.25 + DISTANCE_OFFSET }, + { -2.73 + CAMERA_ANGLE_OFFSET, 43.75 + DISTANCE_OFFSET }, + { -0.05 + CAMERA_ANGLE_OFFSET, 50.0 + DISTANCE_OFFSET }, + { 2.09 + CAMERA_ANGLE_OFFSET, 56.0 + DISTANCE_OFFSET }, + { 3.55 + CAMERA_ANGLE_OFFSET, 61.5 + DISTANCE_OFFSET }, + { 5.87 + CAMERA_ANGLE_OFFSET, 71.75 + DISTANCE_OFFSET } + }; + + /** + * Interpolates a value from a lookup table based on the given xValue. + * + * @param xIndex The index of the x-values in the lookup table. + * @param yIndex The index of the y-values in the lookup table. + * @param xValue The x-value for which to interpolate a y-value. + * @return The interpolated y-value corresponding to the given x-value. + */ + public static double getInterpolatedValue(int xIndex, int yIndex, double xValue) { + int lastIndex = CORAL_LOOKUP_TABLE.length - 1; + if (xValue < CORAL_LOOKUP_TABLE[0][xIndex]) { + // If the xValue is closer than the first setpoint + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } else if (xValue > CORAL_LOOKUP_TABLE[lastIndex][xIndex]) { + // If the xValue is farther than the last setpoint + double returnValue = CORAL_LOOKUP_TABLE[lastIndex][yIndex]; + return returnValue; + } else { + for (int i = 0; i < CORAL_LOOKUP_TABLE.length; i++) { + if (xValue > CORAL_LOOKUP_TABLE[i][xIndex] + && xValue < CORAL_LOOKUP_TABLE[i + 1][xIndex]) { + // If the xValue is in the table of setpoints + // Calculate where xValue is between setpoints + double leftDif = xValue - CORAL_LOOKUP_TABLE[i][xIndex]; + double percent = leftDif / (CORAL_LOOKUP_TABLE[i + 1][xIndex] + - CORAL_LOOKUP_TABLE[i][xIndex]); + + double value1 = CORAL_LOOKUP_TABLE[i][yIndex]; + double value2 = CORAL_LOOKUP_TABLE[i + 1][yIndex]; + + // Interpolate in-between values for value xValue and shooter rpm + double newValue = value1 + (percent * (value2 - value1)); + + double returnValue = newValue; + return returnValue; + } + } + // Should never run + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } + } + + /** + * Calculates shooter values (flywheel velocity and note velocity) based on the + * given angle. + * + * @param angle The angle of the shooter. + * @return An array containing the calculated flywheel velocity and note + * velocity. + */ + public static double getCoralDistanceFromPitch(double angle) { + return getInterpolatedValue(0, 1, angle); + // return new double[] {25, getInterpolatedValue(1, 3, angle)}; + } + } + + // Gear ratios and conversions + public static final class Ratios { + // pivot + public static final double PIVOT_GEAR_RATIO = 23 * 64 / 24; + + // drive + public static final double DRIVE_GEAR_RATIO = 6.12; + public static final double STEER_GEAR_RATIO = 21.43; + + // elevator + public static final double ELEVATOR_FIRST_STAGE = Constants.inchesToMeters(20); + public static final double ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE = 18.067; + public static final double ELEVATOR_MOTOR_ROTATIONS_PER_METER = ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE + * (1 / ELEVATOR_FIRST_STAGE); + + public static double elevatorRotationsToMeters(double rotations) { + return rotations / ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + public static double elevatorMetersToRotations(double meters) { + return meters * ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + // intake + public static final double INTAKE_PIVOT_GEAR_RATIO = 45.0; + } + + public static boolean isReady = false; + public static final ArrayList paths = new ArrayList(); + + // Can info such as IDs + public static final class CANInfo { + public static final String CANBUS_NAME = "Canivore"; + + // drive + public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 1; + public static final int FRONT_RIGHT_ANGLE_MOTOR_ID = 2; + public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 3; + public static final int FRONT_LEFT_ANGLE_MOTOR_ID = 4; + public static final int BACK_LEFT_DRIVE_MOTOR_ID = 5; + public static final int BACK_LEFT_ANGLE_MOTOR_ID = 6; + public static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; + public static final int BACK_RIGHT_ANGLE_MOTOR_ID = 8; + public static final int FRONT_RIGHT_MODULE_CANCODER_ID = 1; + public static final int FRONT_LEFT_MODULE_CANCODER_ID = 2; + public static final int BACK_LEFT_MODULE_CANCODER_ID = 3; + public static final int BACK_RIGHT_MODULE_CANCODER_ID = 4; + + // Elevator + public static final int LEFT_ELEVATOR_MOTOR_ID = 9; + public static final int RIGHT_ELEVATOR_MOTOR_ID = 10; + + // Manipulator + public static final int MANIPULATOR_MOTOR_ID = 13; + + // Intake + public static final int INTAKE_ROLLER_MOTOR_ID = 17; + public static final int INTAKE_PIVOT_MOTOR_ID = 16; + public static final int INTAKE_BEAM_BREAK_PORT = 0; + + // Straightenator + public static final int LEFT_STRAIGHTENATOR_MOTOR_ID = 12; + public static final int RIGHT_STRAIGHTENATOR_MOTOR_ID = 13; + public static final int FAR_BEAM_BREAK_SENSOR = 0; + public static final int CLOSE_BEAM_BREAK_SENSOR = 2; + + // Arm + public static final int ARM_PIVOT_MOTOR_ID = 15; + public static final int ARM_MANIPULATOR_MOTOR_ID = 14; + } + + public static final class IntakeConstants { + public static double PIVOT_LOWER_LIMIT = 0; + public static double PIVOT_UPPER_LIMIT = 10; + } + + // Misc. controller values + public static final class OperatorConstants { + public static final double RIGHT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_STICK_DEADZONE = 0.03; + public static final double RIGHT_STICK_DEADZONE = 0.05; + } + + /** + * Converts inches to meters. + * + * @param inches The length in inches to be converted. + * @return The equivalent length in meters. + */ + public static double inchesToMeters(double inches) { + return inches / 39.37; + } + + public static double metersToInches(double meters) { + return meters * 39.37; + } + + /** + * Converts feet to meters. + * + * @param inches The length in feet to be converted. + * @return The equivalent length in meters. + */ + public static double feetToMeters(double feet) { + return feet / 3.281; + } + + /** + * Calculates the Euclidean distance between two points in a 2D plane. + * + * @param x1 The x-coordinate of the first point. + * @param y1 The y-coordinate of the first point. + * @param x2 The x-coordinate of the second point. + * @param y2 The y-coordinate of the second point. + * @return The Euclidean distance between the two points. + */ + public static double getDistance(double x1, double y1, double x2, double y2) { + return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); + } + + public static double getAngleToPoint(double x1, double y1, double x2, double y2) { + double deltaX = x2 - x1; + double deltaY = y2 - y1; + + double angleInRadians = Math.atan2(deltaY, deltaX); + + double angleInDegrees = Math.toDegrees(angleInRadians); + + double standardizeAngleDegrees = standardizeAngleDegrees(angleInDegrees); + + return 180 + standardizeAngleDegrees; + } + + /** + * Converts a quantity in rotations to radians. + * + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in radians. + */ + public static double rotationsToRadians(double rotations) { + return rotations * 2 * Math.PI; + } + + /** + * Converts a quantity in degrees to rotations. + * + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in rotations. + */ + public static double degreesToRotations(double degrees) { + return degrees / 360; + } + + /** + * Converts a quantity in rotations to degrees. + * + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in degrees. + */ + public static double rotationsToDegrees(double rotations) { + return rotations * 360; + } + + /** + * Converts a quantity in degrees to radians. + * + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in radians. + */ + public static double degreesToRadians(double degrees) { + return degrees * Math.PI / 180; + } + + /** + * Standardizes an angle to be within the range [0, 360) degrees. + * + * @param angleDegrees The input angle in degrees. + * @return The standardized angle within the range [0, 360) degrees. + */ + public static double standardizeAngleDegrees(double angleDegrees) { + return ((angleDegrees % 360) + 360) % 360; + } + + /** + * Calculates the x-component of a unit vector given an angle in radians. + * + * @param angle The angle in radians. + * @return The x-component of the unit vector. + */ + public static double angleToUnitVectorI(double angle) { + return (Math.cos(angle)); + } + + /** + * Calculates the y-component of a unit vector given an angle in radians. + * + * @param angle The angle in radians. + * @return The y-component of the unit vector. + */ + public static double angleToUnitVectorJ(double angle) { + return (Math.sin(angle)); + } + + /** + * Converts revolutions per minute (RPM) to revolutions per second (RPS). + * + * @param RPM The value in revolutions per minute (RPM) to be converted. + * @return The equivalent value in revolutions per second (RPS). + */ + public static double RPMToRPS(double RPM) { + return RPM / 60; + } + + /** + * Converts revolutions per second (RPS) to revolutions per minute (RPM). + * + * @param RPM The value in revolutions per second (RPS) to be converted. + * @return The equivalent value in revolutions per minute (RPM). + */ + public static double RPSToRPM(double RPS) { + return RPS * 60; + } + + /** + * Standardizes an angle to be within the range [otherAngle - pi, otherAngle + + * pi) radians. + * + * @param angle The input angle in radians. + * @param otherAngle The reference angle in radians. + * @return The standardized angle within the range [otherAngle - pi, otherAngle + * + + * pi) radians. + */ + public static double standardizeAngleToOther(double angle, double otherAngle) { + double delta = angle - otherAngle; + delta = Math.IEEEremainder(delta, 2 * Math.PI); // gives value in [-π, π] + return otherAngle + delta; + } + + /** + * Standardizes an angle to be within the range [otherAngle - 180, otherAngle + + * 180) degrees. + * + * @param angle The input angle in degrees. + * @param otherAngle The reference angle in degrees. + * @return The standardized angle within the range [otherAngle - 180, otherAngle + * + + * 180) degrees. + */ + public static double standardizeAngleToOtherDegrees(double angle, double otherAngle) { + return Math.toDegrees(standardizeAngleToOther(degreesToRadians(angle), degreesToRadians(otherAngle))); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Main.java b/2026-Robot/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..8776e5d --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/Main.java @@ -0,0 +1,25 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() {} + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/OI.java b/2026-Robot/src/main/java/frc/robot/OI.java new file mode 100644 index 0000000..cf4a0a5 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/OI.java @@ -0,0 +1,300 @@ +// Copyrights (c) 2018-2019 FIRST, 2020 Highlanders FRC. All Rights Reserved. +//hi om + +package frc.robot; + +import java.io.File; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.tools.TriggerButton; + +public class OI { + public static SendableChooser fieldSide = new SendableChooser(); + public static SendableChooser leftRight = new SendableChooser(); + public static SendableChooser auto = new SendableChooser(); + public static XboxController driverController = new XboxController(0); + public static XboxController operatorController = new XboxController(1); + + public static BooleanSupplier driveRTSupplier = () -> getDriverRTPercent() > Constants.OperatorConstants.RIGHT_TRIGGER_DEADZONE; + public static BooleanSupplier driverLTSupplier = () -> getDriverLTPercent() > Constants.OperatorConstants.LEFT_TRIGGER_DEADZONE; + + public static BooleanSupplier povUp = () -> getPOV() == 0; + public static BooleanSupplier povRight = () -> getPOV() == 90; + public static BooleanSupplier povDown = () -> getPOV() == 180; + public static BooleanSupplier povLeft = () -> getPOV() == 270; + + public static TriggerButton driverPOVUp = new TriggerButton(povUp); + public static TriggerButton driverPOVRight = new TriggerButton(povRight); + public static TriggerButton driverPOVDown = new TriggerButton(povDown); + public static TriggerButton driverPOVLeft = new TriggerButton(povLeft); + + public static BooleanSupplier operatorPovUp = () -> getOperatorPOV() == 0; + public static BooleanSupplier operatorPovRight = () -> getOperatorPOV() == 90; + public static BooleanSupplier operatorPovDown = () -> getOperatorPOV() == 180; + public static BooleanSupplier operatorPovLeft = () -> getOperatorPOV() == 270; + + public static TriggerButton operatorPOVUp = new TriggerButton(operatorPovUp); + public static TriggerButton operatorPOVRight = new TriggerButton(operatorPovRight); + public static TriggerButton operatorPOVDown = new TriggerButton(operatorPovDown); + public static TriggerButton operatorPOVLeft = new TriggerButton(operatorPovLeft); + + public static TriggerButton driverRT = new TriggerButton(driveRTSupplier); + public static TriggerButton driverLT = new TriggerButton(driverLTSupplier); + + public static JoystickButton driverA = new JoystickButton(driverController, 1); + public static JoystickButton driverB = new JoystickButton(driverController, 2); + + public static JoystickButton driverY = new JoystickButton(driverController, 4); + public static JoystickButton driverX = new JoystickButton(driverController, 3); + + public static JoystickButton driverRB = new JoystickButton(driverController, 6); + public static JoystickButton driverLB = new JoystickButton(driverController, 5); + + public static JoystickButton driverLJ = new JoystickButton(driverController, 9); + public static JoystickButton driverRJ = new JoystickButton(driverController, 10); + + public static JoystickButton operatorX = new JoystickButton(operatorController, 3); + public static JoystickButton operatorB = new JoystickButton(operatorController, 2); + + public static JoystickButton operatorY = new JoystickButton(operatorController, 4); + public static JoystickButton operatorA = new JoystickButton(operatorController, 1); + + public static BooleanSupplier operatorRTSupplier = () -> getOperatorRTPercent() > Constants.OperatorConstants.RIGHT_TRIGGER_DEADZONE; + public static BooleanSupplier operatorLTSupplier = () -> getOperatorLTPercent() > Constants.OperatorConstants.LEFT_TRIGGER_DEADZONE; + + public static TriggerButton operatorRT = new TriggerButton(operatorRTSupplier); + public static TriggerButton operatorLT = new TriggerButton(operatorLTSupplier); + + public static JoystickButton operatorRB = new JoystickButton(operatorController, 6); + public static JoystickButton operatorLB = new JoystickButton(operatorController, 5); + + public static JoystickButton operatorLJ = new JoystickButton(operatorController, 9); + public static JoystickButton operatorRJ = new JoystickButton(operatorController, 10); + + public static JoystickButton driverViewButton = new JoystickButton(driverController, 7); + + public static JoystickButton operatorViewButton = new JoystickButton(operatorController, 7); + public static JoystickButton driverMenuButton = new JoystickButton(driverController, 8); + + public static JoystickButton operatorMenuButton = new JoystickButton(operatorController, 8); + + public static Joystick autoChooser = new Joystick(2); + + public static JoystickButton autoChooserIsBlue = new JoystickButton(autoChooser, 8); + + static { + fieldSide.addOption("red", "red"); + fieldSide.addOption("blue", "blue"); + fieldSide.setDefaultOption("blue", "blue"); + SmartDashboard.putData(fieldSide); + leftRight.addOption("processor", "processor"); + leftRight.addOption("net", "net"); + leftRight.setDefaultOption("net", "net"); + SmartDashboard.putData(leftRight); + for (String path : Constants.Autonomous.paths) { + auto.addOption(path, path); + } + auto.setDefaultOption("None", "None"); + SmartDashboard.putData(auto); + } + + public static void printAutoChooserInputs() { + java.util.logging.Logger.getGlobal().info("Driver Controller Connected: " + driverController.isConnected()); + java.util.logging.Logger.getGlobal().info("Operator Controller Connected: " + operatorController.isConnected()); + java.util.logging.Logger.getGlobal().info("Auto Chooser Connected: " + autoChooser.isConnected()); + java.util.logging.Logger.getGlobal().info("Auto Chooser Num Buttons: " + autoChooser.getButtonCount()); + java.util.logging.Logger.getGlobal().info("Is Blue: " + autoChooserIsBlue.getAsBoolean()); + for (int i = 1; i <= 16; i++) { + java.util.logging.Logger.getGlobal().info("Auto Chooser Button " + i + " : " + autoChooser.getRawButton(i)); + } + } + + public static String getSelectedPath() { + return auto.getSelected(); + } + + public static double getDriverLeftX() { + double leftX = -driverController.getLeftX(), leftY = driverController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftX = 0; + } + return leftX; + } + + public static double getDriverLeftY() { + double leftX = -driverController.getLeftX(), leftY = driverController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftY = 0; + } + return leftY; + } + + public static double getDriverRightX() { + double rightX = driverController.getRightX(), rightY = driverController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + rightX = 0; + } + return rightX; + } + + public static double getDriverRightY() { + double rightX = driverController.getRightX(), rightY = driverController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + rightY = 0; + } + return rightY; + } + + public static double getOperatorLeftX() { + double leftX = -operatorController.getLeftX(), leftY = operatorController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftX = 0; + } + return leftX; + } + + public static double getOperatorLeftY() { + double leftX = -operatorController.getLeftX(), leftY = operatorController.getLeftY(); + if (Math.hypot(leftX, leftY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + leftY = 0; + } + return leftY; + } + + public static double getOperatorRightX() { + double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + rightX = 0; + } + return rightX; + } + + public static double getOperatorRightY() { + double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + rightY = 0; + } + return rightY; + } + + public static double getDriverRTPercent() { + return driverController.getRightTriggerAxis(); + } + + // public static double getDriverLTPercent() { + // return driverController.getLeftTriggerAxis(); + // } + + /** + * This is for using the backup controller, the LT is 1.0 when it should be at + * zero + * Set the trigger depth on the controller to T2 (middle) for LT when using this + * code + */ + public static double getDriverLTPercent() { + double raw = driverController.getLeftTriggerAxis(); + if (raw > 0.9) { + raw = 0.0; + } + double refined = raw * (4.0 / 3.0); + return refined; + } + + public static boolean getDriverA() { + return driverController.getAButton(); + } + + public static boolean getDriverRB() { + return driverController.getRightBumperButton(); + } + + public static boolean getDriverLB() { + return driverController.getLeftBumperButton(); + } + + public static double getOperatorRTPercent() { + return operatorController.getRightTriggerAxis(); + } + + public static double getOperatorLTPercent() { + return operatorController.getLeftTriggerAxis(); + } + + public static boolean getOperatorLB() { + return operatorController.getLeftBumperButton(); + } + + public static int getPOV() { + return driverController.getPOV(); + } + + public static int getOperatorPOV() { + return operatorController.getPOV(); + } + + public static boolean getPOVUp() { + if (getPOV() == 0) { + return true; + } else { + return false; + } + } + + public static boolean isRedSide() { + if (autoChooserConnected()) { + return !autoChooser.getRawButton(8); + } else if (DriverStation.isDSAttached()) { + return DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } else { + return true; + } + } + + public static boolean isProcessorSide() { + return leftRight.getSelected().equals("processor"); + } + + public static boolean isRecalculateMode() { + return autoChooser.getRawButton(7); + } + + public static boolean autoChooserConnected() { + return autoChooser.isConnected(); + } + + public static boolean isBlueSide() { + return fieldSide.getSelected().equals("blue"); + } + + public static boolean is4PieceFarBottom231Auto() { + return autoChooser.getRawButton(2); + } + + public static boolean is5PieceAuto() { + return autoChooser.getRawButton(1); + } + + public static boolean is3PieceFarBottomAuto() { + return autoChooser.getRawButton(3); + } + + public static boolean is1PieceAuto() { + return autoChooser.getRawButton(5); + } + + public static boolean is4PieceAmpSideAuto() { + return autoChooser.getRawButton(4); + } + + public static boolean getDriverRightJoystickPressed() { + return driverController.getRightStickButton(); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Robot.java b/2026-Robot/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..12fef72 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/Robot.java @@ -0,0 +1,98 @@ +package frc.robot; + +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; +import org.littletonrobotics.junction.LogFileUtil; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.AutoRunner; +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; +import frc.robot.subsystems.Drive.DriveState; + +import java.io.IOException; + +public class Robot extends LoggedRobot { + + private RobotContainer m_robotContainer; + private Command m_autonomousCommand; + private Drive drive; + private Peripherals peripherals; + + @Override + public void robotInit() { + // Start AdvantageScope logging + Logger.recordMetadata("Robot", "Robot"); + if (isReal()) { + Logger.addDataReceiver(new NT4Publisher()); + } else { + setUseTiming(false); + String replayLog = LogFileUtil.findReplayLog(); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(replayLog, "_sim"))); + + } + Logger.start(); + + m_robotContainer = new RobotContainer(); + drive = m_robotContainer.drive; + peripherals = m_robotContainer.peripherals; + drive.init("blue"); // your team color or alliance color + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void autonomousInit() { + CommandScheduler.getInstance().schedule(new AutoRunner("sample_path", drive, peripherals)); + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + @Override + public void teleopPeriodic() { + drive.setWantedState(DriveState.DEFAULT); + if (OI.driverMenuButton.getAsBoolean()) { + peripherals.zeroPigeon(); + } + } + + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() { + } + + @Override + public void simulationInit() { + } + + @Override + public void simulationPeriodic() { + } +} diff --git a/2026-Robot/src/main/java/frc/robot/RobotContainer.java b/2026-Robot/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..3c18b15 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,50 @@ +package frc.robot; + +import frc.robot.commands.AutoRunner; +import frc.robot.commands.ZeroPigeon; +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; + +import java.io.File; +import java.io.FileReader; +import java.util.HashMap; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import org.json.JSONObject; +import org.json.JSONTokener; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; + +public class RobotContainer { + + public final Peripherals peripherals = new Peripherals(); + public final Drive drive = new Drive(peripherals); + + private JSONObject[] autoJSONs; + + public RobotContainer() { + } + + private void loadAutoJSONs() { + autoJSONs = new JSONObject[Constants.paths.size()]; + for (int i = 0; i < Constants.paths.size(); i++) { + try { + File file = new File(Filesystem.getDeployDirectory(), Constants.paths.get(i)); + FileReader reader = new FileReader(file); + autoJSONs[i] = new JSONObject(new JSONTokener(reader)); + } catch (Exception e) { + System.out.println("ERROR LOADING PATH " + Constants.paths.get(i) + ": " + e); + autoJSONs[i] = null; + } + } + } + + public Command getAutonomousCommand() { + if (Constants.paths.isEmpty() || autoJSONs[0] == null) + return null; + return new AutoRunner(Constants.paths.get(0), drive, peripherals); + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java b/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java new file mode 100644 index 0000000..7fa3a76 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/AutoRunner.java @@ -0,0 +1,316 @@ +package frc.robot.commands; + +import java.io.File; +import java.io.FileReader; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Iterator; +import java.util.List; +import java.util.Set; +import java.util.function.Function; + +import org.json.JSONArray; +import org.json.JSONObject; +import org.json.JSONTokener; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +import frc.robot.subsystems.Drive; +import frc.robot.subsystems.Peripherals; + +import frc.robot.commands.conditionals.Conditional; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public class AutoRunner extends Command { + private final Drive drive; + private final Peripherals peripherals; + private final String pathName; + + private JSONArray rootCommands = new JSONArray(); + private final HashMap> conditionalMap = new HashMap<>(); + + private final List activeCommands = new ArrayList<>(); + private final List stack = new ArrayList<>(); + private final Set triggerFired = new HashSet<>(); + + private boolean odometryInitialized = false; + + private static class StackEntry { + JSONArray array; + int index; + final Conditional switchConditional; + final JSONObject switchObject; + + StackEntry(JSONArray a, int i, Conditional c, JSONObject o) { + array = a; + index = i; + switchConditional = c; + switchObject = o; + } + } + + public AutoRunner(String pathName, Drive drive, Peripherals peripherals) { + this.drive = drive; + this.peripherals = peripherals; + this.pathName = pathName; + initConditionals(); + loadPathFile(); + } + + private void initConditionals() { + conditionalMap.put("should_turn_left", args -> new frc.robot.commands.conditionals.LeftPath()); + conditionalMap.put("start_main_path", args -> new frc.robot.commands.conditionals.StartMainPath()); + conditionalMap.put("start_left_path", args -> new frc.robot.commands.conditionals.LeftPath()); + conditionalMap.put("start_right_path", args -> new frc.robot.commands.conditionals.StartRightPath()); + } + + private void loadPathFile() { + try { + FileReader r = new FileReader(new File(Filesystem.getDeployDirectory(), pathName + ".json")); + JSONObject json = new JSONObject(new JSONTokener(r)); + rootCommands = json.getJSONArray("commands"); + } catch (Exception e) { + rootCommands = new JSONArray(); + } + } + + @Override + public void initialize() { + activeCommands.clear(); + stack.clear(); + triggerFired.clear(); + odometryInitialized = false; + stack.add(new StackEntry(rootCommands, 0, null, null)); + scheduleNextIfIdle(); + } + + private Command buildSingleCommand(String name, JSONObject args) { + if (args == null) + args = new JSONObject(); + switch (name) { + case "FollowPath": + return new FollowPath(args, drive); + case "PurePursuitFollowPath": + return new PurePursuitFollowPath(args, drive); + case "Zero": + return new ZeroPigeon(peripherals); + case "Log": + return new InstantCommand(); + default: + return new InstantCommand(); + } + } + + private void tryInitializeOdometry(JSONObject args) { + if (odometryInitialized) + return; + if (args == null) + return; + JSONArray pts = args.optJSONArray("points"); + if (pts == null || pts.length() == 0) + return; + JSONObject p = pts.getJSONObject(0); + double x = p.optDouble("x", 0); + double y = p.optDouble("y", 0); + double angle = p.optDouble("angle", 0); + drive.setOdometry(new Pose2d(x, y, Rotation2d.fromDegrees(angle))); + Logger.recordOutput("AutoRunner/OdometryInitX", x); + Logger.recordOutput("AutoRunner/OdometryInitY", y); + Logger.recordOutput("AutoRunner/OdometryInitAngle", angle); + odometryInitialized = true; + } + + private Command buildCommandBlock(JSONObject block) { + JSONArray names = block.optJSONArray("commands"); + JSONObject args = block.optJSONObject("arguments"); + if (names == null || names.length() == 0) + return null; + List list = new ArrayList<>(); + for (int i = 0; i < names.length(); i++) { + String n = names.getString(i); + if (n.equals("FollowPath") || n.equals("PurePursuitFollowPath")) + tryInitializeOdometry(args); + Command c = buildSingleCommand(n, args); + if (c != null) + list.add(c); + } + if (list.isEmpty()) + return null; + Command[] arr = list.toArray(new Command[0]); + if (arr.length == 1) + return arr[0]; + return new ParallelCommandGroup(arr); + } + + private void scheduleCommandAndTrack(Command c) { + if (c == null) + return; + CommandScheduler.getInstance().schedule(c); + activeCommands.add(c); + } + + private Conditional buildConditionalInstance(String condName, Object condArgs) { + Function factory = conditionalMap.get(condName); + if (factory == null) + return null; + Conditional instance; + try { + instance = factory.apply(condArgs); + } catch (Exception e) { + instance = factory.apply(null); + } + if (instance != null && condArgs instanceof JSONObject) { + try { + instance.setArguments((JSONObject) condArgs); + } catch (Exception ignored) { + } + } + return instance; + } + + private boolean triggerConditionTrueForNode(JSONObject node) { + JSONObject trigger = node.optJSONObject("trigger"); + if (trigger == null) + return true; + if (!trigger.has("condition")) + return true; + String condName = trigger.optString("condition", ""); + if (condName.isEmpty()) + return true; + Object condArgs = trigger.has("conditionArguments") ? trigger.get("conditionArguments") : null; + Conditional c = buildConditionalInstance(condName, condArgs); + if (c == null) + return false; + return c.evaluate(); + } + + private void runTriggerCommandsIfAny(JSONObject node, String pathId) { + if (triggerFired.contains(pathId)) + return; + JSONObject trigger = node.optJSONObject("trigger"); + if (trigger == null) + return; + JSONArray cmds = trigger.optJSONArray("commands"); + if (cmds == null) + return; + for (int i = 0; i < cmds.length(); i++) { + String name = cmds.getString(i); + Command c = buildSingleCommand(name, null); + scheduleCommandAndTrack(c); + } + triggerFired.add(pathId); + } + + private String currentPathId() { + StringBuilder sb = new StringBuilder(); + for (StackEntry e : stack) + sb.append("/").append(e.index); + return sb.toString(); + } + + private void scheduleNextIfIdle() { + if (!activeCommands.isEmpty()) + return; + while (!stack.isEmpty()) { + StackEntry top = stack.get(stack.size() - 1); + if (top.index >= top.array.length()) { + stack.remove(stack.size() - 1); + continue; + } + JSONObject node = top.array.getJSONObject(top.index); + String type = node.optString("type", ""); + String pathId = currentPathId(); + if (type.equals("CommandBlock")) { + if (!triggerConditionTrueForNode(node)) + return; + runTriggerCommandsIfAny(node, pathId); + Command block = buildCommandBlock(node); + if (block != null) + scheduleCommandAndTrack(block); + top.index++; + return; + } + if (type.equals("Switch")) { + String condName = node.optString("condition", ""); + if (condName.isEmpty()) { + top.index++; + continue; + } + Object condArgs = node.has("conditionArguments") ? node.get("conditionArguments") : null; + Conditional condInstance = buildConditionalInstance(condName, condArgs); + top.index++; + if (condInstance == null) + continue; + boolean result = condInstance.evaluate(); + JSONArray branch = result ? node.optJSONArray("onTrue") : node.optJSONArray("onFalse"); + if (branch == null) + continue; + stack.add(new StackEntry(branch, 0, condInstance, node)); + return; + } + top.index++; + } + } + + private void handleActiveSwitchFlipIfNeeded() { + if (stack.isEmpty()) + return; + StackEntry top = stack.get(stack.size() - 1); + if (top.switchConditional == null) + return; + boolean now = top.switchConditional.evaluate(); + JSONObject switchObj = top.switchObject; + boolean currentlyTrue = top.array == switchObj.optJSONArray("onTrue"); + if (now != currentlyTrue) { + for (Command c : new ArrayList<>(activeCommands)) + CommandScheduler.getInstance().cancel(c); + activeCommands.clear(); + JSONArray newBranch = now ? switchObj.optJSONArray("onTrue") : switchObj.optJSONArray("onFalse"); + top.array = newBranch != null ? newBranch : new JSONArray(); + top.index = 0; + } + } + + private void cleanupFinishedActiveCommands() { + Iterator it = activeCommands.iterator(); + while (it.hasNext()) { + Command c = it.next(); + if (!CommandScheduler.getInstance().isScheduled(c)) + it.remove(); + } + } + + @Override + public void execute() { + cleanupFinishedActiveCommands(); + if (!activeCommands.isEmpty()) { + handleActiveSwitchFlipIfNeeded(); + return; + } + handleActiveSwitchFlipIfNeeded(); + scheduleNextIfIdle(); + } + + @Override + public void end(boolean interrupted) { + for (Command c : new ArrayList<>(activeCommands)) + CommandScheduler.getInstance().cancel(c); + activeCommands.clear(); + stack.clear(); + triggerFired.clear(); + drive.stop(); + } + + @Override + public boolean isFinished() { + return stack.isEmpty() && activeCommands.isEmpty(); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/Autos.java b/2026-Robot/src/main/java/frc/robot/commands/Autos.java new file mode 100644 index 0000000..b32acdb --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Autos.java @@ -0,0 +1,16 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public final class Autos { + /** Example static factory for an autonomous command. */ + + private Autos() { + throw new UnsupportedOperationException("This is a utility class!"); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java b/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java new file mode 100644 index 0000000..ef29a32 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/CommandBlock.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class CommandBlock extends Command { + public CommandBlock() { + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + } + + @Override + public void end(boolean interrupted) { + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java new file mode 100644 index 0000000..6688c4f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java @@ -0,0 +1,101 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Drive; +import frc.robot.tools.PathLoader; +import frc.robot.tools.math.Vector; + +import java.util.ArrayList; +import java.util.List; +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; + +public class FollowPath extends Command { + private final Drive drive; + private final List points = new ArrayList<>(); + private final Timer timer = new Timer(); + private int currentIndex = 0; + private static final double POSITION_TOLERANCE = 0.02; + private static final double ANGLE_TOLERANCE = 0.05; + private static final double MAX_SPEED = 1.0; // meters/sec + private static final double MAX_ANGULAR = 2.0; // rad/sec + + public FollowPath(JSONObject arguments, Drive driveSubsystem) { + this.drive = driveSubsystem; + if (arguments != null && arguments.has("points")) { + JSONArray pointsArray = arguments.getJSONArray("points"); + for (int i = 0; i < pointsArray.length(); i++) { + JSONObject p = pointsArray.getJSONObject(i); + points.add(new PathLoader.PosePoint( + p.getDouble("x"), + p.getDouble("y"), + p.getDouble("angle"), + p.getDouble("time"))); + } + } + } + + @Override + public void initialize() { + currentIndex = 0; + timer.reset(); + timer.start(); + } + + private double wrapAngle(double angle) { + while (angle > Math.PI) + angle -= 2 * Math.PI; + while (angle < -Math.PI) + angle += 2 * Math.PI; + return angle; + } + + @Override + public void execute() { + if (currentIndex >= points.size()) + return; + + PathLoader.PosePoint target = points.get(currentIndex); + Pose2d pose = drive.getMT2Odometry(); + + double dx = target.x - pose.getX(); + double dy = target.y - pose.getY(); + double distance = Math.hypot(dx, dy); + double dtheta = wrapAngle(target.theta - pose.getRotation().getRadians()); + + if (distance < POSITION_TOLERANCE && Math.abs(dtheta) < ANGLE_TOLERANCE) { + currentIndex++; + return; + } + + double vx = (distance > 0) ? (dx / distance) * Math.min(distance, MAX_SPEED) : 0; + double vy = (distance > 0) ? (dy / distance) * Math.min(distance, MAX_SPEED) : 0; + double omega = Math.signum(dtheta) * Math.min(Math.abs(dtheta), MAX_ANGULAR); + + double cos = Math.cos(-pose.getRotation().getRadians()); + double sin = Math.sin(-pose.getRotation().getRadians()); + double robotX = vx * cos - vy * sin; + double robotY = vx * sin + vy * cos; + + drive.autoDrive(new Vector(robotX, -robotY), -omega); + + Logger.recordOutput("FollowPath/X", target.x); + Logger.recordOutput("FollowPath/Y", target.y); + Logger.recordOutput("FollowPath/Theta", target.theta); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + drive.stop(); + } + + @Override + public boolean isFinished() { + return currentIndex >= points.size(); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java new file mode 100644 index 0000000..0cbf04c --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java @@ -0,0 +1,243 @@ +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Drive; +import frc.robot.tools.PathLoader; +import frc.robot.tools.math.Vector; +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; +import java.util.List; + +public class PurePursuitFollowPath extends Command { + private final Drive drive; + private final JSONArray rawPoints; + private final JSONArray pointsWithVels; + private final List points = new ArrayList<>(); + private int currentIndex = 0; + private boolean odometrySet = false; + + private final double kP_x = 3.0; + private final double kP_y = 3.0; + private final double kP_theta = 2.7; + + private static final double POSITION_TOLERANCE = 0.02; + private static final double ANGLE_TOLERANCE = 0.05; + + public PurePursuitFollowPath(JSONObject arguments, Drive driveSubsystem) { + this.drive = driveSubsystem; + if (arguments != null && arguments.has("points")) + rawPoints = arguments.getJSONArray("points"); + else + rawPoints = new JSONArray(); + + pointsWithVels = buildPointsWithVelocities(rawPoints); + + for (int i = 0; i < rawPoints.length(); i++) { + JSONObject p = rawPoints.getJSONObject(i); + points.add(new PathLoader.PosePoint( + p.optDouble("x", 0.0), + p.optDouble("y", 0.0), + p.optDouble("angle", 0.0), + p.optDouble("time", 0.0))); + } + } + + private JSONArray buildPointsWithVelocities(JSONArray raw) { + JSONArray out = new JSONArray(); + int n = raw.length(); + if (n == 0) + return out; + + double[] xs = new double[n], ys = new double[n], th = new double[n], t = new double[n]; + for (int i = 0; i < n; i++) { + JSONObject p = raw.getJSONObject(i); + xs[i] = p.optDouble("x", 0.0); + ys[i] = p.optDouble("y", 0.0); + th[i] = p.optDouble("angle", 0.0); + double tt = p.has("time") ? p.optDouble("time", Double.NaN) : Double.NaN; + t[i] = Double.isNaN(tt) ? i : tt; + } + + double[] vx = new double[n], vy = new double[n], vth = new double[n]; + if (n == 1) { + vx[0] = vy[0] = vth[0] = 0.0; + } else { + for (int i = 0; i < n; i++) { + if (i == 0) { + double dt = t[1] - t[0]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[0] = (xs[1] - xs[0]) / dt; + vy[0] = (ys[1] - ys[0]) / dt; + vth[0] = wrap(th[1] - th[0]) / dt; + } else if (i == n - 1) { + double dt = t[n - 1] - t[n - 2]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[i] = (xs[i] - xs[i - 1]) / dt; + vy[i] = (ys[i] - ys[i - 1]) / dt; + vth[i] = wrap(th[i] - th[i - 1]) / dt; + } else { + double dt = t[i + 1] - t[i - 1]; + if (Math.abs(dt) < 1e-6) + dt = 0.02; + vx[i] = (xs[i + 1] - xs[i - 1]) / dt; + vy[i] = (ys[i + 1] - ys[i - 1]) / dt; + vth[i] = wrap(th[i + 1] - th[i - 1]) / dt; + } + } + } + + for (int i = 0; i < n; i++) { + int cnt = 0; + double sx = 0, sy = 0, st = 0; + for (int j = i - 1; j <= i + 1; j++) { + if (j < 0 || j >= n) + continue; + sx += vx[j]; + sy += vy[j]; + st += vth[j]; + cnt++; + } + double svx = sx / cnt, svy = sy / cnt, svt = st / cnt; + JSONObject p = raw.getJSONObject(i); + JSONObject c = new JSONObject(); + c.put("x", p.optDouble("x", 0.0)); + c.put("y", p.optDouble("y", 0.0)); + c.put("angle", p.optDouble("angle", 0.0)); + c.put("time", p.optDouble("time", i)); + c.put("x_velocity", svx); + c.put("y_velocity", svy); + c.put("angular_velocity", svt); + out.put(c); + } + return out; + } + + private double wrap(double a) { + while (a > Math.PI) + a -= 2 * Math.PI; + while (a < -Math.PI) + a += 2 * Math.PI; + return a; + } + + private boolean insideRadius(double ax, double ay, double atheta, double radius) { + double norm = Math.sqrt(ax * ax + ay * ay + atheta * atheta); + return norm <= radius; + } + + @Override + public void initialize() { + currentIndex = 0; + if (rawPoints.length() > 0 && !odometrySet) { + JSONObject first = rawPoints.getJSONObject(0); + drive.setOdometry(new Pose2d(first.optDouble("x", 0.0), first.optDouble("y", 0.0), + new Rotation2d(first.optDouble("angle", 0.0)))); + odometrySet = true; + } + } + + @Override + public void execute() { + if (pointsWithVels.length() == 0) + return; + + Pose2d pose = drive.getMT2Odometry(); + double currentX = pose.getX(); + double currentY = pose.getY(); + double currentTheta = pose.getRotation().getRadians(); + + JSONObject last = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + double dxFinal = last.getDouble("x") - currentX; + double dyFinal = last.getDouble("y") - currentY; + double dthetaFinal = wrap(last.getDouble("angle") - currentTheta); + if (Math.hypot(dxFinal, dyFinal) < POSITION_TOLERANCE && Math.abs(dthetaFinal) < ANGLE_TOLERANCE) { + drive.stop(); + return; // skip all velocity calculations + } + + JSONObject targetPoint = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + int targetIndex = pointsWithVels.length() - 1; + + for (int i = currentIndex; i < pointsWithVels.length(); i++) { + JSONObject point = pointsWithVels.getJSONObject(i); + double targetX = point.getDouble("x"); + double targetY = point.getDouble("y"); + double targetTheta = point.getDouble("angle"); + double targetXvel = point.optDouble("x_velocity", 0.0); + double targetYvel = point.optDouble("y_velocity", 0.0); + double targetThetavel = point.optDouble("angular_velocity", 0.0); + + targetTheta = wrap(targetTheta - currentTheta) + currentTheta; + double linearVelMag = Math.hypot(targetYvel, targetXvel); + double targetVelMag = Math.hypot(linearVelMag, targetThetavel); + double lookaheadRadius = 0.6 * targetVelMag + 0.2; + double deltaX = currentX - targetX; + double deltaY = currentY - targetY; + double deltaTheta = currentTheta - targetTheta; + + if (!insideRadius(deltaX, deltaY, deltaTheta, lookaheadRadius)) { + targetIndex = i; + targetPoint = pointsWithVels.getJSONObject(i); + break; + } + } + + double targetX = targetPoint.getDouble("x"); + double targetY = targetPoint.getDouble("y"); + double targetTheta = wrap(targetPoint.getDouble("angle") - currentTheta) + currentTheta; + + double errX = targetX - currentX; + double errY = targetY - currentY; + double errTheta = wrap(targetTheta - currentTheta); + + double xVelNoFF = errX * kP_x; + double yVelNoFF = errY * kP_y; + double thetaVelNoFF = -errTheta * kP_theta; + + double feedForwardX = 0; + double feedForwardY = 0; + double feedForwardTheta = 0; + + double finalX = xVelNoFF + feedForwardX; + double finalY = yVelNoFF + feedForwardY; + double finalTheta = (thetaVelNoFF + feedForwardTheta) * 1.25; + + double cos = Math.cos(-currentTheta); + double sin = Math.sin(-currentTheta); + double robotX = finalX * cos - finalY * sin; + double robotY = finalX * sin + finalY * cos; + + drive.autoDrive(new Vector(robotX, -robotY), finalTheta); + currentIndex = Math.max(currentIndex, Math.min(targetIndex, pointsWithVels.length() - 1)); + + Logger.recordOutput("PurePursuit/targetIndex", targetIndex); + Logger.recordOutput("PurePursuit/currentIndex", currentIndex); + Logger.recordOutput("PurePursuit/targetPose", new Pose2d(targetX, targetY, new Rotation2d(targetTheta))); + Logger.recordOutput("PurePursuit/vel_field", new double[] { finalX, finalY, finalTheta }); + Logger.recordOutput("PurePursuit/vel_robot", new double[] { robotX, robotY }); + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + } + + @Override + public boolean isFinished() { + if (pointsWithVels.length() == 0) + return true; + Pose2d pose = drive.getMT2Odometry(); + JSONObject last = pointsWithVels.getJSONObject(pointsWithVels.length() - 1); + double dx = last.getDouble("x") - pose.getX(); + double dy = last.getDouble("y") - pose.getY(); + double dtheta = wrap(last.getDouble("angle") - pose.getRotation().getRadians()); + return Math.hypot(dx, dy) < POSITION_TOLERANCE && Math.abs(dtheta) < ANGLE_TOLERANCE; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java new file mode 100644 index 0000000..c731e60 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotState.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotState extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotState(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + superstructure.setWantedState(state); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (DriverStation.isAutonomousEnabled()) { + superstructure.setWantedState(SuperState.IDLE); + } else { + superstructure.setWantedState(SuperState.DEFAULT); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java new file mode 100644 index 0000000..9accf6a --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicated.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateComplicated extends Command { + Superstructure superstructure; + SuperState startState; + SuperState endState; + + /** Creates a new SetShootingState. */ + public SetRobotStateComplicated(Superstructure superstructure, SuperState startState, SuperState endState) { + this.superstructure = superstructure; + this.startState = startState; + this.endState = endState; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(startState); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + superstructure.setWantedState(endState); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java new file mode 100644 index 0000000..b9a6a76 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateComplicatedContinuous.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateComplicatedContinuous extends Command { + Superstructure superstructure; + SuperState startState; + SuperState endState; + + /** Creates a new SetShootingState. */ + public SetRobotStateComplicatedContinuous(Superstructure superstructure, SuperState startState, + SuperState endState) { + this.superstructure = superstructure; + this.startState = startState; + this.endState = endState; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + superstructure.setWantedState(startState); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + superstructure.setWantedState(endState); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java new file mode 100644 index 0000000..d1d6193 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateOnce.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateOnce extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotStateOnce(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(state); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + System.out.println("Interrupted? " + interrupted); + if (DriverStation.isAutonomousEnabled()) { + superstructure.setWantedState(SuperState.IDLE); + } else { + superstructure.setWantedState(SuperState.DEFAULT); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java new file mode 100644 index 0000000..770df11 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimple.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SetRobotStateSimple extends Command { + /** Creates a new L3Setup. */ + Superstructure superstructure; + SuperState superState; + + public SetRobotStateSimple(Superstructure superstructure, SuperState superState) { + this.superstructure = superstructure; + this.superState = superState; + addRequirements(this.superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(superState); + System.out.println("Robot State Updating: " + superState); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java new file mode 100644 index 0000000..b7d354f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetRobotStateSimpleOnce.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Superstructure; +import frc.robot.subsystems.Superstructure.SuperState; + +public class SetRobotStateSimpleOnce extends Command { + Superstructure superstructure; + SuperState state; + + /** Creates a new SetShootingState. */ + public SetRobotStateSimpleOnce(Superstructure superstructure, SuperState state) { + this.superstructure = superstructure; + this.state = state; + addRequirements(superstructure); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + superstructure.setWantedState(state); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/Trigger.java b/2026-Robot/src/main/java/frc/robot/commands/Trigger.java new file mode 100644 index 0000000..d659176 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Trigger.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** Add your docs here. */ +public class Trigger { + private BooleanSupplier condition; + private List commands = new ArrayList<>(); + + public Trigger(BooleanSupplier condition, Command... commands) { + this.condition = condition; + for (Command command : commands) { + this.commands.add(command); + } + } + + private void runCommands() { + for (Command command : commands) { + CommandScheduler.getInstance().schedule(command); + } + } + + private void check() { + if (condition.getAsBoolean()) { + runCommands(); + } + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java b/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java new file mode 100644 index 0000000..41e9094 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/ZeroPigeon.java @@ -0,0 +1,41 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Peripherals; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class ZeroPigeon extends Command { + /** Creates a new ZeroPigeon. */ + Peripherals peripheralSubsystem; + + public ZeroPigeon(Peripherals peripherals) { + peripheralSubsystem = peripherals; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + peripheralSubsystem.zeroPigeon(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java new file mode 100644 index 0000000..e2b5c94 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/Conditional.java @@ -0,0 +1,17 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public interface Conditional { + + boolean evaluate(); + + Command onTrue(); + + Command onFalse(); + + void setArguments(JSONObject args); + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java new file mode 100644 index 0000000..f11ad0b --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/LeftPath.java @@ -0,0 +1,38 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; +import edu.wpi.first.wpilibj2.command.Command; + +public class LeftPath implements Conditional { + + private double meow = 0; + + public LeftPath() { + + } + + @Override + public boolean evaluate() { + System.out.println("Evaluating LeftPath Conditional: " + (meow == 67)); + return meow == 67; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + if (args != null) { + if (args.has("value")) { + meow = args.getDouble("value"); + } + } + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java new file mode 100644 index 0000000..4f58363 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartMainPath.java @@ -0,0 +1,29 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public class StartMainPath implements Conditional { + + @Override + public boolean evaluate() { + return false; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java new file mode 100644 index 0000000..be2434d --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/conditionals/StartRightPath.java @@ -0,0 +1,28 @@ +package frc.robot.commands.conditionals; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public class StartRightPath implements Conditional { + + @Override + public boolean evaluate() { + return false; + } + + @Override + public Command onTrue() { + return null; + } + + @Override + public Command onFalse() { + return null; + } + + @Override + public void setArguments(JSONObject args) { + } + +} diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java new file mode 100644 index 0000000..8d759e2 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java @@ -0,0 +1,4459 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; + +import org.json.JSONArray; +import org.json.JSONObject; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.OI; +import frc.robot.tools.controlloops.PID; +import frc.robot.tools.math.Vector; + +// **Zero Wheels with the bolt head showing on the left when the front side(battery) is facing down/away from you** + +public class Drive extends SubsystemBase { + private final TalonFX frontRightDriveMotor = new TalonFX(Constants.CANInfo.FRONT_RIGHT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontRightAngleMotor = new TalonFX(Constants.CANInfo.FRONT_RIGHT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontLeftDriveMotor = new TalonFX(Constants.CANInfo.FRONT_LEFT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX frontLeftAngleMotor = new TalonFX(Constants.CANInfo.FRONT_LEFT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backLeftDriveMotor = new TalonFX(Constants.CANInfo.BACK_LEFT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backLeftAngleMotor = new TalonFX(Constants.CANInfo.BACK_LEFT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backRightDriveMotor = new TalonFX(Constants.CANInfo.BACK_RIGHT_DRIVE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + private final TalonFX backRightAngleMotor = new TalonFX(Constants.CANInfo.BACK_RIGHT_ANGLE_MOTOR_ID, + Constants.CANInfo.CANBUS_NAME); + + private final CANcoder frontRightCanCoder = new CANcoder(Constants.CANInfo.FRONT_RIGHT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder frontLeftCanCoder = new CANcoder(Constants.CANInfo.FRONT_LEFT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder backLeftCanCoder = new CANcoder(Constants.CANInfo.BACK_LEFT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + private final CANcoder backRightCanCoder = new CANcoder(Constants.CANInfo.BACK_RIGHT_MODULE_CANCODER_ID, + Constants.CANInfo.CANBUS_NAME); + + public boolean getSwerveCAN() { // checks to see if all of the swerve motors and encoders are connected + if (getSwerveMotorsConnected() == 8 && getSwerveCANCodersConnected() == 4) { + return true; + } else + return false; + } + + public int getSwerveMotorsConnected() { // counts all of the swerve motors that are connected + int count = 0; + if (frontRightDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontLeftDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backRightDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backLeftDriveMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontRightAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (frontLeftAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backRightAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + if (backLeftAngleMotor.clearStickyFault_BootDuringEnable() == StatusCode.OK) { + count++; + } + return count; + } + + public int getSwerveCANCodersConnected() { // counts all of the swerve encoders that are connected + int count = 0; + if (frontRightCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (frontLeftCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (backRightCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + if (backLeftCanCoder.clearStickyFault_BadMagnet() == StatusCode.OK) { + count++; + } + return count; + } + + // creates all 4 modules + private final SwerveModule frontRight = new SwerveModule(1, frontRightAngleMotor, frontRightDriveMotor, + frontRightCanCoder); + private final SwerveModule frontLeft = new SwerveModule(2, frontLeftAngleMotor, frontLeftDriveMotor, + frontLeftCanCoder); + private final SwerveModule backLeft = new SwerveModule(3, backLeftAngleMotor, backLeftDriveMotor, backLeftCanCoder); + private final SwerveModule backRight = new SwerveModule(4, backRightAngleMotor, backRightDriveMotor, + backRightCanCoder); + + Peripherals peripherals; + boolean firstClimb = false; + + // xy position of module based on robot width and distance from edge of robot + private final double moduleX = ((Constants.Physical.ROBOT_LENGTH) / 2) - Constants.Physical.MODULE_OFFSET; + private final double moduleY = ((Constants.Physical.ROBOT_WIDTH) / 2) - Constants.Physical.MODULE_OFFSET; + + // Locations for the swerve drive modules relative to the robot center. + Translation2d m_frontLeftLocation = new Translation2d(moduleX, moduleY); + Translation2d m_frontRightLocation = new Translation2d(moduleX, -moduleY); + Translation2d m_backLeftLocation = new Translation2d(-moduleX, moduleY); + Translation2d m_backRightLocation = new Translation2d(-moduleX, -moduleY); + + // odometry + + private double m_initTime; + private double m_currentTime; + + // Creating my kinematics object using the module locations + SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + + SwerveDrivePoseEstimator mt2Odometry; + Pose2d mt2Pose; + + PhotonPoseEstimator photonPoseEstimator; + PhotonPoseEstimator backPhotonPoseEstimator; + PhotonPoseEstimator backLeftPhotonPoseEstimator; + PhotonPoseEstimator backRightPhotonPoseEstimator; + + // PhotonPoseEstimator rightPhotonPoseEstimator; + // PhotonPoseEstimator leftPhotonPoseEstimator; + PhotonPoseEstimator swervePhotonPoseEstimator; + PhotonPoseEstimator gamePiecePhotonPoseEstimator; + AprilTagFieldLayout aprilTagFieldLayout; + + // *********************NOTE THE PITCH IS POSITIVE DOWNWARDS + // ********************************** + + Transform3d frontReefRobotToCam = new Transform3d( // top front reef cam + new Translation3d(Constants.inchesToMeters(2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(23.625)), + new Rotation3d(Math.toRadians(0.3), Math.toRadians(25.6), Math.toRadians(15.0))); + + Transform3d frontSwerveRobotToCam = new Transform3d( // front reef cam on swerve module + new Translation3d(Constants.inchesToMeters(11.75), + Constants.inchesToMeters(-8.5), + Constants.inchesToMeters(8.75)), + new Rotation3d(Math.toRadians(1.1), Math.toRadians(15.4), + Math.toRadians(35.0))); + + Transform3d backReefRobotToCam = new Transform3d( // top back reef cam + new Translation3d(Constants.inchesToMeters(-2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(23.625)), + new Rotation3d(Math.toRadians(2.8), Math.toRadians(25.9), Math.toRadians(165.0))); + + Transform3d backLeftReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(6.4), Constants.inchesToMeters(7.0), + Constants.inchesToMeters(18.7)), + new Rotation3d(Math.toRadians(0.2), Math.toRadians(10.1), Math.toRadians(0.0))); // 0.4, -20.5 + + Transform3d backRightReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters( + 6.4), Constants.inchesToMeters(-7.0), + Constants.inchesToMeters(18.7)), + new Rotation3d(Math.toRadians(0.4), Math.toRadians(10.6), Math.toRadians(0.0))); + + Transform3d gamePieceReefRobotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(2.0), Constants.inchesToMeters(-11.5), + Constants.inchesToMeters(20.25)), + new Rotation3d(Math.toRadians(1.0), Math.toRadians(21.4), Math.toRadians(15.0))); + + double initAngle; + double setAngle; + double diffAngle; + + // path following PID values + private double kXP = 4.00; + private double kXI = 0.00; + private double kXD = 1.20; + + private double kYP = kXP; + private double kYI = kXI; + private double kYD = kXD; + + private double kThetaP = 2.90; + private double kThetaI = 0.00; + private double kThetaD = 2.00; + + // auto placement PID values + private double kkXP = 5.50; + private double kkXI = 0.00; + private double kkXD = 1.60; + + private double kkYP = kkXP; + private double kkYI = kkXI; + private double kkYD = kkXD; + + private double kkThetaP = 1.79; + private double kkThetaI = 0.00; + private double kkThetaD = 0.971; + + // l4 pid values + private double kkXP4 = 4.30; + private double kkXI4 = 0.00; + private double kkXD4 = 1.70; + + private double kkYP4 = kkXP4; + private double kkYI4 = kkXI4; + private double kkYD4 = kkXD4; + + private double kkThetaP4 = 2.056; + private double kkThetaI4 = 0.00; + private double kkThetaD4 = 0.971; + + // l4 auto pids + private double scalar = 0.9; + private double kkXP4A = 4.30 * scalar; + private double kkXI4A = 0.00 * scalar; + private double kkXD4A = 1.70 * scalar; + + private double kkYP4A = kkXP4A; + private double kkYI4A = kkXI4A; + private double kkYD4A = kkXD4A; + + private double kkThetaP4A = 2.056; + private double kkThetaI4A = 0.00; + private double kkThetaD4A = 0.971; + + // l23 pid values + private double kkXP23 = 4.00; + private double kkXI23 = 0.00; + private double kkXD23 = 1.60; + + private double kkYP23 = kkXP23; + private double kkYI23 = kkXI23; + private double kkYD23 = kkXD23; + + private double kkThetaP23 = 2.481; + private double kkThetaI23 = 0.00; + private double kkThetaD23 = 0.971; + + // l1 pid values + private double kkXP1 = 4.00; + private double kkXI1 = 0.00; + private double kkXD1 = 1.40; + + private double kkYP1 = kkXP1; + private double kkYI1 = kkXI1; + private double kkYD1 = kkXD1; + + private double kkThetaP1 = 3.005; + private double kkThetaI1 = 0.00; + private double kkThetaD1 = 0.971; + + // Piece pickup values + private double kkkXPPickup = 3.30; + private double kkkXIPickup = 0.00; + private double kkkXDPickup = 1.70; + + private double kkkYPPickup = kkkXPPickup; + private double kkkYIPickup = kkkXIPickup; + private double kkkYDPickup = kkkXDPickup; + + private double kkkThetaPPickup = 2.056; + private double kkkThetaIPickup = 0.00; + private double kkkThetaDPickup = 0.971; + + // teleop targeting PID values + private double kTurningP = 0.04; + private double kTurningI = 0; + private double kTurningD = 0.06; + private double kRotateP = 0.04; + private double kRotateI = 0.0; + private double kRotateD = 0.06; + + private PID xxPID = new PID(kkXP, kkXI, kkXD); + private PID yyPID = new PID(kkYP, kkYI, kkYD); + private PID thetaaPID = new PID(kkThetaP, kkThetaI, kkThetaD); + + private PID xxPID4 = new PID(kkXP4, kkXI4, kkXD4); + private PID yyPID4 = new PID(kkYP4, kkYI4, kkYD4); + private PID thetaaPID4 = new PID(kkThetaP4, kkThetaI4, kkThetaD4); + + private PID xxPID4A = new PID(kkXP4A, kkXI4A, kkXD4A); + private PID yyPID4A = new PID(kkYP4A, kkYI4A, kkYD4A); + private PID thetaaPID4A = new PID(kkThetaP4A, kkThetaI4A, kkThetaD4A); + + private PID xxPID23 = new PID(kkXP23, kkXI23, kkXD23); + private PID yyPID23 = new PID(kkYP23, kkYI23, kkYD23); + private PID thetaaPID23 = new PID(kkThetaP23, kkThetaI23, kkThetaD23); + + private PID xxPID1 = new PID(kkXP1, kkXI1, kkXD1); + private PID yyPID1 = new PID(kkYP1, kkYI1, kkYD1); + private PID thetaaPID1 = new PID(kkThetaP1, kkThetaI1, kkThetaD1); + + private PID xxPIDPickup = new PID(kkkXPPickup, kkkXIPickup, kkkXDPickup); + private PID yyPIDPickup = new PID(kkkYPPickup, kkkYIPickup, kkkYDPickup); + private PID thetaaPIDPickup = new PID(kkkThetaPPickup, kkkThetaIPickup, kkkThetaDPickup); + + private PID xPID = new PID(kXP, kXI, kXD); + private PID yPID = new PID(kYP, kYI, kYD); + private PID thetaPID = new PID(kThetaP, kThetaI, kThetaD); + private PID turningPID = new PID(kTurningP, kTurningI, kTurningD); + private PID rotatePID = new PID(kRotateP, kRotateI, kRotateD); + + private String m_fieldSide = "blue"; + + private double angleSetpoint = 0; + double startX; + double startY; + + public boolean algaeMode = false; + + public enum DriveState { + DEFAULT, + IDLE, + STOP, + REEF, + REEF_MORE, + BACK, + L3_REEF, + L4_REEF, + ALGAE, + ALGAE_MORE, + ALGAE_MORE_MORE, + PROCESSOR, + PROCESSOR_MORE, + NET, + NET_MORE, + FEEDER, + SCORE_L23, + FEEDER_ALIGN, + AUTO_FEEDER, + AUTO_L1, + AUTO_L1_MORE, + FEEDER_AUTO, + PIECE_PICKUP, + AUTO_CLIMB + } + + private DriveState wantedState = DriveState.IDLE; + private DriveState systemState = DriveState.IDLE; + + /** + * Creates a new instance of the Swerve Drive subsystem. + * Initializes the Swerve Drive subsystem with the provided peripherals. + * + * @param peripherals The peripherals used by the Swerve Drive subsystem. + */ + + public Drive(Peripherals peripherals) { + this.peripherals = peripherals; + + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(0, new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(0, new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(0, new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(0, new Rotation2d(backRight.getCanCoderPositionRadians())); + + Pose2d m_pose = new Pose2d(); + mt2Odometry = new SwerveDrivePoseEstimator(m_kinematics, + new Rotation2d(Math.toRadians(peripherals.getPigeonAngle())), swerveModulePositions, m_pose); + } + + // public boolean atSetpoint() { + // double currentAngle = peripherals.getPigeonAngle(); + // if (getFieldSide().equals("red")) { + // currentAngle -= 180; + // } + // return (Math.abs(Constants.standardizeAngleDegrees(currentAngle) + // - Constants.standardizeAngleDegrees(angleSetpoint)) < 2); + // } + + public void setWantedState(DriveState wantedState) { + this.wantedState = wantedState; + } + + public void setWantedState(DriveState wantedState, double angle) { + this.wantedState = wantedState; + setSetpointAngle(angle); + } + + public void setSetpointAngle(double angle) { + this.angleSetpoint = angle; + // Logger.recordOutput("Setpoint Theta: ", angle); + } + + /** + * Initializes the robot with the specified field side configuration. + * It sets up configurations when run on robot initialization, such as setting + * the field side, + * initializing each swerve module, configuring motor inversions, and setting + * PID controller output limits. + * Additionally, it sets the default command to the DriveDefault command. + * + * @param fieldSide The side of the field (e.g., "red" or "blue"). + */ + public void init(String fieldSide) { + // sets configurations when run on robot initalization + this.m_fieldSide = fieldSide; + + try { + aprilTagFieldLayout = new AprilTagFieldLayout( + Filesystem.getDeployDirectory().getPath() + "/" + "2025-reefscape-andymark.json"); + } catch (Exception e) { + java.util.logging.Logger.getGlobal().warning("error with april tag: " + e.getMessage()); + } + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontReefRobotToCam); + + backPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backReefRobotToCam); + + backLeftPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backLeftReefRobotToCam); + + backRightPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backRightReefRobotToCam); + + swervePhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontSwerveRobotToCam); + + // rightPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + // PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, frontBargeRobotToCam); + + // leftPhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + // PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, backBargeRobotToCam); + + gamePiecePhotonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, gamePieceReefRobotToCam); + + frontRight.init(); + frontLeft.init(); + backRight.init(); + backLeft.init(); + + xxPID.setMinOutput(-3.0); + xxPID.setMaxOutput(3.0); + + yyPID.setMinOutput(-3.0); + yyPID.setMaxOutput(3.0); + + thetaaPID.setMinOutput(-3.0); + thetaaPID.setMaxOutput(3.0); + + xxPID4.setMinOutput(-2.0); + xxPID4.setMaxOutput(2.0); + + yyPID4.setMinOutput(-2.0); + yyPID4.setMaxOutput(2.0); + + thetaaPID4.setMinOutput(-2.0); + thetaaPID4.setMaxOutput(2.0); + + xxPID4A.setMinOutput(-1.4); + xxPID4A.setMaxOutput(1.4); + + yyPID4A.setMinOutput(-1.4); + yyPID4A.setMaxOutput(1.4); + + thetaaPID4A.setMinOutput(-1.4); + thetaaPID4A.setMaxOutput(1.4); + + xxPID23.setMinOutput(-4.0); + xxPID23.setMaxOutput(4.0); + + yyPID23.setMinOutput(-4.0); + yyPID23.setMaxOutput(4.0); + + thetaaPID23.setMinOutput(-4.0); + thetaaPID23.setMaxOutput(4.0); + + xxPID1.setMinOutput(-4.0); + xxPID1.setMaxOutput(4.0); + + yyPID1.setMinOutput(-4.0); + yyPID1.setMaxOutput(4.0); + + thetaaPID1.setMinOutput(-4.0); + thetaaPID1.setMaxOutput(4.0); + + xxPIDPickup.setMinOutput(-1.0); + xxPIDPickup.setMaxOutput(1.0); + + yyPIDPickup.setMinOutput(-1.0); + yyPIDPickup.setMaxOutput(1.0); + + thetaaPIDPickup.setMinOutput(-2.0); + thetaaPIDPickup.setMaxOutput(2.0); + + xPID.setMinOutput(-4.9); + xPID.setMaxOutput(4.9); + + yPID.setMinOutput(-4.9); + yPID.setMaxOutput(4.9); + + thetaPID.setMinOutput(-3); + thetaPID.setMaxOutput(3); + + turningPID.setMinOutput(-3); + turningPID.setMaxOutput(3); + + rotatePID.setMinOutput(-2); + rotatePID.setMaxOutput(2); + } + + public void teleopInit() { + angleSetpoint = peripherals.getPigeonAngle(); + if (getFieldSide() == "red") { + angleSetpoint -= 180; + } + turningPID.setSetPoint(angleSetpoint); + if (Math.abs(turningPID.getSetPoint() - angleSetpoint) > 100) { + turningPID.setSetPoint(angleSetpoint); + } + + frontLeft.setDriveCurrentLimits(120, 120); // if robot starts browning out in a placement sequence, just drop + // these + // to 40 + frontRight.setDriveCurrentLimits(120, 120); + backLeft.setDriveCurrentLimits(120, 120); + backRight.setDriveCurrentLimits(120, 120); + } + + public void teleopPeriodic() { + if (Math.abs(turningPID.getSetPoint() - angleSetpoint) > 100) { + turningPID.setSetPoint(angleSetpoint); + } + } + + /** + * Zeros the IMU (Inertial Measurement Unit) mid-match and resets the odometry + * with a zeroed angle. + * It resets the angle reported by the pigeon sensor to zero and updates the + * odometry with this new zeroed angle. + */ + public void zeroIMU() { + angleSetpoint = 0; + turningPID.setSetPoint(angleSetpoint); + peripherals.zeroPigeon(); + // SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + // swerveModulePositions[0] = new + // SwerveModulePosition(frontLeft.getModuleDistance(), + // new Rotation2d(frontLeft.getCanCoderPositionRadians())); + // swerveModulePositions[1] = new + // SwerveModulePosition(frontRight.getModuleDistance(), + // new Rotation2d(frontRight.getCanCoderPositionRadians())); + // swerveModulePositions[2] = new + // SwerveModulePosition(backLeft.getModuleDistance(), + // new Rotation2d(backLeft.getCanCoderPositionRadians())); + // swerveModulePositions[3] = new + // SwerveModulePosition(backRight.getModuleDistance(), + // new Rotation2d(backRight.getCanCoderPositionRadians())); + + // m_pose = m_odometry.update(new + // Rotation2d((Math.toRadians(peripherals.getPigeonAngle()))), + // swerveModulePositions); + // loggingPose = loggingOdometry.update(new Rotation2d( + // Math.toRadians(peripherals.getPigeonAngle())), swerveModulePositions); + // mt2Pose = mt2Odometry.update(new + // Rotation2d(Math.toRadians(peripherals.getPigeonAngle())), + // swerveModulePositions); + } + + /** + * Adjusts the angle reported by the pigeon sensor after an autonomous routine. + * It adds 180 degrees to the current angle reported by the pigeon sensor and + * wraps it around 360 degrees. + */ + public void setPigeonAfterAuto() { + peripherals.setPigeonAngle((peripherals.getPigeonAngle() + 180) % 360); + } + + /** + * Sets the angle reported by the pigeon sensor to the specified value. + * + * @param angle The angle to set for the pigeon sensor in degrees. + */ + public void setPigeonAngle(double angle) { + peripherals.setPigeonAngle(angle); + } + + /** + * Retrieves the current angle reported by the pigeon sensor. + * + * @return The current angle reported by the pigeon sensor in degrees. + */ + public double getPigeonAngle() { + return peripherals.getPigeonAngle(); + } + + /** + * Sets the PID values for all swerve modules to zero, keeping the wheels + * straight. + */ + public void setWheelsStraight() { + frontRight.setWheelPID(0.0, 0.0); + frontLeft.setWheelPID(0.0, 0.0); + backLeft.setWheelPID(0.0, 0.0); + backRight.setWheelPID(0.0, 0.0); + } + + /** + * Initializes the robot's state for autonomous mode based on the provided path + * points. + * + * @param pathPoints The array of path points representing the trajectory for + * the autonomous routine. + */ + public void autoInit(JSONArray pathPoints) { + // runs at start of autonomous + java.util.logging.Logger.getGlobal().info("Auto init"); + JSONObject firstPoint = pathPoints.getJSONObject(0); + double firstPointX = firstPoint.getDouble("x"); + double firstPointY = firstPoint.getDouble("y"); + double firstPointAngle = firstPoint.getDouble("angle"); + + // changing odometry if on red side, don't need to change y because it will be + // the same for autos on either side + if (this.m_fieldSide == "blue") { + firstPointX = Constants.Physical.FIELD_LENGTH - firstPointX; + firstPointY = Constants.Physical.FIELD_WIDTH - firstPointY; + firstPointAngle = Math.PI + firstPointAngle; + } + + if (OI.isProcessorSide()) { + firstPointY = Constants.Physical.FIELD_WIDTH - firstPointY; + firstPointAngle = -firstPointAngle; + } + + frontLeft.setDriveCurrentLimits(60, 120); + frontRight.setDriveCurrentLimits(60, 120); + backLeft.setDriveCurrentLimits(60, 120); + backRight.setDriveCurrentLimits(60, 120); + + peripherals.setPigeonAngle(Math.toDegrees(firstPointAngle)); + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(frontLeft.getModuleDistance(), + new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(frontRight.getModuleDistance(), + new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(backLeft.getModuleDistance(), + new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(backRight.getModuleDistance(), + new Rotation2d(backRight.getCanCoderPositionRadians())); + mt2Odometry.resetPosition(new Rotation2d(firstPointAngle), swerveModulePositions, + new Pose2d(new Translation2d(firstPointX, firstPointY), new Rotation2d(firstPointAngle))); + + m_initTime = Timer.getFPGATimestamp(); + + updateOdometryFusedArray(); + } + + /** + * Sets the current field side designation. + * + * @param side The field side designation to set, indicating whether the robot + * is positioned on the "blue" or "red" side of the field. + */ + public void setFieldSide(String side) { + m_fieldSide = side; + } + + /** + * Retrieves the current field side designation. + * + * @return The current field side designation, indicating whether the robot is + * positioned on the "blue" or "red" side of the field. + */ + public String getFieldSide() { + return m_fieldSide; + } + + /** + * Retrieves the current timestamp relative to the start of the robot operation. + * + * @return The current timestamp in seconds since the start of the robot + * operation. + */ + public double getCurrentTime() { + return m_currentTime; + } + + public void setOdometry(Pose2d pose) { + // Logger.recordOutput("Odometry Reset to:", pose.toString()); + java.util.logging.Logger.getGlobal().fine("New Odometry Pose: " + pose.toString()); + mt2Odometry.resetPose(pose); + } + + public boolean isPoseInField(Pose2d pose) { + if (pose.getY() < 0 || pose.getY() > Constants.Physical.FIELD_WIDTH || pose.getX() < 0 + || pose.getX() > Constants.Physical.FIELD_LENGTH) { + return false; + } else { + return true; + } + } + + private int getClosestTagId(Pose2d pose) { + int closestTag = 0; + double closestDistance = Double.MAX_VALUE; + + for (int i = 1; i <= aprilTagFieldLayout.getTags().size(); i++) { + Optional tagPose = aprilTagFieldLayout.getTagPose(i); + if (tagPose.isPresent()) { + double distance = Constants.Vision.distBetweenPose2d(pose, tagPose.get().toPose2d()); + if (distance < closestDistance) { + closestDistance = distance; + closestTag = i; + } + } + } + return closestTag; + } + + private boolean inReefInteractionState() { + return systemState == DriveState.L4_REEF || systemState == DriveState.L3_REEF || + systemState == DriveState.REEF || systemState == DriveState.AUTO_L1 || + systemState == DriveState.AUTO_L1_MORE || systemState == DriveState.ALGAE || + systemState == DriveState.ALGAE_MORE || systemState == DriveState.ALGAE_MORE_MORE; + } + + private boolean closeToReef() { + double dist = DriverStation.isAutonomousEnabled() ? 2.7 : 2.3; + if (distanceFromCenterOfReef() < dist) { + return true; + } else { + return false; + } + } + + /** + * Updates the fused odometry array with current robot position and orientation + * information. + * Calculates the robot's position and orientation using swerve module positions + * and the gyro angle. + * Updates the current X, Y, and theta values, as well as previous values and + * time differences. + */ + public void updateOdometryFusedArray() { + double navxOffset = Math.toRadians(peripherals.getPigeonAngle()); + + SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[4]; + swerveModulePositions[0] = new SwerveModulePosition(frontLeft.getModuleDistance(), + new Rotation2d(frontLeft.getCanCoderPositionRadians())); + swerveModulePositions[1] = new SwerveModulePosition(frontRight.getModuleDistance(), + new Rotation2d(frontRight.getCanCoderPositionRadians())); + swerveModulePositions[2] = new SwerveModulePosition(backLeft.getModuleDistance(), + new Rotation2d(backLeft.getCanCoderPositionRadians())); + swerveModulePositions[3] = new SwerveModulePosition(backRight.getModuleDistance(), + new Rotation2d(backRight.getCanCoderPositionRadians())); + mt2Pose = mt2Odometry.update(new Rotation2d(navxOffset), swerveModulePositions); + + Matrix standardDeviation = new Matrix<>(Nat.N3(), Nat.N1()); + Logger.recordOutput("Closde to reef", closeToReef()); + + if (((closeToReef()) || inReefInteractionState()) + && systemState != DriveState.REEF_MORE) { + photonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backLeftPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + backRightPhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + swervePhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + gamePiecePhotonPoseEstimator.setPrimaryStrategy(PoseStrategy.PNP_DISTANCE_TRIG_SOLVE); + + Rotation2d robotRotation = new Rotation2d(navxOffset); + double time = Timer.getFPGATimestamp(); + photonPoseEstimator.addHeadingData(time, robotRotation); + backPhotonPoseEstimator.addHeadingData(time, robotRotation); + backLeftPhotonPoseEstimator.addHeadingData(time, robotRotation); + backRightPhotonPoseEstimator.addHeadingData(time, robotRotation); + swervePhotonPoseEstimator.addHeadingData(time, robotRotation); + gamePiecePhotonPoseEstimator.addHeadingData(time, robotRotation); + } + Logger.recordOutput("Back Strategy: ", + backPhotonPoseEstimator.getPrimaryStrategy().toString()); + Logger.recordOutput("Back left strat: ", backLeftPhotonPoseEstimator.getPrimaryStrategy().toString()); + Logger.recordOutput("Back right strat: ", + backRightPhotonPoseEstimator.getPrimaryStrategy().toString()); + + if (getRobotSpeed() < 2.4) { + var backResult = peripherals.getBackReefCamResult(); + Optional backMultiTagResult = backPhotonPoseEstimator.update(backResult); + if (backMultiTagResult.isPresent()) { + if (backResult.getBestTarget().getPoseAmbiguity() < 0.3 && backResult.getBestTarget().fiducialId != 5 + && backResult.getBestTarget().fiducialId != 4 && backResult.getBestTarget().fiducialId != 14 + && backResult.getBestTarget().fiducialId != 15 && backResult.getBestTarget().fiducialId != 3 + && backResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backMultiTagResult.get().estimatedPose; + Logger.recordOutput("multitag result", robotPose); + int numFrontTracks = backResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backResult.getBestTarget().getFiducialId()).get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var swerveResult = peripherals.getFrontSwerveCamResult(); + Optional swerveMultiTagResult = swervePhotonPoseEstimator.update(swerveResult); + if (swerveMultiTagResult.isPresent() + && (!inReefInteractionState() + || getAutoPlacementSideIsFront())) { + if (swerveResult.getBestTarget().getPoseAmbiguity() < 0.3) { + Pose3d robotPose = swerveMultiTagResult.get().estimatedPose; + int numFrontTracks = swerveResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(swerveResult.getBestTarget().getFiducialId()).get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (swerveResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + swerveResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + swerveResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var backLeftResult = peripherals.getBackLeftReefCamResult(); + Optional backLeftMultiTagResult = backLeftPhotonPoseEstimator.update(backLeftResult); + if (backLeftMultiTagResult.isPresent()) { + if (backLeftResult.getBestTarget().getPoseAmbiguity() < 0.3 + && backLeftResult.getBestTarget().fiducialId != 5 + && backLeftResult.getBestTarget().fiducialId != 4 + && backLeftResult.getBestTarget().fiducialId != 14 + && backLeftResult.getBestTarget().fiducialId != 15 + && backLeftResult.getBestTarget().fiducialId != 3 + && backLeftResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backLeftMultiTagResult.get().estimatedPose; + int numFrontTracks = backLeftResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backLeftResult.getBestTarget().getFiducialId()) + .get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backLeftResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backLeftResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backLeftResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + + var backRightResult = peripherals.getBackRightReefCamResult(); + Optional backRightMultiTagResult = backRightPhotonPoseEstimator.update(backRightResult); + if (backRightMultiTagResult.isPresent()) { + if (backRightResult.getBestTarget().getPoseAmbiguity() < 0.3 + && backRightResult.getBestTarget().fiducialId != 5 + && backRightResult.getBestTarget().fiducialId != 4 + && backRightResult.getBestTarget().fiducialId != 14 + && backRightResult.getBestTarget().fiducialId != 15 + && backRightResult.getBestTarget().fiducialId != 3 + && backRightResult.getBestTarget().fiducialId != 16) { + Pose3d robotPose = backRightMultiTagResult.get().estimatedPose; + int numFrontTracks = backRightResult.getTargets().size(); + Pose3d tagPose = aprilTagFieldLayout.getTagPose(backRightResult.getBestTarget().getFiducialId()) + .get(); + double distToTag = Constants.Vision.distBetweenPose(tagPose, robotPose); + // Logger.recordOutput("Distance to tag", distToTag); + if (distToTag < 3.2) { + if (inReefInteractionState()) { + standardDeviation.set(0, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + 0.5 + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + if (backRightResult.getBestTarget().getFiducialId() == getClosestTagId(getMt2Pose2d())) { + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backRightResult.getTimestampSeconds()); + } + } else { + standardDeviation.set(0, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(1, 0, + Constants.Vision.getNumTagStdDevScalar(numFrontTracks) + * Constants.Vision.getTagDistStdDevScalar(distToTag)); + // + Math.pow(dif, Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE) + // * Constants.Vision.ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR); + standardDeviation.set(2, 0, 0.9); + + mt2Odometry.addVisionMeasurement(robotPose.toPose2d(), + backRightResult.getTimestampSeconds()); + } + // Pose2d poseWithoutAngle = new Pose2d(robotPose.toPose2d().getTranslation(), + // new Rotation2d(Math.toRadians(peripherals.getPigeonAngle()))); + } + } + } + } + m_currentTime = Timer.getFPGATimestamp() - m_initTime; + } + + /** + * Retrieves the states of the modules (position and ground speed) of the + * robot's swerve drive system. + * + * @return An array containing the states of each wheel module, consisting of: + * front right module position in degrees, + * front right module ground speed in meters per second, + * front left module position in degrees, + * front left module ground speed in meters per second, + * back left module position in degrees, + * back left module ground speed in meters per second, + * back right module position in degrees, + * back right module ground speed in meters per second. + */ + public double[] getModuleStates() { + double[] states = { + frontLeft.getCanCoderPosition() * 360.0, frontLeft.getGroundSpeed(), + frontRight.getCanCoderPosition() * 360.0, frontRight.getGroundSpeed(), + backLeft.getCanCoderPosition() * 360.0, backLeft.getGroundSpeed(), + backRight.getCanCoderPosition() * 360.0, backRight.getGroundSpeed(), + }; + return states; + } + + /** + * Retrieves the setpoints of the modules (angle and drive motors) of the + * robot's swerve drive system. + * + * @return An array containing the setpoints of the angle and drive motors for + * each wheel module, in the order: + * front right angle motor, front right drive motor, + * front left angle motor, front left drive motor, + * back left angle motor, back left drive motor, + * back right angle motor, back right drive motor. + */ + public double[] getModuleSetpoints() { + double[] setpoints = { + frontLeft.getAngleMotorSetpoint() * 360, frontLeft.getDriveMotorSetpoint(), + frontRight.getAngleMotorSetpoint() * 360, frontRight.getDriveMotorSetpoint(), + backLeft.getAngleMotorSetpoint() * 360, backLeft.getDriveMotorSetpoint(), + backRight.getAngleMotorSetpoint() * 360, backRight.getDriveMotorSetpoint(), + }; + return setpoints; + } + + public double getRobotSpeed() { + return (Math.abs(frontLeft.getGroundSpeed()) + Math.abs(frontRight.getGroundSpeed()) + + Math.abs(backLeft.getGroundSpeed()) + + Math.abs(backRight.getGroundSpeed())) / 4.0; + } + + /** + * Retrieves the current velocity of the angle motors of the robot. + * + * @return An array containing the current velocity of the angle motors for + * front right, front left, back left, and back right wheels. + */ + public double[] getAngleMotorVelocity() { + double[] velocity = { + frontRight.getAngleVelocity(), frontLeft.getAngleVelocity(), backLeft.getAngleVelocity(), + backRight.getAngleVelocity() + }; + return velocity; + } + + public Pose2d getMT2Odometry() { + return mt2Odometry.getEstimatedPosition(); + } + + private boolean autoPlacingFront = true; + + public boolean getAutoPlacementSideIsFront() { + return autoPlacingFront; + } + + public double getAngleDifferenceDegrees(double angle1, double angle2) { + double difference = Math.abs(angle1 - angle2) % 360; + return difference > 180 ? 360 - difference : difference; + } + + public boolean isGoingForL3Algae() { + double thetaSetpoint = getAlgaeClosestSetpoint(getMT2Odometry()).getRotation().getRadians(); + if (autoPlacingFront) { + if (!isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 60.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 180.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 300.0) <= 30) { + return true; + } else { + return false; + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 0.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 120.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 240.0) <= 30) { + return true; + } else { + return false; + } + } + } else { + if (!isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 60.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 180.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 300.0) <= 30) { + return false; + } else { + return true; + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), 0.0) <= 30.0 || getAngleDifferenceDegrees( + Math.toDegrees(thetaSetpoint), 120.0) <= 30.0 + || getAngleDifferenceDegrees(Math.toDegrees(thetaSetpoint), + 240.0) <= 30) { + return false; + } else { + return true; + } + } + } + } + + public Pose2d getAlgaeMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositionsMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositionsMore.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositionsMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getAlgaeMoreMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(Math.toDegrees(currentOdometry.getRotation().getRadians())); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositionsMoreMore.get(i).getX()), 2) + // + Math.pow((y - + // Constants.Reef.redFrontPlacingPositionsMoreMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositionsMoreMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositionsMoreMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i).getRotation() + .getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositionsMoreMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositionsMoreMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getAlgaeClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (!isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.algaeRedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - Constants.Reef.algaeRedFrontPlacingPositions.get(i).getX(), + y - Constants.Reef.algaeRedFrontPlacingPositions.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeRedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeRedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeRedBackPlacingPositions.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.algaeBlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getX(), + y - Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getY()); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.algaeBlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.algaeBlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.algaeBlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getReefClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositions.get(i).getX() + + Constants.Reef.redBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositions.get(i).getY() + + Constants.Reef.redBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.redFrontPlacingPositions.get(i).getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.redFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.redFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.redBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.redBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.redFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.redBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositions.get(i).getX() + + Constants.Reef.blueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositions.get(i).getY() + + Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.blueFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.blueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.blueFrontPlacingPositions.get(i) + .getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.blueBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.blueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.blueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getL1ReefClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redL1FrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.redL1BackPlacingPositions.get(i).getX() + + Constants.Reef.redL1BackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redL1BackPlacingPositions.get(i).getY() + + Constants.Reef.redL1BackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redL1FrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redL1FrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redL1BackPlacingPositions.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueL1FrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueL1BackPlacingPositions.get(i).getX() + + Constants.Reef.blueL1BackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueL1BackPlacingPositions.get(i).getY() + + Constants.Reef.blueL1BackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueL1FrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueL1FrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueL1BackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getL1ReefClosestSetpointMore(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redL1FrontPlacingPositionsMore.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getY()), + // 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redL1BackPlacingPositionsMore.get(i).getX() + + Constants.Reef.redL1BackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redL1BackPlacingPositionsMore.get(i).getY() + + Constants.Reef.redL1BackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redL1FrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redL1FrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redL1BackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueL1FrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueL1BackPlacingPositionsMore.get(i).getX() + + Constants.Reef.blueL1BackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueL1BackPlacingPositionsMore.get(i).getY() + + Constants.Reef.blueL1BackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueL1FrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueL1FrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueL1BackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L2 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefMoreClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositionsMore.get(i).getX() + + Constants.Reef.redBackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositionsMore.get(i).getY() + + Constants.Reef.redBackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.redFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.redFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.redBackPlacingPositionsMore.get(i); + } + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositionsMore.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositionsMore.get(i).getX() + + Constants.Reef.blueBackPlacingPositionsMore + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositionsMore.get(i).getY() + + Constants.Reef.blueBackPlacingPositionsMore.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositionsMore.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositionsMore.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.blueBackPlacingPositionsMore.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 2.0) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public Pose2d getReefL44ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l4RedFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l4RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l4BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } else if (notClosest + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX()) < 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getY()) < 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueBackPlacingPositions.get(i).getX()) < 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) < 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L4 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefL3ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l3RedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l3RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l3BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((Math.hypot( + origionalSetpointPose.getX() - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getX(), + origionalSetpointPose.getY() - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getY()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs( + origionalSetpointPose.getX() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L3 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public Pose2d getReefL33ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l3RedFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l3RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l3BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l3BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l3BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } else if (notClosest + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i).getX()) < 0.01 + || Math + .abs( + origionalSetpointPose.getY() + - Constants.Reef.l3BlueFrontPlacingPositions.get(i) + .getY()) < 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l3BlueBackPlacingPositions.get(i).getX()) < 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l3BlueBackPlacingPositions.get(i) + .getY()) < 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l3BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l3BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l3BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l3BlueBackPlacingPositions.get(i); + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L3 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + public void stop() { + frontLeftAngleMotor.stopMotor(); + frontRightAngleMotor.stopMotor(); + backLeftAngleMotor.stopMotor(); + backRightAngleMotor.stopMotor(); + frontLeftDriveMotor.stopMotor(); + frontRightDriveMotor.stopMotor(); + backLeftDriveMotor.stopMotor(); + } + + public Pose2d getReefL4ClosestSetpoint(Pose2d currentOdometry /* {x, y, thetaRadians} */, boolean notClosest) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.l4RedFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4RedBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4RedFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((origionalSetpointPose.getTranslation() + .getDistance(Constants.Reef.l4RedFrontPlacingPositions.get(i) + .getTranslation()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4RedBackPlacingPositions.get(i).getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4RedBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4RedBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4RedFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4RedFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4RedBackPlacingPositions.get(i); + } + } + } + } + } else { + for (int i = 0; i < Constants.Reef.l4BlueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX() + + Constants.Reef.l4BlueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY() + + Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist && !notClosest) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.blueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } else if (notClosest) { + if ((Math.hypot( + origionalSetpointPose.getX() - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getX(), + origionalSetpointPose.getY() - Constants.Reef.l4BlueFrontPlacingPositions.get(i) + .getY()) < 0.9) + && ((Math + .abs(origionalSetpointPose.getX() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getX()) > 0.01 + || Math + .abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueFrontPlacingPositions.get(i).getY()) > 0.01) + && (Math + .abs( + origionalSetpointPose.getX() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getX()) > 0.01 + || Math.abs(origionalSetpointPose.getY() + - Constants.Reef.l4BlueBackPlacingPositions.get(i) + .getY()) > 0.01)) + && (Math.abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueFrontPlacingPositions + .get(i).getRotation().getRadians()) < 0.01 + || Math + .abs(origionalSetpointPose.getRotation().getRadians() + - Constants.Reef.l4BlueBackPlacingPositions + .get(i).getRotation().getRadians()) < 0.01)) { + dist = currentDist; + if (getAngleDifferenceDegrees(theta, + Constants.Reef.l4BlueFrontPlacingPositions.get(i).getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + chosenSetpoint = Constants.Reef.l4BlueFrontPlacingPositions.get(i); + } else { + autoPlacingFront = false; + chosenSetpoint = Constants.Reef.l4BlueBackPlacingPositions.get(i); + } + } + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + Logger.recordOutput("L4 target pose", chosenSetpoint); + return chosenSetpoint; + } + } + + boolean firstTimeAutoPickup = false; + double firstTimePickupAngle = 0.0; + boolean firstTimeCalculated = false; + boolean firstTimeGoingInCalculated = false; + boolean firstTimeGoingIn = false; + boolean hasTrack = false; + + Pose2d targetPose = new Pose2d(); + Pose2d c1 = new Pose2d(); + Pose2d c2 = new Pose2d(); + + public Pose2d getGamePiecePosition() { + if (!firstTimeAutoPickup) { + firstTimePickupAngle = getMT2OdometryAngle(); + firstTimeAutoPickup = true; + } + + double yaw = 0.0; + double pitch = 0.0; + var result = peripherals.getFrontGamePieceCamResult(); + if (result.hasTargets()) { + List tracks = result.getTargets(); + for (int i = 0; i < tracks.size(); i++) { + int id = tracks.get(i).getDetectedObjectClassID(); + if (id == 0) { + tracks.remove(i); + i--; + } + } + if (tracks.isEmpty()) { + return new Pose2d(); + } else { + double minPitch = tracks.get(0).getPitch(); + int index = 0; + for (int i = 1; i < tracks.size(); i++) { + int id = tracks.get(0).getDetectedObjectClassID(); + java.util.logging.Logger.getGlobal().finer("id: " + id); + if (tracks.get(i).getPitch() < minPitch) { + minPitch = tracks.get(i).getPitch(); + index = i; + } + } + pitch = minPitch; + yaw = tracks.get(index).getYaw(); + } + } + // double yFromIntake = 0.0; + + if (yaw != 0.0 && pitch != 0.0) { + java.util.logging.Logger.getGlobal().finer("calculating%"); + double cameraYaw = 15.0; + double limelightXOffset = Constants.inchesToMeters(2.25); + double limelightYOffset = Constants.inchesToMeters(-11.5); + double intakeXOffset = Constants.inchesToMeters(20.5); + double intakeYOffset = Constants.inchesToMeters(3.0); // 4.0 + + double robotX = getMT2OdometryX(); + double robotY = getMT2OdometryY(); + // double robotAngle = getMT2OdometryAngle(); + double robotAngle = firstTimePickupAngle; + Pose2d robotPose = new Pose2d(robotX, robotY, new Rotation2d(robotAngle)); + + // double targetDistance = Math + // .abs((limelightHeight - gamePieceHeight) / + // Math.tan(Math.toRadians(-limelightAngle + pitch))); + double targetDistance = Constants.inchesToMeters(Constants.Vision.getCoralDistanceFromPitch(pitch)); + Logger.recordOutput("Distance to Coral", targetDistance); + double noteY = -(targetDistance * Math.sin(Math.toRadians(-cameraYaw + yaw))); + double noteX = ((targetDistance * Math.cos(Math.toRadians(cameraYaw - yaw)))); + Logger.recordOutput("coral x", noteX); + Logger.recordOutput("coral y", noteY); + double xFromRobot = noteX + limelightXOffset; + double yFromRobot = noteY + limelightYOffset; + Pose2d coralPose = robotPose.transformBy(new Transform2d(xFromRobot, yFromRobot, new Rotation2d())); + + Logger.recordOutput("coral rc x", xFromRobot); + Logger.recordOutput("coral rc y", yFromRobot); + Logger.recordOutput("coral pose", coralPose); + + double xFromIntake = xFromRobot - intakeXOffset; + double yFromIntake = yFromRobot - intakeYOffset; + if (!firstTimeCalculated) { + hasTrack = true; + targetPose = robotPose.transformBy(new Transform2d(xFromIntake, + yFromIntake, new Rotation2d())); + Logger.recordOutput("coral intake x", xFromIntake); + Logger.recordOutput("coral intake y", yFromIntake); + double projectedTx = Math.atan2(Math.tan(Math.toRadians(yaw)), Math.cos(Math.toRadians(pitch))); + java.util.logging.Logger.getGlobal().finer("Projected: " + Math.toDegrees(projectedTx)); + java.util.logging.Logger.getGlobal().finer("actual: " + yaw); + Pose2d intakeFieldPose = robotPose + .transformBy(new Transform2d(intakeXOffset, intakeYOffset, new Rotation2d())); + double angleToPiece = Constants.standardizeAngleDegrees(Math.toDegrees(Math.atan2( + intakeFieldPose.getY() - coralPose.getY(), intakeFieldPose.getX() - coralPose.getX()) + - Math.PI)); + Logger.recordOutput("Angle to Piece", (angleToPiece)); + Logger.recordOutput("Intake field pose", intakeFieldPose); + + double deltaX = coralPose.getX() - intakeFieldPose.getX(); + double deltaY = coralPose.getY() - intakeFieldPose.getY(); + double targetAngle = Math.atan2(deltaY, deltaX); + Pose2d poseFromRobotFront = new Pose2d( + Constants.metersToInches(xFromRobot - Constants.inchesToMeters(16.5)), + Constants.metersToInches(yFromRobot), new Rotation2d()); + Logger.recordOutput("Pose from robot front", poseFromRobotFront); + + // Adjust for the robot's current heading and camera yaw + double requiredTurn = targetAngle - Math.toRadians(cameraYaw); + + // Normalize to range [-π, π] for minimal turning + requiredTurn = (requiredTurn + (2 * Math.PI)) % (2 * Math.PI); + // driveToPoint(targetPose.getX(), targetPose.getY(), + Logger.recordOutput("new angle", Math.toDegrees(requiredTurn)); + // Pose2d targetPose = robotPose + // .transformBy(new Transform2d(xFromIntake, yFromIntake, new + // Rotation2d(requiredTurn))); + Logger.recordOutput("target pickup", targetPose); + + double mx = (robotX + targetPose.getX()) / 2; + double my = (robotY + targetPose.getY()) / 2; + + // Compute the length of AB (hypotenuse) + double abLength = Math + .sqrt(Math.pow(robotX - targetPose.getX(), 2) + Math.pow(robotY - targetPose.getY(), 2)); + + // The expected right triangle height from midpoint to C + double d = Math.sqrt(xFromIntake * xFromIntake + yFromIntake * yFromIntake - (abLength * abLength) / 2); + + // Compute the perpendicular direction (normalized) + double perpX = -(robotY - targetPose.getY()) / abLength; + double perpY = (robotX - targetPose.getX()) / abLength; + + // Compute the two possible C points + double c1X = mx + d * perpX; + double c1Y = my + d * perpY; + + double c2X = mx - d * perpX; + double c2Y = my - d * perpY; + c1 = new Pose2d(c1X, c1Y, new Rotation2d(robotAngle)); + c2 = new Pose2d(c2X, c2Y, new Rotation2d(robotAngle)); + Logger.recordOutput("c1x", c1); + Logger.recordOutput("c1y", c2); + firstTimeCalculated = true; + java.util.logging.Logger.getGlobal().finer("Running once"); + } + // targetPose.getRotation().getRadians()); + // if (){ + + // } else + // if (Math.abs(yFromIntake) < 0.2) { + java.util.logging.Logger.getGlobal().finer("going in"); + // if (!firstTimeGoingInCalculated) { + // firstTimeCalculated = false; + // } + firstTimeGoingIn = true; + return targetPose; + // } else if (yFromIntake < 0.0 && !firstTimeGoingIn) { + // System.out.println("to the left"); + // return c1; + // } else { + // System.out.println("to the right"); + // return c2; + // } + // double pieceXFromIntake = noteX - intakeXOffset; + // double pieceYFromIntake = noteY - intakeYOffset; + } else + + { + // System.out.println("default game piece"); + return new Pose2d(); + } + } + + public void goToCoral() { + double yaw = 0.0; + double pitch = 0.0; + var result = peripherals.getFrontGamePieceCamResult(); + List tracks = new ArrayList<>(result.getTargets()); + List coralTargets = new ArrayList<>(); + for (PhotonTrackedTarget track : tracks) { + if (track.objDetectId == 1) { + coralTargets.add(track); + } + } + + if (result.hasTargets() && !coralTargets.isEmpty()) { + PhotonTrackedTarget bestTrack = coralTargets.get(0); + yaw = bestTrack.getYaw(); + pitch = bestTrack.getPitch(); + } + + if (yaw != 0.0 && pitch != 0.0) { + double currentAngle = -yaw; + java.util.logging.Logger.getGlobal().finer("currentAngle: " + currentAngle); + double wantedAngleInFrame = 14.7; + double cameraToRobotAngleOffset = 35.0; + rotatePID.setSetPoint(wantedAngleInFrame); + rotatePID.updatePID(currentAngle); + double r = -rotatePID.getResult(); + double driveAngleDeg = wantedAngleInFrame + cameraToRobotAngleOffset; + double wantedSpeedMPS = 1.0; + Vector v = new Vector(Math.cos(Math.toRadians(driveAngleDeg)), Math.sin(Math.toRadians(driveAngleDeg))) + .scaled(wantedSpeedMPS); + autoRobotCentricDrive(v, r); + } + } + + public Pose2d getMt2Pose2d() { + return mt2Odometry.getEstimatedPosition(); + } + + /** + * Retrieves the current X-coordinate of the robot from odometry. + * + * @return The current X-coordinate of the robot. + */ + public double getMT2OdometryX() { + return mt2Odometry.getEstimatedPosition().getX(); + } + + /** + * Retrieves the current Y-coordinate of the robot from odometry. + * + * @return The current Y-coordinate of the robot. + */ + public double getMT2OdometryY() { + return mt2Odometry.getEstimatedPosition().getY(); + } + + /** + * Retrieves the current orientation angle of the robot from odometry. + * + * @return The current orientation angle of the robot in radians. + */ + public double getMT2OdometryAngle() { + return mt2Odometry.getEstimatedPosition().getRotation().getRadians(); + } + + /** + * Drives the robot with alignment adjustment based on the specified angle from + * placement. + * + * @param degreesFromPlacement The angle in degrees from the placement + * orientation to align with. + */ + public void driveAutoAligned(double degreesFromPlacement) { + + double turn = degreesFromPlacement; + + double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double originalY = -(Math.copySign(OI.getDriverLeftX() * OI.getDriverLeftX(), OI.getDriverLeftX())); + + if (Math.abs(originalX) < 0.05) { + originalX = 0; + } + if (Math.abs(originalY) < 0.05) { + originalY = 0; + } + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + } + + /** + * Turns the robot in robot-centric mode. + * + * @param turn The rate at which the robot should turn in radians per second. + */ + public void autoRobotCentricTurn(double turn) { + frontLeft.drive(new Vector(0, 0), turn, 0.0); + frontRight.drive(new Vector(0, 0), turn, 0.0); + backLeft.drive(new Vector(0, 0), turn, 0.0); + backRight.drive(new Vector(0, 0), turn, 0.0); + } + + /** + * Drives the robot in robot-centric mode using velocity vector and turning + * rate. + * + * @param velocityVector The velocity vector containing x and y velocities in + * meters per second (m/s). + * @param turnRadiansPerSec The rate at which the robot should spin in radians + * per second. + */ + public void autoRobotCentricDrive(Vector velocityVector, double turnRadiansPerSec) { + frontLeft.drive(velocityVector, turnRadiansPerSec, 0); + frontRight.drive(velocityVector, turnRadiansPerSec, 0); + backLeft.drive(velocityVector, turnRadiansPerSec, 0); + backRight.drive(velocityVector, turnRadiansPerSec, 0); + } + + /** + * Drives the robot during teleoperation. + * + * @apiNote This method updates the fused odometry array and controls the + * robot's movement based on joystick inputs. + */ + public void teleopDrive() { + double oiRX = OI.getDriverRightX(); + double oiLX = OI.getDriverLeftX(); + double oiRY = OI.getDriverRightY(); + double oiLY = OI.getDriverLeftY(); + if (OI.operatorLT.getAsBoolean() && OI.operatorRT.getAsBoolean()) { + oiRX = OI.getOperatorRightX(); + oiLX = OI.getOperatorLeftX(); + oiRY = OI.getOperatorRightY(); + oiLY = OI.getOperatorLeftY(); + } + + double turnLimit = 0.17; + + if (OI.driverController.getRightTriggerAxis() > 0.2 || OI.getDriverRB()) { + // activate slowy spin + turnLimit = 0.1; + oiRX = oiRX * 0.8; + oiLX = oiLX * 0.8; + oiRY = oiRY * 0.8; + oiLY = oiLY * 0.8; + } + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + double turn = turnLimit + * (oiRX * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.05) { + turn = 0.0; + } + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + // } + } + + public void robotCentricDrive(double angle) { + double oiRX = OI.getDriverRightX(); + double oiLX = OI.getDriverLeftX(); + double oiRY = OI.getDriverRightY(); + double oiLY = OI.getDriverLeftY(); + + // Logger.recordOutput("Adjusted Right X", oiRX); + // Logger.recordOutput("Adjusted Left X", oiLX); + // Logger.recordOutput("Adjusted Right Y", oiRY); + // Logger.recordOutput("Adjusted Left Y", oiLY); + + double turnLimit = 0.17; + // 0.35 before + + if (OI.driverController.getRightTriggerAxis() > 0.2) { + // activate slowy spin + turnLimit = 0.1; + oiRX = oiRX * 0.5; + oiLX = oiLX * 0.5; + oiRY = oiRY * 0.5; + oiLY = oiLY * 0.5; + } + + // this is correct, X is forward in field, so originalX should be the y on the + // // joystick + // double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), + // OI.getDriverLeftY())); + // double originalY = -(Math.copySign(OI.getDriverLeftX() * OI.getDriverLeftX(), + // OI.getDriverLeftX())); + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + // if (Math.abs(originalX) < 0.005) { + // originalX = 0; + // } + // if (Math.abs(originalY) < 0.005) { + // originalY = 0; + // } + + // double turn = turnLimit * ((Math.copySign(OI.getDriverRightX() * + // OI.getDriverRightX() * OI.getDriverRightX(), OI.getDriverRightX())) * + // (Constants.Physical.TOP_SPEED)/(Constants.Physical.ROBOT_RADIUS)); + double turn = turnLimit + * (oiRX * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.05) { + turn = 0.0; + } + + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, Math.toRadians(angle)); + frontRight.drive(controllerVector, turn, Math.toRadians(angle)); + backLeft.drive(controllerVector, turn, Math.toRadians(angle)); + backRight.drive(controllerVector, turn, Math.toRadians(angle)); + // } + } + + public void teleopDriveToPiece(double yToPiece) { + double turnLimit = 0.17; + double kP = 0.8; + // 0.35 before + + // if (OI.driverController.getLeftBumper()) { + // // activate speedy spin + // // turnLimit = 1; //TODO: find a different keybind for this + // } + + // this is correct, X is forward in field, so originalX should be the y on the + // joystick + double originalX = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double originalY = yToPiece * kP; + + if (Math.abs(originalX) < 0.075) { + originalX = 0; + } + + double turn = turnLimit + * (OI.getDriverRightX() * (Constants.Physical.TOP_SPEED) / (Constants.Physical.ROBOT_RADIUS)); + + if (Math.abs(turn) < 0.15) { + turn = 0.0; + } + + if (turn == 0.0) { + turningPID.setSetPoint(angleSetpoint); + double yaw = peripherals.getPigeonAngle(); + + yaw = Constants.standardizeAngleToOtherDegrees(yaw, angleSetpoint); + double result = -2 * turningPID.updatePID(yaw); + // Logger.recordOutput("result", result); + + double x = -(Math.copySign(OI.getDriverLeftY() * OI.getDriverLeftY(), OI.getDriverLeftY())); + double y = yToPiece * kP; + + if (Math.abs(originalX) < 0.05) { + originalX = 0; + } + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(x, y); + double yPower = getAdjustedY(x, y); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, result, pigeonAngle); + frontRight.drive(controllerVector, result, pigeonAngle); + backLeft.drive(controllerVector, result, pigeonAngle); + backRight.drive(controllerVector, result, pigeonAngle); + } else { + angleSetpoint = peripherals.getPigeonAngle(); + double compensation = peripherals.getPigeonAngularVelocityW() * 0.050; + angleSetpoint += compensation; + // Logger.recordOutput("setpoint", angleSetpoint); + turningPID.setSetPoint(angleSetpoint); + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-xSpeed); + controllerVector.setJ(-ySpeed); + } + frontLeft.drive(controllerVector, turn, pigeonAngle); + frontRight.drive(controllerVector, turn, pigeonAngle); + backLeft.drive(controllerVector, turn, pigeonAngle); + backRight.drive(controllerVector, turn, pigeonAngle); + } + } + + private int hitNumber = 0; + private int hitNumberSemiGenerous = 0; + private int hitNumberGenerous = 0; + private int hitNumberUltraGenerous = 0; + + public boolean hitSetPoint(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.045 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 1.5) { + hitNumber += 1; + } else { + hitNumber = 0; + } + if (hitNumber > 1) { + return true; + } else { + return false; + } + } + + public boolean hitSetPointSemiGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + Logger.recordOutput("Error for semi-generous", Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2))); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.05 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 2) { + hitNumberSemiGenerous += 1; + } else { + hitNumberSemiGenerous = 0; + } + if (hitNumberSemiGenerous > 3) { + return true; + } else { + return false; + } + } + + public Pose2d getReefClosestSetpointFrontOnly(Pose2d currentOdometry /* {x, y, thetaRadians} */) { + double x = currentOdometry.getX(); + double y = currentOdometry.getY(); + double theta = Constants.standardizeAngleDegrees(currentOdometry.getRotation().getDegrees()); + double dist = 100.0; + double currentDist = 100.0; + Pose2d chosenSetpoint = new Pose2d(x, y, new Rotation2d(Math.toRadians(theta))); + if (getFieldSide() == "red") { + for (int i = 0; i < Constants.Reef.redFrontPlacingPositions.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + x - (Constants.Reef.redFrontPlacingPositions.get(i).getX() + + Constants.Reef.redBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.redFrontPlacingPositions.get(i).getY() + + Constants.Reef.redBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint = Constants.Reef.redFrontPlacingPositions.get(i); + } + } + } else { + for (int i = 0; i < Constants.Reef.blueFrontPlacingPositions.size(); i++) { + currentDist = Math.hypot( + x - (Constants.Reef.blueFrontPlacingPositions.get(i).getX() + + Constants.Reef.blueBackPlacingPositions + .get(i) + .getX()) + / 2, + y - (Constants.Reef.blueFrontPlacingPositions.get(i).getY() + + Constants.Reef.blueBackPlacingPositions.get(i) + .getY()) + / 2); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint = Constants.Reef.blueFrontPlacingPositions.get(i); + } + } + } + if (chosenSetpoint.getTranslation().getDistance(currentOdometry.getTranslation()) > 5) { + return getMT2Odometry(); + } else { + return chosenSetpoint; + } + } + + public boolean hitSetPointGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.10 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 2.5) { + hitNumberGenerous += 1; + } else { + hitNumberGenerous = 0; + } + if (hitNumberGenerous > 3) { + return true; + } else { + return false; + } + } + + public boolean hitSetPointUltraGenerous(Pose2d pose) { // adjust for l4 TODO: + // Logger.recordOutput("Error for setpoint", + // Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2))); + // System.out.println("X Y error: " + // + Math.sqrt(Math.pow((x - getMT2OdometryX()), 2) + // + Math.pow((y - getMT2OdometryY()), 2)) + // + " Angle error: " + getAngleDifferenceDegrees(Math.toDegrees(theta), + // Math.toDegrees(getMT2OdometryAngle())) + // + " Hits: " + // + hitNumber); + double x = pose.getX(); + double y = pose.getY(); + double theta = pose.getRotation().getRadians(); + if (Math + .sqrt(Math.pow((x - getMT2OdometryX()), 2) + + Math.pow((y - getMT2OdometryY()), 2)) < 0.10 + && getAngleDifferenceDegrees(Math.toDegrees(theta), + Math.toDegrees(getMT2OdometryAngle())) < 10.0) { + hitNumberUltraGenerous += 1; + } else { + hitNumberUltraGenerous = 0; + } + if (hitNumberUltraGenerous > 2) { + return true; + } else { + return false; + } + } + + public void driveToPoint(Pose2d targetPoint) { + Logger.recordOutput("Goal X, Y, Theta", targetPoint); + // Logger.recordOutput("Magnitude Error Inches", + // Constants.metersToInches(Math.sqrt(Math.pow(x - getMT2OdometryX(), 2) + + // Math.pow(y - getMT2OdometryY(), 2)))); + // Logger.recordOutput("Theta Error Degrees", Math.toDegrees(theta - + // getMT2OdometryAngle())); + double x = targetPoint.getX(); + double y = targetPoint.getY(); + double theta = targetPoint.getRotation().getRadians(); + theta = Constants.standardizeAngleToOther(theta, getMT2OdometryAngle()); + + double xVelNoFF = 0.0; + double yVelNoFF = 0.0; + double thetaVelNoFF = 0.0; + + if (DriverStation.isAutonomousEnabled() && systemState.equals(DriveState.L4_REEF)) { + xxPID4A.setSetPoint(x); + yyPID4A.setSetPoint(y); + thetaaPID4A.setSetPoint(theta); + + xxPID4A.updatePID(getMT2OdometryX()); + yyPID4A.updatePID(getMT2OdometryY()); + thetaaPID4A.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID4A.getResult(); + yVelNoFF = yyPID4A.getResult(); + // double velmag = Math.hypot(xVelNoFF, yVelNoFF); + // double maxvel = 0.1; + // if (velmag > maxvel) { + // xVelNoFF = xVelNoFF * maxvel / velmag; + // yVelNoFF = yVelNoFF * maxvel / velmag; + // } + thetaVelNoFF = -thetaaPID4A.getResult(); + } else if (OI.driverPOVRight.getAsBoolean()) { + xxPID4.setSetPoint(x); + yyPID4.setSetPoint(y); + thetaaPID4.setSetPoint(theta); + + xxPID4.updatePID(getMT2OdometryX()); + yyPID4.updatePID(getMT2OdometryY()); + thetaaPID4.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID4.getResult(); + yVelNoFF = yyPID4.getResult(); + thetaVelNoFF = -thetaaPID4.getResult(); + + } else if (DriverStation.isTeleopEnabled() + && (OI.driverPOVLeft.getAsBoolean() || OI.driverPOVDown.getAsBoolean())) { + + xxPID23.setSetPoint(x); + yyPID23.setSetPoint(y); + thetaaPID23.setSetPoint(theta); + + xxPID23.updatePID(getMT2OdometryX()); + yyPID23.updatePID(getMT2OdometryY()); + thetaaPID23.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID23.getResult(); + yVelNoFF = yyPID23.getResult(); + thetaVelNoFF = -thetaaPID23.getResult(); + + } else if (DriverStation.isTeleopEnabled() && OI.driverPOVUp.getAsBoolean()) { + + xxPID1.setSetPoint(x); + yyPID1.setSetPoint(y); + thetaaPID1.setSetPoint(theta); + + xxPID1.updatePID(getMT2OdometryX()); + yyPID1.updatePID(getMT2OdometryY()); + thetaaPID1.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID1.getResult(); + yVelNoFF = yyPID1.getResult(); + thetaVelNoFF = -thetaaPID1.getResult(); + + } else if (systemState.equals(DriveState.PIECE_PICKUP)) { + + xxPIDPickup.setSetPoint(x); + yyPIDPickup.setSetPoint(y); + thetaaPIDPickup.setSetPoint(theta); + + xxPIDPickup.updatePID(getMT2OdometryX()); + yyPIDPickup.updatePID(getMT2OdometryY()); + thetaaPIDPickup.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPIDPickup.getResult(); + yVelNoFF = yyPIDPickup.getResult(); + thetaVelNoFF = -thetaaPIDPickup.getResult(); + + } else { + + xxPID.setSetPoint(x); + yyPID.setSetPoint(y); + thetaaPID.setSetPoint(theta); + + xxPID.updatePID(getMT2OdometryX()); + yyPID.updatePID(getMT2OdometryY()); + thetaaPID.updatePID(getMT2OdometryAngle()); + + xVelNoFF = xxPID.getResult(); + yVelNoFF = yyPID.getResult(); + thetaVelNoFF = -thetaaPID.getResult(); + } + + // double feedForwardX = targetPoint.getDouble("x_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + // double feedForwardY = targetPoint.getDouble("y_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + // double feedForwardTheta = -targetPoint.getDouble("angular_velocity") * + // Constants.Autonomous.FEED_FORWARD_MULTIPLIER; + + double finalX = xVelNoFF; + double finalY = yVelNoFF; + double finalTheta = thetaVelNoFF; + // if (m_fieldSide == "blue") { + // finalX = -finalX; + // finalTheta = -finalTheta; + // } + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + }; + + Vector velocityVector = new Vector(); + double desiredThetaChange = 0; + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(velocityArray[1].doubleValue()); + desiredThetaChange = velocityArray[2].doubleValue(); + + autoDrive(velocityVector, desiredThetaChange); + + } + + public void orbitDrive(Pose2d end, Pose2d pre, Pose2d current) { + // end: C, pre: B, current: A + Translation2d p = end.minus(pre).getTranslation(); // a vector in direction BC + Translation2d d = pre.minus(current).getTranslation(); // a vector in direction AB + Rotation2d alpha = p.getAngle().minus(d.getAngle()); // find angle between BC and AB + Rotation2d direction = d.getAngle().minus(alpha); + double velocity = getSpeedUsingPhysics(end.minus(current).getTranslation().getNorm(), 0); + double xVel = velocity * Math.cos(direction.getRadians()); + double yVel = velocity * Math.sin(direction.getRadians()); + Vector velocityVector = new Vector(xVel, yVel); + thetaaPID4.setSetPoint(end.getRotation().getRadians()); + double desiredThetaChange = thetaaPID4.getResult(); // TODO: Test if this PID is good. + autoDrive(velocityVector, desiredThetaChange); + } + + public double getSpeedUsingPhysics(double distance, double finalVel) { + // Max Velocity at which the robot can slow down given the Max Deceleration + // (-Constants.Physical.MAX_ACCELERATION) + return Math.sqrt(finalVel * finalVel - (2 * (-Constants.Physical.MAX_ACCELERATION) * distance)); + } + + public double clampToForwardAccelerationLimit(double currentVelocity, double wantedAcceleration) { + return Math.min(wantedAcceleration, + Constants.Physical.MAX_ACCELERATION * (1 - (currentVelocity / Constants.Physical.TOP_SPEED))); + } + + public void driveToXTheta(double x, double theta) { + java.util.logging.Logger.getGlobal().finer(theta + ""); + // theta = Math.toRadians(theta); + theta = Constants.standardizeAngleToOther(theta, getMT2OdometryAngle()); + xxPID.setSetPoint(x); + thetaaPID.setSetPoint(theta); + + xxPID.updatePID(getMT2OdometryX()); + thetaaPID.updatePID(getMT2OdometryAngle()); + + double xVelNoFF = xxPID.getResult(); + double yVelNoFF = OI.getDriverLeftX() * 2.9; + double thetaVelNoFF = -thetaaPID.getResult(); + double finalX = xVelNoFF; + double finalY = yVelNoFF; + double finalTheta = thetaVelNoFF; + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + }; + + Vector velocityVector = new Vector(); + double desiredThetaChange = 0; + if (getFieldSide().equals("red")) { + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(-velocityArray[1].doubleValue()); + } else { + velocityVector.setI(velocityArray[0].doubleValue()); + velocityVector.setJ(velocityArray[1].doubleValue()); + } + desiredThetaChange = velocityArray[2].doubleValue(); + + autoDrive(velocityVector, desiredThetaChange); + } + + public void driveOnLine(Vector lineVector, Translation2d pointOnLine, double angrad) { + Logger.recordOutput("Point On Line", new Pose2d(pointOnLine, new Rotation2d())); + Logger.recordOutput("LineVector", + new Pose2d(pointOnLine.plus(new Translation2d(lineVector.getI(), lineVector.getJ())), + new Rotation2d())); + double oiLY = OI.getDriverLeftY(); + double oiLX = OI.getDriverLeftX(); + double originalX = -(Math.copySign(oiLY * oiLY, oiLY)); + double originalY = -(Math.copySign(oiLX * oiLX, oiLX)); + double xPower = getAdjustedX(originalX, originalY); + double yPower = getAdjustedY(originalX, originalY); + + double xSpeed = xPower * Constants.Physical.TOP_SPEED; + double ySpeed = yPower * Constants.Physical.TOP_SPEED; + + Vector controllerVector = new Vector(xSpeed, ySpeed); + if (getFieldSide().equals("red")) { + controllerVector.setI(-ySpeed); + controllerVector.setJ(xSpeed); + } + Vector projectedJoystick = lineVector.projectOther(controllerVector); + Logger.recordOutput("controllerProjection", + new Pose2d(new Translation2d(projectedJoystick.getI() + getMT2OdometryX(), + projectedJoystick.getJ() + getMT2OdometryY()), new Rotation2d(angrad))); + Translation2d currentPoint = new Translation2d(getMT2OdometryX(), getMT2OdometryY()); + Translation2d closestPointOnLine = lineVector.getClosestPointOnLine(pointOnLine, currentPoint); + Logger.recordOutput("closestPointOnLine", new Pose2d(closestPointOnLine, new Rotation2d(angrad))); + xxPID.setSetPoint(closestPointOnLine.getX()); + yyPID.setSetPoint(closestPointOnLine.getY()); + angrad = Constants.standardizeAngleToOther(angrad, getMT2OdometryAngle()); + thetaaPID.setSetPoint(angrad); + double toPointXVel = xxPID.updatePID(getMT2OdometryX()); + double toPointYVel = -yyPID.updatePID(getMT2OdometryY()); + double thetaVel = -thetaaPID.updatePID(getMT2OdometryAngle()); + Logger.recordOutput("feederAngle", Math.toDegrees(angrad)); + Logger.recordOutput("thetaVel", thetaVel); + Logger.recordOutput("toPointYVel", toPointYVel); + Logger.recordOutput("toPointXVel", toPointXVel); + Vector toPointVector = new Vector(toPointXVel, toPointYVel); + Vector driveVector = toPointVector.add(projectedJoystick); + Logger.recordOutput("driveVector", new Pose2d(new Translation2d(driveVector.getI() + getMT2OdometryX(), + driveVector.getJ() + getMT2OdometryY()), new Rotation2d(angrad))); + autoDrive(driveVector, thetaVel); + } + + public void driveToTheta(double theta) { + theta = Constants.standardizeAngleToOtherDegrees(theta, getMT2OdometryAngle()); + + // Logger.recordOutput("Drive Angle Setpoint", theta); + turningPID.setSetPoint(theta); + turningPID.updatePID(Math.toDegrees(getMT2OdometryAngle())); + + double result = -turningPID.getResult(); + if (Math.abs(Math.toDegrees(getMT2OdometryAngle()) - theta) < 2) { + result = 0; + } + driveAutoAligned(result); + } + + /** + * Runs autonomous driving by providing velocity vector and turning rate. + * + * @param vector The velocity vector containing xy velocities. + * @param turnRadiansPerSec The rate at which the robot should spin in radians + * per second. + */ + public void autoDrive(Vector vector, double turnRadiansPerSec) { + + double pigeonAngle = Math.toRadians(peripherals.getPigeonAngle()); + + frontLeft.drive(vector, turnRadiansPerSec, pigeonAngle); + frontRight.drive(vector, turnRadiansPerSec, pigeonAngle); + backLeft.drive(vector, turnRadiansPerSec, pigeonAngle); + backRight.drive(vector, turnRadiansPerSec, pigeonAngle); + } + + /** + * Retrieves the current velocity vector of the robot in field coordinates. + * The velocity vector is calculated based on the individual wheel speeds and + * orientations. + * + * @return The current velocity vector of the robot in meters per second (m/s). + */ + public Vector getRobotVelocityVector() { + Vector velocityVector = new Vector(0, 0); + double pigeonAngleRadians = Math.toRadians(this.peripherals.getPigeonAngle()); + + double frV = this.frontRight.getGroundSpeed(); + double frTheta = this.frontRight.getWheelPosition() + pigeonAngleRadians; + double frVX = frV * Math.cos(frTheta); + double frVY = frV * Math.sin(frTheta); + double flV = this.frontLeft.getGroundSpeed(); + double flTheta = this.frontLeft.getWheelPosition() + pigeonAngleRadians; + double flVX = flV * Math.cos(flTheta); + double flVY = flV * Math.sin(flTheta); + double blV = this.backLeft.getGroundSpeed(); + double blTheta = this.backLeft.getWheelPosition() + pigeonAngleRadians; + double blVX = blV * Math.cos(blTheta); + double blVY = blV * Math.sin(blTheta); + double brV = this.backRight.getGroundSpeed(); + double brTheta = this.backRight.getWheelPosition() + pigeonAngleRadians; + double brVX = brV * Math.cos(brTheta); + double brVY = brV * Math.sin(brTheta); + + velocityVector.setI(frVX + flVX + blVX + brVX); + velocityVector.setJ(frVY + flVY + blVY + brVY); + + // System.out.println("VVector: <" + velocityVector.getI() + ", " + + // velocityVector.getJ() + ">"); + + return velocityVector; + } + + /** + * Retrieves the acceleration vector of the robot. + * The acceleration is calculated based on the linear acceleration measured by + * the Pigeon IMU. + * The acceleration is expressed in field-centric coordinates. + * + * @return The acceleration vector of the robot in meters per second squared + * (m/s^2). + */ + public Vector getRobotAccelerationVector() { + Vector accelerationVector = new Vector(); + + Vector robotCentricAccelerationVector = this.peripherals.getPigeonLinAccel(); + double accelerationMagnitude = Constants.getDistance(robotCentricAccelerationVector.getI(), + robotCentricAccelerationVector.getJ(), 0, 0); + accelerationVector.setI(accelerationMagnitude * Math.cos(Math.toRadians(this.peripherals.getPigeonAngle()))); + accelerationVector.setJ(accelerationMagnitude * Math.sin(Math.toRadians(this.peripherals.getPigeonAngle()))); + + return accelerationVector; + } + + /** + * Retrieves the path point closest to the specified time from the given path. + * If the specified time is before the first path point, the first point is + * returned. + * If the specified time is after the last path point, the last point is + * returned. + * + * @param path The array containing path points, each represented as a + * JSONArray. + * @param time The time for which the closest path point is required. + * @return The closest path point to the specified time. + */ + public JSONArray getPathPoint(JSONArray path, double time) { + for (int i = 0; i < path.length() - 1; i++) { + JSONArray currentPoint = path.getJSONArray(i + 1); + JSONArray previousPoint = path.getJSONArray(i); + double currentPointTime = currentPoint.getDouble(0); + double previousPointTime = previousPoint.getDouble(0); + if (time >= previousPointTime && time < currentPointTime) { + return currentPoint; + } + } + if (time < path.getJSONArray(0).getDouble(0)) { + return path.getJSONArray(0); + } else { + return path.getJSONArray(path.length() - 1); + } + } + + public Number[] purePursuitController(double currentX, double currentY, double currentTheta, int currentIndex, + JSONArray pathPoints, boolean fullSend, boolean accurate) { + JSONObject targetPoint = pathPoints.getJSONObject(pathPoints.length() - 1); + int targetIndex = pathPoints.length() - 1; + if (this.m_fieldSide == "blue") { + currentX = Constants.Physical.FIELD_LENGTH - currentX; + currentY = Constants.Physical.FIELD_WIDTH - currentY; + currentTheta = Math.PI + currentTheta; + } + + if (OI.isProcessorSide()) { + currentY = Constants.Physical.FIELD_WIDTH - currentY; + currentTheta = -currentTheta; + } + + for (int i = currentIndex; i < pathPoints.length(); i++) { + JSONObject point = pathPoints.getJSONObject(i); + double targetX = point.getDouble("x"), targetY = point.getDouble("y"), + targetTheta = point.getDouble("angle"), targetXvel = point.getDouble("x_velocity"), + targetYvel = point.getDouble("y_velocity"), targetThetavel = point.getDouble("angular_velocity"); + targetTheta = Constants.standardizeAngleToOther(targetTheta, currentTheta); + double linearVelMag = Math.hypot(targetYvel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + targetXvel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS); + double targetVelMag = Math.hypot(linearVelMag, + targetThetavel / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS); + double lookaheadRadius = fullSend ? Constants.Autonomous.FULL_SEND_LOOKAHEAD + : Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_DISTANCE * targetVelMag + + Constants.Autonomous.MIN_LOOKAHEAD_DISTANCE;// If full send mode is enabled, use the full + // send lookahead + double deltaX = (currentX - targetX), deltaY = (currentY - targetY), + deltaTheta = (currentTheta - targetTheta); + if (!insideRadius(deltaX / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + deltaY / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + deltaTheta / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS, + lookaheadRadius)) { + targetIndex = i; + targetPoint = pathPoints.getJSONObject(i); + break; + } + } + double targetX = targetPoint.getDouble("x"), targetY = targetPoint.getDouble("y"), + targetTheta = targetPoint.getDouble("angle"); + + targetTheta = Constants.standardizeAngleToOther(targetTheta, currentTheta); + + xPID.setSetPoint(targetX); + yPID.setSetPoint(targetY); + thetaPID.setSetPoint(targetTheta); + + xPID.updatePID(currentX); + yPID.updatePID(currentY); + thetaPID.updatePID(currentTheta); + double pidScaler = 1; + double xVelNoFF = xPID.getResult() * pidScaler; + double yVelNoFF = yPID.getResult() * pidScaler; + double thetaVelNoFF = -thetaPID.getResult(); + double f = (accurate ? Constants.Autonomous.ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY + : Constants.Autonomous.FEED_FORWARD_MULTIPLIER); + double feedForwardX = targetPoint.getDouble("x_velocity") * f; + double feedForwardY = targetPoint.getDouble("y_velocity") * f; + double feedForwardTheta = -targetPoint.getDouble("angular_velocity") * f * 0.1; + + double finalX = xVelNoFF + feedForwardX; + double finalY = yVelNoFF + feedForwardY; + double finalTheta = (thetaVelNoFF + feedForwardTheta) * 1.25; + if (m_fieldSide == "blue") { + finalX = -finalX; + finalY = -finalY; + } + + if (OI.isProcessorSide()) { + finalY = -finalY; + finalTheta = -finalTheta; + } + + Number[] velocityArray = new Number[] { + finalX, + -finalY, + finalTheta, + targetIndex, + }; + double linearVelMag = Math.hypot( + targetPoint.getDouble("x_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS, + targetPoint.getDouble("y_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS); + double targetVelMag = Math.hypot(linearVelMag, + targetPoint.getDouble("angular_velocity") / Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS); + double lookaheadRadius = fullSend ? Constants.Autonomous.FULL_SEND_LOOKAHEAD + : Constants.Autonomous.AUTONOMOUS_LOOKAHEAD_DISTANCE * targetVelMag + + Constants.Autonomous.MIN_LOOKAHEAD_DISTANCE; + + Logger.recordOutput("x-vel", xVelNoFF); + Logger.recordOutput("y-vel", yVelNoFF); + Logger.recordOutput("theta-vel", thetaVelNoFF); + Logger.recordOutput("wanted-theta-vel", + targetPoint.getDouble("angular_velocity")); + Logger.recordOutput("FF-theta-vel", feedForwardTheta); + Logger.recordOutput("FF-x-vel", feedForwardX); + Logger.recordOutput("FF-y-vel", feedForwardY); + Logger.recordOutput("current point idx", currentIndex); + Logger.recordOutput("point idx", velocityArray[3].intValue()); + Logger.recordOutput("look-ahead", lookaheadRadius); + Logger.recordOutput("target-point", new Pose2d(targetX, targetY, new Rotation2d(targetTheta))); + Logger.recordOutput("Velocity Array", + new double[] { finalX, -finalY, finalTheta }); + Logger.recordOutput("dx", targetX - currentX); + Logger.recordOutput("dy", targetY - currentY); + Logger.recordOutput("dtheta", targetTheta - currentTheta); + return velocityArray; + } + + public boolean insideRadius(double deltaX, double deltaY, double deltaTheta, double radius) { + // Logger.recordOutput("deltax", deltaX); + // Logger.recordOutput("deltay", deltaY); + // Logger.recordOutput("deltaTheta", deltaTheta); + Logger.recordOutput("Error", Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2) + Math.pow(deltaTheta, 2))); + return Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2) + Math.pow(deltaTheta, 2)) < radius; + } + + public void calculateAngleChange(double angle) { + double pigeonAngleDegrees = this.peripherals.getPigeonAngle(); + double targetAngle = 0; + if (getFieldSide() == "red") { + targetAngle = angle + 180; + } else { + targetAngle = angle; + } + + if (DriverStation.isAutonomousEnabled() && getFieldSide() == "red") { + pigeonAngleDegrees = 180 + pigeonAngleDegrees; + } + this.turningPID.setSetPoint(Constants.standardizeAngleDegrees(targetAngle)); + this.turningPID.updatePID(Constants.standardizeAngleDegrees(pigeonAngleDegrees)); + double turnResult = -turningPID.getResult(); + + this.driveAutoAligned(turnResult); + } + + private DriveState handleStateTransition() { + switch (wantedState) { + case DEFAULT: + return DriveState.DEFAULT; + case IDLE: + return DriveState.IDLE; + case REEF: + return DriveState.REEF; + case REEF_MORE: + return DriveState.REEF_MORE; + case BACK: + return DriveState.BACK; + case L3_REEF: + return DriveState.L3_REEF; + case L4_REEF: + return DriveState.L4_REEF; + case ALGAE: + return DriveState.ALGAE; + case ALGAE_MORE: + return DriveState.ALGAE_MORE; + case ALGAE_MORE_MORE: + return DriveState.ALGAE_MORE_MORE; + case PROCESSOR: + return DriveState.PROCESSOR; + case PROCESSOR_MORE: + return DriveState.PROCESSOR_MORE; + case NET: + return DriveState.NET; + case NET_MORE: + return DriveState.NET_MORE; + case FEEDER: + return DriveState.FEEDER; + case SCORE_L23: + return DriveState.SCORE_L23; + case AUTO_FEEDER: + return DriveState.AUTO_FEEDER; + case FEEDER_ALIGN: + return DriveState.FEEDER_ALIGN; + case AUTO_L1: + return DriveState.AUTO_L1; + case AUTO_L1_MORE: + return DriveState.AUTO_L1_MORE; + case FEEDER_AUTO: + return DriveState.FEEDER_AUTO; + case PIECE_PICKUP: + return DriveState.PIECE_PICKUP; + case AUTO_CLIMB: + return DriveState.AUTO_CLIMB; + case STOP: + return DriveState.STOP; + default: + return DriveState.IDLE; + } + } + + Vector scoreL23Vector = new Vector(2.5, 0); + Pose2d l23Setpoint = new Pose2d(); + + Vector pickupAlgaeFrontVector = new Vector(2.5, 0); + Pose2d algaeSetpoint = new Pose2d(); + Vector pickupAlgaeBackVector = new Vector(-2.5, 0); + + public double getDistanceFromL23Setpoint() { + return l23Setpoint.getTranslation().getDistance(getMT2Odometry().getTranslation()); + } + + public double getDistanceFromAlgaeSetpoint() { + return algaeSetpoint.getTranslation().getDistance(getMT2Odometry().getTranslation()); + } + + public double getThetaToCenterReef() { + double theta = 0.0; + if (OI.isRedSide()) { + theta = Math.atan2(Constants.Reef.centerRed.getY() - getMT2OdometryY(), + Constants.Reef.centerRed.getX() - getMT2OdometryX()); + } else { + theta = Math.atan2((Constants.Reef.centerBlue.getY() - getMT2OdometryY()), + (Constants.Reef.centerBlue.getX() - getMT2OdometryX())); + } + if (getAngleDifferenceDegrees(Math.toDegrees(theta), Math.toDegrees(getMT2OdometryAngle())) > 90.0) { + theta -= Math.PI; + } + return theta; + } + + public double[] getClosestPoint(double[] lineStart, double[] lineEnd) { // chatGPT ahh code + // System.out.println("X1 - X2; " + (lineStart[0] - lineEnd[0]) + "Y1 - Y2" + + // (lineStart[1] - lineEnd[1])); + double x1 = lineStart[0]; + double y1 = lineStart[1]; + double x2 = lineEnd[0]; + double y2 = lineEnd[1]; + double px = getMT2OdometryX(); + double py = getMT2OdometryY(); + + double dx = x2 - x1; + double dy = y2 - y1; + double lenSq = dx * dx + dy * dy; + + if (lenSq == 0) { + return new double[] { x1, y1 }; + } + + double t = ((px - x1) * dx + (py - y1) * dy) / lenSq; + t = Math.max(0, Math.min(1, t)); + + double closestX = x1 + t * dx; + double closestY = y1 + t * dy; + + return new double[] { closestX, closestY }; + } + + public double getThetaToPoint(double xMeters, double yMeters) { + return Math.atan2(yMeters - getMT2OdometryY(), + xMeters - getMT2OdometryX()); + } + + public double[] getClosestL1PointXY() { // returns the closest point on the trough for l1 + // double[] xy = { 0.0, 0.0 }; + // if (isL1Face()) { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(1)); + // } else { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(0)); + // } + // return xy; + // System.out.println("X1: " + getClosestL1Points().get(0)[0] + " Y1: " + + // getClosestL1Points().get(0)[1] + "X2: " + // + getClosestL1Points().get(1)[0] + " Y2: " + getClosestL1Points().get(1)[1]); + double[] xy = getClosestPoint(getClosestL1Points().get(0), getClosestL1Points().get(1)); + return xy; + } + + public double[] getClosestL1CornerPointXY() { // returns the closest point on the trough for l1 + // double[] xy = { 0.0, 0.0 }; + // if (isL1Face()) { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(1)); + // } else { + // xy = getClosestPoint(getClosestL1Points().get(0), + // getClosestL1Points().get(0)); + // } + // return xy; + // System.out.println("X1: " + getClosestL1Points().get(0)[0] + " Y1: " + + // getClosestL1Points().get(0)[1] + "X2: " + // + getClosestL1Points().get(1)[0] + " Y2: " + getClosestL1Points().get(1)[1]); + double[] xy = getClosestPoint(getClosestL1CornerPoints().get(0), getClosestL1CornerPoints().get(1)); + return xy; + } + + public ArrayList getClosestL1Points() { + ArrayList list = new ArrayList<>(); + double currentDist = 100.0; + double dist = 100.0; + double[] chosenSetpoint = { 0.0, 0.0 }; + double[] secondPoint = { 0.0, 0.0 }; + if (isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueDrivePoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1BlueDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1BlueDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + // System.out.println("Closest Point: " + list.get(0)[0] + " " + + // list.get(0)[1]); + currentDist = 100.0; + dist = 100.0; + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueDrivePoints.get(i).getY())); + if (currentDist < dist + && Math.hypot(Math.abs(Constants.Reef.l1BlueDrivePoints.get(i).getX() - list.get(0)[0]), + Math.abs(Constants.Reef.l1BlueDrivePoints.get(i).getY() - list.get(0)[1])) > 0.1) { + dist = currentDist; + // System.out.println(" "); + // System.out.println("new X2: " + Constants.Reef.l1BluePoints.get(i).getX() + + // "new Y2: " + // + Constants.Reef.l1BluePoints.get(i).getY()); + // System.out.println("old X2: " + list.get(0)[0] + "old Y2: " + // + list.get(0)[1]); + secondPoint[0] = Constants.Reef.l1BlueDrivePoints.get(i).getX(); + secondPoint[1] = Constants.Reef.l1BlueDrivePoints.get(i).getY(); + } + } + // System.out.println("->->->->->->->->->"); + list.add(secondPoint); + return list; + } else { + for (int i = 0; i < Constants.Reef.l1RedDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedDrivePoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + currentDist = 100.0; + dist = 100.0; + chosenSetpoint[0] = 0.0; + chosenSetpoint[1] = 0.0; + for (int i = 0; i < Constants.Reef.l1RedDrivePoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedDrivePoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedDrivePoints.get(i).getY())); + if (currentDist < dist && Math.abs(Constants.Reef.l1RedDrivePoints.get(i).getX() - list.get(0)[0]) > 0.1 + && Math.abs(Constants.Reef.l1RedDrivePoints.get(i).getY() - list.get(0)[1]) > 0.1) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedDrivePoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedDrivePoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + return list; + + } + } + + public ArrayList getClosestL1CornerPoints() { + ArrayList list = new ArrayList<>(); + double currentDist = 100.0; + double dist = 100.0; + double[] chosenSetpoint = { 0.0, 0.0 }; + double[] secondPoint = { 0.0, 0.0 }; + if (isOnBlueSide()) { + for (int i = 0; i < Constants.Reef.l1BlueCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueCornerPoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1BlueCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1BlueCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + // System.out.println("Closest Point: " + list.get(0)[0] + " " + + // list.get(0)[1]); + currentDist = 100.0; + dist = 100.0; + for (int i = 0; i < Constants.Reef.l1BlueCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1BlueCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1BlueCornerPoints.get(i).getY())); + if (currentDist < dist + && Math.hypot(Math.abs(Constants.Reef.l1BlueCornerPoints.get(i).getX() - list.get(0)[0]), + Math.abs(Constants.Reef.l1BlueCornerPoints.get(i).getY() - list.get(0)[1])) > 0.1) { + dist = currentDist; + // System.out.println(" "); + // System.out.println("new X2: " + + // Constants.Reef.l1BlueCornerPoints.get(i).getX() + "new Y2: " + // + Constants.Reef.l1BlueCornerPoints.get(i).getY()); + // System.out.println("old X2: " + list.get(0)[0] + "old Y2: " + // + list.get(0)[1]); + secondPoint[0] = Constants.Reef.l1BlueCornerPoints.get(i).getX(); + secondPoint[1] = Constants.Reef.l1BlueCornerPoints.get(i).getY(); + } + } + // System.out.println("->->->->->->->->->"); + list.add(secondPoint); + return list; + } else { + for (int i = 0; i < Constants.Reef.l1RedCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedCornerPoints.get(i).getY())); + if (currentDist < dist) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + currentDist = 100.0; + dist = 100.0; + chosenSetpoint[0] = 0.0; + chosenSetpoint[1] = 0.0; + for (int i = 0; i < Constants.Reef.l1RedCornerPoints.size(); i++) { + // currentDist = Math.sqrt(Math.pow((x - + // Constants.Reef.redFrontPlacingPositions.get(i).getX()), 2) + // + Math.pow((y - Constants.Reef.redFrontPlacingPositions.get(i).getY()), 2)); + currentDist = Math.hypot( + getMT2OdometryX() - (Constants.Reef.l1RedCornerPoints.get(i).getX()), + getMT2OdometryY() - (Constants.Reef.l1RedCornerPoints.get(i).getY())); + if (currentDist < dist + && Math.abs(Constants.Reef.l1RedCornerPoints.get(i).getX() - list.get(0)[0]) > 0.1 + && Math.abs(Constants.Reef.l1RedCornerPoints.get(i).getY() - list.get(0)[1]) > 0.1) { + dist = currentDist; + chosenSetpoint[0] = Constants.Reef.l1RedCornerPoints.get(i).getX(); + chosenSetpoint[1] = Constants.Reef.l1RedCornerPoints.get(i).getY(); + } + } + list.add(chosenSetpoint); + return list; + + } + } + + public boolean isL1Face() { // returns true if the robot is by a face of the L1, returns false if the robot + // is around the point on the edge of l1 faces + if (isOnBlueSide()) { + + } else { + + } + return true; // TODO: theoretically this should work because the closest + } + + public Pose2d getClosestPose(Pose2d pose1, Pose2d pose2) { + double dist1 = Math.hypot(Math.abs(pose1.getX() - getMT2OdometryX()), + Math.abs(pose1.getY() - getMT2OdometryY())); + double dist2 = Math.hypot(Math.abs(pose2.getX() - getMT2OdometryX()), + Math.abs(pose2.getY() - getMT2OdometryY())); + if (dist1 <= dist2) { + return pose1; + } else { + return pose2; + } + } + + public Pose2d origionalSetpointPose = new Pose2d(); + public boolean firstTimeReef = true; + + public double getNetXSetpoint() { + if (isOnBlueSide()) { + return Constants.Reef.netBlueXM; + } else { + return Constants.Reef.netRedXM; + } + } + + public double getNetXMoreSetpoint() { + if (isOnBlueSide()) { + return Constants.Reef.netBlueXMore; + } else { + return Constants.Reef.netRedXMore; + } + } + + public double getNetThetaSetpoint() { + if (isOnBlueSide()) { + if (autoPlacingFront) { + return Constants.Reef.netBlueFrontThetaR; + } else { + return Constants.Reef.netBlueBackThetaR; + } + } else { + if (autoPlacingFront) { + return Constants.Reef.netRedFrontThetaR; + } else { + return Constants.Reef.netRedBackThetaR; + } + } + } + + public boolean isOnBlueSide() { + if (getMT2OdometryX() < Constants.Physical.FIELD_LENGTH / 2.0) { + return true; + } else { + return false; + } + } + + public double[] getNetXTheta() { + double goalX = getMT2OdometryX(); + double goalTheta = getMT2OdometryAngle(); + + if (isOnBlueSide()) { + goalX = Constants.Reef.netBlueXM; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netBlueFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netBlueBackThetaR; + } + } else { + goalX = Constants.Reef.netRedXM; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netRedFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netRedBackThetaR; + } + } + + double[] xTheta = { goalX, goalTheta }; + return xTheta; + } + + public double[] getNetMoreXTheta() { + double goalX = getMT2OdometryX(); + double goalTheta = getMT2OdometryAngle(); + + if (isOnBlueSide()) { + goalX = Constants.Reef.netBlueXMore; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netBlueFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netBlueBackThetaR; + } + } else { + goalX = Constants.Reef.netRedXMore; + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + autoPlacingFront = true; + goalTheta = Constants.Reef.netRedFrontThetaR; + } else { + autoPlacingFront = false; + goalTheta = Constants.Reef.netRedBackThetaR; + } + } + + double[] xTheta = { goalX, goalTheta }; + return xTheta; + } + + public double distanceFromCenterOfReef() { + if (isOnBlueSide()) { + return Math.hypot((getMT2OdometryX() - Constants.Reef.centerBlue.getX()), + ((getMT2OdometryY() - Constants.Reef.centerBlue.getY()))); + } else { + return Math.hypot((getMT2OdometryX() - Constants.Reef.centerRed.getX()), + ((getMT2OdometryY() - Constants.Reef.centerRed.getY()))); + } + } + + private final Vector backupVector = new Vector(-20.0, 0.0); + private final Vector otherBackupVector = new Vector(20.0, 0.0); + + @Override + public void periodic() { + Logger.recordOutput("Extra Pigeon Angle", peripherals.getPigeonExtraAngle()); + Logger.recordOutput("Robot Velocity", getRobotSpeed()); + Logger.recordOutput("MT2 Odometry", getMT2Odometry()); + // Pose2d target = getGamePiecePosition(); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + // Translation2d t1 = new Translation2d(getMT2OdometryX(), getMT2OdometryY()); + // Rotation2d r1 = new Rotation2d(getThetaToCenterReef()); + // Pose2d p1 = new Pose2d(t1, r1); + // Logger.recordOutput("L1 Auto Angle", p1); + l23Setpoint = getReefClosestSetpoint(getMT2Odometry(), false); + algaeSetpoint = getAlgaeClosestSetpoint(getMT2Odometry()); + // Logger.recordOutput("Robot Odometry", getMT2Odometry()); + updateOdometryFusedArray(); + // process inputs + DriveState newState = handleStateTransition(); + Pose2d setpoint = new Pose2d(); + double standardizedAngle = Constants.standardizeAngleDegrees(Math.toDegrees(getMT2OdometryAngle())); + if (newState != systemState) { + systemState = newState; + } + Logger.recordOutput("Drive State", systemState); + // Stop moving when disabled + if (DriverStation.isDisabled()) { + systemState = DriveState.DEFAULT; + } + + if (!systemState.equals(DriveState.AUTO_CLIMB)) { + firstClimb = false; + } + if (!systemState.equals(DriveState.PIECE_PICKUP)) { + firstTimeAutoPickup = false; + firstTimeCalculated = false; + firstTimeGoingInCalculated = false; + firstTimeGoingIn = false; + hasTrack = false; + } + if (!OI.getDriverA()) { + firstTimeReef = true; + } + // Logger.recordOutput("L1 Vector", + // new Pose2d(new Translation2d(2.0, 2.0), + // new Rotation2d( + // Math.atan2(getClosestL1Points().get(0)[1] - getClosestL1Points().get( + // 1)[1], getClosestL1Points().get(0)[0] - getClosestL1Points().get(1)[0])))); + + // Logger.recordOutput("Point 1", new Pose2d(new + // Translation2d(getClosestL1Points().get(0)[0], + // getClosestL1Points().get(0)[1]), new Rotation2d(0.0))); + // Logger.recordOutput("Point 2", new Pose2d(new + // Translation2d(getClosestL1Points().get(1)[0], + // getClosestL1Points().get(1)[1]), new Rotation2d(0.0))); + + // Pose2d l1Pose = new Pose2d(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1], + // new Rotation2d(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + + switch (systemState) { + case DEFAULT: + teleopDrive(); + break; + case IDLE: + break; + case STOP: + Vector velocityVector = new Vector(); + velocityVector.setI(0); + velocityVector.setJ(0); + double desiredThetaChange = 0.0; + autoDrive(velocityVector, desiredThetaChange); + break; + case AUTO_L1: + // driveToTheta(Math.toDegrees(getThetaToCenterReef())); + // driveToTheta(Math.toDegrees(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + // driveOnLine(new Vector(getClosestL1Points().get(0)[0] - + // getClosestL1Points().get( + // 1)[0], getClosestL1Points().get(0)[1] + // - getClosestL1Points().get( + // 1)[1]), + // new Translation2d(getClosestL1Points().get(0)[0], getClosestL1Points().get( + // 0)[1]), + // getThetaToPoint(getClosestL1CornerPointXY()[0], + // getClosestL1CornerPointXY()[1])); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getL1ReefClosestSetpoint(getMT2Odometry()); + } + setpoint = getL1ReefClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case AUTO_L1_MORE: + // driveToTheta(Math.toDegrees(getThetaToCenterReef())); + // driveToTheta(Math.toDegrees(getThetaToPoint(getClosestL1PointXY()[0], + // getClosestL1PointXY()[1]))); + // driveOnLine(new Vector(getClosestL1Points().get(0)[0] - + // getClosestL1Points().get( + // 1)[0], getClosestL1Points().get(0)[1] + // - getClosestL1Points().get( + // 1)[1]), + // new Translation2d(getClosestL1Points().get(0)[0], getClosestL1Points().get( + // 0)[1]), + // getThetaToPoint(getClosestL1CornerPointXY()[0], + // getClosestL1CornerPointXY()[1])); + // System.out.println(Math.toDegrees(getThetaToCenterReef())); + // if (firstTimeReef) { + // firstTimeReef = false; + // origionalSetpointPose = getL1ReefClosestSetpoint(getMT2Odometry(), false); + // } + setpoint = getL1ReefClosestSetpointMore(getMT2Odometry()); + driveToPoint(setpoint); + break; + case REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefClosestSetpoint(getMT2Odometry(), false); + } + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case REEF_MORE: + setpoint = getReefMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case BACK: + if (autoPlacingFront) { + autoRobotCentricDrive(backupVector, 0.0); + } else { + autoRobotCentricDrive(otherBackupVector, 0.0); + } + break; + case L4_REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefL4ClosestSetpoint(getMT2Odometry(), false); + } + // System.out.println(origionalSetpointPose); + // Logger.recordOutput("Targeted L4 Point", + // getReefL4ClosestSetpoint(getMT2Odometry(), OI.getDriverA())); + // TODO: make this work + setpoint = getReefL4ClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case L3_REEF: + if (firstTimeReef) { + firstTimeReef = false; + origionalSetpointPose = getReefL3ClosestSetpoint(getMT2Odometry(), false); + } + setpoint = getReefL3ClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + + setpoint = getReefClosestSetpoint(getMT2Odometry(), OI.getDriverA()); + if (Math.abs(OI.getDriverLeftX()) > 0.2 || Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + driveToPoint(setpoint); + } + break; + case PIECE_PICKUP: + // Pose2d target = getGamePiecePosition(); + // if (!target.equals(new Pose2d())) { + // targetPointPickup = target; + // } + // if (hasTrack) { + goToCoral(); + java.util.logging.Logger.getGlobal().finer("Driving to point"); + // } else { + // System.out.println("forward"); + // Vector v = new Vector(); + // v.setI(0.4); + // v.setJ(0); + // double d = 0.0; + // autoRobotCentricDrive(v, d); + // } + break; + case ALGAE: + setpoint = getAlgaeClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case ALGAE_MORE: + // if + // (getAngleDifferenceDegrees(Math.toDegrees(getAlgaeClosestSetpoint(getMT2Odometry())[2]), + // Math.toDegrees(getMT2OdometryAngle())) <= 90) { + // autoRobotCentricDrive(scoreL23Vector, 0); + // } else { + // autoRobotCentricDrive(scoreL23Vector, 0); + // } + setpoint = getAlgaeMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case ALGAE_MORE_MORE: + setpoint = getAlgaeMoreMoreClosestSetpoint(getMT2Odometry()); + driveToPoint(setpoint); + break; + case PROCESSOR: + if (isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorBlueFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorBlueFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorBlueFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + // driveToTheta((Constants.Reef.processorBlueBackPlacingPosition.getRotation().getDegrees())); + driveToPoint(Constants.Reef.processorBlueBackPlacingPosition); + } + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorRedFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorRedFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorRedFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorRedBackPlacingPosition); + } + // driveToTheta((Constants.Reef.processorRedBackPlacingPosition.getRotation().getDegrees())); + } + } + break; + case PROCESSOR_MORE: + if (isOnBlueSide()) { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorMoreBlueFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreBlueFrontPlacingPosition); + } + // driveToTheta((Constants.Reef.processorMoreBlueFrontPlacingPosition.getRotation().getDegrees())); + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + // driveToTheta((Constants.Reef.processorMoreBlueBackPlacingPosition.getRotation().getDegrees())); + driveToPoint(Constants.Reef.processorMoreBlueBackPlacingPosition); + } + } + } else { + if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + Constants.Reef.processorMoreRedFrontPlacingPosition.getRotation().getDegrees()) <= 90) { + autoPlacingFront = true; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreRedFrontPlacingPosition); + // driveToTheta((Constants.Reef.processorMoreRedFrontPlacingPosition.getRotation().getDegrees())); + } + } else { + autoPlacingFront = false; + if (OI.driverA.getAsBoolean() || OI.driverLT.getAsBoolean()) { + teleopDrive(); + } else { + driveToPoint(Constants.Reef.processorMoreRedBackPlacingPosition); + } + // driveToTheta((Constants.Reef.processorMoreRedBackPlacingPosition.getRotation().getDegrees())); + } + } + break; + case NET_MORE: + if (OI.getDriverA()) { + teleopDrive(); + } else if (Math.abs(OI.getDriverLeftY()) > 0.2) { + teleopDrive(); + } else { + double[] setpointA = getNetMoreXTheta(); + driveToXTheta(setpointA[0], setpointA[1]); + } + + // if (OI.isBlueSide()) { // TODO: uncomment to revert to colorado algae code + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXMore, + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXMore, + // Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // } else { + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXMore, + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXMore, + // Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // } + + break; + case NET: + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + double[] setpointA = getNetXTheta(); + driveToXTheta(setpointA[0], setpointA[1]); + } + + // if (OI.isBlueSide()) { // TODO: uncomment to revert to colorado algae code + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXM, + // Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netBlueXM, + // Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netBlueBackThetaR)); + // } + // } else { + // if (getAngleDifferenceDegrees(Math.toDegrees(getMT2OdometryAngle()), + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)) <= 90) { + // autoPlacingFront = true; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXM, + // Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedFrontThetaR)); + // } else { + // autoPlacingFront = false; + // if (OI.driverA.getAsBoolean()) { + // teleopDrive(); + // } else { + // driveToXTheta(Constants.Reef.netRedXM, + // Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // // driveToTheta(Math.toDegrees(Constants.Reef.netRedBackThetaR)); + // } + // } + + break; + case SCORE_L23: + autoRobotCentricDrive(scoreL23Vector, 0); + break; + case FEEDER: + if (getFieldSide() == "red") { // red side + if (getMT2OdometryY() > 4.026) { // redside right feeder (field top right) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + driveToTheta(234); + } else { // robot back side redside left feeder (fieldside top right) + driveToTheta(54); + } + } else { // redside left feeder (fieldside bottom right) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + driveToTheta(306); + } else { // robot back side redside left (fieldside bottom right) + driveToTheta(126); + } + } + } else { // blue side + if (getMT2OdometryY() < 4.026) { // blue side right feeder (fieldside bottom left) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + driveToTheta(234); + } else { // robot back side blueside right (fieldside bottom left) + driveToTheta(54); + } + } else { // blue side left feeder (fieldside top left) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + driveToTheta(306); + } else { // robot back side blueside left (fieldside top left) + driveToTheta(126); + } + } + } + + break; + case AUTO_FEEDER: + break; + case FEEDER_ALIGN: + if (getFieldSide() == "red") { // red side + if (getMT2OdometryY() > 4.026) { // redside right feeder (field top right) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + Vector feederLine = new Vector( + -Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getRadians()); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_RIGHT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } + } else { // redside left feeder (fieldside bottom right) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + Vector feederLine = new Vector( + -Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.RED_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.RED_LEFT_FEEDER_TELEOP.getRotation().getRadians()); + } + } + } + } else { // blue side + if (getMT2OdometryY() < 4.026) { // blue side right feeder (fieldside bottom left) + if ((standardizedAngle <= 324 + && + standardizedAngle >= 144)) { + Vector feederLine = new Vector( + -Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_RIGHT_FEEDER_TELEOP.getRotation().getRadians()); + } + } + } else { // blue side left feeder (fieldside top left) + if ((standardizedAngle <= 36 + && + standardizedAngle >= 0) + || + (standardizedAngle <= 360 + && + standardizedAngle >= 216)) { + Vector feederLine = new Vector( + -Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getRadians()); + } + } else { // robot back side redside left feeder (fieldside top right) + Vector feederLine = new Vector( + -Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getSin(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getCos()); + if (OI.driverA.getAsBoolean()) { + teleopDrive(); + } else { + driveOnLine(feederLine, Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getTranslation(), + Constants.Reef.BLUE_LEFT_FEEDER_TELEOP.getRotation().getRadians() + Math.PI); + } + } + } + } + break; + case FEEDER_AUTO: + break; + case AUTO_CLIMB: + Vector moveBack = new Vector(0.2, 0); + + if (!firstClimb) { + startX = getMT2OdometryX(); + startY = getMT2OdometryY(); + firstClimb = true; + } + + if (Math.hypot((Math.abs(getMT2OdometryX() - startX)), Math.abs(getMT2OdometryY() - startY)) < 0.15) { + autoRobotCentricDrive(moveBack, 0); + } else { + autoRobotCentricDrive(new Vector(0, 0), 0); + } + break; + + default: + + break; + } + } + + /** + * Calculates the adjusted y-coordinate based on the original x and y + * coordinates. + * + * @param originalX The original x-coordinate. + * @param originalY The original y-coordinate. + * @return The adjusted y-coordinate. + */ + public double getAdjustedY(double originalX, double originalY) { + double adjustedY = originalY * Math.sqrt((1 - (Math.pow(originalX, 2)) / 2)); + return adjustedY; + } + + /** + * Calculates the adjusted x-coordinate based on the original x and y + * coordinates. + * + * @param originalX The original x-coordinate. + * @param originalY The original y-coordinate. + * @return The adjusted x-coordinate. + */ + public double getAdjustedX(double originalX, double originalY) { + double adjustedX = originalX * Math.sqrt((1 - (Math.pow(originalY, 2)) / 2)); + return adjustedX; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java b/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java new file mode 100644 index 0000000..ae97fd7 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Peripherals.java @@ -0,0 +1,496 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import org.littletonrobotics.junction.Logger; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj.Filesystem; +import frc.robot.Constants; +import frc.robot.tools.math.Vector; + +public class Peripherals { + private PhotonCamera frontReefCam = new PhotonCamera("Front_Reef"); + private PhotonCamera frontSwerveCam = new PhotonCamera("Front_Swerve"); + private PhotonCamera backReefCam = new PhotonCamera("Back_Reef"); + private PhotonCamera backLeftReefCam = new PhotonCamera("Back_Left_Reef"); + private PhotonCamera backRightReefCam = new PhotonCamera("Back_Right_Reef"); + private PhotonCamera frontBargeCam = new PhotonCamera("Front_Barge"); + private PhotonCamera backBargeCam = new PhotonCamera("Back_Barge"); + private PhotonCamera gamePieceCamera = new PhotonCamera("Front_Game_Piece_Cam"); + + AprilTagFieldLayout aprilTagFieldLayout; + + private Pigeon2 pigeon = new Pigeon2(0, "Canivore"); + private Pigeon2 pigeonExtra = new Pigeon2(1, "Canivore"); + + private Pigeon2Configuration pigeonConfig = new Pigeon2Configuration(); + private Pigeon2Configuration pigeonExtraConfig = new Pigeon2Configuration(); + Transform3d robotToCam = new Transform3d( + new Translation3d(Constants.inchesToMeters(1.75), Constants.inchesToMeters(11.625), + Constants.inchesToMeters(33.5)), + new Rotation3d(0, Math.toRadians(30.6), 0)); + PhotonPoseEstimator photonPoseEstimator; + + double pigeonSetpoint = 0.0; + + boolean frontReefCamTrack = false; + boolean backReefCamTrack = false; + boolean frontBargeCamTrack = false; + boolean backBargeCamTrack = false; + + public Peripherals() { + } + + /** + * Initializes the Peripherals subsystem. + * + * This method sets up the IMU configuration, mount pose, and zeroes the IMU. + * It also applies the default command to the Peripherals subsystem. + */ + public void init() { + try { + aprilTagFieldLayout = new AprilTagFieldLayout( + Filesystem.getDeployDirectory().getPath() + "/" + "2025-reefscape.json"); + } catch (Exception e) { + java.util.logging.Logger.getGlobal().warning("error with april tag: " + e.getMessage()); + } + photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); + // Set the mount pose configuration for the IMU + pigeonConfig.MountPose.MountPosePitch = 0.3561480641365051; + pigeonConfig.MountPose.MountPoseRoll = -0.10366992652416229; + pigeonConfig.MountPose.MountPoseYaw = -0.24523599445819855; + + pigeonExtraConfig.MountPose.MountPosePitch = 2.9378318786621094; + pigeonExtraConfig.MountPose.MountPoseRoll = -1.7237101793289185; + pigeonExtraConfig.MountPose.MountPoseYaw = -1.0769075155258179; + + // Apply the IMU configuration + pigeon.getConfigurator().apply(pigeonConfig); + pigeonExtra.getConfigurator().apply(pigeonExtraConfig); + + // Zero the IMU angle + zeroPigeon(); + + setPigeonPitchOffset(getPigeonPitch()); + + } + + public double getFrontReefCamYaw() { + double yaw = 0.0; + var result = frontReefCam.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + yaw = target.getYaw(); + } + return yaw; + } + + public double getFrontReefCamPitch() { + double pitch = 0.0; + var result = frontReefCam.getLatestResult(); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + pitch = target.getPitch(); + } + + return pitch; + } + + public void setBackCamPipline(int index) { + backReefCam.setPipelineIndex(index); + } + + public void setGamePieceCamPipline(int index) { + gamePieceCamera.setPipelineIndex(index); + } + + public double getGamePieceCamYaw() { + double yaw = 0.0; + var result = gamePieceCamera.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + yaw = target.getYaw(); + } + return yaw; + } + + public List getGamePieceCamCorners() { + List corners = new ArrayList(); + var result = gamePieceCamera.getLatestResult(); + // Logger.recordOutput("has target", result.hasTargets()); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + corners = target.getDetectedCorners(); + } + return corners; + } + + public double getGamePiecePitch() { + double pitch = 0.0; + var result = gamePieceCamera.getLatestResult(); + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + pitch = target.getPitch(); + } + + return pitch; + } + + // private Pose2d getRobotPoseViaTrig(PhotonTrackedTarget trackedTarget, + // double[] cameraPositionOnRobot, + // double robotAngle) { + // double pitch = trackedTarget.getPitch(); + // double yaw = trackedTarget.getYaw(); + // int id = trackedTarget.getFiducialId(); + // double cameraXOffset = cameraPositionOnRobot[0]; + // double cameraYOffset = cameraPositionOnRobot[1]; + // double cameraZOffset = cameraPositionOnRobot[2]; + // double cameraRYOffset = cameraPositionOnRobot[4]; + // double cameraRZOffset = cameraPositionOnRobot[5]; + + // double[] tagPose = Constants.Vision.TAG_POSES[id - 1]; + // double tagHeight = tagPose[2]; + // double tagX = tagPose[0]; + // double tagY = tagPose[1]; + // double tagYaw = tagPose[3]; + + // double distToTag = (tagHeight - cameraZOffset) / + // Math.tan(Math.toRadians(pitch + cameraRYOffset)); + // Logger.recordOutput("Distance to Tag", distToTag); + // Logger.recordOutput("yaw to Tag", yaw); + // double txProjOntoGroundPlane = Math.atan((Math.tan(yaw)) / Math.cos(pitch)); + // double xFromTag = distToTag * Math.cos(Math.toRadians(txProjOntoGroundPlane + + // robotAngle + cameraRZOffset)); + // double yFromTag = distToTag * Math.sin(Math.toRadians(txProjOntoGroundPlane + + // robotAngle + cameraRZOffset)); + // Logger.recordOutput("x to Tag", xFromTag); + // Logger.recordOutput("y to Tag", yFromTag); + + // double fieldPoseX = -xFromTag + tagX - cameraXOffset; + // double fieldPoseY = -yFromTag + tagY - cameraYOffset; + // Pose2d pose = new Pose2d(fieldPoseX, fieldPoseY, new + // Rotation2d(Math.toRadians(getPigeonAngle()))); + // return pose; + // } + + /** + * Calculates the robot's position based on the camera offset, angles to the + * tag, and tag position. + * + * @param cameraOffset Pose3d representing the camera's position and + * orientation relative to the robot center. + * @param horizontalAngle Horizontal angle (radians) from the camera to the tag. + * @param verticalAngle Vertical angle (radians) from the camera to the tag. + * @param tagPosition Pose3d representing the position of the tag in the + * field coordinate system. + * @param robotYaw Yaw (rotation around the Z-axis) of the robot in + * radians. + * @return Pose3d representing the robot's position in the field coordinate + * system. + */ + // public static Pose3d calculateRobotPosition( + // Pose3d cameraOffset, double horizontalAngle, double verticalAngle, Pose3d + // tagPosition, double robotYaw) { + + // // Extract the camera's offset from the robot in its local frame + // Translation3d cameraTranslation = cameraOffset.getTranslation(); + + // // Rotate the camera's offset into the field frame using the robot's yaw + // double cosYaw = Math.cos(robotYaw); + // double sinYaw = Math.sin(robotYaw); + // Translation3d cameraInFieldTranslation = new Translation3d( + // cosYaw * cameraTranslation.getX() - sinYaw * cameraTranslation.getY(), + // sinYaw * cameraTranslation.getX() + cosYaw * cameraTranslation.getY(), + // cameraTranslation.getZ()); + + // // Calculate the relative position of the tag to the camera + // double dx = tagPosition.getTranslation().getX() - + // cameraInFieldTranslation.getX(); + // double dy = tagPosition.getTranslation().getY() - + // cameraInFieldTranslation.getY(); + // double dz = tagPosition.getTranslation().getZ() - + // cameraInFieldTranslation.getZ(); + + // // Calculate the distance from the camera to the tag + // double distanceToTag = Math.sqrt(dx * dx + dy * dy + dz * dz); + + // // Translation from the camera to the tag in the camera's local space + // Translation3d tagInCameraSpace = new Translation3d( + // distanceToTag * Math.cos(horizontalAngle) * Math.cos(verticalAngle), // X + // distanceToTag * Math.sin(horizontalAngle) * Math.cos(verticalAngle), // Y + // distanceToTag * Math.sin(verticalAngle) // Z + // ); + + // // Transform the tag position from the camera space to the field space + // Translation3d tagInFieldSpace = + // cameraInFieldTranslation.plus(tagInCameraSpace); + + // // Calculate the robot's position in the field frame + // Translation3d robotTranslation = + // tagPosition.getTranslation().minus(tagInFieldSpace); + + // // Return the robot's position with its yaw in the field coordinate system + // return new Pose3d(robotTranslation, new Rotation3d(0, 0, robotYaw)); + // } + + // public Pose2d getFrontReefCamTrigPose() { + // var result = frontReefCam.getLatestResult(); + // if (result.hasTargets() && result.getBestTarget().getPoseAmbiguity() < 0.3) { + // PhotonTrackedTarget target = result.getBestTarget(); + // // Pose3d robotPose = calculateRobotPosition(cameraOffset, target.getYaw(), + // // target.getPitch(), + // // new Pose3d( + // // new Translation3d(Constants.Vision.TAG_POSES[10][0], + // // Constants.Vision.TAG_POSES[10][1], + // // Constants.Vision.TAG_POSES[10][2]), + // // new Rotation3d(0.0, Constants.Vision.TAG_POSES[10][4], + // // Constants.Vision.TAG_POSES[10][3])), + // // Math.toRadians(getPigeonAngle())); + // Pose2d robotPose = getRobotPoseViaTrig(target, + // Constants.Vision.FRONT_CAMERA_POSE, getPigeonAngle()); + // Logger.recordOutput("Trig Localiazation", robotPose); + // return robotPose; + // } else { + // Pose2d defaultPose = new Pose2d(0.0, 0.0, new Rotation2d(0.0)); + // return defaultPose; + // } + // } + + public PhotonPipelineResult getFrontReefCamResult() { + var result = frontReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + frontReefCamTrack = true; + return result.get(0); + } else { + frontReefCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackReefCamResult() { + var result = backReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + backReefCamTrack = true; + return result.get(0); + } else { + backReefCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackLeftReefCamResult() { + var result = backLeftReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackRightReefCamResult() { + var result = backRightReefCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontSwerveCamResult() { + var result = frontSwerveCam.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontBargeCamResult() { + var result = frontBargeCam.getAllUnreadResults(); + if (!result.isEmpty()) { + frontBargeCamTrack = true; + return result.get(0); + } else { + frontBargeCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getBackBargeCamResult() { + var result = backBargeCam.getAllUnreadResults(); + if (!result.isEmpty()) { + backBargeCamTrack = true; + return result.get(0); + } else { + backBargeCamTrack = false; + return new PhotonPipelineResult(); + } + } + + public PhotonPipelineResult getFrontGamePieceCamResult() { + var result = gamePieceCamera.getAllUnreadResults(); + if (!result.isEmpty()) { + return result.get(0); + } else { + return new PhotonPipelineResult(); + } + } + + // public Pose3d getFrontReefCamPnPPose() { + // // Logger.recordOutput("April Tag", aprilTagFieldLayout.getTagPose(1).get()); + // var result = frontReefCam.getLatestResult(); + // Optional multiTagResult = + // photonPoseEstimator.update(result); + // if (multiTagResult.isPresent()) { + // Pose3d robotPose = multiTagResult.get().estimatedPose; + // Logger.recordOutput("multitag result", robotPose); + // return robotPose; + // } else { + // Pose3d robotPose = new Pose3d(); + // return robotPose; + // } + // // var result = frontReefCam.getLatestResult(); + // // Logger.recordOutput("Is presetn", result.getMultiTagResult().isPresent()); + // // if (result.getMultiTagResult().isPresent()) { + // // Transform3d fieldToCamera = + // // result.getMultiTagResult().get().estimatedPose.best; + // // Logger.recordOutput("Camera pose: ", fieldToCamera); + // // return fieldToCamera; + // // } else { + // // System.out.println("in else"); + // // return new Transform3d(); + // // } + // } + + public double getFrontReefCamLatency() { + return frontReefCam.getLatestResult().getTimestampSeconds(); + } + + /** + * Sets the IMU angle to 0 + */ + public void zeroPigeon() { + setPigeonAngle(0.0); + } + + /** + * Sets the angle of the IMU + * + * @param degrees - Angle to be set to the IMU + */ + public void setPigeonAngle(double degrees) { + pigeon.setYaw(degrees); + pigeonExtra.setYaw(degrees); + } + + /** + * Retrieves the yaw of the robot + * + * @return Yaw in degrees + */ + public double getPigeonAngle() { + return pigeon.getYaw().getValueAsDouble(); + } + + public double getPigeonExtraAngle() { + return pigeonExtra.getYaw().getValueAsDouble(); + } + + /** + * Retrieves the absolute angular velocity of the IMU's Z-axis in device + * coordinates. + * + * @return The absolute angular velocity of the IMU's Z-axis in device + * coordinates. + * The value is in degrees per second. + */ + public double getPigeonAngularVelocity() { + return Math.abs(pigeon.getAngularVelocityZDevice().getValueAsDouble()); + } + + /** + * Retrieves the absolute angular velocity of the IMU's Z-axis in world + * coordinates. + * + * @return The absolute angular velocity of the IMU's Z-axis in world + * coordinates. + * The value is in radians per second. + */ + public double getPigeonAngularVelocityW() { + return pigeon.getAngularVelocityZWorld().getValueAsDouble(); + } + + /** + * Retrieves the acceleration vector of the robot + * + * @return Current acceleration vector of the robot + */ + public Vector getPigeonLinAccel() { + Vector accelVector = new Vector(); + accelVector.setI(pigeon.getAccelerationX().getValueAsDouble() / Constants.Physical.GRAVITY_ACCEL_MS2); + accelVector.setJ(pigeon.getAccelerationY().getValueAsDouble() / Constants.Physical.GRAVITY_ACCEL_MS2); + return accelVector; + } + + public double getPigeonPitch() { + return pigeon.getPitch().getValueAsDouble(); + } + + public double getPigeonPitchAdjusted() { + return getPigeonPitch() - pigeonPitchOffset; + } + + double pigeonPitchOffset = 0.0; + + public void setPigeonPitchOffset(double newOffset) { + pigeonPitchOffset = newOffset; + } + + double cameraScreenshotTime = 0.0; + + public void periodic() { + Logger.recordOutput("Pigeon Pitch", getPigeonPitchAdjusted()); + + // Use to take snapshots of camera stream (Output means processed stream, input + // means raw stream) + // if (Timer.getFPGATimestamp() - cameraScreenshotTime > 1.0 && + // (DriverStation.isEnabled())) { + // gamePieceCamera.takeOutputSnapshot(); + // cameraScreenshotTime = Timer.getFPGATimestamp(); + // } + + // Logger.recordOutput("Pidgeon Yaw?", pigeon.getYaw().getValueAsDouble()); + // Logger.recordOutput("Pidgeon Pitch?", pigeon.getPitch().getValueAsDouble()); + // Logger.recordOutput("Pidgeon Roll?", pigeon.getRoll().getValueAsDouble()); + // TODO: uncomment if you want to see if the cameras have a track + // Logger.recordOutput("Front Cam Track", frontReefCamTrack); + // Logger.recordOutput("Back Cam Track", backReefCamTrack); + // Logger.recordOutput("Right Cam Track", frontBargeCamTrack); + // Logger.recordOutput("Left Cam Track", backBargeCamTrack); + + // getFrontReefCamPnPPose(); //TODO: uncomment when using camera + // var result = frontReefCam.getLatestResult(); + // if (result.hasTargets()) { + // PhotonTrackedTarget target = result.getBestTarget(); + // Pose2d robotPose = getRobotPoseViaTrig(target, + // Constants.Vision.FRONT_CAMERA_POSE, getPigeonAngle()); + // Logger.recordOutput("Trig Localiazation", robotPose); + // } + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java b/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..b8bbb83 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,111 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import java.util.function.BooleanSupplier; +import java.util.function.IntFunction; + +import javax.lang.model.util.ElementScanner14; + +import org.littletonrobotics.junction.Logger; + +import com.fasterxml.jackson.databind.ser.BeanSerializer; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.OI; +import frc.robot.Robot; +import frc.robot.subsystems.Drive.DriveState; + +public class Superstructure extends SubsystemBase { + /** Creates a new Superstructure. */ + public enum SuperState { + DEFAULT, + IDLE + } + + private SuperState wantedSuperState = SuperState.IDLE; + private SuperState currentSuperState = SuperState.IDLE; + private boolean pathCompleted = false; + + public boolean algaeMode = false; + + private boolean manipulatorHasCoral = false; + private BooleanSupplier coralInManipulator = () -> manipulatorHasCoral; + + Drive drive; + Peripherals peripherals; + + public enum CurrentMode { + ALGAE, + CORAL + } + + public CurrentMode currentMode = CurrentMode.CORAL; + + public Superstructure(Drive driveSubsystem, Peripherals peripheralSubsystem) { + drive = driveSubsystem; + peripherals = peripheralSubsystem; + + } + + public BooleanSupplier hasCoralInManipulator() { + return coralInManipulator; + } + + public void setManipulatorHasCoral(boolean hasCoral) { + coralInManipulator = () -> hasCoral; + } + + public void setCurrentMode(CurrentMode mode) { + currentMode = mode; + } + + public void setWantedState(SuperState wantedState) { + System.out.println("Wanted State: " + wantedState); + this.wantedSuperState = wantedState; + + } + + public SuperState getCurrentSuperState() { + return currentSuperState; + } + + public boolean isPathCompleted() { + return pathCompleted; + } + + private double backUpTime = Timer.getFPGATimestamp(); + + private void applyStates() { + switch (currentSuperState) { + case DEFAULT: + drive.setWantedState(DriveState.DEFAULT); + break; + } + } + + private SuperState handleStateTransitions() { + switch (wantedSuperState) { + case DEFAULT: + currentSuperState = SuperState.DEFAULT; + break; + + default: + currentSuperState = SuperState.DEFAULT; + break; + } + return currentSuperState; + } + + @Override + public void periodic() { + Logger.recordOutput("SuperStructure State", currentSuperState); + currentSuperState = handleStateTransitions(); + applyStates(); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java b/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..19e6198 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,534 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.tools.math.Vector; + +// **Zero Wheels with the bolt head showing on the left when the front side(battery) is facing down/away from you** + +public class SwerveModule extends SubsystemBase { + private final TalonFX angleMotor; + private final TalonFX driveMotor; + private final int moduleNumber; + private final CANcoder canCoder; + + PositionTorqueCurrentFOC positionTorqueFOCRequest = new PositionTorqueCurrentFOC(0); + VelocityTorqueCurrentFOC velocityTorqueFOCRequest = new VelocityTorqueCurrentFOC(0); + VelocityTorqueCurrentFOC velocityTorqueFOCRequestAngleMotor = new VelocityTorqueCurrentFOC(0); + // VelocityTorqueCurrentFOC velocityTorqueFOCRequestDriveMotorStop = new + // VelocityTorqueCurrentFOC(0); + + boolean swerveCan1; + boolean swerveCan2; + boolean swerveCan3; + boolean swerveCan4; + boolean frontRight; + boolean frontLeft; + boolean backLeft; + boolean backRight; + boolean can1; + boolean can2; + boolean can3; + boolean can4; + + /** + * Constructs a new SwerveModule instance. + * + * @param mModuleNum The module number. + * @param mAngleMotor The TalonFX motor used for controlling the angle of the + * module. + * @param mDriveMotor The TalonFX motor used for driving the module. + * @param mCanCoder The CANCoder sensor used for feedback control. + */ + public SwerveModule(int mModuleNum, TalonFX mAngleMotor, TalonFX mDriveMotor, CANcoder mCanCoder) { + // creates values for a single module + moduleNumber = mModuleNum; + angleMotor = mAngleMotor; + driveMotor = mDriveMotor; + canCoder = mCanCoder; + } + + /** + * Calculates the torque angle for the swerve module. + * + * @return The torque angle in radians. + */ + public double torqueAngle() { + // math to find turn angle + double length = Constants.Physical.ROBOT_LENGTH / 2, width = Constants.Physical.ROBOT_WIDTH / 2, angle; + length -= Constants.Physical.MODULE_OFFSET; + width -= Constants.Physical.MODULE_OFFSET; + + switch (moduleNumber) { + case 1: + angle = (Math.atan2(-width, length)) - Math.PI; + break; + case 2: + angle = Math.atan2(width, length); + break; + case 3: + angle = Math.PI + Math.atan2(width, -length); + break; + case 4: + angle = (2 * Math.PI) + (Math.atan2(-width, -length)); + break; + default: + angle = 1; + } + return angle; + } + + public void init() { + // sets all of the configurations for the motors + TalonFXConfiguration angleMotorConfig = new TalonFXConfiguration(); + TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration(); + + angleMotorConfig.Slot0.kP = 370.0; + angleMotorConfig.Slot0.kI = 0.0; + angleMotorConfig.Slot0.kD = 15; + + angleMotorConfig.Slot1.kP = 3.0; + angleMotorConfig.Slot1.kI = 0.0; + angleMotorConfig.Slot1.kD = 0.0; + + angleMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 70; + angleMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -70; + + angleMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + angleMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; + + angleMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + angleMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + angleMotorConfig.Feedback.FeedbackRemoteSensorID = canCoder.getDeviceID(); + angleMotorConfig.Feedback.SensorToMechanismRatio = 1.0; + angleMotorConfig.Feedback.RotorToSensorRatio = Constants.Ratios.STEER_GEAR_RATIO; + + driveMotorConfig.Slot0.kP = 8.0; + driveMotorConfig.Slot0.kI = 0.0; + driveMotorConfig.Slot0.kD = 0.0; + driveMotorConfig.Slot0.kV = 0.0; + + // driveMotorConfig.Slot1.kP = 4.0; + // driveMotorConfig.Slot1.kI = 0.0; + // driveMotorConfig.Slot1.kD = 0.0; + // driveMotorConfig.Slot1.kV = 0.0; + + driveMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 120; + driveMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -120; + driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + driveMotorConfig.CurrentLimits.StatorCurrentLimit = 120; + driveMotorConfig.CurrentLimits.SupplyCurrentLimit = 120; + + driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + driveMotorConfig.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1; + + driveMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + double absolutePosition = canCoder.getAbsolutePosition().getValueAsDouble(); + angleMotor.setPosition(absolutePosition); + driveMotor.setPosition(0.0); + + angleMotor.getConfigurator().apply(angleMotorConfig); + driveMotor.getConfigurator().apply(driveMotorConfig); + + velocityTorqueFOCRequestAngleMotor.Slot = 1; + // velocityTorqueFOCRequestDriveMotorStop.Slot = 1; + } + + public void setDriveCurrentLimits(double supply, double stator) { + CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); + currentLimitsConfigs.StatorCurrentLimitEnable = true; + currentLimitsConfigs.SupplyCurrentLimitEnable = true; + currentLimitsConfigs.StatorCurrentLimit = stator; + currentLimitsConfigs.SupplyCurrentLimit = supply; + driveMotor.getConfigurator().apply(currentLimitsConfigs); + } + + /** + * Sets the position and velocity PID for the swerve module's wheel. + * + * @param angle The desired angle for the wheel. + * @param velocity The desired velocity for the wheel. + */ + public void setWheelPID(double angle, double velocity) { + // method used to move wheel + angleMotor.setControl(positionTorqueFOCRequest.withPosition(degreesToRotations(Math.toDegrees(angle)))); + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(wheelToDriveMotorRotations(velocity))); + } + + /** + * Sets the drive motor velocity + * + * @param velocity - Wheel Velocity in RPS + */ + public void setDrivePID(double velocity) { + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(wheelToDriveMotorRotations(velocity))); + } + + /** + * Converts wheel to motor rotations for the steer motor + * + * @param rotations - Wheel angle in rotations + * @return - Motor angle in rotations + */ + public double wheelToSteerMotorRotations(double rotations) { + return (rotations * Constants.Ratios.STEER_GEAR_RATIO); + } + + /** + * Converts motor to wheel rotations for the steer motor + * + * @param rotations - Motor angle in rotations + * @return Wheel angle in rotations + */ + public double steerMotorToWheelRotations(double rotations) { + return (rotations / Constants.Ratios.STEER_GEAR_RATIO); + } + + /** + * Converts wheel to motor rotations for the drive motor + * + * @param rotations - Wheel rotations + * @return Motor rotations + */ + public double wheelToDriveMotorRotations(double rotations) { + return (rotations * Constants.Ratios.DRIVE_GEAR_RATIO); + } + + /** + * Converts wheel to motor rotations for the drive motor + * + * @param rotations - Motor rotations + * @return Wheel rotations + */ + public double driveMotorToWheelRotations(double rotations) { + return (rotations / Constants.Ratios.DRIVE_GEAR_RATIO); + } + + public double degreesToRotations(double degrees) { + return degrees / 360; + } + + public double rotationsToDegrees(double rotations) { + return rotations * 360; + } + + /** + * Ground meters to wheel rotations + * + * @param mps - Meters Per Second + * @return - Wheel Rotations per Second + */ + public double MPSToRPS(double mps) { + return (mps * Constants.Physical.WHEEL_ROTATION_PER_METER); + } + + /** + * Ground meters to wheel rotations + * + * @param rps - Meters Per Second + * @return - Wheel Rotations per Second + */ + public double RPSToMPS(double rps) { + return (rps / Constants.Physical.WHEEL_ROTATION_PER_METER); + } + + public void moveAngleMotor(double speed) { + angleMotor.set(0.2); + } + + public void moveDriveMotor(double speed) { + driveMotor.set(-0.5); + } + + /** + * Gets the distance traveled by the swerve module's wheel. + * + * @return The distance traveled by the wheel in meters. + */ + public double getModuleDistance() { + double position = driveMotor.getPosition().getValueAsDouble(); + // double wheelRotations = (position * Constants.Wheel_Rotations_In_A_Meter) / + // Constants.GEAR_RATIO; + double wheelRotations = driveMotorToWheelRotations(position); + double distance = RPSToMPS(wheelRotations); + return distance; + } + + /** + * Retrieves Wheel Angle + * + * @return Wheel angle in radians + */ + public double getWheelPosition() { + double position = angleMotor.getPosition().getValueAsDouble(); + return Constants.rotationsToRadians(position); + } + + /** + * Retrieves the current wheel velocity + * + * @return Wheel velocity in RPS + */ + public double getWheelSpeed() { + double speed = driveMotorToWheelRotations(driveMotor.getVelocity().getValueAsDouble()); + return speed; + } + + /** + * Retrieves the velocity of the angle of the wheel + * + * @return Wheel Angle Velocity in RPS + */ + public double getAngleVelocity() { + return angleMotor.getVelocity().getValueAsDouble(); + } + + /** + * Retrieves the velocity of the drive motor rotor + * + * @return Rotor velocity (RPS) + */ + public double getWheelSpeedWithoutGearRatio() { + return driveMotor.getVelocity().getValueAsDouble(); + } + + /** + * Retrieves the angle of the motor rotor + * + * @return Angle Rotor Position (radians) + */ + public double getAngleMotorPosition() { + double degrees = rotationsToDegrees(wheelToSteerMotorRotations(angleMotor.getPosition().getValueAsDouble())); + return (Math.toRadians(degrees)); + } + + /** + * Retrieves the absolute position of the CANCoder + * + * @return CANCoder absolute position (rotations) + */ + public double getCanCoderPosition() { + return canCoder.getAbsolutePosition().getValueAsDouble(); + } + + /** + * Retrieves the absolute position of the CANCoder + * + * @return CANCoder absolute position (radians) + */ + public double getCanCoderPositionRadians() { + return Constants.rotationsToRadians(canCoder.getAbsolutePosition().getValueAsDouble()); + } + + /** + * Retrieves the current ground speed of the swerve module + * + * @return Speed (MPS) + */ + public double getGroundSpeed() { + return RPSToMPS(getWheelSpeed()); + } + + /** + * Retrieves the setpoint of the angle motor + * + * @return Angle motor setpoint (rotations) + */ + public double getAngleMotorSetpoint() { + return angleMotor.getClosedLoopReference().getValue(); + } + + /** + * Retrieves the setpoint of the drive motor + * + * @return Drive motor setpoint (MPS) + */ + public double getDriveMotorSetpoint() { + return RPSToMPS(driveMotorToWheelRotations(driveMotor.getClosedLoopReference().getValue())); + } + + /** + * Calculates the angle of the joystick input relative to the positive y-axis. + * + * @param joystickY The y-component of the joystick input. + * @param joystickX The x-component of the joystick input. + * @return The angle of the joystick input in radians, relative to the positive + * y-axis. + */ + public double getJoystickAngle(double joystickY, double joystickX) { + double joystickAngle = Math.atan2(-joystickX, -joystickY); + return joystickAngle; + } + + /** + * Calculates the position magnitude of the joystick input. + * + * @param joystickY The y-component of the joystick input. + * @param joystickX The x-component of the joystick input. + * @return The magnitude of the joystick position. + */ + public double getJoystickPosition(double joystickY, double joystickX) { + double position = joystickY * joystickY + joystickX * joystickX; + return position; + } + + /** + * Drives the swerve module based on the given control vector, turn value, and + * current orientation. + * + * @param vector The control vector representing the desired direction and + * magnitude of movement. + * @param turnValue The turn value representing the rotation to be applied. + * @param navxAngle The current orientation angle from the IMU sensor. + */ + public void drive(Vector vector, double turnValue, double navxAngle) { + // turnValue = -turnValue; + if (Math.abs(vector.getI()) < 0.001 && Math.abs(vector.getJ()) < 0.001 && Math.abs(turnValue) < 0.01) { + // stops motors when joysticks are at 0 + driveMotor.setControl(velocityTorqueFOCRequest.withVelocity(0.0)); + // driveMotor.setControl(velocityTorqueFOCRequestDriveMotorStop.withVelocity(0.0)); + angleMotor.setControl(velocityTorqueFOCRequestAngleMotor.withVelocity(0.0)); + } else { + double angleWanted = Math.atan2(vector.getJ(), vector.getI()); + double wheelPower = Math.sqrt(Math.pow(vector.getI(), 2) + Math.pow(vector.getJ(), 2)); + + // field centric math + double angleWithNavx = angleWanted + navxAngle; + + double xValueWithNavx = wheelPower * Math.cos(angleWithNavx); + double yValueWithNavx = wheelPower * Math.sin(angleWithNavx); + + double turnX = turnValue * (Constants.angleToUnitVectorI(torqueAngle())); + double turnY = turnValue * (Constants.angleToUnitVectorJ(torqueAngle())); + + // adds turn and strafe vectors + Vector finalVector = new Vector(); + finalVector.setI(xValueWithNavx + turnX); + finalVector.setJ(yValueWithNavx + turnY); + + double finalAngle = -Math.atan2(finalVector.getJ(), finalVector.getI()); + double finalVelocity = Math.sqrt(Math.pow(finalVector.getI(), 2) + Math.pow(finalVector.getJ(), 2)); + + if (finalVelocity > Constants.Physical.TOP_SPEED) { + finalVelocity = Constants.Physical.TOP_SPEED; + } + + double velocityRPS = (MPSToRPS(finalVelocity)); + + double currentAngle = getWheelPosition(); + double currentAngleBelow360 = (getWheelPosition()) % (Math.toRadians(360)); + + // runs angle through optimizer to optimize wheel motion + double setpointAngle = findClosestAngle(currentAngleBelow360, finalAngle); + double setpointAngleFlipped = findClosestAngle(currentAngleBelow360, finalAngle + Math.PI); + + // used to make drive motor move less the more the angle motor is from its + // setpoint + double angleDifference = Math.abs(currentAngleBelow360 - finalAngle); + double adjustedVelocity = ((Math.cos(angleDifference)) * velocityRPS); + + // moves wheel + if (Math.abs(setpointAngle) <= Math.abs(setpointAngleFlipped)) { + setWheelPID(currentAngle + setpointAngle, adjustedVelocity); + } else { + setWheelPID(currentAngle + setpointAngleFlipped, adjustedVelocity); + } + } + } + + /** + * Finds the closest angle between two given angles, considering wrap-around at + * 360 degrees. + * + * @param angleA The first angle. + * @param angleB The second angle. + * @return The closest angle from angle A to angle B, taking into account + * wrap-around. + */ + public double findClosestAngle(double angleA, double angleB) { + double direction = angleB - angleA; + + if (Math.abs(direction) > Math.PI) { + direction = -(Math.signum(direction) * (2 * Math.PI)) + direction; + } + return direction; + } + + @Override + public void periodic() { + // if(driveMotor.getDeviceID() == 1 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 2 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // frontRight = true; + // // System.out.println("front right - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 3 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 4 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // frontLeft = true; + // // System.out.println("front left - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 5 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 6 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // backLeft = true; + // // System.out.println("back left - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + // if(driveMotor.getDeviceID() == 7 && driveMotor.getSupplyVoltage().getValue() + // != 0.0 && angleMotor.getDeviceID() == 8 && + // angleMotor.getSupplyVoltage().getValue() != 0.0){ + // backRight = true; + // // System.out.println("back right - " + + // driveMotor.getSupplyVoltage().getValue() + " + " + + // angleMotor.getSupplyVoltage().getValue()); + // } + + // if(canCoder.getDeviceID() == 1 && canCoder.getSupplyVoltage().getValue() != 0 + // && frontRight == true){ + // swerveCan1 = true; + // // System.out.println("can 1 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 2 && + // canCoder.getSupplyVoltage().getValue() != 0 && frontLeft == true){ + // swerveCan2 = true; + // // System.out.println("can 2 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 3 && + // canCoder.getSupplyVoltage().getValue() != 0 && backLeft == true){ + // swerveCan3 = true; + // // System.out.println("can 3 - " + canCoder.getSupplyVoltage().getValue()); + // } else if(canCoder.getDeviceID() == 4 && + // canCoder.getSupplyVoltage().getValue() != 0 && backRight == true){ + // swerveCan4 = true; + // // System.out.println("can 4 - " + canCoder.getSupplyVoltage().getValue()); + // } + + // SmartDashboard.putBoolean(" Swerve can(1)", swerveCan1); + // SmartDashboard.putBoolean(" Swerve can(2)", swerveCan2); + // SmartDashboard.putBoolean(" Swerve can(3)", swerveCan3); + // SmartDashboard.putBoolean(" Swerve can(4)", swerveCan4); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java new file mode 100644 index 0000000..076c8f5 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java @@ -0,0 +1,52 @@ +package frc.robot.tools; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.util.ArrayList; +import java.util.List; + +import org.json.JSONArray; +import org.json.JSONObject; + +import edu.wpi.first.wpilibj.Filesystem; + +public class PathLoader { + + public static class PosePoint { + public final double x, y, theta, time; + + public PosePoint(double x, double y, double theta, double time) { + this.x = x; + this.y = y; + this.theta = theta; + this.time = time; + } + } + + public static List loadPath(String relativePath) throws IOException { + File file = new File(Filesystem.getDeployDirectory() + "/" + "Paths/" + relativePath); + String content = Files.readString(file.toPath()); + System.out.println(content); + JSONObject root = new JSONObject(content); + JSONArray arr = root.getJSONArray("sampled_points"); + List points = new ArrayList<>(); + for (int i = 0; i < arr.length(); i++) { + JSONObject p = arr.getJSONObject(i); + points.add(new PosePoint( + p.getDouble("x"), + p.getDouble("y"), + p.getDouble("angle"), + p.getDouble("time"))); + } + return points; + } + + public static JSONObject getJSON(String relativePath) throws IOException { + File file = new File(Filesystem.getDeployDirectory() + "/" + "Paths/" + relativePath); + String content = Files.readString(file.toPath()); + JSONObject root = new JSONObject(content); + System.out.println(root); + return root; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java b/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java new file mode 100644 index 0000000..083b805 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/TriggerButton.java @@ -0,0 +1,10 @@ +package frc.robot.tools; + +import java.util.function.BooleanSupplier; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class TriggerButton extends Trigger { + public TriggerButton(BooleanSupplier bs) { + super(bs); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java b/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java new file mode 100644 index 0000000..5b9292d --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java @@ -0,0 +1,233 @@ +package frc.robot.tools.controlloops; + +public class PID { + private double error; + private double totalError; + private double prevError; + + private double PValue; + private double IValue; + private double DValue; + + // Dictates the inputs and outputs + private double maxInput; + private double minInput; + private double maxOutput = 1500;// defaults to 100% and -100% motor power + private double minOutput = -1500; + + private boolean continuous = false; // only for absolute encoders + private double setPoint; // this will be set continuously + private double result; + + public PID(double kp, double ki, double kd) { + PValue = kp; + IValue = ki; + DValue = kd; + } + + /** + * Updates the PID controller with the current sensor value and calculates the + * output. + * + * @param value The current sensor value. + * @return The calculated output based on the PID algorithm. + * + * The PID controller calculates the error between the setpoint and the + * current sensor value. + * It then applies proportional, integral, and derivative terms to the + * error to determine the output. + * + * If the 'continuous' flag is set to true, the error is adjusted to + * handle wraparound situations + * (e.g., when using absolute encoders). + * + * The total error is clamped to prevent integral windup, and the output + * is also clamped to the + * specified minimum and maximum output values. + */ + public double updatePID(double value) { + error = setPoint - value; + if (continuous) { + if (Math.abs(error) > (maxInput - minInput) / 2) { + if (error > 0) { + error = error - maxInput + minInput; + } else { + error = error + maxInput - minInput; + } + } + } + + if ((error * PValue < maxOutput) && (error * PValue > minOutput)) { + totalError += error; + } else { + totalError = 0; + } + + result = (PValue * error + IValue * totalError + DValue * (error - prevError)); + prevError = error; + result = clamp(result); + return result; + } + + /** + * Sets the proportional, integral, and derivative values for the PID + * controller. + * + * @param p The proportional gain. This value determines how much the controller + * responds to the current error. + * @param i The integral gain. This value accumulates the error over time and + * helps to eliminate steady-state errors. + * @param d The derivative gain. This value calculates the rate of change of the + * error and helps to dampen oscillations. + * + * The setPID() function updates the PValue, IValue, and DValue + * variables with the provided values. + * These values are used in the PID control algorithm to calculate the + * output based on the current sensor value and setpoint. + */ + public void setPID(double p, double i, double d) { + PValue = p; + IValue = i; + DValue = d; + } + + /** + * Retrieves the calculated output based on the PID algorithm. + * + * @return The calculated output, which is the result of the PID control + * algorithm. + * This value is clamped to the specified minimum and maximum output + * values. + * + * The getResult() function returns the result of the PID control + * algorithm, which is + * calculated by the updatePID() function. The result is clamped to + * ensure it falls within + * the specified minimum and maximum output values. + */ + public double getResult() { + return result; + } + + /** + * Sets the maximum output value for the PID controller. + * + * @param output The maximum output value. This value is used to clamp the + * calculated PID output. + * The output will be limited to this value if it exceeds it. + * + * The setMaxOutput() function updates the maxOutput variable with + * the provided value. + * This value is used in the PID control algorithm to ensure that + * the calculated output does not exceed the specified maximum + * output value. + * By limiting the output, the PID controller can prevent + * overshooting or saturation issues in the system. + */ + public void setMaxOutput(double output) { + maxOutput = output; + } + + /** + * Sets the minimum output value for the PID controller. + * + * @param output The minimum output value. This value is used to clamp the + * calculated PID output. + * The output will be limited to this value if it falls below it. + * + * The setMinOutput() function updates the minOutput variable with + * the provided value. + * This value is used in the PID control algorithm to ensure that + * the calculated output does not fall below the specified minimum + * output value. + * By limiting the output, the PID controller can prevent + * undershooting or saturation issues in the system. + */ + public void setMinOutput(double output) { + minOutput = output; + } + + /** + * Sets the minimum input value for the PID controller. + * + * @param input The minimum input value. This value is used to handle wraparound + * situations + * when using absolute encoders. If the current sensor value + * exceeds the maximum + * input value minus the minimum input value by more than half, the + * error is adjusted + * to account for the wraparound. + * + * The setMinInput() function updates the minInput variable with + * the provided value. + * This value is used in the PID control algorithm to handle + * wraparound situations when using + * absolute encoders. By setting the minimum input value, the PID + * controller can accurately + * calculate the error and prevent integral windup in such cases. + */ + public void setMinInput(double input) { + minInput = input; + } + + /** + * Sets the maximum input value for the PID controller. + * + * @param input The maximum input value. This value is used to handle wraparound + * situations + * when using absolute encoders. If the current sensor value + * exceeds the maximum + * input value minus the minimum input value by more than half, the + * error is adjusted + * to account for the wraparound. + * + * The setMaxInput() function updates the maxInput variable with + * the provided value. + * This value is used in the PID control algorithm to handle + * wraparound situations when using + * absolute encoders. By setting the minimum input value, the PID + * controller can accurately + * calculate the error and prevent integral windup in such cases. + */ + public void setMaxInput(double input) { + maxInput = input; + } + + /** + * Sets the target value of the PID controller + * + * @param target + */ + public void setSetPoint(double target) { + setPoint = target; + } + + public void setContinuous(boolean value) { + continuous = value; + } + + /** + * Gives the current setpoint of the PID controller + * + * @return The setpoint value of the PID controller + */ + public double getSetPoint() { + return setPoint; + } + + /** + * Clamps the value given between the maximum and minimum output values + * + * @param input The value to clamp + * @return The clamped value + */ + public double clamp(double input) { + if (input > maxOutput) { + return maxOutput; + } + if (input < minOutput) { + return minOutput; + } + return input; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/math/PID.java b/2026-Robot/src/main/java/frc/robot/tools/math/PID.java new file mode 100644 index 0000000..0ed1a29 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/math/PID.java @@ -0,0 +1,100 @@ +package frc.robot.tools.math; + +public class PID { + private double error; + private double totalError; + private double prevError; + + private double PValue; + private double IValue; + private double DValue; + + private double maxInput; + private double minInput; + private double maxOutput = 1.0; + private double minOutput = -1.0; + + private boolean continuous = false; + private double setPoint; + private double result; + + public PID(double kp, double ki, double kd) { + PValue = kp; + IValue = ki; + DValue = kd; + totalError = 0.0; // Initialize total error + } + + public double updatePID(double value) { + error = setPoint - value; + + if (continuous) { + if (Math.abs(error) > (maxInput - minInput) / 2) { + error = error > 0 ? error - (maxInput - minInput) : error + (maxInput - minInput); + } + } + + if (Math.abs(error) < 0.1) { + totalError += error; + totalError = clamp(totalError); + } + + result = PValue * error + IValue * totalError + DValue * (error - prevError); + prevError = error; + result = clamp(result); + return result; + } + + public void setPID(double p, double i, double d) { + PValue = p; + IValue = i; + DValue = d; + } + + public void setSetPoint(double target) { + setPoint = target; + totalError = 0; + } + + public double getSetPoint() { + return setPoint; + } + + public double getResult() { + return result; + } + + public void setMaxOutput(double output) { + maxOutput = output; + } + + public void setMinOutput(double output) { + minOutput = output; + } + + public void setMinInput(double input) { + minInput = input; + } + + public void setMaxInput(double input) { + maxInput = input; + } + + public void setContinuous(boolean value) { + continuous = value; + } + + public double clamp(double input) { + if (input > maxOutput) { + return maxOutput; + } + if (input < minOutput) { + return minOutput; + } + return input; + } + + public double getError() { + return error; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java b/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java new file mode 100644 index 0000000..0da0d96 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/math/Vector.java @@ -0,0 +1,138 @@ +// Copyrights (c) 2018-2019 FIRST, 2020 Highlanders FRC. All Rights Reserved. + +package frc.robot.tools.math; + +import edu.wpi.first.math.geometry.Translation2d; + +public class Vector { + + private double i; + private double j; + + public Vector() { + i = 0; + j = 0; + } + + public Vector(double i, double j) { + this.i = i; + this.j = j; + } + + /** + * Returns the i-component of the vector. + * + * @return The i-component of the vector. + */ + public double getI() { + return i; + } + + /** + * Returns the j-component of the vector. + * + * @return The j-component of the vector. + */ + public double getJ() { + return j; + } + + /** + * Multiplies the vector by a scalar value. + * + *

+ * This method modifies the current vector by multiplying each component (i and + * j) by the given scalar value. + * + * @param s The scalar value to multiply the vector by. + */ + public void scalarMultiple(double s) { + this.i = this.i * s; + this.j = this.j * s; + } + + /** + * Calculates the dot product of this vector with another vector. + * + *

+ * The dot product of two vectors is a scalar value that represents the product + * of their magnitudes and the cosine of the angle + * between them. It is calculated as the sum of the product of corresponding + * components of the two vectors. + * + * @param u The other vector with which to calculate the dot product. + * @return The dot product of this vector with the given vector {@code u}. + */ + public double dot(Vector u) { + return i * u.getI() + j * u.getJ(); + } + + /** + * Calculates the magnitude (or length) of the vector. + * + *

+ * The magnitude of a vector is a non-negative scalar value that represents the + * length of the vector. + * It is calculated as the square root of the sum of the squares of its + * components (i and j). + * + * @return The magnitude (or length) of the vector. + */ + public double magnitude() { + return Math.sqrt(this.dot(this)); + } + + /** + * Sets the i-component of the vector. + * + *

+ * This method modifies the current vector by setting the i-component to the + * given value. + * + * @param i The new value for the i-component of the vector. + */ + public void setI(double i) { + this.i = i; + } + + /** + * Sets the j-component of the vector. + * + *

+ * This method modifies the current vector by setting the j-component to the + * given value. + * + * @param j The new value for the j-component of the vector. + */ + public void setJ(double j) { + this.j = j; + } + + public Vector add(Vector other) { + return new Vector(this.i + other.getI(), this.j + other.getJ()); + } + + public Vector subtract(Vector other) { + return new Vector(this.i - other.getI(), this.j - other.getJ()); + } + + public Vector scaled(double scalar) { + return new Vector(this.i * scalar, this.j * scalar); + } + + public Vector projectOther(Vector other) { + double dotProduct = dot(other); + double magnitudeSquared = dot(this); + return scaled(dotProduct / magnitudeSquared); + } + + public Translation2d getClosestPointOnLine(Translation2d pointOnLine, Translation2d pointOffLine) { + Vector toPoint = new Vector(pointOffLine.getX() - pointOnLine.getX(), pointOffLine.getY() - pointOnLine.getY()); + Vector projVector = projectOther(toPoint); // Project onto the line direction + return new Translation2d(pointOnLine.getX() + projVector.i, pointOnLine.getY() + projVector.j); + } + + public Vector perpendicular() { + return new Vector(-j, i); + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java b/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java new file mode 100644 index 0000000..aa65cc0 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/wrappers/AutoFollower.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.tools.wrappers; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +public abstract class AutoFollower extends Command { + /** Creates a new PolarTakeDrive. */ + public abstract int getPathPointIndex(); + + public abstract void initialize(); + + @Override + public abstract void execute(); + + @Override + public abstract void end(boolean interrupted); + + public abstract void from(int pointIndex, JSONObject pathJSON, int toIndex); + + @Override + abstract public boolean isFinished(); +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/AdvantageKit.json b/2026-Robot/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..bef4a15 --- /dev/null +++ b/2026-Robot/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "4.1.2", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2025", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "4.1.2" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "4.1.2", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json b/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json new file mode 100644 index 0000000..6f40c84 --- /dev/null +++ b/2026-Robot/vendordeps/Phoenix6-frc2025-latest.json @@ -0,0 +1,479 @@ +{ + "fileName": "Phoenix6-frc2025-latest.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "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-latest.json", + "conflictsWith": [ + { + "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-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/2026-Robot/vendordeps/WPILibNewCommands.json b/2026-Robot/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..3718e0a --- /dev/null +++ b/2026-Robot/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/2026-Robot/vendordeps/photonlib.json b/2026-Robot/vendordeps/photonlib.json new file mode 100644 index 0000000..2d7b1d8 --- /dev/null +++ b/2026-Robot/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +} \ No newline at end of file From 057e2d00d840fda1039b60023d15d256aa211f04 Mon Sep 17 00:00:00 2001 From: Dhairya Trivedi <140292400+Dhairya-exe@users.noreply.github.com> Date: Fri, 12 Dec 2025 18:25:08 -0700 Subject: [PATCH 2/2] fixes for the pr --- 2026-Robot/src/main/deploy/sample_path.json | 29 +- .../src/main/java/frc/robot/Constants.java | 8452 ++++++++--------- 2026-Robot/src/main/java/frc/robot/OI.java | 40 +- .../java/frc/robot/commands/FollowPath.java | 14 +- .../src/main/java/frc/robot/commands/Log.java | 43 + .../robot/commands/PurePursuitFollowPath.java | 5 +- .../java/frc/robot/commands/SetOdometry.java | 34 + .../main/java/frc/robot/subsystems/Drive.java | 22 +- .../main/java/frc/robot/tools/PathLoader.java | 11 - .../main/java/frc/robot/tools/PosePoint.java | 12 + .../frc/robot/tools/controlloops/PID.java | 233 - 11 files changed, 4359 insertions(+), 4536 deletions(-) create mode 100644 2026-Robot/src/main/java/frc/robot/commands/Log.java create mode 100644 2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java create mode 100644 2026-Robot/src/main/java/frc/robot/tools/PosePoint.java delete mode 100644 2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java diff --git a/2026-Robot/src/main/deploy/sample_path.json b/2026-Robot/src/main/deploy/sample_path.json index 6e1c0db..2da78a2 100644 --- a/2026-Robot/src/main/deploy/sample_path.json +++ b/2026-Robot/src/main/deploy/sample_path.json @@ -7,7 +7,8 @@ "index": 0, "type": "CommandBlock", "commands": [ - "PurePursuitFollowPath" + "PurePursuitFollowPath", + "Log" ], "arguments": { "points": [ @@ -20,12 +21,13 @@ }, { "index": 1, - "x": 1, + "x": 2, "y": 0, "angle": 0, "time": 1.0 } - ] + ], + "message": "Path One" } }, { @@ -33,7 +35,7 @@ "type": "Switch", "condition": "should_turn_left", "conditionArguments": { - "value": 41 + "value": 67 }, "onTrue": [ { @@ -45,14 +47,14 @@ "points": [ { "index": 0, - "x": 1, + "x": 2, "y": 0, "angle": 0, "time": 1.0 }, { "index": 1, - "x": 1, + "x": 2, "y": 1, "angle": 0, "time": 2.0 @@ -67,18 +69,19 @@ "commands": [ "FollowPath" ], + "condition": "should_turn_left", "arguments": { "points": [ { "index": 0, - "x": 1, + "x": 2, "y": 0, "angle": 0, "time": 1.0 }, { "index": 1, - "x": 1, + "x": 2, "y": -1, "angle": 0, "time": 2.0 @@ -87,6 +90,16 @@ } } ] + }, + { + "index": 2, + "type": "CommandBlock", + "commands": [ + "Log" + ], + "arguments": { + "message": "Auto Complete" + } } ] } \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/Constants.java b/2026-Robot/src/main/java/frc/robot/Constants.java index adafbec..10c7c8c 100644 --- a/2026-Robot/src/main/java/frc/robot/Constants.java +++ b/2026-Robot/src/main/java/frc/robot/Constants.java @@ -18,4299 +18,4299 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public final class Constants { - public static final class Autonomous { - public static final int STAGNATE_BOOST = 25; - public static final int STAGNATE_THRESHOLD = 8; // Number of cycles of stagnation before ending path - // lookahead distance is a function: - // LOOKAHEAD = AUTONOMOUS_LOOKAHEAD_DISTANCE * velocity + MIN_LOOKAHEAD_DISTANCE - // their constants - public static final double AUTONOMOUS_LOOKAHEAD_DISTANCE = 0.04; // Lookahead at 1m/s scaled by wanted - // velocity - public static final double FULL_SEND_LOOKAHEAD = 0.60; - public static final double MIN_LOOKAHEAD_DISTANCE = 0.05; // Lookahead distance at 0m/s - // Path follower will end if within this radius of the final point - public static final double AUTONOMOUS_END_ACCURACY = 0.40; - public static final double ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY = 0.05; - // When calculating the point distance, will divide x and y by this constant - public static final double AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS = 1.0; - // When calculating the point distance, will divide theta by this constant - public static final double AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS = 4 * Math.PI; - // Feed Forward Multiplier - public static final double FEED_FORWARD_MULTIPLIER = 0.8044; - public static final double ACCURATE_FOLLOWER_FEED_FORWARD_MULTIPLIER = 1; - public static String[] paths; - - static { - ArrayList autoPaths = new ArrayList<>(); - File[] dir = Filesystem.getDeployDirectory().listFiles(); - for (File file : dir) { - if (file.getName().contains(".polarauto")) { - autoPaths.add(file.getName()); + public static final class Autonomous { + public static final int STAGNATE_BOOST = 25; + public static final int STAGNATE_THRESHOLD = 8; // Number of cycles of stagnation before ending path + // lookahead distance is a function: + // LOOKAHEAD = AUTONOMOUS_LOOKAHEAD_DISTANCE * velocity + MIN_LOOKAHEAD_DISTANCE + // their constants + public static final double AUTONOMOUS_LOOKAHEAD_DISTANCE = 0.04; // Lookahead at 1m/s scaled by wanted + // velocity + public static final double FULL_SEND_LOOKAHEAD = 0.60; + public static final double MIN_LOOKAHEAD_DISTANCE = 0.05; // Lookahead distance at 0m/s + // Path follower will end if within this radius of the final point + public static final double AUTONOMOUS_END_ACCURACY = 0.40; + public static final double ACCURATE_FOLLOWER_AUTONOMOUS_END_ACCURACY = 0.05; + // When calculating the point distance, will divide x and y by this constant + public static final double AUTONOMOUS_LOOKAHEAD_LINEAR_RADIUS = 1.0; + // When calculating the point distance, will divide theta by this constant + public static final double AUTONOMOUS_LOOKAHEAD_ANGULAR_RADIUS = 4 * Math.PI; + // Feed Forward Multiplier + public static final double FEED_FORWARD_MULTIPLIER = 0.8044; + public static final double ACCURATE_FOLLOWER_FEED_FORWARD_MULTIPLIER = 1; + public static String[] paths; + + static { + ArrayList autoPaths = new ArrayList<>(); + File[] dir = Filesystem.getDeployDirectory().listFiles(); + for (File file : dir) { + if (file.getName().contains(".json")) { + autoPaths.add(file.getName()); + } + } + paths = new String[autoPaths.size()]; + for (int i = 0; i < autoPaths.size(); i++) { + paths[i] = autoPaths.get(i); + } } - } - paths = new String[autoPaths.size()]; - for (int i = 0; i < autoPaths.size(); i++) { - paths[i] = autoPaths.get(i); - } - } - public static int getSelectedPathIndex() { - String path = OI.getSelectedPath(); - if (path.equals("None")) { - return -1; - } - for (int i = 0; i < paths.length; i++) { - if (path.equals(paths[i])) { - return i; + public static int getSelectedPathIndex() { + String path = OI.getSelectedPath(); + if (path.equals("None")) { + return -1; + } + for (int i = 0; i < paths.length; i++) { + if (path.equals(paths[i])) { + return i; + } + } + return -1; } - } - return -1; - } - - } - public static void periodic() { - int index = Autonomous.getSelectedPathIndex(); - if (index == -1 || index > Constants.Autonomous.paths.length) { - Logger.recordOutput("Selected Auto", "Do Nothing"); - } else { - Logger.recordOutput("Selected Auto", Autonomous.paths[index]); } - } - - public static void init() { - - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint1); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint2); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint3); - // /////// - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint4); - // // Only have these 4 now - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint5); - // // The rest are 0, 0 - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint6); - // /////// - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint7); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint8); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint9); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint10); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint11); - // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint12); - - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint1); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint2); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint3); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint4); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint5); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint6); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint7); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint8); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint9); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint10); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint11); - // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint12); - - // for (int i = 0; i < Constants.Vision.redSideReefTags.length; i++) { - // Vector tagVector = new Vector(Constants.Vision.redSideReefTags[i][0], - // Constants.Vision.redSideReefTags[i][1]); - // Vector offsetXVector = new Vector( - // Constants.Physical.CORAL_PLACEMENT_X * - // Math.cos(Constants.Vision.redSideReefTags[i][3]), - // Constants.Physical.CORAL_PLACEMENT_X * - // Math.sin(Constants.Vision.redSideReefTags[i][3])); - // Vector offsetYVector = new Vector( - // Constants.Physical.CORAL_PLACEMENT_Y * - // Math.sin(Constants.Vision.redSideReefTags[i][3]), - // Constants.Physical.CORAL_PLACEMENT_Y * - // Math.cos(Constants.Vision.redSideReefTags[i][3])); - // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); - // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); - // Constants.Physical.redCoralScoringPositions - // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), - // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); - // Constants.Physical.redCoralScoringPositions - // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), - // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); - // } - // for (int i = 0; i < Constants.Vision.blueSideReefTags.length; i++) { - // Vector tagVector = new Vector(Constants.Vision.blueSideReefTags[i][0], - // Constants.Vision.blueSideReefTags[i][1]); - // Vector offsetXVector = new Vector( - // Constants.Physical.CORAL_PLACEMENT_X * - // Math.cos(Constants.Vision.blueSideReefTags[i][3]), - // Constants.Physical.CORAL_PLACEMENT_X * - // Math.sin(Constants.Vision.blueSideReefTags[i][3])); - // Vector offsetYVector = new Vector( - // Constants.Physical.CORAL_PLACEMENT_Y * - // Math.sin(Constants.Vision.blueSideReefTags[i][3]), - // Constants.Physical.CORAL_PLACEMENT_Y * - // Math.cos(Constants.Vision.blueSideReefTags[i][3])); - // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); - // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); - // Constants.Physical.blueCoralScoringPositions - // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), - // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); - // Constants.Physical.blueCoralScoringPositions - // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), - // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); - // } - - // Logger.recordOutput("red side scoring", - // Constants.Physical.redCoralScoringPositions.toString()); - // Logger.recordOutput("blue side scoring", - // Constants.Physical.blueCoralScoringPositions.toString()); - System.out.println("blue algae front positions: " - + Constants.Reef.algaeBlueFrontPlacingPositions.toString()); - System.out.println("red algae front positions: " - + Constants.Reef.algaeRedFrontPlacingPositions.toString()); - System.out.println("blue algae back positions: " - + Constants.Reef.algaeBlueBackPlacingPositions.toString()); - System.out.println( - "red algae back positions: " + Constants.Reef.algaeRedBackPlacingPositions.toString()); - - System.out.println("blue positions: " + Constants.Reef.blueFrontPlacingPositions.toString()); - System.out.println("red positions: " + Constants.Reef.redFrontPlacingPositions.toString()); - System.out.println("blue back positions: " + Constants.Reef.blueBackPlacingPositions.toString()); - System.out.println("red back positions: " + Constants.Reef.redBackPlacingPositions.toString()); - - System.out.println("l4 blue positions: " + Constants.Reef.l4BlueFrontPlacingPositions.toString()); - System.out.println("l4 red positions: " + Constants.Reef.l4RedFrontPlacingPositions.toString()); - System.out.println("l4 blue back positions: " + Constants.Reef.l4BlueBackPlacingPositions.toString()); - System.out.println("l4 red back positions: " + Constants.Reef.l4RedBackPlacingPositions.toString()); - - System.out.println("l3 blue positions: " + Constants.Reef.l3BlueFrontPlacingPositions.toString()); - System.out.println("l3 red positions: " + Constants.Reef.l3RedFrontPlacingPositions.toString()); - System.out.println("l3 blue back positions: " + Constants.Reef.l3BlueBackPlacingPositions.toString()); - System.out.println("l3 red back positions: " + Constants.Reef.l3RedBackPlacingPositions.toString()); - - System.out.println("L1 Blue Corners: " + Constants.Reef.l1BlueCornerPoints.toString()); - System.out.println("L1 Red Corners: " + Constants.Reef.l1RedCornerPoints.toString()); - - System.out.println("L1 Blue Drive: " + Constants.Reef.l1BlueDrivePoints.toString()); - System.out.println("L1 Red Drive: " + Constants.Reef.l1RedDrivePoints.toString()); - - for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { - Logger.recordOutput("L1 Blue Corners " + i + " ", Constants.Reef.l1BlueDrivePoints.get(i)); + + public static void periodic() { + int index = Autonomous.getSelectedPathIndex(); + if (index == -1 || index > Constants.Autonomous.paths.length) { + Logger.recordOutput("Selected Auto", "Do Nothing"); + } else { + Logger.recordOutput("Selected Auto", Autonomous.paths[index]); + } } - Logger.recordOutput("feeder Positions", new Pose2d[] { Constants.Reef.RED_LEFT_FEEDER_LEFT, - Constants.Reef.RED_RIGHT_FEEDER_RIGHT, Constants.Reef.RED_RIGHT_FEEDER_LEFT, - Constants.Reef.RED_LEFT_FEEDER_RIGHT, }); - } - - public static class Reef { - public static final double PERFECT_BRANCH_OFFSET_L23 = inchesToMeters(1.625); - public static final double PERFECT_BRANCH_OFFSET_L4 = inchesToMeters(1.125); - - // positive is from face of reef towards center of reef - // negative means futher from reef - // public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.5); - // public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.875); - // public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.125); - // public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.25); - // public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.125); - // public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.125); - // public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.125); - // public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.125); - // public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(-0.5940); - // public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(0.375); - // public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(0.7157); - // public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - // inchesToMeters(1.75); - - // public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.5); - // public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.0); - // public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.75); - // public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.0); - // public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.125); - // public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.5); - // public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(3.0); - // public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(3.0); - - // public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.5); - // public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.5); - // public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.875); - // public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.25); - // public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.625); - // public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.25); - // public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(1.75); - // public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.0); - // public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - // inchesToMeters(2.0); - - // // // right when facing the reef side is positive - // // // negative makes robot go more to the left - // public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); // - // TODO: make red and blue - // // seperate - // public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - // public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - - // public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - // public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - - // public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - // public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - - public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - - inchesToMeters(1.125); - - public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - - public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - - inchesToMeters(1.625); - - // right when facing the reef side is positive - // negative makes robot go more to the left - public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); - - public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); - - public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); - public static final Translation2d centerBlue = new Translation2d(inchesToMeters(176.746), - inchesToMeters(158.501)); - public static final Translation2d centerRed = new Translation2d( - Constants.Physical.FIELD_LENGTH - inchesToMeters(176.746), - inchesToMeters(158.501)); - public static final double faceToZoneLine = inchesToMeters(12); // Side of the reef to the inside of the - // reef - // zone line - - public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in - // clockwise - // order - // public static final List> - // blueBranchPositions = new - // ArrayList<>(); // Starting at the right - // // branch facing the driver - // // station in clockwise - // public static final List> - // redBranchPositions = new - // ArrayList<>(); // Starting at the right - - public static final double RED_LEFT_FEEDER_X = 16.28; - public static final double RED_LEFT_FEEDER_Y = 0.92; - public static final double RED_LEFT_FEEDER_THETA = Math.toRadians(126.0); - - public static final double RED_LEFT_FEEDER_X_TELEOP = 16.544; - public static final double RED_LEFT_FEEDER_Y_TELEOP = 0.890; - public static final double RED_LEFT_FEEDER_THETA_TELEOP = Math.toRadians(126.0); - - public static final double RED_LEFT_FEEDER_LEFT_X = 16.873; - public static final double RED_LEFT_FEEDER_LEFT_Y = 1.209; - public static final double RED_LEFT_FEEDER_LEFT_THETA = Math.toRadians(126.0); - public static final double RED_LEFT_FEEDER_RIGHT_X = 15.952; - public static final double RED_LEFT_FEEDER_RIGHT_Y = 0.564; - public static final double RED_LEFT_FEEDER_RIGHT_THETA = Math.toRadians(126.0); - - public static final Pose2d RED_LEFT_FEEDER_LEFT = new Pose2d(RED_LEFT_FEEDER_LEFT_X, - RED_LEFT_FEEDER_LEFT_Y, - new Rotation2d(RED_LEFT_FEEDER_LEFT_THETA)); - public static final Pose2d RED_RIGHT_FEEDER_LEFT = new Pose2d( - RED_LEFT_FEEDER_LEFT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, - new Rotation2d(Math.toRadians(234.0))); - public static final Pose2d BLUE_RIGHT_FEEDER_LEFT = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, RED_LEFT_FEEDER_LEFT_Y, - new Rotation2d(Math.toRadians(54.0))); - public static final Pose2d BLUE_LEFT_FEEDER_LEFT = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, - Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, - new Rotation2d(Math.toRadians(-54.0))); - public static final Pose2d RED_LEFT_FEEDER_RIGHT = new Pose2d(RED_LEFT_FEEDER_RIGHT_X, - RED_LEFT_FEEDER_RIGHT_Y, - new Rotation2d(RED_LEFT_FEEDER_RIGHT_THETA)); - public static final Pose2d RED_RIGHT_FEEDER_RIGHT = new Pose2d( - RED_LEFT_FEEDER_RIGHT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, - new Rotation2d(Math.toRadians(234.0))); - public static final Pose2d BLUE_RIGHT_FEEDER_RIGHT = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, RED_LEFT_FEEDER_RIGHT_Y, - new Rotation2d(Math.toRadians(54.0))); - public static final Pose2d BLUE_LEFT_FEEDER_RIGHT = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, - Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, - new Rotation2d(Math.toRadians(-54.0))); - - public static final Pose2d RED_LEFT_FEEDER = new Pose2d(RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, - new Rotation2d(RED_LEFT_FEEDER_THETA)); - public static final Pose2d RED_RIGHT_FEEDER = new Pose2d( - RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, - new Rotation2d(Math.toRadians(234.0))); - - public static final Pose2d BLUE_RIGHT_FEEDER = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, - new Rotation2d(Math.toRadians(54.0))); - public static final Pose2d BLUE_LEFT_FEEDER = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, - new Rotation2d(Math.toRadians(-54.0))); - - public static final Pose2d RED_LEFT_FEEDER_TELEOP = new Pose2d(RED_LEFT_FEEDER_X_TELEOP, - RED_LEFT_FEEDER_Y_TELEOP, - new Rotation2d(RED_LEFT_FEEDER_THETA_TELEOP)); - public static final Pose2d RED_RIGHT_FEEDER_TELEOP = new Pose2d( - RED_LEFT_FEEDER_X_TELEOP, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, - new Rotation2d(Math.toRadians(234.0))); - - public static final Pose2d BLUE_RIGHT_FEEDER_TELEOP = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, RED_LEFT_FEEDER_Y_TELEOP, - new Rotation2d(Math.toRadians(54.0))); - public static final Pose2d BLUE_LEFT_FEEDER_TELEOP = new Pose2d( - Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, - Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, - new Rotation2d(Math.toRadians(-54.0))); - - public static final double PROCESSOR_Y_OFFSET_M = inchesToMeters(50.0); - public static final double PROCESSOR_MORE_Y_OFFSET_M = inchesToMeters(25.0); - public static final double NET_X_OFFSET_M = inchesToMeters(53.0); - public static final double NET_X_OFFSET_MORE = inchesToMeters(34.0); - - public static final Translation2d processorBlueFrontPlacingTranslation = new Translation2d( - inchesToMeters(238.79), - PROCESSOR_Y_OFFSET_M); - public static final Rotation2d processorBlueFrontPlacingRotation = new Rotation2d( - degreesToRadians(270)); - - public static final Translation2d processorRedFrontPlacingTranslation = new Translation2d( - inchesToMeters(452.40), - inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); - public static final Rotation2d processorRedFrontPlacingRotation = new Rotation2d(degreesToRadians(90)); - - public static final Translation2d processorBlueBackPlacingTranslation = new Translation2d( - inchesToMeters(238.79), - PROCESSOR_Y_OFFSET_M); - public static final Rotation2d processorBlueBackPlacingRotation = new Rotation2d(degreesToRadians(90)); - - public static final Translation2d processorRedBackPlacingTranslation = new Translation2d( - inchesToMeters(452.40), - inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); - public static final Rotation2d processorRedBackPlacingRotation = new Rotation2d(degreesToRadians(270)); - - public static final Translation2d processorMoreBlueFrontPlacingTranslation = new Translation2d( - inchesToMeters(238.79), - PROCESSOR_MORE_Y_OFFSET_M); - public static final Rotation2d processorMoreBlueFrontPlacingRotation = new Rotation2d( - degreesToRadians(270)); - - public static final Translation2d processorMoreRedFrontPlacingTranslation = new Translation2d( - inchesToMeters(452.40), - inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); - public static final Rotation2d processorMoreRedFrontPlacingRotation = new Rotation2d( - degreesToRadians(90)); - - public static final Translation2d processorMoreBlueBackPlacingTranslation = new Translation2d( - inchesToMeters(238.79), - PROCESSOR_MORE_Y_OFFSET_M); - public static final Rotation2d processorMoreBlueBackPlacingRotation = new Rotation2d( - degreesToRadians(90)); - - public static final Translation2d processorMoreRedBackPlacingTranslation = new Translation2d( - inchesToMeters(452.40), - inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); - public static final Rotation2d processorMoreRedBackPlacingRotation = new Rotation2d( - degreesToRadians(270)); - - public static final Pose2d processorBlueFrontPlacingPosition = new Pose2d( - processorBlueFrontPlacingTranslation, - processorBlueFrontPlacingRotation); - public static final Pose2d processorRedFrontPlacingPosition = new Pose2d( - processorRedFrontPlacingTranslation, - processorRedFrontPlacingRotation); - public static final Pose2d processorBlueBackPlacingPosition = new Pose2d( - processorBlueBackPlacingTranslation, - processorBlueBackPlacingRotation); - public static final Pose2d processorRedBackPlacingPosition = new Pose2d( - processorRedBackPlacingTranslation, - processorRedBackPlacingRotation); - - public static final Pose2d processorMoreBlueFrontPlacingPosition = new Pose2d( - processorMoreBlueFrontPlacingTranslation, - processorMoreBlueFrontPlacingRotation); - public static final Pose2d processorMoreRedFrontPlacingPosition = new Pose2d( - processorMoreRedFrontPlacingTranslation, - processorMoreRedFrontPlacingRotation); - public static final Pose2d processorMoreBlueBackPlacingPosition = new Pose2d( - processorMoreBlueBackPlacingTranslation, - processorMoreBlueBackPlacingRotation); - public static final Pose2d processorMoreRedBackPlacingPosition = new Pose2d( - processorMoreRedBackPlacingTranslation, - processorMoreRedBackPlacingRotation); - - public static final double netBlueXM = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_M; - public static final double netRedXM = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_M; - public static final double netBlueXMore = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_MORE; - public static final double netRedXMore = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_MORE; - - public static final double netBlueFrontThetaR = degreesToRadians(0.0); - public static final double netRedFrontThetaR = degreesToRadians(180.0); - public static final double netBlueBackThetaR = degreesToRadians(180.0); - public static final double netRedBackThetaR = degreesToRadians(0.0); - - public static final List blueL1FrontPlacingPositions = new ArrayList<>(); - public static final List redL1FrontPlacingPositions = new ArrayList<>(); - public static final List blueL1BackPlacingPositions = new ArrayList<>(); - public static final List redL1BackPlacingPositions = new ArrayList<>(); - - public static final List blueL1FrontPlacingPositionsMore = new ArrayList<>(); - public static final List redL1FrontPlacingPositionsMore = new ArrayList<>(); - public static final List blueL1BackPlacingPositionsMore = new ArrayList<>(); - public static final List redL1BackPlacingPositionsMore = new ArrayList<>(); - - public static final List blueFrontPlacingPositions = new ArrayList<>(); - public static final List redFrontPlacingPositions = new ArrayList<>(); - public static final List blueBackPlacingPositions = new ArrayList<>(); - public static final List redBackPlacingPositions = new ArrayList<>(); - - public static final List blueFrontPlacingPositionsMore = new ArrayList<>(); - public static final List redFrontPlacingPositionsMore = new ArrayList<>(); - public static final List blueBackPlacingPositionsMore = new ArrayList<>(); - public static final List redBackPlacingPositionsMore = new ArrayList<>(); - - public static final List algaeBlueFrontPlacingPositionsMoreMore = new ArrayList<>(); - public static final List algaeRedFrontPlacingPositionsMoreMore = new ArrayList<>(); - public static final List algaeBlueBackPlacingPositionsMoreMore = new ArrayList<>(); - public static final List algaeRedBackPlacingPositionsMoreMore = new ArrayList<>(); - - public static final List l4BlueFrontPlacingPositions = new ArrayList<>(); - public static final List l4RedFrontPlacingPositions = new ArrayList<>(); - public static final List l4BlueBackPlacingPositions = new ArrayList<>(); - public static final List l4RedBackPlacingPositions = new ArrayList<>(); - - public static final List l3BlueFrontPlacingPositions = new ArrayList<>(); - public static final List l3RedFrontPlacingPositions = new ArrayList<>(); - public static final List l3BlueBackPlacingPositions = new ArrayList<>(); - public static final List l3RedBackPlacingPositions = new ArrayList<>(); - - public static final List algaeBlueFrontPlacingPositions = new ArrayList<>(); - public static final List algaeRedFrontPlacingPositions = new ArrayList<>(); - public static final List algaeBlueBackPlacingPositions = new ArrayList<>(); - public static final List algaeRedBackPlacingPositions = new ArrayList<>(); - - public static final List algaeBlueFrontPlacingPositionsMore = new ArrayList<>(); - public static final List algaeRedFrontPlacingPositionsMore = new ArrayList<>(); - public static final List algaeBlueBackPlacingPositionsMore = new ArrayList<>(); - public static final List algaeRedBackPlacingPositionsMore = new ArrayList<>(); - - public static final List l1BlueCornerPoints = new ArrayList<>(); - public static final List l1RedCornerPoints = new ArrayList<>(); - public static final List l1BlueDrivePoints = new ArrayList<>(); - public static final List l1RedDrivePoints = new ArrayList<>(); - - static { - calculateReefPoints(); + public static void init() { + + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint1); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint2); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint3); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint4); + // // Only have these 4 now + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint5); + // // The rest are 0, 0 + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint6); + // /////// + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint7); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint8); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint9); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint10); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint11); + // Constants.Physical.redCoralScoringPositions.add(Constants.Physical.redSetpoint12); + + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint1); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint2); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint3); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint4); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint5); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint6); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint7); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint8); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint9); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint10); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint11); + // Constants.Physical.blueCoralScoringPositions.add(Constants.Physical.blueSetpoint12); + + // for (int i = 0; i < Constants.Vision.redSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.redSideReefTags[i][0], + // Constants.Vision.redSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.redSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.redSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.redSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // Constants.Physical.redCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.redSideReefTags[i][3] + Math.PI))); + // } + // for (int i = 0; i < Constants.Vision.blueSideReefTags.length; i++) { + // Vector tagVector = new Vector(Constants.Vision.blueSideReefTags[i][0], + // Constants.Vision.blueSideReefTags[i][1]); + // Vector offsetXVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.cos(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_X * + // Math.sin(Constants.Vision.blueSideReefTags[i][3])); + // Vector offsetYVector = new Vector( + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.sin(Constants.Vision.blueSideReefTags[i][3]), + // Constants.Physical.CORAL_PLACEMENT_Y * + // Math.cos(Constants.Vision.blueSideReefTags[i][3])); + // Vector leftVector = tagVector.add(offsetXVector.add(offsetYVector)); + // Vector rightVector = tagVector.add(offsetXVector.subtract(offsetYVector)); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(leftVector.getI(), leftVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // Constants.Physical.blueCoralScoringPositions + // .add(new Pose2d(new Translation2d(rightVector.getI(), rightVector.getJ()), + // new Rotation2d(Constants.Vision.blueSideReefTags[i][3] + Math.PI))); + // } + + // Logger.recordOutput("red side scoring", + // Constants.Physical.redCoralScoringPositions.toString()); + // Logger.recordOutput("blue side scoring", + // Constants.Physical.blueCoralScoringPositions.toString()); + System.out.println("blue algae front positions: " + + Constants.Reef.algaeBlueFrontPlacingPositions.toString()); + System.out.println("red algae front positions: " + + Constants.Reef.algaeRedFrontPlacingPositions.toString()); + System.out.println("blue algae back positions: " + + Constants.Reef.algaeBlueBackPlacingPositions.toString()); + System.out.println( + "red algae back positions: " + Constants.Reef.algaeRedBackPlacingPositions.toString()); + + System.out.println("blue positions: " + Constants.Reef.blueFrontPlacingPositions.toString()); + System.out.println("red positions: " + Constants.Reef.redFrontPlacingPositions.toString()); + System.out.println("blue back positions: " + Constants.Reef.blueBackPlacingPositions.toString()); + System.out.println("red back positions: " + Constants.Reef.redBackPlacingPositions.toString()); + + System.out.println("l4 blue positions: " + Constants.Reef.l4BlueFrontPlacingPositions.toString()); + System.out.println("l4 red positions: " + Constants.Reef.l4RedFrontPlacingPositions.toString()); + System.out.println("l4 blue back positions: " + Constants.Reef.l4BlueBackPlacingPositions.toString()); + System.out.println("l4 red back positions: " + Constants.Reef.l4RedBackPlacingPositions.toString()); + + System.out.println("l3 blue positions: " + Constants.Reef.l3BlueFrontPlacingPositions.toString()); + System.out.println("l3 red positions: " + Constants.Reef.l3RedFrontPlacingPositions.toString()); + System.out.println("l3 blue back positions: " + Constants.Reef.l3BlueBackPlacingPositions.toString()); + System.out.println("l3 red back positions: " + Constants.Reef.l3RedBackPlacingPositions.toString()); + + System.out.println("L1 Blue Corners: " + Constants.Reef.l1BlueCornerPoints.toString()); + System.out.println("L1 Red Corners: " + Constants.Reef.l1RedCornerPoints.toString()); + + System.out.println("L1 Blue Drive: " + Constants.Reef.l1BlueDrivePoints.toString()); + System.out.println("L1 Red Drive: " + Constants.Reef.l1RedDrivePoints.toString()); + + for (int i = 0; i < Constants.Reef.l1BlueDrivePoints.size(); i++) { + Logger.recordOutput("L1 Blue Corners " + i + " ", Constants.Reef.l1BlueDrivePoints.get(i)); + } + + Logger.recordOutput("feeder Positions", new Pose2d[] { Constants.Reef.RED_LEFT_FEEDER_LEFT, + Constants.Reef.RED_RIGHT_FEEDER_RIGHT, Constants.Reef.RED_RIGHT_FEEDER_LEFT, + Constants.Reef.RED_LEFT_FEEDER_RIGHT, }); } - // Angle to face red side: - // 180=AB - // 120=CD - // 60=EF - // 0=GH - // -60=IJ - // -120=KL - - public static void calculateReefPoints() { - blueL1FrontPlacingPositions.clear(); - redL1FrontPlacingPositions.clear(); - blueL1BackPlacingPositions.clear(); - redL1BackPlacingPositions.clear(); - blueL1FrontPlacingPositionsMore.clear(); - redL1FrontPlacingPositionsMore.clear(); - blueL1BackPlacingPositionsMore.clear(); - redL1BackPlacingPositionsMore.clear(); - blueFrontPlacingPositions.clear(); - redFrontPlacingPositions.clear(); - blueBackPlacingPositions.clear(); - redBackPlacingPositions.clear(); - blueFrontPlacingPositionsMore.clear(); - redFrontPlacingPositionsMore.clear(); - blueBackPlacingPositionsMore.clear(); - redBackPlacingPositionsMore.clear(); - l4BlueFrontPlacingPositions.clear(); - l4RedFrontPlacingPositions.clear(); - l4BlueBackPlacingPositions.clear(); - l4RedBackPlacingPositions.clear(); - l3BlueFrontPlacingPositions.clear(); - l3RedFrontPlacingPositions.clear(); - l3BlueBackPlacingPositions.clear(); - l3RedBackPlacingPositions.clear(); - algaeBlueFrontPlacingPositions.clear(); - algaeRedFrontPlacingPositions.clear(); - algaeBlueBackPlacingPositions.clear(); - algaeRedBackPlacingPositions.clear(); - algaeBlueFrontPlacingPositionsMore.clear(); - algaeRedFrontPlacingPositionsMore.clear(); - algaeBlueBackPlacingPositionsMore.clear(); - algaeRedBackPlacingPositionsMore.clear(); - algaeBlueFrontPlacingPositionsMoreMore.clear(); - algaeRedFrontPlacingPositionsMoreMore.clear(); - algaeBlueBackPlacingPositionsMoreMore.clear(); - algaeRedBackPlacingPositionsMoreMore.clear(); - l1BlueCornerPoints.clear(); - l1RedCornerPoints.clear(); - l1BlueDrivePoints.clear(); - l1RedDrivePoints.clear(); - centerFaces[0] = new Pose2d( - inchesToMeters(144.003), - inchesToMeters(158.500), - Rotation2d.fromDegrees(180)); - centerFaces[1] = new Pose2d( - inchesToMeters(160.373), - inchesToMeters(186.857), - Rotation2d.fromDegrees(120)); - centerFaces[2] = new Pose2d( - inchesToMeters(193.116), - inchesToMeters(186.858), - Rotation2d.fromDegrees(60)); - centerFaces[3] = new Pose2d( - inchesToMeters(209.489), - inchesToMeters(158.502), - Rotation2d.fromDegrees(0)); - centerFaces[4] = new Pose2d( - inchesToMeters(193.118), - inchesToMeters(130.145), - Rotation2d.fromDegrees(-60)); - centerFaces[5] = new Pose2d( - inchesToMeters(160.375), - inchesToMeters(130.144), - Rotation2d.fromDegrees(-120)); - - for (int face = 0; face < 6; face++) { - Pose2d l1FrontRight = new Pose2d(); - Pose2d l1FrontLeft = new Pose2d(); - Pose2d l1BackRight = new Pose2d(); - Pose2d l1BackLeft = new Pose2d(); - Pose2d l1FrontRightMore = new Pose2d(); - Pose2d l1FrontLeftMore = new Pose2d(); - Pose2d l1BackRightMore = new Pose2d(); - Pose2d l1BackLeftMore = new Pose2d(); - Pose2d l2FrontRight = new Pose2d(); - Pose2d l2FrontLeft = new Pose2d(); - Pose2d frontRightMore = new Pose2d(); - Pose2d frontLeftMore = new Pose2d(); - Pose2d backRightMore = new Pose2d(); - Pose2d backLeftMore = new Pose2d(); - Pose2d l2BackRight = new Pose2d(); - Pose2d l2BackLeft = new Pose2d(); - Pose2d l3FrontRight = new Pose2d(); - Pose2d l3FrontLeft = new Pose2d(); - Pose2d l3BackRight = new Pose2d(); - Pose2d l3BackLeft = new Pose2d(); - Pose2d l4FrontRight = new Pose2d(); - Pose2d l4FrontLeft = new Pose2d(); - Pose2d l4BackRight = new Pose2d(); - Pose2d l4BackLeft = new Pose2d(); - Pose2d algaeFront = new Pose2d(); - Pose2d algaeBack = new Pose2d(); - Pose2d algaeFrontMore = new Pose2d(); - Pose2d algaeBackMore = new Pose2d(); - Pose2d algaeFrontMoreMore = new Pose2d(); - Pose2d algaeBackMoreMore = new Pose2d(); - Pose2d l1Corner = new Pose2d(); - Pose2d l1Drive = new Pose2d(); - Pose2d poseDirection = new Pose2d(centerBlue, - Rotation2d.fromDegrees(180 - (60 * face))); - double adjustX = inchesToMeters(30.738); - double adjustY = inchesToMeters(6.469); - double adjustXL1 = inchesToMeters(30.738); - double adjustYL1 = inchesToMeters(6.469); - double adjustXMore = inchesToMeters(41.67); - double adjustYMore = inchesToMeters(6.469); - double adjustAlgaeX = inchesToMeters(45.738); - double adjustAlgaeY = inchesToMeters(0.0); - double adjustAlgaeMoreX = inchesToMeters(16.738); - double adjustAlgaeMoreY = inchesToMeters(0.0); - double adjustAlgaeMoreMoreX = inchesToMeters(56.738); - double adjustAlgaeMoreMoreY = inchesToMeters(0.0); - double adjustL1CornerX = inchesToMeters(9.0); - double adjustL1CornerY = inchesToMeters(18.0); - double adjustL1DriveX = inchesToMeters(38.0); - double adjustL1DriveY = inchesToMeters(25.0); - - l1FrontRightMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, - Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, - Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l1BackRightMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK_MORE, - Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK_MORE, - Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - l1FrontLeftMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, - Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, - Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l1BackLeftMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK_MORE, - Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK_MORE, - Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - l1FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT, - Physical.L1_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT, - Physical.L1_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l1BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK, - Physical.L1_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK, - Physical.L1_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - l1FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT, - Physical.L1_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_FRONT, - Physical.L1_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l1BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK, - Physical.L1_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXL1, - -adjustYL1, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L1_INTAKE_X_OFFSET_BACK, - Physical.L1_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - algaeFront = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustAlgaeX, - adjustAlgaeY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustAlgaeX, - adjustAlgaeY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - - algaeBack = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustAlgaeX, - adjustAlgaeY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustAlgaeX, - adjustAlgaeY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - algaeFrontMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreX, - adjustAlgaeMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreX, - adjustAlgaeMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - - algaeBackMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreX, - adjustAlgaeMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO: - // why - // is - // this - // pi - // and - // not - // 0 - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreX, - adjustAlgaeMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - algaeFrontMoreMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreMoreX, - adjustAlgaeMoreMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreMoreX, - adjustAlgaeMoreMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, - Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - - l1Corner = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustL1CornerX, - adjustL1CornerY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustL1CornerX, - adjustL1CornerY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - - l1Drive = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustL1DriveX, - adjustL1DriveY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustL1DriveX, - adjustL1DriveY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - - algaeBackMoreMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreMoreX, - adjustAlgaeMoreMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO: - // why - // is - // this - // pi - // and - // not - // 0 - .getX(), - poseDirection - .transformBy( - new Transform2d(adjustAlgaeMoreMoreX, - adjustAlgaeMoreMoreY, - new Rotation2d())) - .transformBy( - new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, - Physical.INTAKE_Y_OFFSET_BACK_ALGAE, - new Rotation2d(Math.PI))) // TODO - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - frontRightMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXMore, - adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXMore, - adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - backRightMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXMore, - adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_BACK, - Physical.INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXMore, - adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_BACK, - Physical.INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - - // l2FrontRight = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_FRONT, - // Physical.L2_INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_FRONT, - // Physical.L2_INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians() - Math.PI)); - // l2BackRight = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_BACK, - // Physical.L2_INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_BACK, - // Physical.L2_INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians())); + public static class Reef { + public static final double PERFECT_BRANCH_OFFSET_L23 = inchesToMeters(1.625); + public static final double PERFECT_BRANCH_OFFSET_L4 = inchesToMeters(1.125); + + // positive is from face of reef towards center of reef + // negative means futher from reef + // public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.875); + // public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.25); + // public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.125); + // public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(-0.5940); + // public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.375); + // public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(0.7157); + // public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + // inchesToMeters(1.75); + + // public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.125); + // public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + // public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(3.0); + + // public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.5); + // public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.5); + // public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.875); + // public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.25); + // public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.625); + // public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.25); + // public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(1.75); + // public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + // public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + // inchesToMeters(2.0); + + // // // right when facing the reef side is positive + // // // negative makes robot go more to the left + // public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); // + // TODO: make red and blue + // // seperate + // public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + // public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + // public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double B_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double C_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double D_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double E_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double F_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double G_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double H_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double I_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double J_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double K_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + public static final double L_BRANCH_OFFSET = PERFECT_BRANCH_OFFSET_L4 - + inchesToMeters(1.125); + + public static final double A_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L3 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + public static final double A_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double B_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double C_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double D_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double E_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double F_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double G_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double H_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double I_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double J_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double K_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + public static final double L_BRANCH_OFFSET_L2 = PERFECT_BRANCH_OFFSET_L23 - + inchesToMeters(1.625); + + // right when facing the reef side is positive + // negative makes robot go more to the left + public static final double A_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L3 = inchesToMeters(0.0); + + public static final double A_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double B_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double C_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double D_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double E_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double F_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double G_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double H_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double I_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double J_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double K_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final double L_BRANCH_OFFSET_SIDE_L2 = inchesToMeters(0.0); + public static final Translation2d centerBlue = new Translation2d(inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final Translation2d centerRed = new Translation2d( + Constants.Physical.FIELD_LENGTH - inchesToMeters(176.746), + inchesToMeters(158.501)); + public static final double faceToZoneLine = inchesToMeters(12); // Side of the reef to the inside of the + // reef + // zone line + + public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in + // clockwise + // order + // public static final List> + // blueBranchPositions = new + // ArrayList<>(); // Starting at the right + // // branch facing the driver + // // station in clockwise + // public static final List> + // redBranchPositions = new + // ArrayList<>(); // Starting at the right + + public static final double RED_LEFT_FEEDER_X = 16.28; + public static final double RED_LEFT_FEEDER_Y = 0.92; + public static final double RED_LEFT_FEEDER_THETA = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_X_TELEOP = 16.544; + public static final double RED_LEFT_FEEDER_Y_TELEOP = 0.890; + public static final double RED_LEFT_FEEDER_THETA_TELEOP = Math.toRadians(126.0); + + public static final double RED_LEFT_FEEDER_LEFT_X = 16.873; + public static final double RED_LEFT_FEEDER_LEFT_Y = 1.209; + public static final double RED_LEFT_FEEDER_LEFT_THETA = Math.toRadians(126.0); + public static final double RED_LEFT_FEEDER_RIGHT_X = 15.952; + public static final double RED_LEFT_FEEDER_RIGHT_Y = 0.564; + public static final double RED_LEFT_FEEDER_RIGHT_THETA = Math.toRadians(126.0); + + public static final Pose2d RED_LEFT_FEEDER_LEFT = new Pose2d(RED_LEFT_FEEDER_LEFT_X, + RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(RED_LEFT_FEEDER_LEFT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_LEFT = new Pose2d( + RED_LEFT_FEEDER_LEFT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_LEFT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_LEFT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_LEFT_Y, + new Rotation2d(Math.toRadians(-54.0))); + public static final Pose2d RED_LEFT_FEEDER_RIGHT = new Pose2d(RED_LEFT_FEEDER_RIGHT_X, + RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(RED_LEFT_FEEDER_RIGHT_THETA)); + public static final Pose2d RED_RIGHT_FEEDER_RIGHT = new Pose2d( + RED_LEFT_FEEDER_RIGHT_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(234.0))); + public static final Pose2d BLUE_RIGHT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_RIGHT = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_RIGHT_X, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_RIGHT_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER = new Pose2d(RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(RED_LEFT_FEEDER_THETA)); + public static final Pose2d RED_RIGHT_FEEDER = new Pose2d( + RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y, + new Rotation2d(Math.toRadians(-54.0))); + + public static final Pose2d RED_LEFT_FEEDER_TELEOP = new Pose2d(RED_LEFT_FEEDER_X_TELEOP, + RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(RED_LEFT_FEEDER_THETA_TELEOP)); + public static final Pose2d RED_RIGHT_FEEDER_TELEOP = new Pose2d( + RED_LEFT_FEEDER_X_TELEOP, Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(234.0))); + + public static final Pose2d BLUE_RIGHT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(54.0))); + public static final Pose2d BLUE_LEFT_FEEDER_TELEOP = new Pose2d( + Physical.FIELD_LENGTH - RED_LEFT_FEEDER_X_TELEOP, + Physical.FIELD_WIDTH - RED_LEFT_FEEDER_Y_TELEOP, + new Rotation2d(Math.toRadians(-54.0))); + + public static final double PROCESSOR_Y_OFFSET_M = inchesToMeters(50.0); + public static final double PROCESSOR_MORE_Y_OFFSET_M = inchesToMeters(25.0); + public static final double NET_X_OFFSET_M = inchesToMeters(53.0); + public static final double NET_X_OFFSET_MORE = inchesToMeters(34.0); + + public static final Translation2d processorBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedFrontPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorBlueBackPlacingRotation = new Rotation2d(degreesToRadians(90)); + + public static final Translation2d processorRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_Y_OFFSET_M); + public static final Rotation2d processorRedBackPlacingRotation = new Rotation2d(degreesToRadians(270)); + + public static final Translation2d processorMoreBlueFrontPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueFrontPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Translation2d processorMoreRedFrontPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedFrontPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreBlueBackPlacingTranslation = new Translation2d( + inchesToMeters(238.79), + PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreBlueBackPlacingRotation = new Rotation2d( + degreesToRadians(90)); + + public static final Translation2d processorMoreRedBackPlacingTranslation = new Translation2d( + inchesToMeters(452.40), + inchesToMeters(316.21) - PROCESSOR_MORE_Y_OFFSET_M); + public static final Rotation2d processorMoreRedBackPlacingRotation = new Rotation2d( + degreesToRadians(270)); + + public static final Pose2d processorBlueFrontPlacingPosition = new Pose2d( + processorBlueFrontPlacingTranslation, + processorBlueFrontPlacingRotation); + public static final Pose2d processorRedFrontPlacingPosition = new Pose2d( + processorRedFrontPlacingTranslation, + processorRedFrontPlacingRotation); + public static final Pose2d processorBlueBackPlacingPosition = new Pose2d( + processorBlueBackPlacingTranslation, + processorBlueBackPlacingRotation); + public static final Pose2d processorRedBackPlacingPosition = new Pose2d( + processorRedBackPlacingTranslation, + processorRedBackPlacingRotation); + + public static final Pose2d processorMoreBlueFrontPlacingPosition = new Pose2d( + processorMoreBlueFrontPlacingTranslation, + processorMoreBlueFrontPlacingRotation); + public static final Pose2d processorMoreRedFrontPlacingPosition = new Pose2d( + processorMoreRedFrontPlacingTranslation, + processorMoreRedFrontPlacingRotation); + public static final Pose2d processorMoreBlueBackPlacingPosition = new Pose2d( + processorMoreBlueBackPlacingTranslation, + processorMoreBlueBackPlacingRotation); + public static final Pose2d processorMoreRedBackPlacingPosition = new Pose2d( + processorMoreRedBackPlacingTranslation, + processorMoreRedBackPlacingRotation); + + public static final double netBlueXM = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_M; + public static final double netRedXM = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_M; + public static final double netBlueXMore = (Constants.Physical.FIELD_LENGTH / 2) - NET_X_OFFSET_MORE; + public static final double netRedXMore = (Constants.Physical.FIELD_LENGTH / 2) + NET_X_OFFSET_MORE; + + public static final double netBlueFrontThetaR = degreesToRadians(0.0); + public static final double netRedFrontThetaR = degreesToRadians(180.0); + public static final double netBlueBackThetaR = degreesToRadians(180.0); + public static final double netRedBackThetaR = degreesToRadians(0.0); + + public static final List blueL1FrontPlacingPositions = new ArrayList<>(); + public static final List redL1FrontPlacingPositions = new ArrayList<>(); + public static final List blueL1BackPlacingPositions = new ArrayList<>(); + public static final List redL1BackPlacingPositions = new ArrayList<>(); + + public static final List blueL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List redL1FrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueL1BackPlacingPositionsMore = new ArrayList<>(); + public static final List redL1BackPlacingPositionsMore = new ArrayList<>(); + + public static final List blueFrontPlacingPositions = new ArrayList<>(); + public static final List redFrontPlacingPositions = new ArrayList<>(); + public static final List blueBackPlacingPositions = new ArrayList<>(); + public static final List redBackPlacingPositions = new ArrayList<>(); + + public static final List blueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List redFrontPlacingPositionsMore = new ArrayList<>(); + public static final List blueBackPlacingPositionsMore = new ArrayList<>(); + public static final List redBackPlacingPositionsMore = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMoreMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMoreMore = new ArrayList<>(); + + public static final List l4BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l4RedFrontPlacingPositions = new ArrayList<>(); + public static final List l4BlueBackPlacingPositions = new ArrayList<>(); + public static final List l4RedBackPlacingPositions = new ArrayList<>(); + + public static final List l3BlueFrontPlacingPositions = new ArrayList<>(); + public static final List l3RedFrontPlacingPositions = new ArrayList<>(); + public static final List l3BlueBackPlacingPositions = new ArrayList<>(); + public static final List l3RedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositions = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositions = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositions = new ArrayList<>(); + public static final List algaeRedBackPlacingPositions = new ArrayList<>(); + + public static final List algaeBlueFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedFrontPlacingPositionsMore = new ArrayList<>(); + public static final List algaeBlueBackPlacingPositionsMore = new ArrayList<>(); + public static final List algaeRedBackPlacingPositionsMore = new ArrayList<>(); + + public static final List l1BlueCornerPoints = new ArrayList<>(); + public static final List l1RedCornerPoints = new ArrayList<>(); + public static final List l1BlueDrivePoints = new ArrayList<>(); + public static final List l1RedDrivePoints = new ArrayList<>(); + + static { + calculateReefPoints(); + } // Angle to face red side: // 180=AB - // 120=KL - // 60=IJ + // 120=CD + // 60=EF // 0=GH - // -60=EF - // -120=CD - if (poseDirection.getRotation().getDegrees() > 179.0 - && poseDirection.getRotation().getDegrees() < 181.0) { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET, - adjustY + B_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET, - adjustY + B_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET, - adjustY + B_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET, - adjustY + B_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L3, - adjustY + B_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L3, - adjustY + B_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L3, - adjustY + B_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L3, - adjustY + B_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L2, - adjustY + B_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L2, - adjustY + B_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L2, - adjustY + B_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + B_BRANCH_OFFSET_L2, - adjustY + B_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > 119.0 - && poseDirection.getRotation().getDegrees() < 121.0) { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET, - adjustY + L_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET, - adjustY + L_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET, - adjustY + L_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET, - adjustY + L_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L3, - adjustY + L_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L3, - adjustY + L_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L3, - adjustY + L_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L3, - adjustY + L_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L2, - adjustY + L_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L2, - adjustY + L_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L2, - adjustY + L_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + L_BRANCH_OFFSET_L2, - adjustY + L_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > 59.0 - && poseDirection.getRotation().getDegrees() < 61.0) { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET, - adjustY + J_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET, - adjustY + J_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET, - adjustY + J_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET, - adjustY + J_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L3, - adjustY + J_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L3, - adjustY + J_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L3, - adjustY + J_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L3, - adjustY + J_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L2, - adjustY + J_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L2, - adjustY + J_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L2, - adjustY + J_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + J_BRANCH_OFFSET_L2, - adjustY + J_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > -1.0 - && poseDirection.getRotation().getDegrees() < 1.0) { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET, - adjustY + H_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET, - adjustY + H_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET, - adjustY + H_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET, - adjustY + H_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L3, - adjustY + H_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L3, - adjustY + H_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L3, - adjustY + H_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L3, - adjustY + H_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L2, - adjustY + H_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L2, - adjustY + H_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L2, - adjustY + H_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + H_BRANCH_OFFSET_L2, - adjustY + H_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > -61.0 - && poseDirection.getRotation().getDegrees() < -59.0) { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET, - adjustY + F_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET, - adjustY + F_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET, - adjustY + F_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET, - adjustY + F_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L3, - adjustY + F_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L3, - adjustY + F_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L3, - adjustY + F_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L3, - adjustY + F_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L2, - adjustY + F_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L2, - adjustY + F_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L2, - adjustY + F_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + F_BRANCH_OFFSET_L2, - adjustY + F_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else { - l4FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET, - adjustY + D_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET, - adjustY + D_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET, - adjustY + D_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET, - adjustY + D_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L3, - adjustY + D_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L3, - adjustY + D_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L3, - adjustY + D_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L3, - adjustY + D_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L2, - adjustY + D_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L2, - adjustY + D_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackRight = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L2, - adjustY + D_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + D_BRANCH_OFFSET_L2, - adjustY + D_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); + // -60=IJ + // -120=KL + + public static void calculateReefPoints() { + blueL1FrontPlacingPositions.clear(); + redL1FrontPlacingPositions.clear(); + blueL1BackPlacingPositions.clear(); + redL1BackPlacingPositions.clear(); + blueL1FrontPlacingPositionsMore.clear(); + redL1FrontPlacingPositionsMore.clear(); + blueL1BackPlacingPositionsMore.clear(); + redL1BackPlacingPositionsMore.clear(); + blueFrontPlacingPositions.clear(); + redFrontPlacingPositions.clear(); + blueBackPlacingPositions.clear(); + redBackPlacingPositions.clear(); + blueFrontPlacingPositionsMore.clear(); + redFrontPlacingPositionsMore.clear(); + blueBackPlacingPositionsMore.clear(); + redBackPlacingPositionsMore.clear(); + l4BlueFrontPlacingPositions.clear(); + l4RedFrontPlacingPositions.clear(); + l4BlueBackPlacingPositions.clear(); + l4RedBackPlacingPositions.clear(); + l3BlueFrontPlacingPositions.clear(); + l3RedFrontPlacingPositions.clear(); + l3BlueBackPlacingPositions.clear(); + l3RedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositions.clear(); + algaeRedFrontPlacingPositions.clear(); + algaeBlueBackPlacingPositions.clear(); + algaeRedBackPlacingPositions.clear(); + algaeBlueFrontPlacingPositionsMore.clear(); + algaeRedFrontPlacingPositionsMore.clear(); + algaeBlueBackPlacingPositionsMore.clear(); + algaeRedBackPlacingPositionsMore.clear(); + algaeBlueFrontPlacingPositionsMoreMore.clear(); + algaeRedFrontPlacingPositionsMoreMore.clear(); + algaeBlueBackPlacingPositionsMoreMore.clear(); + algaeRedBackPlacingPositionsMoreMore.clear(); + l1BlueCornerPoints.clear(); + l1RedCornerPoints.clear(); + l1BlueDrivePoints.clear(); + l1RedDrivePoints.clear(); + centerFaces[0] = new Pose2d( + inchesToMeters(144.003), + inchesToMeters(158.500), + Rotation2d.fromDegrees(180)); + centerFaces[1] = new Pose2d( + inchesToMeters(160.373), + inchesToMeters(186.857), + Rotation2d.fromDegrees(120)); + centerFaces[2] = new Pose2d( + inchesToMeters(193.116), + inchesToMeters(186.858), + Rotation2d.fromDegrees(60)); + centerFaces[3] = new Pose2d( + inchesToMeters(209.489), + inchesToMeters(158.502), + Rotation2d.fromDegrees(0)); + centerFaces[4] = new Pose2d( + inchesToMeters(193.118), + inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)); + centerFaces[5] = new Pose2d( + inchesToMeters(160.375), + inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)); + + for (int face = 0; face < 6; face++) { + Pose2d l1FrontRight = new Pose2d(); + Pose2d l1FrontLeft = new Pose2d(); + Pose2d l1BackRight = new Pose2d(); + Pose2d l1BackLeft = new Pose2d(); + Pose2d l1FrontRightMore = new Pose2d(); + Pose2d l1FrontLeftMore = new Pose2d(); + Pose2d l1BackRightMore = new Pose2d(); + Pose2d l1BackLeftMore = new Pose2d(); + Pose2d l2FrontRight = new Pose2d(); + Pose2d l2FrontLeft = new Pose2d(); + Pose2d frontRightMore = new Pose2d(); + Pose2d frontLeftMore = new Pose2d(); + Pose2d backRightMore = new Pose2d(); + Pose2d backLeftMore = new Pose2d(); + Pose2d l2BackRight = new Pose2d(); + Pose2d l2BackLeft = new Pose2d(); + Pose2d l3FrontRight = new Pose2d(); + Pose2d l3FrontLeft = new Pose2d(); + Pose2d l3BackRight = new Pose2d(); + Pose2d l3BackLeft = new Pose2d(); + Pose2d l4FrontRight = new Pose2d(); + Pose2d l4FrontLeft = new Pose2d(); + Pose2d l4BackRight = new Pose2d(); + Pose2d l4BackLeft = new Pose2d(); + Pose2d algaeFront = new Pose2d(); + Pose2d algaeBack = new Pose2d(); + Pose2d algaeFrontMore = new Pose2d(); + Pose2d algaeBackMore = new Pose2d(); + Pose2d algaeFrontMoreMore = new Pose2d(); + Pose2d algaeBackMoreMore = new Pose2d(); + Pose2d l1Corner = new Pose2d(); + Pose2d l1Drive = new Pose2d(); + Pose2d poseDirection = new Pose2d(centerBlue, + Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = inchesToMeters(30.738); + double adjustY = inchesToMeters(6.469); + double adjustXL1 = inchesToMeters(30.738); + double adjustYL1 = inchesToMeters(6.469); + double adjustXMore = inchesToMeters(41.67); + double adjustYMore = inchesToMeters(6.469); + double adjustAlgaeX = inchesToMeters(45.738); + double adjustAlgaeY = inchesToMeters(0.0); + double adjustAlgaeMoreX = inchesToMeters(16.738); + double adjustAlgaeMoreY = inchesToMeters(0.0); + double adjustAlgaeMoreMoreX = inchesToMeters(56.738); + double adjustAlgaeMoreMoreY = inchesToMeters(0.0); + double adjustL1CornerX = inchesToMeters(9.0); + double adjustL1CornerY = inchesToMeters(18.0); + double adjustL1DriveX = inchesToMeters(38.0); + double adjustL1DriveY = inchesToMeters(25.0); + + l1FrontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT_MORE, + Physical.L1_INTAKE_Y_OFFSET_FRONT_MORE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK_MORE, + Physical.L1_INTAKE_Y_OFFSET_BACK_MORE, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + l1FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_FRONT, + Physical.L1_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l1BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXL1, + -adjustYL1, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L1_INTAKE_X_OFFSET_BACK, + Physical.L1_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + algaeFront = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBack = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustAlgaeX, + adjustAlgaeY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreX, + adjustAlgaeMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + algaeFrontMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT_ALGAE, + Physical.INTAKE_Y_OFFSET_FRONT_ALGAE, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Corner = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1CornerX, + adjustL1CornerY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + l1Drive = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustL1DriveX, + adjustL1DriveY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + + algaeBackMoreMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO: + // why + // is + // this + // pi + // and + // not + // 0 + .getX(), + poseDirection + .transformBy( + new Transform2d(adjustAlgaeMoreMoreX, + adjustAlgaeMoreMoreY, + new Rotation2d())) + .transformBy( + new Transform2d(Physical.INTAKE_X_OFFSET_BACK_ALGAE, + Physical.INTAKE_Y_OFFSET_BACK_ALGAE, + new Rotation2d(Math.PI))) // TODO + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + frontRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backRightMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + + // l2FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + + // Angle to face red side: + // 180=AB + // 120=KL + // 60=IJ + // 0=GH + // -60=EF + // -120=CD + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET, + adjustY + B_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L3, + adjustY + B_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + B_BRANCH_OFFSET_L2, + adjustY + B_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET, + adjustY + L_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L3, + adjustY + L_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + L_BRANCH_OFFSET_L2, + adjustY + L_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET, + adjustY + J_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L3, + adjustY + J_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + J_BRANCH_OFFSET_L2, + adjustY + J_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET, + adjustY + H_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L3, + adjustY + H_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + H_BRANCH_OFFSET_L2, + adjustY + H_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET, + adjustY + F_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L3, + adjustY + F_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + F_BRANCH_OFFSET_L2, + adjustY + F_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET, + adjustY + D_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L3, + adjustY + D_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackRight = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + D_BRANCH_OFFSET_L2, + adjustY + D_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackRight = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + // l2FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_FRONT, + // Physical.L2_INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l2BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.L2_INTAKE_X_OFFSET_BACK, + // Physical.L2_INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + frontLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_FRONT, + Physical.INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() - Math.PI)); + backLeftMore = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustXMore, + -adjustYMore, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.INTAKE_X_OFFSET_BACK, + Physical.INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + if (poseDirection.getRotation().getDegrees() > 179.0 + && poseDirection.getRotation().getDegrees() < 181.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET, + -adjustY + A_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L3, + -adjustY + A_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + A_BRANCH_OFFSET_L2, + -adjustY + A_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 119.0 + && poseDirection.getRotation().getDegrees() < 121.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET, + -adjustY + K_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L3, + -adjustY + K_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + K_BRANCH_OFFSET_L2, + -adjustY + K_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > 59.0 + && poseDirection.getRotation().getDegrees() < 61.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET, + -adjustY + I_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L3, + -adjustY + I_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + I_BRANCH_OFFSET_L2, + -adjustY + I_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -1.0 + && poseDirection.getRotation().getDegrees() < 1.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET, + -adjustY + G_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L3, + -adjustY + G_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + G_BRANCH_OFFSET_L2, + -adjustY + G_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else if (poseDirection.getRotation().getDegrees() > -61.0 + && poseDirection.getRotation().getDegrees() < -59.0) { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET, + -adjustY + E_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L3, + -adjustY + E_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + E_BRANCH_OFFSET_L2, + -adjustY + E_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } else { + l4FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_FRONT, + Physical.L4_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l4BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET, + -adjustY + C_BRANCH_OFFSET_SIDE, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L4_INTAKE_X_OFFSET_BACK, + Physical.L4_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l3FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_FRONT, + Physical.L3_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l3BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L3, + -adjustY + C_BRANCH_OFFSET_SIDE_L3, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L3_INTAKE_X_OFFSET_BACK, + Physical.L3_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + l2FrontLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_FRONT, + Physical.L2_INTAKE_Y_OFFSET_FRONT, + new Rotation2d(Math.PI))) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians() + - Math.PI)); + l2BackLeft = new Pose2d( + new Translation2d( + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d( + adjustX + C_BRANCH_OFFSET_L2, + -adjustY + C_BRANCH_OFFSET_SIDE_L2, + new Rotation2d())) + .transformBy(new Transform2d( + Physical.L2_INTAKE_X_OFFSET_BACK, + Physical.L2_INTAKE_Y_OFFSET_BACK, + new Rotation2d())) + .getY()), + new Rotation2d( + poseDirection.getRotation().getRadians())); + } + // l3FrontLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_FRONT, + // Physical.INTAKE_Y_OFFSET_FRONT, + // new Rotation2d(Math.PI))) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians() - Math.PI)); + // l3BackLeft = new Pose2d( + // new Translation2d( + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getX(), + // poseDirection + // .transformBy(new Transform2d(adjustX, + // -adjustY, + // new Rotation2d())) + // .transformBy(new Transform2d( + // Physical.INTAKE_X_OFFSET_BACK, + // Physical.INTAKE_Y_OFFSET_BACK, + // new Rotation2d())) + // .getY()), + // new Rotation2d( + // poseDirection.getRotation().getRadians())); + blueFrontPlacingPositions.add(l2FrontRight); + blueFrontPlacingPositions.add(l2FrontLeft); + blueBackPlacingPositions.add(l2BackRight); + blueBackPlacingPositions.add(l2BackLeft); + blueFrontPlacingPositionsMore.add(frontRightMore); + blueFrontPlacingPositionsMore.add(frontLeftMore); + blueBackPlacingPositionsMore.add(backRightMore); + blueBackPlacingPositionsMore.add(backLeftMore); + l4BlueFrontPlacingPositions.add(l4FrontRight); + l4BlueFrontPlacingPositions.add(l4FrontLeft); + l4BlueBackPlacingPositions.add(l4BackRight); + l4BlueBackPlacingPositions.add(l4BackLeft); + l3BlueFrontPlacingPositions.add(l3FrontRight); + l3BlueFrontPlacingPositions.add(l3FrontLeft); + l3BlueBackPlacingPositions.add(l3BackRight); + l3BlueBackPlacingPositions.add(l3BackLeft); + algaeBlueFrontPlacingPositions.add(algaeFront); + algaeBlueBackPlacingPositions.add(algaeBack); + algaeBlueFrontPlacingPositionsMore.add(algaeFrontMore); + algaeBlueBackPlacingPositionsMore.add(algaeBackMore); + algaeBlueFrontPlacingPositionsMoreMore.add(algaeFrontMoreMore); + algaeBlueBackPlacingPositionsMoreMore.add(algaeBackMoreMore); + l1BlueCornerPoints.add(l1Corner); + l1BlueDrivePoints.add(l1Drive); + blueL1FrontPlacingPositions.add(l1FrontLeft); + blueL1FrontPlacingPositions.add(l1FrontRight); + blueL1BackPlacingPositions.add(l1BackLeft); + blueL1BackPlacingPositions.add(l1BackRight); + blueL1FrontPlacingPositionsMore.add(l1FrontLeftMore); + blueL1FrontPlacingPositionsMore.add(l1FrontRightMore); + blueL1BackPlacingPositionsMore.add(l1BackLeftMore); + blueL1BackPlacingPositionsMore.add(l1BackRightMore); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueL1FrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1FrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueL1BackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redL1BackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l1BlueCornerPoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedCornerPoints.add(redPose); + } + + for (Pose2d bluePose : l1BlueDrivePoints) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l1RedDrivePoints.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedFrontPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : algaeBlueBackPlacingPositionsMoreMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + algaeRedBackPlacingPositionsMoreMore.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : blueFrontPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redFrontPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : blueBackPlacingPositionsMore) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + redBackPlacingPositionsMore.add(redPose); + } + + for (Pose2d bluePose : l4BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l4BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l4RedBackPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueFrontPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() + Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedFrontPlacingPositions.add(redPose); + } + + for (Pose2d bluePose : l3BlueBackPlacingPositions) { + Pose2d redPose = new Pose2d(); + Translation2d mirroredTranslation = new Translation2d( + Constants.Physical.FIELD_LENGTH - bluePose.getX(), + Constants.Physical.FIELD_WIDTH - bluePose.getY()); + Rotation2d mirroredRotation = new Rotation2d( + bluePose.getRotation().getRadians() - Math.PI); + redPose = new Pose2d(mirroredTranslation, mirroredRotation); + l3RedBackPlacingPositions.add(redPose); + } } - // l3FrontRight = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_FRONT, - // Physical.INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_FRONT, - // Physical.INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians() - Math.PI)); - // l3BackRight = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_BACK, - // Physical.INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_BACK, - // Physical.INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians())); - // l2FrontLeft = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_FRONT, - // Physical.L2_INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_FRONT, - // Physical.L2_INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians() - Math.PI)); - // l2BackLeft = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_BACK, - // Physical.L2_INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.L2_INTAKE_X_OFFSET_BACK, - // Physical.L2_INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians())); - frontLeftMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXMore, - -adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXMore, - -adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_FRONT, - Physical.INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - Math.PI)); - backLeftMore = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustXMore, - -adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_BACK, - Physical.INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustXMore, - -adjustYMore, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.INTAKE_X_OFFSET_BACK, - Physical.INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - if (poseDirection.getRotation().getDegrees() > 179.0 - && poseDirection.getRotation().getDegrees() < 181.0) { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET, - -adjustY + A_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET, - -adjustY + A_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET, - -adjustY + A_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET, - -adjustY + A_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L3, - -adjustY + A_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L3, - -adjustY + A_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L3, - -adjustY + A_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L3, - -adjustY + A_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L2, - -adjustY + A_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L2, - -adjustY + A_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L2, - -adjustY + A_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + A_BRANCH_OFFSET_L2, - -adjustY + A_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > 119.0 - && poseDirection.getRotation().getDegrees() < 121.0) { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET, - -adjustY + K_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET, - -adjustY + K_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET, - -adjustY + K_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET, - -adjustY + K_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L3, - -adjustY + K_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L3, - -adjustY + K_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L3, - -adjustY + K_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L3, - -adjustY + K_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L2, - -adjustY + K_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L2, - -adjustY + K_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L2, - -adjustY + K_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + K_BRANCH_OFFSET_L2, - -adjustY + K_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > 59.0 - && poseDirection.getRotation().getDegrees() < 61.0) { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET, - -adjustY + I_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET, - -adjustY + I_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET, - -adjustY + I_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET, - -adjustY + I_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L3, - -adjustY + I_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L3, - -adjustY + I_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L3, - -adjustY + I_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L3, - -adjustY + I_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L2, - -adjustY + I_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L2, - -adjustY + I_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L2, - -adjustY + I_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + I_BRANCH_OFFSET_L2, - -adjustY + I_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > -1.0 - && poseDirection.getRotation().getDegrees() < 1.0) { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET, - -adjustY + G_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET, - -adjustY + G_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET, - -adjustY + G_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET, - -adjustY + G_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L3, - -adjustY + G_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L3, - -adjustY + G_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L3, - -adjustY + G_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L3, - -adjustY + G_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L2, - -adjustY + G_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L2, - -adjustY + G_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L2, - -adjustY + G_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + G_BRANCH_OFFSET_L2, - -adjustY + G_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else if (poseDirection.getRotation().getDegrees() > -61.0 - && poseDirection.getRotation().getDegrees() < -59.0) { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET, - -adjustY + E_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET, - -adjustY + E_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET, - -adjustY + E_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET, - -adjustY + E_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L3, - -adjustY + E_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L3, - -adjustY + E_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L3, - -adjustY + E_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L3, - -adjustY + E_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L2, - -adjustY + E_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L2, - -adjustY + E_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L2, - -adjustY + E_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + E_BRANCH_OFFSET_L2, - -adjustY + E_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - } else { - l4FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET, - -adjustY + C_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET, - -adjustY + C_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_FRONT, - Physical.L4_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l4BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET, - -adjustY + C_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET, - -adjustY + C_BRANCH_OFFSET_SIDE, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L4_INTAKE_X_OFFSET_BACK, - Physical.L4_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l3FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L3, - -adjustY + C_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L3, - -adjustY + C_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_FRONT, - Physical.L3_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l3BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L3, - -adjustY + C_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L3, - -adjustY + C_BRANCH_OFFSET_SIDE_L3, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L3_INTAKE_X_OFFSET_BACK, - Physical.L3_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); - l2FrontLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L2, - -adjustY + C_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L2, - -adjustY + C_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_FRONT, - Physical.L2_INTAKE_Y_OFFSET_FRONT, - new Rotation2d(Math.PI))) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians() - - Math.PI)); - l2BackLeft = new Pose2d( - new Translation2d( - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L2, - -adjustY + C_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getX(), - poseDirection - .transformBy(new Transform2d( - adjustX + C_BRANCH_OFFSET_L2, - -adjustY + C_BRANCH_OFFSET_SIDE_L2, - new Rotation2d())) - .transformBy(new Transform2d( - Physical.L2_INTAKE_X_OFFSET_BACK, - Physical.L2_INTAKE_Y_OFFSET_BACK, - new Rotation2d())) - .getY()), - new Rotation2d( - poseDirection.getRotation().getRadians())); + } + + // Physical constants (e.g. field and robot dimensions) + public static final class Physical { + public static final double FIELD_WIDTH = 8.052; + public static final double FIELD_LENGTH = 17.548; + public static final double WHEEL_DIAMETER = inchesToMeters(4); + public static final double WHEEL_CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER; + public static final double WHEEL_ROTATION_PER_METER = 1 / WHEEL_CIRCUMFERENCE; + public static final double WHEEL_TO_FRAME_DISTANCE = inchesToMeters(2.5); + public static final double TOP_SPEED = feetToMeters(30.0); + public static final double MAX_ACCELERATION = feetToMeters(30.0); // TODO: actually tune the top speed + // and max acceleration. Add a max + // deceleration if needed. + + public static final double ROBOT_LENGTH = inchesToMeters(32); + public static final double ROBOT_WIDTH = inchesToMeters(26); + public static final double MODULE_OFFSET = inchesToMeters(2.625); + public static final double ROBOT_RADIUS = Math.hypot(ROBOT_LENGTH / 2 - WHEEL_TO_FRAME_DISTANCE, + ROBOT_WIDTH / 2 - WHEEL_TO_FRAME_DISTANCE); + public static double INTAKE_X_OFFSET_FRONT = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_BACK = inchesToMeters(33.0); + public static double INTAKE_Y_OFFSET_BACK = inchesToMeters(0.5); + public static double INTAKE_X_OFFSET_FRONT_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_FRONT_ALGAE = inchesToMeters(3.8); + public static double INTAKE_X_OFFSET_BACK_ALGAE = inchesToMeters(23.0 + 5.0); + public static double INTAKE_Y_OFFSET_BACK_ALGAE = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK = inchesToMeters(35.3); + public static double L1_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L1_INTAKE_X_OFFSET_FRONT_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_FRONT_MORE = inchesToMeters(0.0); + public static double L1_INTAKE_X_OFFSET_BACK_MORE = inchesToMeters(24.9); + public static double L1_INTAKE_Y_OFFSET_BACK_MORE = inchesToMeters(-0.0); + public static double L2_INTAKE_X_OFFSET_FRONT = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L2_INTAKE_X_OFFSET_BACK = inchesToMeters(25.4); + public static double L2_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L3_INTAKE_X_OFFSET_FRONT = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); + public static double L3_INTAKE_X_OFFSET_BACK = inchesToMeters(26.0); + public static double L3_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); + public static double L4_INTAKE_X_OFFSET_FRONT = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_FRONT = inchesToMeters(-1.0); + public static double L4_INTAKE_X_OFFSET_BACK = inchesToMeters(27.0); + public static double L4_INTAKE_Y_OFFSET_BACK = inchesToMeters(-1.0); + + public static final double GRAVITY_ACCEL_MS2 = 9.806; + } + + public static final class Arm { + + public static final double L4_Score = -20.0; + public static final double L4_Place = 45.0; + public static final double L3_Score = -10.0; + public static final double L3_Place = 47.0; + public static final double L2_Score = -10.0; + public static final double L2_Place = 47.0; + public static final double L1_Place = -25.0; + public static final double HANDOFF = -91.0; + public static final double DEFAULT = -90.0; + public static final double HORIZONTAL = 5.0; + public static final double VERTICAL = 95.0; + public static final double NET = 135.0; + public static final double IDLE = 0.0; + } + + public static final class Elevator { + public static final double AUTO_L1 = inchesToMeters(18.0); + public static final double AUTO_L2 = inchesToMeters(8.0); + public static final double AUTO_L3 = inchesToMeters(24.25); + public static final double AUTO_L4 = inchesToMeters(50.0); + public static final double AUTO_SCORE_L2_HIGH = inchesToMeters(14.0); + public static final double AUTO_SCORE_L2 = inchesToMeters(5.0); + public static final double AUTO_SCORE_L3 = inchesToMeters(20.0); + public static final double AUTO_SCORE_L4 = inchesToMeters(48.0); + public static final double HANDOFF_HIGH = inchesToMeters(11.0); + public static final double HANDOFF_LOW = inchesToMeters(3.0); + public static final double ALGAE_HIGH = inchesToMeters(33); + public static final double ALGAE_LOW = inchesToMeters(14.5); + public static final double PROCESSOR = inchesToMeters(0.0); + public static final double NET = inchesToMeters(55.0); + } + + // Subsystem setpoint constants + public static final class SetPoints { + public static class IntakeSetpoints { + public static final double INTAKE_ACCELERATION = 500.0; + public static final double INTAKE_CRUISE_VELOCITY = 400.0; + public static final double INTAKE_MOTION_PROFILE_SCALAR = 1.0; + public static final double INTAKE_DOWN = -18.2; // rotations + public static final double INTAKE_UP = 0; // rotations + public static final double INTAKE_ROLLER_MAX_SPEED = 1.0; // percent + public static final double INTAKE_ROLLER_HOLDING_SPEED = 0.1; // percent + public static final double INTAKE_ROLLER_TORQUE = 80.0; // amps + public static final double INTAKE_HOLDING_TORQUE = 60.0; // amps + } + + public static final double ELEVATOR_BOTTOM_POSITION_M = 0.0; + public static final double ELEVATOR_MID_POSITION_M = inchesToMeters(26.0); // L2 after placement + public static final double ELEVATOR_TOP_POSITION_M = inchesToMeters(43.0); + public static final double ELEVATOR_L1_POSITION_M = inchesToMeters(6.6); + public static final double ELEVATOR_L2_POSITION_M = inchesToMeters(15); + public static final double ELEVATOR_AUTO_L2_POSITION_M = inchesToMeters(18.5); + public static final double ELEVATOR_AUTO_L2_POSITION_SCORE_M = inchesToMeters(16); + public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(34.25); + // public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_SCORE_L3_POSITION_M = inchesToMeters(25); + public static final double ELEVATOR_AUTO_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_L3_POSITION_M = inchesToMeters(28); + public static final double ELEVATOR_L4_POSITION_M = inchesToMeters(64.0); + public static final double ELEVATOR_ALGAE_POSITION_M = inchesToMeters(8.0); + public static final double ELEVATOR_GROUND_CORAL_POSITION_M = inchesToMeters(5.4); + public static final double ELEVATOR_GROUND_ALGAE_POSITION_M = inchesToMeters(5.0); + public static final double ELEVATOR_FEEDER_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_OVER_POSITION_M = inchesToMeters(20); + public static final double ELEVATOR_NET_POSITION_M = inchesToMeters(65); + public static final double ELEVATOR_L2_ALGAE_POSITION_M = inchesToMeters(15.7); + public static final double ELEVATOR_L3_ALGAE_POSITION_M = inchesToMeters(37.6741); + public static final double ELEVATOR_PROCESSOR_POSITION_M = inchesToMeters(6.5); + public static final double ELEVATOR_LOLLIPOP_POSITION_M = inchesToMeters(0.0); + public static final double ELEVATOR_PRE_HANDOFF_POSITION_M = inchesToMeters(39.0); + public static final double ELEVATOR_HANDOFF_POSITION_M = inchesToMeters(35.0); + + public enum ElevatorPosition { + kDOWN(ELEVATOR_BOTTOM_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_BOTTOM_POSITION_M)), + kMID(ELEVATOR_MID_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_MID_POSITION_M)), + kUP(ELEVATOR_TOP_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_TOP_POSITION_M)), + kL1(ELEVATOR_L1_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L1_POSITION_M)), + kL2(ELEVATOR_L2_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L2_POSITION_M)), + kAUTOL2(ELEVATOR_AUTO_L2_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_M)), + kAUTOL2SCORE(ELEVATOR_AUTO_L2_POSITION_SCORE_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_SCORE_M)), + kAUTOL3(ELEVATOR_AUTO_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L3_POSITION_M)), + kAUTOL3SCORE(ELEVATOR_AUTO_SCORE_L3_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_SCORE_L3_POSITION_M)), + kAUTOL4(ELEVATOR_AUTO_L4_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L4_POSITION_M)), + kL3(ELEVATOR_L3_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L3_POSITION_M)), + kL4(ELEVATOR_L4_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L4_POSITION_M)), + kALGAE(ELEVATOR_ALGAE_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_ALGAE_POSITION_M)), + kFEEDER(ELEVATOR_FEEDER_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_FEEDER_POSITION_M)), + kGROUNDCORAL(ELEVATOR_GROUND_CORAL_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_CORAL_POSITION_M)), + kGROUNDALGAE(ELEVATOR_GROUND_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_ALGAE_POSITION_M)), + kNET(ELEVATOR_NET_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_NET_POSITION_M)), + kPROCESSOR(ELEVATOR_PROCESSOR_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PROCESSOR_POSITION_M)), + kL2ALGAE(ELEVATOR_L2_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L2_ALGAE_POSITION_M)), + kL3ALGAE(ELEVATOR_L3_ALGAE_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_L3_ALGAE_POSITION_M)), + kOVER(ELEVATOR_OVER_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_OVER_POSITION_M)), + kLOLLIPOP(ELEVATOR_LOLLIPOP_POSITION_M, Ratios.elevatorMetersToRotations( + ELEVATOR_LOLLIPOP_POSITION_M)), + kHANDOFF(ELEVATOR_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_HANDOFF_POSITION_M)), + kPREHANDOFF(ELEVATOR_PRE_HANDOFF_POSITION_M, + Ratios.elevatorMetersToRotations(ELEVATOR_PRE_HANDOFF_POSITION_M)); + + public final double meters; + public final double rotations; + + private ElevatorPosition(double meters, double rotations) { + this.meters = meters; + this.rotations = rotations; + } + } + + public static final double PIVOT_L1_POSITION_D = 65.67; + public static final double PIVOT_L23_POSITION_D = 52.5; + // public static final double PIVOT_AUTO_L23_POSITION_D = 45.0; + public static final double PIVOT_AUTO_L2_POSITION_D = 62.0; + public static final double PIVOT_AUTO_L3_POSITION_D = 62.0; + // public static final double PIVOT_AUTO_L3_POSITION_D = 30.0; + public static final double PIVOT_AUTO_L4_POSITION_D = 0.0; + public static final double PIVOT_AUTO_L4_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D = 70.0; + public static final double PIVOT_AUTO_L3_SCORE_POSITION_D = 100.0; + public static final double PIVOT_AUTO_L2_SCORE_POSITION_D = 100.0; + public static final double PIVOT_L4_POSITION_D = 60.0; + public static final double PIVOT_UPRIGHT_POSITION_D = 45.0; + public static final double PIVOT_GROUND_ALGAE_POSITION_D = 82.5; + public static final double PIVOT_GROUND_CORAL_POSITION_FRONT_D = 111.0; + public static final double PIVOT_GROUND_CORAL_POSITION_BACK_D = -111.0; + public static final double PIVOT_GROUND_CORAL_PREP_BACK_D = -90; + // public static final double PIVOT_DEFAULT_POSITION_D = 30.0; + public static final double PIVOT_DEFAULT_POSITION_D = 0.0; + public static final double PIVOT_DEFAULT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_PREP_POSITION_D = 30.0; + public static final double PIVOT_FEEDER_POSITION_D = 21.0; + public static final double PIVOT_NET_POSITION_D = 15.0; + public static final double PIVOT_PROCESSOR_POSITION_D = 76.0; + public static final double PIVOT_REEF_ALGAE_POSITION_D = 80.0; + public static final double PIVOT_CLIMB_POSITION_D = 45.0; + public static final double PIVOT_LOLLIPOP_POSITION_D = -98.0; + public static final double PIVOT_HANDOFF_POSITION_D = 145.0; + + public enum PivotPosition { + kL1(PIVOT_L1_POSITION_D, Constants.degreesToRotations(PIVOT_L1_POSITION_D)), + kL23(PIVOT_L23_POSITION_D, Constants.degreesToRotations(PIVOT_L23_POSITION_D)), + kAUTOL2(PIVOT_AUTO_L2_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L2_POSITION_D)), + kAUTOL3(PIVOT_AUTO_L3_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L3_POSITION_D)), + kAUTOL4(PIVOT_AUTO_L4_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L4_POSITION_D)), + kL4(PIVOT_L4_POSITION_D, Constants.degreesToRotations(PIVOT_L4_POSITION_D)), + kUP(PIVOT_UPRIGHT_POSITION_D, Constants.degreesToRotations(PIVOT_UPRIGHT_POSITION_D)), + kGROUNDALGAE(PIVOT_GROUND_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_GROUND_ALGAE_POSITION_D)), + kGROUNDCORALFRONT(PIVOT_GROUND_CORAL_POSITION_FRONT_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_FRONT_D)), + kGROUNDCORALBACK(PIVOT_GROUND_CORAL_POSITION_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_BACK_D)), + kGROUNDCORALPREPBACK(PIVOT_GROUND_CORAL_PREP_BACK_D, + Constants.degreesToRotations(PIVOT_GROUND_CORAL_PREP_BACK_D)), + kNET(PIVOT_NET_POSITION_D, + Constants.degreesToRotations(PIVOT_NET_POSITION_D)), + kPROCESSOR(PIVOT_PROCESSOR_POSITION_D, + Constants.degreesToRotations(PIVOT_PROCESSOR_POSITION_D)), + kAUTOL2SCORE(PIVOT_AUTO_L2_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L2_SCORE_POSITION_D)), + kAUTOL3SCORE(PIVOT_AUTO_L3_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L3_SCORE_POSITION_D)), + kAUTOL4SCORE(PIVOT_AUTO_L4_SCORE_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_POSITION_D)), + kAUTOL4SCORESLOW(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D, + Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D)), + kDEFAULT(PIVOT_DEFAULT_POSITION_D, Constants.degreesToRotations(PIVOT_DEFAULT_POSITION_D)), + kDEFAULTCLIMB(PIVOT_DEFAULT_CLIMB_POSITION_D, + Constants.degreesToRotations(PIVOT_DEFAULT_CLIMB_POSITION_D)), + kFEEDER(PIVOT_FEEDER_POSITION_D, Constants.degreesToRotations(PIVOT_FEEDER_POSITION_D)), + kREEFALGAE(PIVOT_REEF_ALGAE_POSITION_D, + Constants.degreesToRotations(PIVOT_REEF_ALGAE_POSITION_D)), + kPREP(PIVOT_PREP_POSITION_D, Constants.degreesToRotations(PIVOT_PREP_POSITION_D)), + kCLIMB(PIVOT_CLIMB_POSITION_D, Constants.degreesToRotations(PIVOT_CLIMB_POSITION_D)), + kLOLLIPOP(PIVOT_LOLLIPOP_POSITION_D, Constants.degreesToRotations(PIVOT_LOLLIPOP_POSITION_D)), + kHANDOFF(PIVOT_HANDOFF_POSITION_D, Constants.degreesToRotations(PIVOT_HANDOFF_POSITION_D)); + + public final double degrees; + public final double rotations; + + private PivotPosition(double degrees, double rotations) { + this.degrees = degrees; + this.rotations = rotations; + } + } + } + + // Vision constants (e.g. camera offsets) + public static final class Vision { + // Poses of all 16 AprilTags, {x, y, z, yaw, pitch}, in meters and radians + public static final double[][] TAG_POSES = { + { inchesToMeters(657.37), inchesToMeters(25.8), inchesToMeters(58.5), + Math.toRadians(126), + Math.toRadians(0) }, + { inchesToMeters(657.37), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(234), + Math.toRadians(0) }, + { inchesToMeters(455.15), inchesToMeters(317.15), inchesToMeters(51.25), + Math.toRadians(270), + Math.toRadians(0) }, + { inchesToMeters(365.2), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(365.2), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(0), + Math.toRadians(30) }, + { inchesToMeters(530.49), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) }, + { inchesToMeters(546.87), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(530.49), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(481.39), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(497.77), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(25.8), inchesToMeters(58.5), Math.toRadians(54), + Math.toRadians(0) }, + { inchesToMeters(33.51), inchesToMeters(291.2), inchesToMeters(58.5), + Math.toRadians(306), + Math.toRadians(0) }, + { inchesToMeters(325.68), inchesToMeters(241.64), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(325.68), inchesToMeters(75.39), inchesToMeters(73.54), + Math.toRadians(180), + Math.toRadians(30) }, + { inchesToMeters(235.73), inchesToMeters(-0.15), inchesToMeters(51.25), + Math.toRadians(90), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(240), + Math.toRadians(0) }, + { inchesToMeters(144), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(180), + Math.toRadians(0) }, + { inchesToMeters(160.39), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(120), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(186.83), inchesToMeters(12.13), + Math.toRadians(60), + Math.toRadians(0) }, + { inchesToMeters(209.49), inchesToMeters(158.5), inchesToMeters(12.13), + Math.toRadians(0), + Math.toRadians(0) }, + { inchesToMeters(193.1), inchesToMeters(130.17), inchesToMeters(12.13), + Math.toRadians(300), + Math.toRadians(0) } + }; + + public static final double[][] redSideReefTags = { + TAG_POSES[6 - 1], TAG_POSES[7 - 1], TAG_POSES[8 - 1], TAG_POSES[9 - 1], + TAG_POSES[10 - 1], + TAG_POSES[11 - 1] }; + + public static final double[][] blueSideReefTags = { + TAG_POSES[6 + 11 - 1], TAG_POSES[7 + 11 - 1], TAG_POSES[8 + 11 - 1], + TAG_POSES[9 + 11 - 1], + TAG_POSES[10 + 11 - 1], TAG_POSES[11 + + 11 - 1] }; + + // Poses of cameras relative to robot, {x, y, z, rx, ry, rz}, in meters and + // radians + public static final double[] FRONT_CAMERA_POSE = { Constants.inchesToMeters(1.75), + Constants.inchesToMeters(11.625), + Constants.inchesToMeters(33.5), 0, -33.5, 0 }; + + // Standard deviation adjustments + public static final double STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR = 1; + public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE = 3; + public static final double TAG_STANDARD_DEVIATION_DISTANCE = 2; // meters + public static final double TAG_STANDARD_DEVIATION_FLATNESS = 5; + // public static final double XY_STD_DEV_SCALAR = 0.1; + + // Standard deviation regressions + /** + * Calculates the standard deviation scalar based on the distance from the tag. + * + * @param dist The distance from the tag. + * @return The standard deviation scalar. + */ + public static double getTagDistStdDevScalar(double dist) { + // double a = TAG_STANDARD_DEVIATION_FLATNESS; + // double b = 1 - a * Math.pow(TAG_STANDARD_DEVIATION_DISTANCE, 2); + // Logger.recordOutput("std devs", + // -0.000277778 * Math.pow(dist, 3) + 0.00988095 * Math.pow(dist, 2) + + // 0.00444444 * dist + 0.0371429); + // return Math.max(1, a * Math.pow(dist, 2) + b); + return 0.0000520833 * Math.pow(dist, 4) + 0.000394571 * Math.pow(dist, 3) + + 0.000440341 * Math.pow(dist, 2) + + 0.0554117 * dist + 0.0298674; + } + + /** + * Calculates the standard deviation scalar based on the number of detected + * tags. + * + * @param numTags The number of detected tags. + * @return The standard deviation scalar. + */ + public static double getNumTagStdDevScalar(int numTags) { + if (numTags == 0) { + return 99999; + } else if (numTags == 1) { + return 1; + } else if (numTags == 2) { + return 0.6; + } else { + return 0.75; + } + } + + /** + * Calculates the standard deviation of the x-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the x-coordinate. + */ + public static double getTagStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.005533021491867763 * (xOffset * xOffset + yOffset * yOffset) + - 0.010807566510145635) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation of the y-coordinate based on the given + * offsets. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation of the y-coordinate. + */ + public static double getTagStdDevY(double xOffset, double yOffset) { + return Math.max(0, 0.0055 * (xOffset * xOffset + yOffset * yOffset) - 0.01941597810542626) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the x-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the x-coordinate. + */ + public static double getTriStdDevX(double xOffset, double yOffset) { + return Math.max(0, + 0.004544133588821881 * (xOffset * xOffset + yOffset * yOffset) + - 0.01955724864971872) + * STANDARD_DEVIATION_SCALAR; + } + + /** + * Calculates the standard deviation in the y-coordinate for triangulation + * measurements. + * + * @param xOffset The x-coordinate offset. + * @param yOffset The y-coordinate offset. + * @return The standard deviation in the y-coordinate. + */ + public static double getTriStdDevY(double xOffset, double yOffset) { + return Math.max(0, + 0.002615358015002413 * (xOffset * xOffset + yOffset * yOffset) + - 0.008955462032388808) + * STANDARD_DEVIATION_SCALAR; + } + + public static double distBetweenPose(Pose3d pose1, Pose3d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static double distBetweenPose2d(Pose2d pose1, Pose2d pose2) { + return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) + + Math.pow(pose1.getY() - pose2.getY(), 2))); + } + + public static final double DISTANCE_OFFSET = 7.0; + public static final double CAMERA_ANGLE_OFFSET = 0.0; + // pitch, distance + public static final double[][] CORAL_LOOKUP_TABLE = { + { -17.72 + CAMERA_ANGLE_OFFSET, 24.5 + DISTANCE_OFFSET }, + { -12.22 + CAMERA_ANGLE_OFFSET, 28.5 + DISTANCE_OFFSET }, + { -8.02 + CAMERA_ANGLE_OFFSET, 33.0 + DISTANCE_OFFSET }, + { -5.69 + CAMERA_ANGLE_OFFSET, 38.25 + DISTANCE_OFFSET }, + { -2.73 + CAMERA_ANGLE_OFFSET, 43.75 + DISTANCE_OFFSET }, + { -0.05 + CAMERA_ANGLE_OFFSET, 50.0 + DISTANCE_OFFSET }, + { 2.09 + CAMERA_ANGLE_OFFSET, 56.0 + DISTANCE_OFFSET }, + { 3.55 + CAMERA_ANGLE_OFFSET, 61.5 + DISTANCE_OFFSET }, + { 5.87 + CAMERA_ANGLE_OFFSET, 71.75 + DISTANCE_OFFSET } + }; + + /** + * Interpolates a value from a lookup table based on the given xValue. + * + * @param xIndex The index of the x-values in the lookup table. + * @param yIndex The index of the y-values in the lookup table. + * @param xValue The x-value for which to interpolate a y-value. + * @return The interpolated y-value corresponding to the given x-value. + */ + public static double getInterpolatedValue(int xIndex, int yIndex, double xValue) { + int lastIndex = CORAL_LOOKUP_TABLE.length - 1; + if (xValue < CORAL_LOOKUP_TABLE[0][xIndex]) { + // If the xValue is closer than the first setpoint + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } else if (xValue > CORAL_LOOKUP_TABLE[lastIndex][xIndex]) { + // If the xValue is farther than the last setpoint + double returnValue = CORAL_LOOKUP_TABLE[lastIndex][yIndex]; + return returnValue; + } else { + for (int i = 0; i < CORAL_LOOKUP_TABLE.length; i++) { + if (xValue > CORAL_LOOKUP_TABLE[i][xIndex] + && xValue < CORAL_LOOKUP_TABLE[i + 1][xIndex]) { + // If the xValue is in the table of setpoints + // Calculate where xValue is between setpoints + double leftDif = xValue - CORAL_LOOKUP_TABLE[i][xIndex]; + double percent = leftDif / (CORAL_LOOKUP_TABLE[i + 1][xIndex] + - CORAL_LOOKUP_TABLE[i][xIndex]); + + double value1 = CORAL_LOOKUP_TABLE[i][yIndex]; + double value2 = CORAL_LOOKUP_TABLE[i + 1][yIndex]; + + // Interpolate in-between values for value xValue and shooter rpm + double newValue = value1 + (percent * (value2 - value1)); + + double returnValue = newValue; + return returnValue; + } + } + // Should never run + double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; + return returnValue; + } + } + + /** + * Calculates shooter values (flywheel velocity and note velocity) based on the + * given angle. + * + * @param angle The angle of the shooter. + * @return An array containing the calculated flywheel velocity and note + * velocity. + */ + public static double getCoralDistanceFromPitch(double angle) { + return getInterpolatedValue(0, 1, angle); + // return new double[] {25, getInterpolatedValue(1, 3, angle)}; } - // l3FrontLeft = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_FRONT, - // Physical.INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_FRONT, - // Physical.INTAKE_Y_OFFSET_FRONT, - // new Rotation2d(Math.PI))) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians() - Math.PI)); - // l3BackLeft = new Pose2d( - // new Translation2d( - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_BACK, - // Physical.INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getX(), - // poseDirection - // .transformBy(new Transform2d(adjustX, - // -adjustY, - // new Rotation2d())) - // .transformBy(new Transform2d( - // Physical.INTAKE_X_OFFSET_BACK, - // Physical.INTAKE_Y_OFFSET_BACK, - // new Rotation2d())) - // .getY()), - // new Rotation2d( - // poseDirection.getRotation().getRadians())); - blueFrontPlacingPositions.add(l2FrontRight); - blueFrontPlacingPositions.add(l2FrontLeft); - blueBackPlacingPositions.add(l2BackRight); - blueBackPlacingPositions.add(l2BackLeft); - blueFrontPlacingPositionsMore.add(frontRightMore); - blueFrontPlacingPositionsMore.add(frontLeftMore); - blueBackPlacingPositionsMore.add(backRightMore); - blueBackPlacingPositionsMore.add(backLeftMore); - l4BlueFrontPlacingPositions.add(l4FrontRight); - l4BlueFrontPlacingPositions.add(l4FrontLeft); - l4BlueBackPlacingPositions.add(l4BackRight); - l4BlueBackPlacingPositions.add(l4BackLeft); - l3BlueFrontPlacingPositions.add(l3FrontRight); - l3BlueFrontPlacingPositions.add(l3FrontLeft); - l3BlueBackPlacingPositions.add(l3BackRight); - l3BlueBackPlacingPositions.add(l3BackLeft); - algaeBlueFrontPlacingPositions.add(algaeFront); - algaeBlueBackPlacingPositions.add(algaeBack); - algaeBlueFrontPlacingPositionsMore.add(algaeFrontMore); - algaeBlueBackPlacingPositionsMore.add(algaeBackMore); - algaeBlueFrontPlacingPositionsMoreMore.add(algaeFrontMoreMore); - algaeBlueBackPlacingPositionsMoreMore.add(algaeBackMoreMore); - l1BlueCornerPoints.add(l1Corner); - l1BlueDrivePoints.add(l1Drive); - blueL1FrontPlacingPositions.add(l1FrontLeft); - blueL1FrontPlacingPositions.add(l1FrontRight); - blueL1BackPlacingPositions.add(l1BackLeft); - blueL1BackPlacingPositions.add(l1BackRight); - blueL1FrontPlacingPositionsMore.add(l1FrontLeftMore); - blueL1FrontPlacingPositionsMore.add(l1FrontRightMore); - blueL1BackPlacingPositionsMore.add(l1BackLeftMore); - blueL1BackPlacingPositionsMore.add(l1BackRightMore); - } - - for (Pose2d bluePose : blueL1FrontPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redL1FrontPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : blueL1BackPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redL1BackPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : blueL1FrontPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redL1FrontPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : blueL1BackPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redL1BackPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : algaeBlueFrontPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedFrontPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : l1BlueCornerPoints) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l1RedCornerPoints.add(redPose); - } - - for (Pose2d bluePose : l1BlueDrivePoints) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l1RedDrivePoints.add(redPose); - } - - for (Pose2d bluePose : algaeBlueBackPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedBackPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedFrontPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : algaeBlueBackPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedBackPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : algaeBlueFrontPlacingPositionsMoreMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedFrontPlacingPositionsMoreMore.add(redPose); - } - - for (Pose2d bluePose : algaeBlueBackPlacingPositionsMoreMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - algaeRedBackPlacingPositionsMoreMore.add(redPose); - } - - for (Pose2d bluePose : blueFrontPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redFrontPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : blueBackPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() - Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redBackPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : blueFrontPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redFrontPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : blueBackPlacingPositionsMore) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() - Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - redBackPlacingPositionsMore.add(redPose); - } - - for (Pose2d bluePose : l4BlueFrontPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l4RedFrontPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : l4BlueBackPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() - Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l4RedBackPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : l3BlueFrontPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() + Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l3RedFrontPlacingPositions.add(redPose); - } - - for (Pose2d bluePose : l3BlueBackPlacingPositions) { - Pose2d redPose = new Pose2d(); - Translation2d mirroredTranslation = new Translation2d( - Constants.Physical.FIELD_LENGTH - bluePose.getX(), - Constants.Physical.FIELD_WIDTH - bluePose.getY()); - Rotation2d mirroredRotation = new Rotation2d( - bluePose.getRotation().getRadians() - Math.PI); - redPose = new Pose2d(mirroredTranslation, mirroredRotation); - l3RedBackPlacingPositions.add(redPose); - } } - } - - // Physical constants (e.g. field and robot dimensions) - public static final class Physical { - public static final double FIELD_WIDTH = 8.052; - public static final double FIELD_LENGTH = 17.548; - public static final double WHEEL_DIAMETER = inchesToMeters(4); - public static final double WHEEL_CIRCUMFERENCE = Math.PI * WHEEL_DIAMETER; - public static final double WHEEL_ROTATION_PER_METER = 1 / WHEEL_CIRCUMFERENCE; - public static final double WHEEL_TO_FRAME_DISTANCE = inchesToMeters(2.5); - public static final double TOP_SPEED = feetToMeters(30.0); - public static final double MAX_ACCELERATION = feetToMeters(30.0); // TODO: actually tune the top speed - // and max acceleration. Add a max - // deceleration if needed. - - public static final double ROBOT_LENGTH = inchesToMeters(32); - public static final double ROBOT_WIDTH = inchesToMeters(26); - public static final double MODULE_OFFSET = inchesToMeters(2.625); - public static final double ROBOT_RADIUS = Math.hypot(ROBOT_LENGTH / 2 - WHEEL_TO_FRAME_DISTANCE, - ROBOT_WIDTH / 2 - WHEEL_TO_FRAME_DISTANCE); - public static double INTAKE_X_OFFSET_FRONT = inchesToMeters(33.0); - public static double INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.5); - public static double INTAKE_X_OFFSET_BACK = inchesToMeters(33.0); - public static double INTAKE_Y_OFFSET_BACK = inchesToMeters(0.5); - public static double INTAKE_X_OFFSET_FRONT_ALGAE = inchesToMeters(23.0 + 5.0); - public static double INTAKE_Y_OFFSET_FRONT_ALGAE = inchesToMeters(3.8); - public static double INTAKE_X_OFFSET_BACK_ALGAE = inchesToMeters(23.0 + 5.0); - public static double INTAKE_Y_OFFSET_BACK_ALGAE = inchesToMeters(-0.0); - public static double L1_INTAKE_X_OFFSET_FRONT = inchesToMeters(35.3); - public static double L1_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); - public static double L1_INTAKE_X_OFFSET_BACK = inchesToMeters(35.3); - public static double L1_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); - public static double L1_INTAKE_X_OFFSET_FRONT_MORE = inchesToMeters(24.9); - public static double L1_INTAKE_Y_OFFSET_FRONT_MORE = inchesToMeters(0.0); - public static double L1_INTAKE_X_OFFSET_BACK_MORE = inchesToMeters(24.9); - public static double L1_INTAKE_Y_OFFSET_BACK_MORE = inchesToMeters(-0.0); - public static double L2_INTAKE_X_OFFSET_FRONT = inchesToMeters(25.4); - public static double L2_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); - public static double L2_INTAKE_X_OFFSET_BACK = inchesToMeters(25.4); - public static double L2_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); - public static double L3_INTAKE_X_OFFSET_FRONT = inchesToMeters(26.0); - public static double L3_INTAKE_Y_OFFSET_FRONT = inchesToMeters(0.0); - public static double L3_INTAKE_X_OFFSET_BACK = inchesToMeters(26.0); - public static double L3_INTAKE_Y_OFFSET_BACK = inchesToMeters(-0.0); - public static double L4_INTAKE_X_OFFSET_FRONT = inchesToMeters(27.0); - public static double L4_INTAKE_Y_OFFSET_FRONT = inchesToMeters(-1.0); - public static double L4_INTAKE_X_OFFSET_BACK = inchesToMeters(27.0); - public static double L4_INTAKE_Y_OFFSET_BACK = inchesToMeters(-1.0); - - public static final double GRAVITY_ACCEL_MS2 = 9.806; - } - - public static final class Arm { - - public static final double L4_Score = -20.0; - public static final double L4_Place = 45.0; - public static final double L3_Score = -10.0; - public static final double L3_Place = 47.0; - public static final double L2_Score = -10.0; - public static final double L2_Place = 47.0; - public static final double L1_Place = -25.0; - public static final double HANDOFF = -91.0; - public static final double DEFAULT = -90.0; - public static final double HORIZONTAL = 5.0; - public static final double VERTICAL = 95.0; - public static final double NET = 135.0; - public static final double IDLE = 0.0; - } - - public static final class Elevator { - public static final double AUTO_L1 = inchesToMeters(18.0); - public static final double AUTO_L2 = inchesToMeters(8.0); - public static final double AUTO_L3 = inchesToMeters(24.25); - public static final double AUTO_L4 = inchesToMeters(50.0); - public static final double AUTO_SCORE_L2_HIGH = inchesToMeters(14.0); - public static final double AUTO_SCORE_L2 = inchesToMeters(5.0); - public static final double AUTO_SCORE_L3 = inchesToMeters(20.0); - public static final double AUTO_SCORE_L4 = inchesToMeters(48.0); - public static final double HANDOFF_HIGH = inchesToMeters(11.0); - public static final double HANDOFF_LOW = inchesToMeters(3.0); - public static final double ALGAE_HIGH = inchesToMeters(33); - public static final double ALGAE_LOW = inchesToMeters(14.5); - public static final double PROCESSOR = inchesToMeters(0.0); - public static final double NET = inchesToMeters(55.0); - } - - // Subsystem setpoint constants - public static final class SetPoints { - public static class IntakeSetpoints { - public static final double INTAKE_ACCELERATION = 500.0; - public static final double INTAKE_CRUISE_VELOCITY = 400.0; - public static final double INTAKE_MOTION_PROFILE_SCALAR = 1.0; - public static final double INTAKE_DOWN = -18.2; // rotations - public static final double INTAKE_UP = 0; // rotations - public static final double INTAKE_ROLLER_MAX_SPEED = 1.0; // percent - public static final double INTAKE_ROLLER_HOLDING_SPEED = 0.1; // percent - public static final double INTAKE_ROLLER_TORQUE = 80.0; // amps - public static final double INTAKE_HOLDING_TORQUE = 60.0; // amps + + // Gear ratios and conversions + public static final class Ratios { + // pivot + public static final double PIVOT_GEAR_RATIO = 23 * 64 / 24; + + // drive + public static final double DRIVE_GEAR_RATIO = 6.12; + public static final double STEER_GEAR_RATIO = 21.43; + + // elevator + public static final double ELEVATOR_FIRST_STAGE = Constants.inchesToMeters(20); + public static final double ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE = 18.067; + public static final double ELEVATOR_MOTOR_ROTATIONS_PER_METER = ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE + * (1 / ELEVATOR_FIRST_STAGE); + + public static double elevatorRotationsToMeters(double rotations) { + return rotations / ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + public static double elevatorMetersToRotations(double meters) { + return meters * ELEVATOR_MOTOR_ROTATIONS_PER_METER; + } + + // intake + public static final double INTAKE_PIVOT_GEAR_RATIO = 45.0; } - public static final double ELEVATOR_BOTTOM_POSITION_M = 0.0; - public static final double ELEVATOR_MID_POSITION_M = inchesToMeters(26.0); // L2 after placement - public static final double ELEVATOR_TOP_POSITION_M = inchesToMeters(43.0); - public static final double ELEVATOR_L1_POSITION_M = inchesToMeters(6.6); - public static final double ELEVATOR_L2_POSITION_M = inchesToMeters(15); - public static final double ELEVATOR_AUTO_L2_POSITION_M = inchesToMeters(18.5); - public static final double ELEVATOR_AUTO_L2_POSITION_SCORE_M = inchesToMeters(16); - public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(34.25); - // public static final double ELEVATOR_AUTO_L3_POSITION_M = inchesToMeters(25); - public static final double ELEVATOR_AUTO_SCORE_L3_POSITION_M = inchesToMeters(25); - public static final double ELEVATOR_AUTO_L4_POSITION_M = inchesToMeters(64.0); - public static final double ELEVATOR_L3_POSITION_M = inchesToMeters(28); - public static final double ELEVATOR_L4_POSITION_M = inchesToMeters(64.0); - public static final double ELEVATOR_ALGAE_POSITION_M = inchesToMeters(8.0); - public static final double ELEVATOR_GROUND_CORAL_POSITION_M = inchesToMeters(5.4); - public static final double ELEVATOR_GROUND_ALGAE_POSITION_M = inchesToMeters(5.0); - public static final double ELEVATOR_FEEDER_POSITION_M = inchesToMeters(0.0); - public static final double ELEVATOR_OVER_POSITION_M = inchesToMeters(20); - public static final double ELEVATOR_NET_POSITION_M = inchesToMeters(65); - public static final double ELEVATOR_L2_ALGAE_POSITION_M = inchesToMeters(15.7); - public static final double ELEVATOR_L3_ALGAE_POSITION_M = inchesToMeters(37.6741); - public static final double ELEVATOR_PROCESSOR_POSITION_M = inchesToMeters(6.5); - public static final double ELEVATOR_LOLLIPOP_POSITION_M = inchesToMeters(0.0); - public static final double ELEVATOR_PRE_HANDOFF_POSITION_M = inchesToMeters(39.0); - public static final double ELEVATOR_HANDOFF_POSITION_M = inchesToMeters(35.0); - - public enum ElevatorPosition { - kDOWN(ELEVATOR_BOTTOM_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_BOTTOM_POSITION_M)), - kMID(ELEVATOR_MID_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_MID_POSITION_M)), - kUP(ELEVATOR_TOP_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_TOP_POSITION_M)), - kL1(ELEVATOR_L1_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L1_POSITION_M)), - kL2(ELEVATOR_L2_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L2_POSITION_M)), - kAUTOL2(ELEVATOR_AUTO_L2_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_M)), - kAUTOL2SCORE(ELEVATOR_AUTO_L2_POSITION_SCORE_M, - Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L2_POSITION_SCORE_M)), - kAUTOL3(ELEVATOR_AUTO_L3_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L3_POSITION_M)), - kAUTOL3SCORE(ELEVATOR_AUTO_SCORE_L3_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_SCORE_L3_POSITION_M)), - kAUTOL4(ELEVATOR_AUTO_L4_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_AUTO_L4_POSITION_M)), - kL3(ELEVATOR_L3_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L3_POSITION_M)), - kL4(ELEVATOR_L4_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_L4_POSITION_M)), - kALGAE(ELEVATOR_ALGAE_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_ALGAE_POSITION_M)), - kFEEDER(ELEVATOR_FEEDER_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_FEEDER_POSITION_M)), - kGROUNDCORAL(ELEVATOR_GROUND_CORAL_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_CORAL_POSITION_M)), - kGROUNDALGAE(ELEVATOR_GROUND_ALGAE_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_GROUND_ALGAE_POSITION_M)), - kNET(ELEVATOR_NET_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_NET_POSITION_M)), - kPROCESSOR(ELEVATOR_PROCESSOR_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_PROCESSOR_POSITION_M)), - kL2ALGAE(ELEVATOR_L2_ALGAE_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_L2_ALGAE_POSITION_M)), - kL3ALGAE(ELEVATOR_L3_ALGAE_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_L3_ALGAE_POSITION_M)), - kOVER(ELEVATOR_OVER_POSITION_M, Ratios.elevatorMetersToRotations(ELEVATOR_OVER_POSITION_M)), - kLOLLIPOP(ELEVATOR_LOLLIPOP_POSITION_M, Ratios.elevatorMetersToRotations( - ELEVATOR_LOLLIPOP_POSITION_M)), - kHANDOFF(ELEVATOR_HANDOFF_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_HANDOFF_POSITION_M)), - kPREHANDOFF(ELEVATOR_PRE_HANDOFF_POSITION_M, - Ratios.elevatorMetersToRotations(ELEVATOR_PRE_HANDOFF_POSITION_M)); - - public final double meters; - public final double rotations; - - private ElevatorPosition(double meters, double rotations) { - this.meters = meters; - this.rotations = rotations; - } + public static boolean isReady = false; + public static final ArrayList paths = new ArrayList(); + + // Can info such as IDs + public static final class CANInfo { + public static final String CANBUS_NAME = "Canivore"; + + // drive + public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 1; + public static final int FRONT_RIGHT_ANGLE_MOTOR_ID = 2; + public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 3; + public static final int FRONT_LEFT_ANGLE_MOTOR_ID = 4; + public static final int BACK_LEFT_DRIVE_MOTOR_ID = 5; + public static final int BACK_LEFT_ANGLE_MOTOR_ID = 6; + public static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; + public static final int BACK_RIGHT_ANGLE_MOTOR_ID = 8; + public static final int FRONT_RIGHT_MODULE_CANCODER_ID = 1; + public static final int FRONT_LEFT_MODULE_CANCODER_ID = 2; + public static final int BACK_LEFT_MODULE_CANCODER_ID = 3; + public static final int BACK_RIGHT_MODULE_CANCODER_ID = 4; + + // Elevator + public static final int LEFT_ELEVATOR_MOTOR_ID = 9; + public static final int RIGHT_ELEVATOR_MOTOR_ID = 10; + + // Manipulator + public static final int MANIPULATOR_MOTOR_ID = 13; + + // Intake + public static final int INTAKE_ROLLER_MOTOR_ID = 17; + public static final int INTAKE_PIVOT_MOTOR_ID = 16; + public static final int INTAKE_BEAM_BREAK_PORT = 0; + + // Straightenator + public static final int LEFT_STRAIGHTENATOR_MOTOR_ID = 12; + public static final int RIGHT_STRAIGHTENATOR_MOTOR_ID = 13; + public static final int FAR_BEAM_BREAK_SENSOR = 0; + public static final int CLOSE_BEAM_BREAK_SENSOR = 2; + + // Arm + public static final int ARM_PIVOT_MOTOR_ID = 15; + public static final int ARM_MANIPULATOR_MOTOR_ID = 14; } - public static final double PIVOT_L1_POSITION_D = 65.67; - public static final double PIVOT_L23_POSITION_D = 52.5; - // public static final double PIVOT_AUTO_L23_POSITION_D = 45.0; - public static final double PIVOT_AUTO_L2_POSITION_D = 62.0; - public static final double PIVOT_AUTO_L3_POSITION_D = 62.0; - // public static final double PIVOT_AUTO_L3_POSITION_D = 30.0; - public static final double PIVOT_AUTO_L4_POSITION_D = 0.0; - public static final double PIVOT_AUTO_L4_SCORE_POSITION_D = 100.0; - public static final double PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D = 70.0; - public static final double PIVOT_AUTO_L3_SCORE_POSITION_D = 100.0; - public static final double PIVOT_AUTO_L2_SCORE_POSITION_D = 100.0; - public static final double PIVOT_L4_POSITION_D = 60.0; - public static final double PIVOT_UPRIGHT_POSITION_D = 45.0; - public static final double PIVOT_GROUND_ALGAE_POSITION_D = 82.5; - public static final double PIVOT_GROUND_CORAL_POSITION_FRONT_D = 111.0; - public static final double PIVOT_GROUND_CORAL_POSITION_BACK_D = -111.0; - public static final double PIVOT_GROUND_CORAL_PREP_BACK_D = -90; - // public static final double PIVOT_DEFAULT_POSITION_D = 30.0; - public static final double PIVOT_DEFAULT_POSITION_D = 0.0; - public static final double PIVOT_DEFAULT_CLIMB_POSITION_D = 45.0; - public static final double PIVOT_PREP_POSITION_D = 30.0; - public static final double PIVOT_FEEDER_POSITION_D = 21.0; - public static final double PIVOT_NET_POSITION_D = 15.0; - public static final double PIVOT_PROCESSOR_POSITION_D = 76.0; - public static final double PIVOT_REEF_ALGAE_POSITION_D = 80.0; - public static final double PIVOT_CLIMB_POSITION_D = 45.0; - public static final double PIVOT_LOLLIPOP_POSITION_D = -98.0; - public static final double PIVOT_HANDOFF_POSITION_D = 145.0; - - public enum PivotPosition { - kL1(PIVOT_L1_POSITION_D, Constants.degreesToRotations(PIVOT_L1_POSITION_D)), - kL23(PIVOT_L23_POSITION_D, Constants.degreesToRotations(PIVOT_L23_POSITION_D)), - kAUTOL2(PIVOT_AUTO_L2_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L2_POSITION_D)), - kAUTOL3(PIVOT_AUTO_L3_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L3_POSITION_D)), - kAUTOL4(PIVOT_AUTO_L4_POSITION_D, Constants.degreesToRotations(PIVOT_AUTO_L4_POSITION_D)), - kL4(PIVOT_L4_POSITION_D, Constants.degreesToRotations(PIVOT_L4_POSITION_D)), - kUP(PIVOT_UPRIGHT_POSITION_D, Constants.degreesToRotations(PIVOT_UPRIGHT_POSITION_D)), - kGROUNDALGAE(PIVOT_GROUND_ALGAE_POSITION_D, - Constants.degreesToRotations(PIVOT_GROUND_ALGAE_POSITION_D)), - kGROUNDCORALFRONT(PIVOT_GROUND_CORAL_POSITION_FRONT_D, - Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_FRONT_D)), - kGROUNDCORALBACK(PIVOT_GROUND_CORAL_POSITION_BACK_D, - Constants.degreesToRotations(PIVOT_GROUND_CORAL_POSITION_BACK_D)), - kGROUNDCORALPREPBACK(PIVOT_GROUND_CORAL_PREP_BACK_D, - Constants.degreesToRotations(PIVOT_GROUND_CORAL_PREP_BACK_D)), - kNET(PIVOT_NET_POSITION_D, - Constants.degreesToRotations(PIVOT_NET_POSITION_D)), - kPROCESSOR(PIVOT_PROCESSOR_POSITION_D, - Constants.degreesToRotations(PIVOT_PROCESSOR_POSITION_D)), - kAUTOL2SCORE(PIVOT_AUTO_L2_SCORE_POSITION_D, - Constants.degreesToRotations(PIVOT_AUTO_L2_SCORE_POSITION_D)), - kAUTOL3SCORE(PIVOT_AUTO_L3_SCORE_POSITION_D, - Constants.degreesToRotations(PIVOT_AUTO_L3_SCORE_POSITION_D)), - kAUTOL4SCORE(PIVOT_AUTO_L4_SCORE_POSITION_D, - Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_POSITION_D)), - kAUTOL4SCORESLOW(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D, - Constants.degreesToRotations(PIVOT_AUTO_L4_SCORE_SLOW_POSITION_D)), - kDEFAULT(PIVOT_DEFAULT_POSITION_D, Constants.degreesToRotations(PIVOT_DEFAULT_POSITION_D)), - kDEFAULTCLIMB(PIVOT_DEFAULT_CLIMB_POSITION_D, - Constants.degreesToRotations(PIVOT_DEFAULT_CLIMB_POSITION_D)), - kFEEDER(PIVOT_FEEDER_POSITION_D, Constants.degreesToRotations(PIVOT_FEEDER_POSITION_D)), - kREEFALGAE(PIVOT_REEF_ALGAE_POSITION_D, - Constants.degreesToRotations(PIVOT_REEF_ALGAE_POSITION_D)), - kPREP(PIVOT_PREP_POSITION_D, Constants.degreesToRotations(PIVOT_PREP_POSITION_D)), - kCLIMB(PIVOT_CLIMB_POSITION_D, Constants.degreesToRotations(PIVOT_CLIMB_POSITION_D)), - kLOLLIPOP(PIVOT_LOLLIPOP_POSITION_D, Constants.degreesToRotations(PIVOT_LOLLIPOP_POSITION_D)), - kHANDOFF(PIVOT_HANDOFF_POSITION_D, Constants.degreesToRotations(PIVOT_HANDOFF_POSITION_D)); - - public final double degrees; - public final double rotations; - - private PivotPosition(double degrees, double rotations) { - this.degrees = degrees; - this.rotations = rotations; - } + public static final class IntakeConstants { + public static double PIVOT_LOWER_LIMIT = 0; + public static double PIVOT_UPPER_LIMIT = 10; } - } - - // Vision constants (e.g. camera offsets) - public static final class Vision { - // Poses of all 16 AprilTags, {x, y, z, yaw, pitch}, in meters and radians - public static final double[][] TAG_POSES = { - { inchesToMeters(657.37), inchesToMeters(25.8), inchesToMeters(58.5), - Math.toRadians(126), - Math.toRadians(0) }, - { inchesToMeters(657.37), inchesToMeters(291.2), inchesToMeters(58.5), - Math.toRadians(234), - Math.toRadians(0) }, - { inchesToMeters(455.15), inchesToMeters(317.15), inchesToMeters(51.25), - Math.toRadians(270), - Math.toRadians(0) }, - { inchesToMeters(365.2), inchesToMeters(241.64), inchesToMeters(73.54), - Math.toRadians(0), - Math.toRadians(30) }, - { inchesToMeters(365.2), inchesToMeters(75.39), inchesToMeters(73.54), - Math.toRadians(0), - Math.toRadians(30) }, - { inchesToMeters(530.49), inchesToMeters(130.17), inchesToMeters(12.13), - Math.toRadians(300), - Math.toRadians(0) }, - { inchesToMeters(546.87), inchesToMeters(158.5), inchesToMeters(12.13), - Math.toRadians(0), - Math.toRadians(0) }, - { inchesToMeters(530.49), inchesToMeters(186.83), inchesToMeters(12.13), - Math.toRadians(60), - Math.toRadians(0) }, - { inchesToMeters(497.77), inchesToMeters(186.83), inchesToMeters(12.13), - Math.toRadians(120), - Math.toRadians(0) }, - { inchesToMeters(481.39), inchesToMeters(158.5), inchesToMeters(12.13), - Math.toRadians(180), - Math.toRadians(0) }, - { inchesToMeters(497.77), inchesToMeters(130.17), inchesToMeters(12.13), - Math.toRadians(240), - Math.toRadians(0) }, - { inchesToMeters(33.51), inchesToMeters(25.8), inchesToMeters(58.5), Math.toRadians(54), - Math.toRadians(0) }, - { inchesToMeters(33.51), inchesToMeters(291.2), inchesToMeters(58.5), - Math.toRadians(306), - Math.toRadians(0) }, - { inchesToMeters(325.68), inchesToMeters(241.64), inchesToMeters(73.54), - Math.toRadians(180), - Math.toRadians(30) }, - { inchesToMeters(325.68), inchesToMeters(75.39), inchesToMeters(73.54), - Math.toRadians(180), - Math.toRadians(30) }, - { inchesToMeters(235.73), inchesToMeters(-0.15), inchesToMeters(51.25), - Math.toRadians(90), - Math.toRadians(0) }, - { inchesToMeters(160.39), inchesToMeters(130.17), inchesToMeters(12.13), - Math.toRadians(240), - Math.toRadians(0) }, - { inchesToMeters(144), inchesToMeters(158.5), inchesToMeters(12.13), - Math.toRadians(180), - Math.toRadians(0) }, - { inchesToMeters(160.39), inchesToMeters(186.83), inchesToMeters(12.13), - Math.toRadians(120), - Math.toRadians(0) }, - { inchesToMeters(193.1), inchesToMeters(186.83), inchesToMeters(12.13), - Math.toRadians(60), - Math.toRadians(0) }, - { inchesToMeters(209.49), inchesToMeters(158.5), inchesToMeters(12.13), - Math.toRadians(0), - Math.toRadians(0) }, - { inchesToMeters(193.1), inchesToMeters(130.17), inchesToMeters(12.13), - Math.toRadians(300), - Math.toRadians(0) } - }; - - public static final double[][] redSideReefTags = { - TAG_POSES[6 - 1], TAG_POSES[7 - 1], TAG_POSES[8 - 1], TAG_POSES[9 - 1], - TAG_POSES[10 - 1], - TAG_POSES[11 - 1] }; - - public static final double[][] blueSideReefTags = { - TAG_POSES[6 + 11 - 1], TAG_POSES[7 + 11 - 1], TAG_POSES[8 + 11 - 1], - TAG_POSES[9 + 11 - 1], - TAG_POSES[10 + 11 - 1], TAG_POSES[11 - + 11 - 1] }; - - // Poses of cameras relative to robot, {x, y, z, rx, ry, rz}, in meters and - // radians - public static final double[] FRONT_CAMERA_POSE = { Constants.inchesToMeters(1.75), - Constants.inchesToMeters(11.625), - Constants.inchesToMeters(33.5), 0, -33.5, 0 }; - - // Standard deviation adjustments - public static final double STANDARD_DEVIATION_SCALAR = 1; - public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_SCALAR = 1; - public static final double ODOMETRY_JUMP_STANDARD_DEVIATION_DEGREE = 3; - public static final double TAG_STANDARD_DEVIATION_DISTANCE = 2; // meters - public static final double TAG_STANDARD_DEVIATION_FLATNESS = 5; - // public static final double XY_STD_DEV_SCALAR = 0.1; - - // Standard deviation regressions + + // Misc. controller values + public static final class OperatorConstants { + public static final double RIGHT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_TRIGGER_DEADZONE = 0.1; + public static final double LEFT_STICK_DEADZONE = 0.03; + public static final double RIGHT_STICK_DEADZONE = 0.05; + } + /** - * Calculates the standard deviation scalar based on the distance from the tag. + * Converts inches to meters. * - * @param dist The distance from the tag. - * @return The standard deviation scalar. + * @param inches The length in inches to be converted. + * @return The equivalent length in meters. */ - public static double getTagDistStdDevScalar(double dist) { - // double a = TAG_STANDARD_DEVIATION_FLATNESS; - // double b = 1 - a * Math.pow(TAG_STANDARD_DEVIATION_DISTANCE, 2); - // Logger.recordOutput("std devs", - // -0.000277778 * Math.pow(dist, 3) + 0.00988095 * Math.pow(dist, 2) + - // 0.00444444 * dist + 0.0371429); - // return Math.max(1, a * Math.pow(dist, 2) + b); - return 0.0000520833 * Math.pow(dist, 4) + 0.000394571 * Math.pow(dist, 3) - + 0.000440341 * Math.pow(dist, 2) - + 0.0554117 * dist + 0.0298674; + public static double inchesToMeters(double inches) { + return inches / 39.37; + } + + public static double metersToInches(double meters) { + return meters * 39.37; } /** - * Calculates the standard deviation scalar based on the number of detected - * tags. + * Converts feet to meters. * - * @param numTags The number of detected tags. - * @return The standard deviation scalar. + * @param inches The length in feet to be converted. + * @return The equivalent length in meters. */ - public static double getNumTagStdDevScalar(int numTags) { - if (numTags == 0) { - return 99999; - } else if (numTags == 1) { - return 1; - } else if (numTags == 2) { - return 0.6; - } else { - return 0.75; - } + public static double feetToMeters(double feet) { + return feet / 3.281; } /** - * Calculates the standard deviation of the x-coordinate based on the given - * offsets. + * Calculates the Euclidean distance between two points in a 2D plane. * - * @param xOffset The x-coordinate offset. - * @param yOffset The y-coordinate offset. - * @return The standard deviation of the x-coordinate. + * @param x1 The x-coordinate of the first point. + * @param y1 The y-coordinate of the first point. + * @param x2 The x-coordinate of the second point. + * @param y2 The y-coordinate of the second point. + * @return The Euclidean distance between the two points. */ - public static double getTagStdDevX(double xOffset, double yOffset) { - return Math.max(0, - 0.005533021491867763 * (xOffset * xOffset + yOffset * yOffset) - - 0.010807566510145635) - * STANDARD_DEVIATION_SCALAR; + public static double getDistance(double x1, double y1, double x2, double y2) { + return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); + } + + public static double getAngleToPoint(double x1, double y1, double x2, double y2) { + double deltaX = x2 - x1; + double deltaY = y2 - y1; + + double angleInRadians = Math.atan2(deltaY, deltaX); + + double angleInDegrees = Math.toDegrees(angleInRadians); + + double standardizeAngleDegrees = standardizeAngleDegrees(angleInDegrees); + + return 180 + standardizeAngleDegrees; } /** - * Calculates the standard deviation of the y-coordinate based on the given - * offsets. + * Converts a quantity in rotations to radians. * - * @param xOffset The x-coordinate offset. - * @param yOffset The y-coordinate offset. - * @return The standard deviation of the y-coordinate. + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in radians. */ - public static double getTagStdDevY(double xOffset, double yOffset) { - return Math.max(0, 0.0055 * (xOffset * xOffset + yOffset * yOffset) - 0.01941597810542626) - * STANDARD_DEVIATION_SCALAR; + public static double rotationsToRadians(double rotations) { + return rotations * 2 * Math.PI; } /** - * Calculates the standard deviation in the x-coordinate for triangulation - * measurements. + * Converts a quantity in degrees to rotations. * - * @param xOffset The x-coordinate offset. - * @param yOffset The y-coordinate offset. - * @return The standard deviation in the x-coordinate. + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in rotations. */ - public static double getTriStdDevX(double xOffset, double yOffset) { - return Math.max(0, - 0.004544133588821881 * (xOffset * xOffset + yOffset * yOffset) - - 0.01955724864971872) - * STANDARD_DEVIATION_SCALAR; + public static double degreesToRotations(double degrees) { + return degrees / 360; } /** - * Calculates the standard deviation in the y-coordinate for triangulation - * measurements. + * Converts a quantity in rotations to degrees. * - * @param xOffset The x-coordinate offset. - * @param yOffset The y-coordinate offset. - * @return The standard deviation in the y-coordinate. + * @param rotations The quantity in rotations to be converted. + * @return The equivalent quantity in degrees. */ - public static double getTriStdDevY(double xOffset, double yOffset) { - return Math.max(0, - 0.002615358015002413 * (xOffset * xOffset + yOffset * yOffset) - - 0.008955462032388808) - * STANDARD_DEVIATION_SCALAR; + public static double rotationsToDegrees(double rotations) { + return rotations * 360; } - public static double distBetweenPose(Pose3d pose1, Pose3d pose2) { - return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) - + Math.pow(pose1.getY() - pose2.getY(), 2))); + /** + * Converts a quantity in degrees to radians. + * + * @param rotations The quantity in degrees to be converted. + * @return The equivalent quantity in radians. + */ + public static double degreesToRadians(double degrees) { + return degrees * Math.PI / 180; } - public static double distBetweenPose2d(Pose2d pose1, Pose2d pose2) { - return (Math.sqrt(Math.pow(pose1.getX() - pose2.getX(), 2) - + Math.pow(pose1.getY() - pose2.getY(), 2))); + /** + * Standardizes an angle to be within the range [0, 360) degrees. + * + * @param angleDegrees The input angle in degrees. + * @return The standardized angle within the range [0, 360) degrees. + */ + public static double standardizeAngleDegrees(double angleDegrees) { + return ((angleDegrees % 360) + 360) % 360; } - public static final double DISTANCE_OFFSET = 7.0; - public static final double CAMERA_ANGLE_OFFSET = 0.0; - // pitch, distance - public static final double[][] CORAL_LOOKUP_TABLE = { - { -17.72 + CAMERA_ANGLE_OFFSET, 24.5 + DISTANCE_OFFSET }, - { -12.22 + CAMERA_ANGLE_OFFSET, 28.5 + DISTANCE_OFFSET }, - { -8.02 + CAMERA_ANGLE_OFFSET, 33.0 + DISTANCE_OFFSET }, - { -5.69 + CAMERA_ANGLE_OFFSET, 38.25 + DISTANCE_OFFSET }, - { -2.73 + CAMERA_ANGLE_OFFSET, 43.75 + DISTANCE_OFFSET }, - { -0.05 + CAMERA_ANGLE_OFFSET, 50.0 + DISTANCE_OFFSET }, - { 2.09 + CAMERA_ANGLE_OFFSET, 56.0 + DISTANCE_OFFSET }, - { 3.55 + CAMERA_ANGLE_OFFSET, 61.5 + DISTANCE_OFFSET }, - { 5.87 + CAMERA_ANGLE_OFFSET, 71.75 + DISTANCE_OFFSET } - }; - /** - * Interpolates a value from a lookup table based on the given xValue. - * - * @param xIndex The index of the x-values in the lookup table. - * @param yIndex The index of the y-values in the lookup table. - * @param xValue The x-value for which to interpolate a y-value. - * @return The interpolated y-value corresponding to the given x-value. + * Calculates the x-component of a unit vector given an angle in radians. + * + * @param angle The angle in radians. + * @return The x-component of the unit vector. */ - public static double getInterpolatedValue(int xIndex, int yIndex, double xValue) { - int lastIndex = CORAL_LOOKUP_TABLE.length - 1; - if (xValue < CORAL_LOOKUP_TABLE[0][xIndex]) { - // If the xValue is closer than the first setpoint - double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; - return returnValue; - } else if (xValue > CORAL_LOOKUP_TABLE[lastIndex][xIndex]) { - // If the xValue is farther than the last setpoint - double returnValue = CORAL_LOOKUP_TABLE[lastIndex][yIndex]; - return returnValue; - } else { - for (int i = 0; i < CORAL_LOOKUP_TABLE.length; i++) { - if (xValue > CORAL_LOOKUP_TABLE[i][xIndex] - && xValue < CORAL_LOOKUP_TABLE[i + 1][xIndex]) { - // If the xValue is in the table of setpoints - // Calculate where xValue is between setpoints - double leftDif = xValue - CORAL_LOOKUP_TABLE[i][xIndex]; - double percent = leftDif / (CORAL_LOOKUP_TABLE[i + 1][xIndex] - - CORAL_LOOKUP_TABLE[i][xIndex]); - - double value1 = CORAL_LOOKUP_TABLE[i][yIndex]; - double value2 = CORAL_LOOKUP_TABLE[i + 1][yIndex]; - - // Interpolate in-between values for value xValue and shooter rpm - double newValue = value1 + (percent * (value2 - value1)); - - double returnValue = newValue; - return returnValue; - } - } - // Should never run - double returnValue = CORAL_LOOKUP_TABLE[0][yIndex]; - return returnValue; - } + public static double angleToUnitVectorI(double angle) { + return (Math.cos(angle)); } /** - * Calculates shooter values (flywheel velocity and note velocity) based on the - * given angle. + * Calculates the y-component of a unit vector given an angle in radians. * - * @param angle The angle of the shooter. - * @return An array containing the calculated flywheel velocity and note - * velocity. + * @param angle The angle in radians. + * @return The y-component of the unit vector. */ - public static double getCoralDistanceFromPitch(double angle) { - return getInterpolatedValue(0, 1, angle); - // return new double[] {25, getInterpolatedValue(1, 3, angle)}; + public static double angleToUnitVectorJ(double angle) { + return (Math.sin(angle)); } - } - // Gear ratios and conversions - public static final class Ratios { - // pivot - public static final double PIVOT_GEAR_RATIO = 23 * 64 / 24; - - // drive - public static final double DRIVE_GEAR_RATIO = 6.12; - public static final double STEER_GEAR_RATIO = 21.43; - - // elevator - public static final double ELEVATOR_FIRST_STAGE = Constants.inchesToMeters(20); - public static final double ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE = 18.067; - public static final double ELEVATOR_MOTOR_ROTATIONS_PER_METER = ELEVATOR_MOTOR_ROTATIONS_FOR_FIRST_STAGE - * (1 / ELEVATOR_FIRST_STAGE); + /** + * Converts revolutions per minute (RPM) to revolutions per second (RPS). + * + * @param RPM The value in revolutions per minute (RPM) to be converted. + * @return The equivalent value in revolutions per second (RPS). + */ + public static double RPMToRPS(double RPM) { + return RPM / 60; + } - public static double elevatorRotationsToMeters(double rotations) { - return rotations / ELEVATOR_MOTOR_ROTATIONS_PER_METER; + /** + * Converts revolutions per second (RPS) to revolutions per minute (RPM). + * + * @param RPM The value in revolutions per second (RPS) to be converted. + * @return The equivalent value in revolutions per minute (RPM). + */ + public static double RPSToRPM(double RPS) { + return RPS * 60; } - public static double elevatorMetersToRotations(double meters) { - return meters * ELEVATOR_MOTOR_ROTATIONS_PER_METER; + /** + * Standardizes an angle to be within the range [otherAngle - pi, otherAngle + + * pi) radians. + * + * @param angle The input angle in radians. + * @param otherAngle The reference angle in radians. + * @return The standardized angle within the range [otherAngle - pi, otherAngle + * + + * pi) radians. + */ + public static double standardizeAngleToOther(double angle, double otherAngle) { + double delta = angle - otherAngle; + delta = Math.IEEEremainder(delta, 2 * Math.PI); // gives value in [-π, π] + return otherAngle + delta; } - // intake - public static final double INTAKE_PIVOT_GEAR_RATIO = 45.0; - } - - public static boolean isReady = false; - public static final ArrayList paths = new ArrayList(); - - // Can info such as IDs - public static final class CANInfo { - public static final String CANBUS_NAME = "Canivore"; - - // drive - public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 1; - public static final int FRONT_RIGHT_ANGLE_MOTOR_ID = 2; - public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 3; - public static final int FRONT_LEFT_ANGLE_MOTOR_ID = 4; - public static final int BACK_LEFT_DRIVE_MOTOR_ID = 5; - public static final int BACK_LEFT_ANGLE_MOTOR_ID = 6; - public static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; - public static final int BACK_RIGHT_ANGLE_MOTOR_ID = 8; - public static final int FRONT_RIGHT_MODULE_CANCODER_ID = 1; - public static final int FRONT_LEFT_MODULE_CANCODER_ID = 2; - public static final int BACK_LEFT_MODULE_CANCODER_ID = 3; - public static final int BACK_RIGHT_MODULE_CANCODER_ID = 4; - - // Elevator - public static final int LEFT_ELEVATOR_MOTOR_ID = 9; - public static final int RIGHT_ELEVATOR_MOTOR_ID = 10; - - // Manipulator - public static final int MANIPULATOR_MOTOR_ID = 13; - - // Intake - public static final int INTAKE_ROLLER_MOTOR_ID = 17; - public static final int INTAKE_PIVOT_MOTOR_ID = 16; - public static final int INTAKE_BEAM_BREAK_PORT = 0; - - // Straightenator - public static final int LEFT_STRAIGHTENATOR_MOTOR_ID = 12; - public static final int RIGHT_STRAIGHTENATOR_MOTOR_ID = 13; - public static final int FAR_BEAM_BREAK_SENSOR = 0; - public static final int CLOSE_BEAM_BREAK_SENSOR = 2; - - // Arm - public static final int ARM_PIVOT_MOTOR_ID = 15; - public static final int ARM_MANIPULATOR_MOTOR_ID = 14; - } - - public static final class IntakeConstants { - public static double PIVOT_LOWER_LIMIT = 0; - public static double PIVOT_UPPER_LIMIT = 10; - } - - // Misc. controller values - public static final class OperatorConstants { - public static final double RIGHT_TRIGGER_DEADZONE = 0.1; - public static final double LEFT_TRIGGER_DEADZONE = 0.1; - public static final double LEFT_STICK_DEADZONE = 0.03; - public static final double RIGHT_STICK_DEADZONE = 0.05; - } - - /** - * Converts inches to meters. - * - * @param inches The length in inches to be converted. - * @return The equivalent length in meters. - */ - public static double inchesToMeters(double inches) { - return inches / 39.37; - } - - public static double metersToInches(double meters) { - return meters * 39.37; - } - - /** - * Converts feet to meters. - * - * @param inches The length in feet to be converted. - * @return The equivalent length in meters. - */ - public static double feetToMeters(double feet) { - return feet / 3.281; - } - - /** - * Calculates the Euclidean distance between two points in a 2D plane. - * - * @param x1 The x-coordinate of the first point. - * @param y1 The y-coordinate of the first point. - * @param x2 The x-coordinate of the second point. - * @param y2 The y-coordinate of the second point. - * @return The Euclidean distance between the two points. - */ - public static double getDistance(double x1, double y1, double x2, double y2) { - return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2)); - } - - public static double getAngleToPoint(double x1, double y1, double x2, double y2) { - double deltaX = x2 - x1; - double deltaY = y2 - y1; - - double angleInRadians = Math.atan2(deltaY, deltaX); - - double angleInDegrees = Math.toDegrees(angleInRadians); - - double standardizeAngleDegrees = standardizeAngleDegrees(angleInDegrees); - - return 180 + standardizeAngleDegrees; - } - - /** - * Converts a quantity in rotations to radians. - * - * @param rotations The quantity in rotations to be converted. - * @return The equivalent quantity in radians. - */ - public static double rotationsToRadians(double rotations) { - return rotations * 2 * Math.PI; - } - - /** - * Converts a quantity in degrees to rotations. - * - * @param rotations The quantity in degrees to be converted. - * @return The equivalent quantity in rotations. - */ - public static double degreesToRotations(double degrees) { - return degrees / 360; - } - - /** - * Converts a quantity in rotations to degrees. - * - * @param rotations The quantity in rotations to be converted. - * @return The equivalent quantity in degrees. - */ - public static double rotationsToDegrees(double rotations) { - return rotations * 360; - } - - /** - * Converts a quantity in degrees to radians. - * - * @param rotations The quantity in degrees to be converted. - * @return The equivalent quantity in radians. - */ - public static double degreesToRadians(double degrees) { - return degrees * Math.PI / 180; - } - - /** - * Standardizes an angle to be within the range [0, 360) degrees. - * - * @param angleDegrees The input angle in degrees. - * @return The standardized angle within the range [0, 360) degrees. - */ - public static double standardizeAngleDegrees(double angleDegrees) { - return ((angleDegrees % 360) + 360) % 360; - } - - /** - * Calculates the x-component of a unit vector given an angle in radians. - * - * @param angle The angle in radians. - * @return The x-component of the unit vector. - */ - public static double angleToUnitVectorI(double angle) { - return (Math.cos(angle)); - } - - /** - * Calculates the y-component of a unit vector given an angle in radians. - * - * @param angle The angle in radians. - * @return The y-component of the unit vector. - */ - public static double angleToUnitVectorJ(double angle) { - return (Math.sin(angle)); - } - - /** - * Converts revolutions per minute (RPM) to revolutions per second (RPS). - * - * @param RPM The value in revolutions per minute (RPM) to be converted. - * @return The equivalent value in revolutions per second (RPS). - */ - public static double RPMToRPS(double RPM) { - return RPM / 60; - } - - /** - * Converts revolutions per second (RPS) to revolutions per minute (RPM). - * - * @param RPM The value in revolutions per second (RPS) to be converted. - * @return The equivalent value in revolutions per minute (RPM). - */ - public static double RPSToRPM(double RPS) { - return RPS * 60; - } - - /** - * Standardizes an angle to be within the range [otherAngle - pi, otherAngle + - * pi) radians. - * - * @param angle The input angle in radians. - * @param otherAngle The reference angle in radians. - * @return The standardized angle within the range [otherAngle - pi, otherAngle - * + - * pi) radians. - */ - public static double standardizeAngleToOther(double angle, double otherAngle) { - double delta = angle - otherAngle; - delta = Math.IEEEremainder(delta, 2 * Math.PI); // gives value in [-π, π] - return otherAngle + delta; - } - - /** - * Standardizes an angle to be within the range [otherAngle - 180, otherAngle + - * 180) degrees. - * - * @param angle The input angle in degrees. - * @param otherAngle The reference angle in degrees. - * @return The standardized angle within the range [otherAngle - 180, otherAngle - * + - * 180) degrees. - */ - public static double standardizeAngleToOtherDegrees(double angle, double otherAngle) { - return Math.toDegrees(standardizeAngleToOther(degreesToRadians(angle), degreesToRadians(otherAngle))); - } + /** + * Standardizes an angle to be within the range [otherAngle - 180, otherAngle + + * 180) degrees. + * + * @param angle The input angle in degrees. + * @param otherAngle The reference angle in degrees. + * @return The standardized angle within the range [otherAngle - 180, otherAngle + * + + * 180) degrees. + */ + public static double standardizeAngleToOtherDegrees(double angle, double otherAngle) { + return Math.toDegrees(standardizeAngleToOther(degreesToRadians(angle), degreesToRadians(otherAngle))); + } } \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/OI.java b/2026-Robot/src/main/java/frc/robot/OI.java index cf4a0a5..80f2a5a 100644 --- a/2026-Robot/src/main/java/frc/robot/OI.java +++ b/2026-Robot/src/main/java/frc/robot/OI.java @@ -139,7 +139,7 @@ public static double getDriverLeftY() { public static double getDriverRightX() { double rightX = driverController.getRightX(), rightY = driverController.getRightY(); - if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { rightX = 0; } return rightX; @@ -147,7 +147,7 @@ public static double getDriverRightX() { public static double getDriverRightY() { double rightX = driverController.getRightX(), rightY = driverController.getRightY(); - if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { rightY = 0; } return rightY; @@ -171,7 +171,7 @@ public static double getOperatorLeftY() { public static double getOperatorRightX() { double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); - if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { rightX = 0; } return rightX; @@ -179,7 +179,7 @@ public static double getOperatorRightX() { public static double getOperatorRightY() { double rightX = operatorController.getRightX(), rightY = operatorController.getRightY(); - if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.LEFT_STICK_DEADZONE) { + if (Math.hypot(rightX, rightY) < Constants.OperatorConstants.RIGHT_STICK_DEADZONE) { rightY = 0; } return rightY; @@ -189,10 +189,6 @@ public static double getDriverRTPercent() { return driverController.getRightTriggerAxis(); } - // public static double getDriverLTPercent() { - // return driverController.getLeftTriggerAxis(); - // } - /** * This is for using the backup controller, the LT is 1.0 when it should be at * zero @@ -200,12 +196,14 @@ public static double getDriverRTPercent() { * code */ public static double getDriverLTPercent() { - double raw = driverController.getLeftTriggerAxis(); - if (raw > 0.9) { - raw = 0.0; - } - double refined = raw * (4.0 / 3.0); - return refined; + // extra deadzone logic for bad controller + // double raw = driverController.getLeftTriggerAxis(); + // if (raw > 0.9) { + // raw = 0.0; + // } + // double refined = raw * (4.0 / 3.0); + // return refined; + return driverController.getLeftTriggerAxis(); } public static boolean getDriverA() { @@ -248,20 +246,6 @@ public static boolean getPOVUp() { } } - public static boolean isRedSide() { - if (autoChooserConnected()) { - return !autoChooser.getRawButton(8); - } else if (DriverStation.isDSAttached()) { - return DriverStation.getAlliance().get() == DriverStation.Alliance.Red; - } else { - return true; - } - } - - public static boolean isProcessorSide() { - return leftRight.getSelected().equals("processor"); - } - public static boolean isRecalculateMode() { return autoChooser.getRawButton(7); } diff --git a/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java index 6688c4f..c782f62 100644 --- a/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java +++ b/2026-Robot/src/main/java/frc/robot/commands/FollowPath.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drive; import frc.robot.tools.PathLoader; +import frc.robot.tools.PosePoint; import frc.robot.tools.math.Vector; import java.util.ArrayList; @@ -16,12 +17,12 @@ public class FollowPath extends Command { private final Drive drive; - private final List points = new ArrayList<>(); + private final List points = new ArrayList<>(); private final Timer timer = new Timer(); private int currentIndex = 0; private static final double POSITION_TOLERANCE = 0.02; private static final double ANGLE_TOLERANCE = 0.05; - private static final double MAX_SPEED = 1.0; // meters/sec + private static final double MAX_SPEED = 6.7; // meters/sec private static final double MAX_ANGULAR = 2.0; // rad/sec public FollowPath(JSONObject arguments, Drive driveSubsystem) { @@ -30,7 +31,7 @@ public FollowPath(JSONObject arguments, Drive driveSubsystem) { JSONArray pointsArray = arguments.getJSONArray("points"); for (int i = 0; i < pointsArray.length(); i++) { JSONObject p = pointsArray.getJSONObject(i); - points.add(new PathLoader.PosePoint( + points.add(new PosePoint( p.getDouble("x"), p.getDouble("y"), p.getDouble("angle"), @@ -59,7 +60,7 @@ public void execute() { if (currentIndex >= points.size()) return; - PathLoader.PosePoint target = points.get(currentIndex); + PosePoint target = points.get(currentIndex); Pose2d pose = drive.getMT2Odometry(); double dx = target.x - pose.getX(); @@ -67,10 +68,7 @@ public void execute() { double distance = Math.hypot(dx, dy); double dtheta = wrapAngle(target.theta - pose.getRotation().getRadians()); - if (distance < POSITION_TOLERANCE && Math.abs(dtheta) < ANGLE_TOLERANCE) { - currentIndex++; - return; - } + currentIndex = (int) (timer.getTimestamp() / 0.5); double vx = (distance > 0) ? (dx / distance) * Math.min(distance, MAX_SPEED) : 0; double vy = (distance > 0) ? (dy / distance) * Math.min(distance, MAX_SPEED) : 0; diff --git a/2026-Robot/src/main/java/frc/robot/commands/Log.java b/2026-Robot/src/main/java/frc/robot/commands/Log.java new file mode 100644 index 0000000..d43418e --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/Log.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class Log extends Command { + /** Creates a new Logger. */ + public Log(JSONObject json) { + String string = ""; + if (json.has("message")) { + string = json.getString("message"); + } + System.out.println(string); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java index 0cbf04c..1486727 100644 --- a/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java +++ b/2026-Robot/src/main/java/frc/robot/commands/PurePursuitFollowPath.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Drive; import frc.robot.tools.PathLoader; +import frc.robot.tools.PosePoint; import frc.robot.tools.math.Vector; import org.json.JSONArray; import org.json.JSONObject; @@ -17,7 +18,7 @@ public class PurePursuitFollowPath extends Command { private final Drive drive; private final JSONArray rawPoints; private final JSONArray pointsWithVels; - private final List points = new ArrayList<>(); + private final List points = new ArrayList<>(); private int currentIndex = 0; private boolean odometrySet = false; @@ -39,7 +40,7 @@ public PurePursuitFollowPath(JSONObject arguments, Drive driveSubsystem) { for (int i = 0; i < rawPoints.length(); i++) { JSONObject p = rawPoints.getJSONObject(i); - points.add(new PathLoader.PosePoint( + points.add(new PosePoint( p.optDouble("x", 0.0), p.optDouble("y", 0.0), p.optDouble("angle", 0.0), diff --git a/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java b/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java new file mode 100644 index 0000000..cc24041 --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/commands/SetOdometry.java @@ -0,0 +1,34 @@ +package frc.robot.commands; + +import org.json.JSONObject; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import frc.robot.subsystems.Drive; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.Logger; + +public class SetOdometry extends InstantCommand { + public SetOdometry(JSONObject args, Drive drive) { + super(() -> { + if (args == null) { + Logger.recordOutput("Auto/OdoArgsNull", 1); + return; + } + + double x = args.optDouble("x", 0.0); + double y = args.optDouble("y", 0.0); + double angle = args.optDouble("angle", 0.0); + + try { + drive.setOdometry(new Pose2d(x, y, Rotation2d.fromDegrees(angle))); + Logger.recordOutput("Auto/OdoX", x); + Logger.recordOutput("Auto/OdoY", y); + Logger.recordOutput("Auto/OdoAngle", angle); + } catch (Exception e) { + Logger.recordOutput("Auto/OdoSetError", 1); + } + }); + } +} diff --git a/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java index 8d759e2..b223a3c 100644 --- a/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java +++ b/2026-Robot/src/main/java/frc/robot/subsystems/Drive.java @@ -38,7 +38,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.OI; -import frc.robot.tools.controlloops.PID; +import frc.robot.tools.math.PID; import frc.robot.tools.math.Vector; // **Zero Wheels with the bolt head showing on the left when the front side(battery) is facing down/away from you** @@ -658,11 +658,6 @@ public void autoInit(JSONArray pathPoints) { firstPointAngle = Math.PI + firstPointAngle; } - if (OI.isProcessorSide()) { - firstPointY = Constants.Physical.FIELD_WIDTH - firstPointY; - firstPointAngle = -firstPointAngle; - } - frontLeft.setDriveCurrentLimits(60, 120); frontRight.setDriveCurrentLimits(60, 120); backLeft.setDriveCurrentLimits(60, 120); @@ -3255,11 +3250,6 @@ public Number[] purePursuitController(double currentX, double currentY, double c currentTheta = Math.PI + currentTheta; } - if (OI.isProcessorSide()) { - currentY = Constants.Physical.FIELD_WIDTH - currentY; - currentTheta = -currentTheta; - } - for (int i = currentIndex; i < pathPoints.length(); i++) { JSONObject point = pathPoints.getJSONObject(i); double targetX = point.getDouble("x"), targetY = point.getDouble("y"), @@ -3315,11 +3305,6 @@ public Number[] purePursuitController(double currentX, double currentY, double c finalY = -finalY; } - if (OI.isProcessorSide()) { - finalY = -finalY; - finalTheta = -finalTheta; - } - Number[] velocityArray = new Number[] { finalX, -finalY, @@ -3454,10 +3439,7 @@ public double getDistanceFromAlgaeSetpoint() { public double getThetaToCenterReef() { double theta = 0.0; - if (OI.isRedSide()) { - theta = Math.atan2(Constants.Reef.centerRed.getY() - getMT2OdometryY(), - Constants.Reef.centerRed.getX() - getMT2OdometryX()); - } else { + { theta = Math.atan2((Constants.Reef.centerBlue.getY() - getMT2OdometryY()), (Constants.Reef.centerBlue.getX() - getMT2OdometryX())); } diff --git a/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java index 076c8f5..f45af9f 100644 --- a/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java +++ b/2026-Robot/src/main/java/frc/robot/tools/PathLoader.java @@ -13,17 +13,6 @@ public class PathLoader { - public static class PosePoint { - public final double x, y, theta, time; - - public PosePoint(double x, double y, double theta, double time) { - this.x = x; - this.y = y; - this.theta = theta; - this.time = time; - } - } - public static List loadPath(String relativePath) throws IOException { File file = new File(Filesystem.getDeployDirectory() + "/" + "Paths/" + relativePath); String content = Files.readString(file.toPath()); diff --git a/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java b/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java new file mode 100644 index 0000000..f46cf0f --- /dev/null +++ b/2026-Robot/src/main/java/frc/robot/tools/PosePoint.java @@ -0,0 +1,12 @@ +package frc.robot.tools; + +public class PosePoint { + public final double x, y, theta, time; + + public PosePoint(double x, double y, double theta, double time) { + this.x = x; + this.y = y; + this.theta = theta; + this.time = time; + } +} \ No newline at end of file diff --git a/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java b/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java deleted file mode 100644 index 5b9292d..0000000 --- a/2026-Robot/src/main/java/frc/robot/tools/controlloops/PID.java +++ /dev/null @@ -1,233 +0,0 @@ -package frc.robot.tools.controlloops; - -public class PID { - private double error; - private double totalError; - private double prevError; - - private double PValue; - private double IValue; - private double DValue; - - // Dictates the inputs and outputs - private double maxInput; - private double minInput; - private double maxOutput = 1500;// defaults to 100% and -100% motor power - private double minOutput = -1500; - - private boolean continuous = false; // only for absolute encoders - private double setPoint; // this will be set continuously - private double result; - - public PID(double kp, double ki, double kd) { - PValue = kp; - IValue = ki; - DValue = kd; - } - - /** - * Updates the PID controller with the current sensor value and calculates the - * output. - * - * @param value The current sensor value. - * @return The calculated output based on the PID algorithm. - * - * The PID controller calculates the error between the setpoint and the - * current sensor value. - * It then applies proportional, integral, and derivative terms to the - * error to determine the output. - * - * If the 'continuous' flag is set to true, the error is adjusted to - * handle wraparound situations - * (e.g., when using absolute encoders). - * - * The total error is clamped to prevent integral windup, and the output - * is also clamped to the - * specified minimum and maximum output values. - */ - public double updatePID(double value) { - error = setPoint - value; - if (continuous) { - if (Math.abs(error) > (maxInput - minInput) / 2) { - if (error > 0) { - error = error - maxInput + minInput; - } else { - error = error + maxInput - minInput; - } - } - } - - if ((error * PValue < maxOutput) && (error * PValue > minOutput)) { - totalError += error; - } else { - totalError = 0; - } - - result = (PValue * error + IValue * totalError + DValue * (error - prevError)); - prevError = error; - result = clamp(result); - return result; - } - - /** - * Sets the proportional, integral, and derivative values for the PID - * controller. - * - * @param p The proportional gain. This value determines how much the controller - * responds to the current error. - * @param i The integral gain. This value accumulates the error over time and - * helps to eliminate steady-state errors. - * @param d The derivative gain. This value calculates the rate of change of the - * error and helps to dampen oscillations. - * - * The setPID() function updates the PValue, IValue, and DValue - * variables with the provided values. - * These values are used in the PID control algorithm to calculate the - * output based on the current sensor value and setpoint. - */ - public void setPID(double p, double i, double d) { - PValue = p; - IValue = i; - DValue = d; - } - - /** - * Retrieves the calculated output based on the PID algorithm. - * - * @return The calculated output, which is the result of the PID control - * algorithm. - * This value is clamped to the specified minimum and maximum output - * values. - * - * The getResult() function returns the result of the PID control - * algorithm, which is - * calculated by the updatePID() function. The result is clamped to - * ensure it falls within - * the specified minimum and maximum output values. - */ - public double getResult() { - return result; - } - - /** - * Sets the maximum output value for the PID controller. - * - * @param output The maximum output value. This value is used to clamp the - * calculated PID output. - * The output will be limited to this value if it exceeds it. - * - * The setMaxOutput() function updates the maxOutput variable with - * the provided value. - * This value is used in the PID control algorithm to ensure that - * the calculated output does not exceed the specified maximum - * output value. - * By limiting the output, the PID controller can prevent - * overshooting or saturation issues in the system. - */ - public void setMaxOutput(double output) { - maxOutput = output; - } - - /** - * Sets the minimum output value for the PID controller. - * - * @param output The minimum output value. This value is used to clamp the - * calculated PID output. - * The output will be limited to this value if it falls below it. - * - * The setMinOutput() function updates the minOutput variable with - * the provided value. - * This value is used in the PID control algorithm to ensure that - * the calculated output does not fall below the specified minimum - * output value. - * By limiting the output, the PID controller can prevent - * undershooting or saturation issues in the system. - */ - public void setMinOutput(double output) { - minOutput = output; - } - - /** - * Sets the minimum input value for the PID controller. - * - * @param input The minimum input value. This value is used to handle wraparound - * situations - * when using absolute encoders. If the current sensor value - * exceeds the maximum - * input value minus the minimum input value by more than half, the - * error is adjusted - * to account for the wraparound. - * - * The setMinInput() function updates the minInput variable with - * the provided value. - * This value is used in the PID control algorithm to handle - * wraparound situations when using - * absolute encoders. By setting the minimum input value, the PID - * controller can accurately - * calculate the error and prevent integral windup in such cases. - */ - public void setMinInput(double input) { - minInput = input; - } - - /** - * Sets the maximum input value for the PID controller. - * - * @param input The maximum input value. This value is used to handle wraparound - * situations - * when using absolute encoders. If the current sensor value - * exceeds the maximum - * input value minus the minimum input value by more than half, the - * error is adjusted - * to account for the wraparound. - * - * The setMaxInput() function updates the maxInput variable with - * the provided value. - * This value is used in the PID control algorithm to handle - * wraparound situations when using - * absolute encoders. By setting the minimum input value, the PID - * controller can accurately - * calculate the error and prevent integral windup in such cases. - */ - public void setMaxInput(double input) { - maxInput = input; - } - - /** - * Sets the target value of the PID controller - * - * @param target - */ - public void setSetPoint(double target) { - setPoint = target; - } - - public void setContinuous(boolean value) { - continuous = value; - } - - /** - * Gives the current setpoint of the PID controller - * - * @return The setpoint value of the PID controller - */ - public double getSetPoint() { - return setPoint; - } - - /** - * Clamps the value given between the maximum and minimum output values - * - * @param input The value to clamp - * @return The clamped value - */ - public double clamp(double input) { - if (input > maxOutput) { - return maxOutput; - } - if (input < minOutput) { - return minOutput; - } - return input; - } -} \ No newline at end of file