From 183db6a30b125c9e9652f4ee3f5f1ed2f82f3ec4 Mon Sep 17 00:00:00 2001 From: Kieran Klukas Date: Mon, 11 Aug 2025 22:05:20 -0400 Subject: [PATCH 1/5] feat: add mill support --- .editorconfig | 35 +++ .githooks/pre-commit | 26 ++ .github/workflows/ci.yml | 34 +++ .vscode/extensions.json | 9 + .vscode/settings.json | 26 +- build.mill | 563 +++++++++++++++++++++++++++++++++++++++ bun.lock | 36 --- package-lock.json | 128 +++++++++ 8 files changed, 820 insertions(+), 37 deletions(-) create mode 100644 .editorconfig create mode 100755 .githooks/pre-commit create mode 100644 .github/workflows/ci.yml create mode 100644 .vscode/extensions.json create mode 100644 build.mill delete mode 100644 bun.lock create mode 100644 package-lock.json diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..7f54946 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,35 @@ +# EditorConfig helps maintain consistent coding styles +# between different editors and IDEs +# See https://editorconfig.org + +root = true + +[*] +charset = utf-8 +end_of_line = lf +insert_final_newline = true +trim_trailing_whitespace = true + +[*.java] +indent_style = space +indent_size = 2 +max_line_length = 100 + +[*.{json,yaml,yml}] +indent_style = space +indent_size = 2 + +[*.{md,markdown}] +trim_trailing_whitespace = false + +[*.{xml,html}] +indent_style = space +indent_size = 2 + +[*.gradle] +indent_style = space +indent_size = 4 + +[*.mill] +indent_style = space +indent_size = 2 \ No newline at end of file diff --git a/.githooks/pre-commit b/.githooks/pre-commit new file mode 100755 index 0000000..84e5d83 --- /dev/null +++ b/.githooks/pre-commit @@ -0,0 +1,26 @@ +#!/usr/bin/env bash +# Pre-commit hook to format Java code with Google Java Format + +echo "Running Google Java Format on staged Java files..." + +# Get list of staged Java files +STAGED_JAVA_FILES=$(git diff --cached --name-only --diff-filter=ACM | grep '\.java$') + +if [ -z "$STAGED_JAVA_FILES" ]; then + echo "No Java files to format." + exit 0 +fi + +# Format the files +mill formatJava + +# Add the formatted files back to staging +for file in $STAGED_JAVA_FILES; do + if [ -f "$file" ]; then + git add "$file" + echo "Formatted and re-staged: $file" + fi +done + +echo "Java formatting complete!" +exit 0 \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..8f1d260 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,34 @@ +name: Code Quality + +on: + push: + branches: [ main, develop ] + pull_request: + branches: [ main ] + +jobs: + format-check: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + java-version: '17' + distribution: 'temurin' + + - name: Setup Mill + uses: jodersky/setup-mill@v0.2.3 + with: + mill-version: 0.12.11 + + - name: Check Java formatting + run: mill checkJavaFormat + + - name: Compile code + run: mill compile + + - name: Run tests + run: mill test \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..13bf90a --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,9 @@ +{ + "recommendations": [ + "redhat.java", + "vscjava.vscode-java-pack", + "esbenp.prettier-vscode", + "editorconfig.editorconfig", + "ms-vscode.vscode-json" + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 60db7ba..9d79f85 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,11 @@ { "java.configuration.updateBuildConfiguration": "automatic", "java.server.launchMode": "Standard", + "java.format.settings.url": "https://raw.githubusercontent.com/google/styleguide/gh-pages/eclipse-java-google-style.xml", + "java.format.settings.profile": "GoogleStyle", + "java.format.enabled": true, + "java.saveActions.organizeImports": true, + "java.compile.nullAnalysis.mode": "automatic", "files.exclude": { "**/.git": true, "**/.svn": true, @@ -26,8 +31,27 @@ } ], "java.test.defaultConfig": "WPIlibUnitTests", + "editor.formatOnSave": true, + "editor.formatOnPaste": true, + "editor.codeActionsOnSave": { + "source.organizeImports": "explicit" + }, + "files.trimTrailingWhitespace": true, + "files.insertFinalNewline": true, + "files.trimFinalNewlines": true, "[java]": { + "editor.defaultFormatter": "redhat.java", + "editor.tabSize": 2, + "editor.insertSpaces": true, + "editor.detectIndentation": false + }, + "[json]": { "editor.defaultFormatter": "esbenp.prettier-vscode" }, - "editor.formatOnSave": true + "[yaml]": { + "editor.defaultFormatter": "esbenp.prettier-vscode" + }, + "[markdown]": { + "editor.defaultFormatter": "esbenp.prettier-vscode" + } } diff --git a/build.mill b/build.mill new file mode 100644 index 0000000..0752bbb --- /dev/null +++ b/build.mill @@ -0,0 +1,563 @@ +package build + +import mill._ +import mill.javalib._ +import coursier.maven.MavenRepository + +object `package` extends RootModule with JavaModule { + + def artifactName = "2025-reefscape" + + def javaVersion = "17" + + def mainClass = Some("frc.robot.Main") + + def javacOptions = super.javacOptions() ++ Seq( + "-XDstringConcat=inline", + "--release", "17" + ) + + def repositoriesTask = Task.Anon { + super.repositoriesTask() ++ Seq( + MavenRepository("file:/home/kierank/wpilib/2025/maven"), + MavenRepository( + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ), + MavenRepository( + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ), + MavenRepository("https://dev.studica.com/maven/release/2025/"), + MavenRepository("https://docs.home.thethriftybot.com"), + MavenRepository("https://frcmaven.wpi.edu/artifactory/release"), + MavenRepository( + "https://frcmaven.wpi.edu/artifactory/vendor-mvn-release" + ), + MavenRepository("https://maven.ctr-electronics.com/release/"), + MavenRepository("https://maven.photonvision.org/repository/internal"), + MavenRepository("https://maven.photonvision.org/repository/snapshots"), + MavenRepository("https://maven.reduxrobotics.com/"), + MavenRepository("https://maven.revrobotics.com/"), + MavenRepository("https://repo1.maven.org/maven2"), + MavenRepository( + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases" + ) + ) + } + + def ivyDeps = T { + import mill.scalalib._ + Agg( + // WPILib Core Dependencies + ivy"edu.wpi.first.wpilibj:wpilibj-java:2025.3.2", + ivy"edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2025.3.2", + ivy"edu.wpi.first.wpimath:wpimath-java:2025.3.2", + ivy"edu.wpi.first.wpiutil:wpiutil-java:2025.3.2", + ivy"edu.wpi.first.wpinet:wpinet-java:2025.3.2", + ivy"edu.wpi.first.wpiunits:wpiunits-java:2025.3.2", + ivy"edu.wpi.first.hal:hal-java:2025.3.2", + ivy"edu.wpi.first.ntcore:ntcore-java:2025.3.2", + ivy"edu.wpi.first.cscore:cscore-java:2025.3.2", + ivy"edu.wpi.first.cameraserver:cameraserver-java:2025.3.2", + ivy"edu.wpi.first.apriltag:apriltag-java:2025.3.2", + ivy"edu.wpi.first.epilogue:epilogue-runtime-java:2025.3.2", + ivy"edu.wpi.first.thirdparty.frc2025.opencv:opencv-java:4.10.0-3", + + // Vendor Dependencies + ivy"com.ctre.phoenix6:wpiapi-java:25.3.1", + ivy"com.ctre.phoenix:api-java:5.35.1", + ivy"com.ctre.phoenix:wpiapi-java:5.35.1", + ivy"com.revrobotics.frc:REVLib-java:2025.0.3", + ivy"com.pathplanner.lib:PathplannerLib-java:2025.2.6", + ivy"com.reduxrobotics.frc:ReduxLib-java:2025.0.1", + ivy"com.studica.frc:Studica-java:2025.0.1", + ivy"com.thethriftybot.frc:ThriftyLib-java:2025.1.0", + ivy"org.photonvision:photonlib-java:v2025.2.1", + ivy"org.photonvision:photontargeting-java:v2025.2.1", + ivy"swervelib:YAGSL-java:2025.7.2", + // Note: maple-sim dependency removed due to checksum issues + + // Supporting Libraries + ivy"com.fasterxml.jackson.core:jackson-annotations:2.15.2", + ivy"com.fasterxml.jackson.core:jackson-core:2.15.2", + ivy"com.fasterxml.jackson.core:jackson-databind:2.15.2", + ivy"org.dyn4j:dyn4j:5.0.2", + ivy"org.ejml:ejml-simple:0.43.1", + ivy"us.hebi.quickbuf:quickbuf-runtime:1.3.3" + ) + } + + // Test dependencies + object test extends JavaTests with TestModule.Junit5 { + def ivyDeps = T { + import mill.scalalib._ + Agg( + ivy"org.junit.jupiter:junit-jupiter:5.10.1", + ivy"org.junit.platform:junit-platform-launcher:1.10.1" + ) + } + + def testFramework = "com.github.sbt.junit.JupiterFramework" + + def forkArgs = T { + Seq( + "-Djunit.jupiter.extensions.autodetection.enabled=true" + ) + } + } + + // Assembly task for creating fat jar (equivalent to Gradle's jar task) + def assembly = T { + val cp = runClasspath() + val mainClassValue = mainClass().getOrElse( + throw new RuntimeException("Main class not specified") + ) + + mill.scalalib.Assembly.createAssembly( + cp.map(_.path), + manifest = mill.util.JarManifest( + Map("Main-Class" -> mainClassValue) + ) + ) + } + + // Team number from wpilib preferences + def teamNumber = T { + val prefsFile = millSourcePath / ".wpilib" / "wpilib_preferences.json" + if (os.exists(prefsFile)) { + val content = os.read(prefsFile) + val teamNumRegex = """"teamNumber":\s*(\d+)""".r + teamNumRegex.findFirstMatchIn(content) match { + case Some(m) => m.group(1).toInt + case None => throw new RuntimeException("Team number not found in wpilib_preferences.json") + } + } else { + throw new RuntimeException("wpilib_preferences.json not found") + } + } + + // RoboRIO IP address + def roborioAddress = T { + val team = teamNumber() + s"roborio-${team}-frc.local" + } + + // Deploy static files task + def deployStaticFiles() = Task.Command { + val team = teamNumber() + val address = roborioAddress() + val deployDir = millSourcePath / "src" / "main" / "deploy" + + if (os.exists(deployDir)) { + println(s"Deploying static files to RoboRIO at $address...") + + // Use scp to copy files to RoboRIO + val scpCmd = Seq( + "scp", "-r", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"${deployDir}/*", s"lvuser@$address:/home/lvuser/deploy/" + ) + + try { + os.proc(scpCmd).call(cwd = millSourcePath) + println("Static files deployed successfully") + } catch { + case e: Exception => + println(s"Failed to deploy static files: ${e.getMessage}") + throw e + } + } else { + println("No deploy directory found, skipping static file deployment") + } + } + + // Deploy robot code task + def deployCode() = Task.Command { + val team = teamNumber() + val address = roborioAddress() + val jarFile = assembly() + val jarName = "FRCUserProgram.jar" + + println(s"Deploying robot code to RoboRIO at $address...") + + try { + // Stop robot program + println("Stopping robot program...") + val stopCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", ". /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t 2> /dev/null" + ) + os.proc(stopCmd).call(cwd = millSourcePath, check = false) + + // Copy jar file + println("Copying robot program...") + val scpCmd = Seq( + "scp", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + jarFile.path.toString, s"lvuser@$address:/home/lvuser/$jarName" + ) + os.proc(scpCmd).call(cwd = millSourcePath) + + // Set executable permissions and create run script + println("Setting up robot program...") + val setupCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", + s"""chmod +x /home/lvuser/$jarName && """ + + s"""echo '#!/bin/bash' > /home/lvuser/robotCommand && """ + + s"""echo 'exec /usr/local/frc/JRE/bin/java -Djava.library.path=/usr/local/frc/third-party/lib -jar /home/lvuser/$jarName' >> /home/lvuser/robotCommand && """ + + s"""chmod +x /home/lvuser/robotCommand""" + ) + os.proc(setupCmd).call(cwd = millSourcePath) + + // Start robot program + println("Starting robot program...") + val startCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", ". /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r 2> /dev/null" + ) + os.proc(startCmd).call(cwd = millSourcePath, check = false) + + println("Robot code deployed successfully!") + + } catch { + case e: Exception => + println(s"Failed to deploy robot code: ${e.getMessage}") + throw e + } + } + + // Main deploy task that deploys both static files and code + def deploy() = Task.Command { + println(s"Deploying to team ${teamNumber()} RoboRIO...") + + // First compile and build + compile() + assembly() + + // Deploy static files first + deployStaticFiles()() + + // Then deploy code + deployCode()() + + println("Deployment complete!") + } + + // Deploy for debugging (same as deploy but with debug info) + def deployDebug() = Task.Command { + println(s"Deploying debug build to team ${teamNumber()} RoboRIO...") + + // First compile and build + compile() + assembly() + + // Deploy static files first + deployStaticFiles()() + + // Deploy code with debug flags + val team = teamNumber() + val address = roborioAddress() + val jarFile = assembly() + val jarName = "FRCUserProgram.jar" + + println(s"Deploying robot code (debug) to RoboRIO at $address...") + + try { + // Stop robot program + println("Stopping robot program...") + val stopCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", ". /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t 2> /dev/null" + ) + os.proc(stopCmd).call(cwd = millSourcePath, check = false) + + // Copy jar file + println("Copying robot program...") + val scpCmd = Seq( + "scp", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + jarFile.path.toString, s"lvuser@$address:/home/lvuser/$jarName" + ) + os.proc(scpCmd).call(cwd = millSourcePath) + + // Set executable permissions and create debug run script + println("Setting up robot program (debug mode)...") + val setupCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", + s"""chmod +x /home/lvuser/$jarName && """ + + s"""echo '#!/bin/bash' > /home/lvuser/robotCommand && """ + + s"""echo 'exec /usr/local/frc/JRE/bin/java -Xdebug -Xrunjdwp:transport=dt_socket,server=y,suspend=n,address=8348 -Djava.library.path=/usr/local/frc/third-party/lib -jar /home/lvuser/$jarName' >> /home/lvuser/robotCommand && """ + + s"""chmod +x /home/lvuser/robotCommand""" + ) + os.proc(setupCmd).call(cwd = millSourcePath) + + println("Robot code deployed successfully in debug mode!") + println("Debug port: 8348") + + } catch { + case e: Exception => + println(s"Failed to deploy robot code: ${e.getMessage}") + throw e + } + } + + // Check RoboRIO connection + def checkRoborio() = Task.Command { + val address = roborioAddress() + println(s"Checking connection to RoboRIO at $address...") + + try { + val pingCmd = Seq("ping", "-c", "1", "-W", "3", address) + os.proc(pingCmd).call(cwd = millSourcePath, stdout = os.Pipe) + println(s"✓ RoboRIO is reachable at $address") + } catch { + case e: Exception => + println(s"✗ Cannot reach RoboRIO at $address") + println("Make sure the RoboRIO is connected and powered on") + throw e + } + } + + // Kill robot program + def killRobot() = Task.Command { + val address = roborioAddress() + println(s"Stopping robot program on $address...") + + try { + val stopCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", ". /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t 2> /dev/null" + ) + os.proc(stopCmd).call(cwd = millSourcePath, check = false) + println("Robot program stopped") + } catch { + case e: Exception => + println(s"Failed to stop robot program: ${e.getMessage}") + throw e + } + } + + // Start robot program + def startRobot() = Task.Command { + val address = roborioAddress() + println(s"Starting robot program on $address...") + + try { + val startCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", ". /etc/profile.d/natinst-path.sh; /usr/local/frc/bin/frcKillRobot.sh -t -r 2> /dev/null" + ) + os.proc(startCmd).call(cwd = millSourcePath, check = false) + println("Robot program started") + } catch { + case e: Exception => + println(s"Failed to start robot program: ${e.getMessage}") + throw e + } + } + + // Show RoboRIO logs + def robotLogs() = Task.Command { + val address = roborioAddress() + println(s"Showing robot logs from $address...") + println("Press Ctrl+C to exit") + + try { + val logsCmd = Seq( + "ssh", "-o", "StrictHostKeyChecking=no", "-o", "UserKnownHostsFile=/dev/null", + s"lvuser@$address", "tail", "-f", "/home/lvuser/FRC_UserProgram.log" + ) + os.proc(logsCmd).call(cwd = millSourcePath, stdin = os.Inherit, stdout = os.Inherit) + () + } catch { + case e: Exception => + println(s"Failed to show robot logs: ${e.getMessage}") + () + } + } + + // Run robot simulation + def simulate() = Task.Command { + val cp = runClasspath() + val mainClassValue = mainClass().getOrElse( + throw new RuntimeException("Main class not specified") + ) + + println("Starting robot simulation...") + + val javaCmd = Seq( + "java", + "-cp", cp.map(_.path.toString).mkString(":"), + "-Djava.library.path=/usr/local/frc/third-party/lib", + "-Dfile.encoding=UTF-8", + "-Djava.awt.headless=false", + mainClassValue + ) + + os.proc(javaCmd).call(cwd = millSourcePath, stdin = os.Inherit, stdout = os.Inherit) + () + } + + // Format Java code using Google Java Format + def formatJava() = Task.Command { + val sourceFiles = os.walk(millSourcePath / "src") + .filter(_.ext == "java") + + if (sourceFiles.nonEmpty) { + println(s"Formatting ${sourceFiles.length} Java files...") + + // Download Google Java Format if not already available + val gfJar = mill.util.Util.download( + "https://github.com/google/google-java-format/releases/download/v1.22.0/google-java-format-1.22.0-all-deps.jar" + ) + + val javaCmd = Seq( + "java", "-jar", gfJar.path.toString, + "--replace" + ) ++ sourceFiles.map(_.toString) + + os.proc(javaCmd).call(cwd = millSourcePath) + println("Java formatting complete!") + } else { + println("No Java files found to format") + } + } + + // Check Java formatting + def checkJavaFormat() = Task.Command { + val sourceFiles = os.walk(millSourcePath / "src") + .filter(_.ext == "java") + + if (sourceFiles.nonEmpty) { + println(s"Checking format of ${sourceFiles.length} Java files...") + + // Download Google Java Format if not already available + val gfJar = mill.util.Util.download( + "https://github.com/google/google-java-format/releases/download/v1.22.0/google-java-format-1.22.0-all-deps.jar" + ) + + val javaCmd = Seq( + "java", "-jar", gfJar.path.toString, + "--dry-run", "--set-exit-if-changed" + ) ++ sourceFiles.map(_.toString) + + try { + os.proc(javaCmd).call(cwd = millSourcePath) + println("✓ All Java files are properly formatted") + } catch { + case e: Exception => + println("✗ Some Java files need formatting. Run 'mill formatJava' to fix.") + throw e + } + } else { + println("No Java files found to check") + } + } + + // Setup git hooks for automatic formatting + def setupGitHooks() = Task.Command { + val hooksDir = millSourcePath / ".git" / "hooks" + val preCommitHook = hooksDir / "pre-commit" + + if (os.exists(millSourcePath / ".git")) { + if (!os.exists(hooksDir)) { + os.makeDir.all(hooksDir) + } + + // Detect platform and install appropriate hook + val isWindows = System.getProperty("os.name").toLowerCase.contains("windows") + + if (isWindows) { + // Windows: Create a batch file hook + val hookContent = """@echo off +REM Pre-commit hook to format Java code with Google Java Format + +echo Running Google Java Format on staged Java files... + +REM Check for staged Java files +git diff --cached --name-only --diff-filter=ACM | findstr "\.java$" >nul +if errorlevel 1 ( + echo No Java files to format. + exit /b 0 +) + +echo Formatting Java files... +mill formatJava +if errorlevel 1 ( + echo Failed to format Java files + exit /b 1 +) + +REM Re-stage the formatted files +for /f "delims=" %%i in ('git diff --cached --name-only --diff-filter=ACM ^| findstr "\.java$"') do ( + git add "%%i" + echo Formatted and re-staged: %%i +) + +echo Java formatting complete! +exit /b 0""" + + os.write(preCommitHook, hookContent) + println("✓ Git pre-commit hook installed") + } else { + // Unix/Linux/macOS: Use bash script + val sourceHook = millSourcePath / ".githooks" / "pre-commit" + if (os.exists(sourceHook)) { + os.copy.over(sourceHook, preCommitHook) + os.perms.set(preCommitHook, "rwxr-xr-x") + println("✓ Git pre-commit hook installed") + } else { + println("✗ Source hook not found at .githooks/pre-commit") + } + } + + println(" Java files will be automatically formatted before each commit") + } else { + println("✗ Not a git repository") + } + } + + // Format non-Java files using prettier (JSON, YAML, Markdown, etc.) + def format() = Task.Command { + os.proc("npm", "run", "format").call(cwd = millSourcePath) + } + + // Show help with available commands + def help() = Task.Command { + println("Available Mill commands for FRC Robot:") + println() + println("Build Commands:") + println(" mill compile - Compile the robot code") + println(" mill test.compile - Compile test code") + println(" mill test - Run tests") + println(" mill assembly - Create deployable JAR file") + println() + println("Formatting Commands:") + println(" mill formatJava - Format Java code with Google Java Format") + println(" mill checkJavaFormat - Check if Java code is properly formatted") + println(" mill format - Format other files with Prettier (JSON, YAML, etc.)") + println(" mill setupGitHooks - Install git pre-commit hook for auto-formatting") + println() + println("Deploy Commands:") + println(" mill deploy - Deploy robot code to RoboRIO") + println(" mill deployDebug - Deploy with debug support (port 8348)") + println(" mill deployStaticFiles - Deploy only static files") + println(" mill deployCode - Deploy only robot code") + println() + println("RoboRIO Commands:") + println(" mill checkRoborio - Check RoboRIO connection") + println(" mill killRobot - Stop robot program") + println(" mill startRobot - Start robot program") + println(" mill robotLogs - Show robot logs (Ctrl+C to exit)") + println() + println("Simulation:") + println(" mill simulate - Run robot simulation") + println() + println("Info Commands:") + println(" mill show teamNumber - Show team number") + println(" mill show roborioAddress - Show RoboRIO address") + println(" mill help - Show this help") + println() + println(s"Team: ${teamNumber()}") + println(s"RoboRIO: ${roborioAddress()}") + } + +} \ No newline at end of file diff --git a/bun.lock b/bun.lock deleted file mode 100644 index 41a500d..0000000 --- a/bun.lock +++ /dev/null @@ -1,36 +0,0 @@ -{ - "lockfileVersion": 1, - "workspaces": { - "": { - "dependencies": { - "prettier": "^3.5.1", - "prettier-plugin-java": "^2.6.7", - }, - }, - }, - "packages": { - "@chevrotain/cst-dts-gen": ["@chevrotain/cst-dts-gen@11.0.3", "", { "dependencies": { "@chevrotain/gast": "11.0.3", "@chevrotain/types": "11.0.3", "lodash-es": "4.17.21" } }, "sha512-BvIKpRLeS/8UbfxXxgC33xOumsacaeCKAjAeLyOn7Pcp95HiRbrpl14S+9vaZLolnbssPIUuiUd8IvgkRyt6NQ=="], - - "@chevrotain/gast": ["@chevrotain/gast@11.0.3", "", { "dependencies": { "@chevrotain/types": "11.0.3", "lodash-es": "4.17.21" } }, "sha512-+qNfcoNk70PyS/uxmj3li5NiECO+2YKZZQMbmjTqRI3Qchu8Hig/Q9vgkHpI3alNjr7M+a2St5pw5w5F6NL5/Q=="], - - "@chevrotain/regexp-to-ast": ["@chevrotain/regexp-to-ast@11.0.3", "", {}, "sha512-1fMHaBZxLFvWI067AVbGJav1eRY7N8DDvYCTwGBiE/ytKBgP8azTdgyrKyWZ9Mfh09eHWb5PgTSO8wi7U824RA=="], - - "@chevrotain/types": ["@chevrotain/types@11.0.3", "", {}, "sha512-gsiM3G8b58kZC2HaWR50gu6Y1440cHiJ+i3JUvcp/35JchYejb2+5MVeJK0iKThYpAa/P2PYFV4hoi44HD+aHQ=="], - - "@chevrotain/utils": ["@chevrotain/utils@11.0.3", "", {}, "sha512-YslZMgtJUyuMbZ+aKvfF3x1f5liK4mWNxghFRv7jqRR9C3R3fAOGTTKvxXDa2Y1s9zSbcpuO0cAxDYsc9SrXoQ=="], - - "chevrotain": ["chevrotain@11.0.3", "", { "dependencies": { "@chevrotain/cst-dts-gen": "11.0.3", "@chevrotain/gast": "11.0.3", "@chevrotain/regexp-to-ast": "11.0.3", "@chevrotain/types": "11.0.3", "@chevrotain/utils": "11.0.3", "lodash-es": "4.17.21" } }, "sha512-ci2iJH6LeIkvP9eJW6gpueU8cnZhv85ELY8w8WiFtNjMHA5ad6pQLaJo9mEly/9qUyCpvqX8/POVUTf18/HFdw=="], - - "chevrotain-allstar": ["chevrotain-allstar@0.3.1", "", { "dependencies": { "lodash-es": "^4.17.21" }, "peerDependencies": { "chevrotain": "^11.0.0" } }, "sha512-b7g+y9A0v4mxCW1qUhf3BSVPg+/NvGErk/dOkrDaHA0nQIQGAtrOjlX//9OQtRlSCy+x9rfB5N8yC71lH1nvMw=="], - - "java-parser": ["java-parser@2.3.3", "", { "dependencies": { "chevrotain": "11.0.3", "chevrotain-allstar": "0.3.1", "lodash": "4.17.21" } }, "sha512-9YY8OGlNGfq5TDDq2SBjtIEHMVLeV8vSSZrXDaQBhQ84hi1zc3/+5l3DF3wW8JGqQT2kNVha05dziSamvN8M/g=="], - - "lodash": ["lodash@4.17.21", "", {}, "sha512-v2kDEe57lecTulaDIuNTPy3Ry4gLGJ6Z1O3vE1krgXZNrsQ+LFTGHVxVjcXPs17LhbZVGedAJv8XZ1tvj5FvSg=="], - - "lodash-es": ["lodash-es@4.17.21", "", {}, "sha512-mKnC+QJ9pWVzv+C4/U3rRsHapFfHvQFoFB92e52xeyGMcX6/OlIl78je1u8vePzYZSkkogMPJ2yjxxsb89cxyw=="], - - "prettier": ["prettier@3.5.1", "", { "bin": { "prettier": "bin/prettier.cjs" } }, "sha512-hPpFQvHwL3Qv5AdRvBFMhnKo4tYxp0ReXiPn2bxkiohEX6mBeBwEpBSQTkD458RaaDKQMYSp4hX4UtfUTA5wDw=="], - - "prettier-plugin-java": ["prettier-plugin-java@2.6.7", "", { "dependencies": { "java-parser": "2.3.3", "lodash": "4.17.21" }, "peerDependencies": { "prettier": "^3.0.0" } }, "sha512-AVm+X7fhAZpYKiUCdUIGZ8HJbkGkTUgYQIKVuCQEplcqpGw2ewVnNGcPb1Kc3+MYMfiEqzhd8ZYhMGVHw6tZdQ=="], - } -} diff --git a/package-lock.json b/package-lock.json new file mode 100644 index 0000000..3bd6cf7 --- /dev/null +++ b/package-lock.json @@ -0,0 +1,128 @@ +{ + "name": "2025-reefscape", + "lockfileVersion": 3, + "requires": true, + "packages": { + "": { + "dependencies": { + "prettier": "^3.5.1", + "prettier-plugin-java": "^2.6.7" + } + }, + "node_modules/@chevrotain/cst-dts-gen": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/cst-dts-gen/-/cst-dts-gen-11.0.3.tgz", + "integrity": "sha512-BvIKpRLeS/8UbfxXxgC33xOumsacaeCKAjAeLyOn7Pcp95HiRbrpl14S+9vaZLolnbssPIUuiUd8IvgkRyt6NQ==", + "license": "Apache-2.0", + "dependencies": { + "@chevrotain/gast": "11.0.3", + "@chevrotain/types": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/@chevrotain/gast": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/gast/-/gast-11.0.3.tgz", + "integrity": "sha512-+qNfcoNk70PyS/uxmj3li5NiECO+2YKZZQMbmjTqRI3Qchu8Hig/Q9vgkHpI3alNjr7M+a2St5pw5w5F6NL5/Q==", + "license": "Apache-2.0", + "dependencies": { + "@chevrotain/types": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/@chevrotain/regexp-to-ast": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/regexp-to-ast/-/regexp-to-ast-11.0.3.tgz", + "integrity": "sha512-1fMHaBZxLFvWI067AVbGJav1eRY7N8DDvYCTwGBiE/ytKBgP8azTdgyrKyWZ9Mfh09eHWb5PgTSO8wi7U824RA==", + "license": "Apache-2.0" + }, + "node_modules/@chevrotain/types": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/types/-/types-11.0.3.tgz", + "integrity": "sha512-gsiM3G8b58kZC2HaWR50gu6Y1440cHiJ+i3JUvcp/35JchYejb2+5MVeJK0iKThYpAa/P2PYFV4hoi44HD+aHQ==", + "license": "Apache-2.0" + }, + "node_modules/@chevrotain/utils": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/@chevrotain/utils/-/utils-11.0.3.tgz", + "integrity": "sha512-YslZMgtJUyuMbZ+aKvfF3x1f5liK4mWNxghFRv7jqRR9C3R3fAOGTTKvxXDa2Y1s9zSbcpuO0cAxDYsc9SrXoQ==", + "license": "Apache-2.0" + }, + "node_modules/chevrotain": { + "version": "11.0.3", + "resolved": "https://registry.npmjs.org/chevrotain/-/chevrotain-11.0.3.tgz", + "integrity": "sha512-ci2iJH6LeIkvP9eJW6gpueU8cnZhv85ELY8w8WiFtNjMHA5ad6pQLaJo9mEly/9qUyCpvqX8/POVUTf18/HFdw==", + "license": "Apache-2.0", + "dependencies": { + "@chevrotain/cst-dts-gen": "11.0.3", + "@chevrotain/gast": "11.0.3", + "@chevrotain/regexp-to-ast": "11.0.3", + "@chevrotain/types": "11.0.3", + "@chevrotain/utils": "11.0.3", + "lodash-es": "4.17.21" + } + }, + "node_modules/chevrotain-allstar": { + "version": "0.3.1", + "resolved": "https://registry.npmjs.org/chevrotain-allstar/-/chevrotain-allstar-0.3.1.tgz", + "integrity": "sha512-b7g+y9A0v4mxCW1qUhf3BSVPg+/NvGErk/dOkrDaHA0nQIQGAtrOjlX//9OQtRlSCy+x9rfB5N8yC71lH1nvMw==", + "license": "MIT", + "dependencies": { + "lodash-es": "^4.17.21" + }, + "peerDependencies": { + "chevrotain": "^11.0.0" + } + }, + "node_modules/java-parser": { + "version": "3.0.1", + "resolved": "https://registry.npmjs.org/java-parser/-/java-parser-3.0.1.tgz", + "integrity": "sha512-sDIR7u9b7O2JViNUxiZRhnRz7URII/eE7g2B+BmGxDeS6Ex3OYAcCyz5oh0H4LQ+hL/BS8OJTz8apMy9xtGmrQ==", + "license": "Apache-2.0", + "dependencies": { + "chevrotain": "11.0.3", + "chevrotain-allstar": "0.3.1", + "lodash": "4.17.21" + } + }, + "node_modules/lodash": { + "version": "4.17.21", + "resolved": "https://registry.npmjs.org/lodash/-/lodash-4.17.21.tgz", + "integrity": "sha512-v2kDEe57lecTulaDIuNTPy3Ry4gLGJ6Z1O3vE1krgXZNrsQ+LFTGHVxVjcXPs17LhbZVGedAJv8XZ1tvj5FvSg==", + "license": "MIT" + }, + "node_modules/lodash-es": { + "version": "4.17.21", + "resolved": "https://registry.npmjs.org/lodash-es/-/lodash-es-4.17.21.tgz", + "integrity": "sha512-mKnC+QJ9pWVzv+C4/U3rRsHapFfHvQFoFB92e52xeyGMcX6/OlIl78je1u8vePzYZSkkogMPJ2yjxxsb89cxyw==", + "license": "MIT" + }, + "node_modules/prettier": { + "version": "3.6.2", + "resolved": "https://registry.npmjs.org/prettier/-/prettier-3.6.2.tgz", + "integrity": "sha512-I7AIg5boAr5R0FFtJ6rCfD+LFsWHp81dolrFD8S79U9tb8Az2nGrJncnMSnys+bpQJfRUzqs9hnA81OAA3hCuQ==", + "license": "MIT", + "bin": { + "prettier": "bin/prettier.cjs" + }, + "engines": { + "node": ">=14" + }, + "funding": { + "url": "https://github.com/prettier/prettier?sponsor=1" + } + }, + "node_modules/prettier-plugin-java": { + "version": "2.7.4", + "resolved": "https://registry.npmjs.org/prettier-plugin-java/-/prettier-plugin-java-2.7.4.tgz", + "integrity": "sha512-RiRNkumIW9vaDpxirgIPI+oLSRmuCmoVZuTax9i3cWzWnxd+uKyAfDe4efS+ce00owAeh0a1DI5eFaH1xYWNPg==", + "license": "Apache-2.0", + "dependencies": { + "java-parser": "3.0.1" + }, + "peerDependencies": { + "prettier": "^3.0.0" + } + } + } +} From c3720a39f55ecfac4d5f3ac13f5595ed10973076 Mon Sep 17 00:00:00 2001 From: Kieran Klukas Date: Mon, 11 Aug 2025 22:05:37 -0400 Subject: [PATCH 2/5] chore: ignore crush --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index ebf75f0..528df41 100644 --- a/.gitignore +++ b/.gitignore @@ -188,3 +188,6 @@ compile_commands.json # bun node_modules/ + +# crush +.crush From cb369d31bd3da7e53968d9781c2629d198555b2f Mon Sep 17 00:00:00 2001 From: Kieran Klukas Date: Mon, 11 Aug 2025 22:05:59 -0400 Subject: [PATCH 3/5] chore: format with google java --- README.md | 2 - docs/README.md | 51 +- src/main/java/frc/robot/Constants.java | 203 +-- .../java/frc/robot/ControllerActions.java | 16 +- src/main/java/frc/robot/Main.java | 23 +- src/main/java/frc/robot/Robot.java | 279 ++- src/main/java/frc/robot/RobotContainer.java | 911 +++++----- .../swervedrive/auto/AutoBalanceCommand.java | 116 +- .../swervedrive/drivebase/AbsoluteDrive.java | 178 +- .../drivebase/AbsoluteDriveAdv.java | 258 +-- .../drivebase/AbsoluteFieldDrive.java | 119 +- .../java/frc/robot/libs/AllianceFlipUtil.java | 59 +- .../java/frc/robot/libs/FieldConstants.java | 280 +-- .../robot/subsystems/ClimberSubsystem.java | 187 +- .../robot/subsystems/ElevatorSubsystem.java | 530 +++--- .../robot/subsystems/ScoringSubsystem.java | 455 ++--- .../robot/subsystems/TargetingSubsystem.java | 499 +++--- .../swervedrive/SwerveSubsystem.java | 1524 ++++++++--------- .../robot/subsystems/swervedrive/Vision.java | 973 +++++------ 19 files changed, 3288 insertions(+), 3375 deletions(-) diff --git a/README.md b/README.md index bd8e725..690aa45 100644 --- a/README.md +++ b/README.md @@ -100,7 +100,6 @@ public class ExampleSubsystem extends SubsystemBase { return new RunCommand(() -> motor.set(speed.getAsDouble()), this); } } - ``` ```java @@ -113,7 +112,6 @@ public class RobotContainer { joystick.trigger().onTrue(elevatorSubsystem.setPos(() -> 0.3)); } } - ``` ## Why AGPL 3.0 as a license? diff --git a/docs/README.md b/docs/README.md index fbbebd5..ad82c0a 100644 --- a/docs/README.md +++ b/docs/README.md @@ -21,21 +21,22 @@ All the fancy quick reference stuff goes here. 4. Score L2 Red (align center red cage) ![Score L2 Red](autos/l2-red.png) + ## Controls ### Teleop #### Xbox Controller (Driver) [`Port 0`] -| Binding | Action | Description | -| --------------- | ------ | ----------- | -| `Button A` | Zero gyro | Resets gyro heading | +| Binding | Action | Description | +| --------------- | ---------------------- | ------------------------------------------------------------------------------ | +| `Button A` | Zero gyro | Resets gyro heading | | `Button X` | Auto Target left Reef | automatically target and drive to left reef; will rumble if further than 1.5m | | `Button B` | Auto Target right Reef | automatically target and drive to right reef; will rumble if further than 1.5m | -| `Button Y` | kill auto drive | restores manual control to the drive system | -| `Left Bumper` | Lock drive | Locks drivebase | -| `Right Trigger` | Play music | Climber plays the duck song | -| `Right Bumper` | Toggle robot relative | Switches between field and robot relative driving | +| `Button Y` | kill auto drive | restores manual control to the drive system | +| `Left Bumper` | Lock drive | Locks drivebase | +| `Right Trigger` | Play music | Climber plays the duck song | +| `Right Bumper` | Toggle robot relative | Switches between field and robot relative driving | #### Left Joystick (Operator) [`Port 1`] @@ -62,28 +63,28 @@ All the fancy quick reference stuff goes here. #### Xbox Controller (Driver) -| Binding | Action | Description | -| --------------- | --------------- | -------------------------------------- | -| `A Button` | Zero gyro | Resets gyro heading | -| `X Button` | Auto Target | Auto-targets the left reef | -| `B Button` | Auto Target | Auto-targets the right reef | -| `Y Button` | cancel align | Escape auto drive to pose | -| `Back Button` | Center modules | Centers swerve modules | -| `Left Bumper` | Play music | Plays duck song | -| `Right Bumper` | Run intake | Runs the intake mechanism | +| Binding | Action | Description | +| -------------- | -------------- | --------------------------- | +| `A Button` | Zero gyro | Resets gyro heading | +| `X Button` | Auto Target | Auto-targets the left reef | +| `B Button` | Auto Target | Auto-targets the right reef | +| `Y Button` | cancel align | Escape auto drive to pose | +| `Back Button` | Center modules | Centers swerve modules | +| `Left Bumper` | Play music | Plays duck song | +| `Right Bumper` | Run intake | Runs the intake mechanism | #### Left Joystick (Operator) - Test Mode -| Binding | Action | Description | -| ----------- | ------------------ | --------------------------------- | -| `Trigger` | Manual elevator | Controls elevator speed manually | -| `Button 3` | lvl 4 | Sets elevator to level 4 | -| `Button 7` | Demo mode | Cycles through elevator positions | -| `Button 8` | Tilt to position 0 | Resets tilt to zero position | -| `Button 9` | Descend | Controls climber down | +| Binding | Action | Description | +| ----------- | ------------------ | -------------------------------------------- | +| `Trigger` | Manual elevator | Controls elevator speed manually | +| `Button 3` | lvl 4 | Sets elevator to level 4 | +| `Button 7` | Demo mode | Cycles through elevator positions | +| `Button 8` | Tilt to position 0 | Resets tilt to zero position | +| `Button 9` | Descend | Controls climber down | | `Joystick` | Tilt | Nudges tilt up/down (no limits in test mode) | -| `Button 12` | zero tilt encoder | zeros tilt encoder | -| `Button 2` | Tilt SysID | Runs tilt system identification | +| `Button 12` | zero tilt encoder | zeros tilt encoder | +| `Button 2` | Tilt SysID | Runs tilt system identification | #### Right Joystick (Operator) [`Port 2`] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 367c104..8a52763 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,117 +13,104 @@ import edu.wpi.first.math.util.Units; import swervelib.math.Matter; -/** ---------- - * Constants - * --- - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean constants. This - * class should not be used for any other purpose. All constants should be declared globally (i.e. public static). Do - * not put anything functional in this class. +/** + * ---------- Constants --- The Constants class provides a convenient place for teams to hold + * robot-wide numerical or boolean constants. This class should not be used for any other purpose. + * All constants should be declared globally (i.e. public static). Do not put anything functional in + * this class. * *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - * --- + * constants are needed, to reduce verbosity. --- */ public final class Constants { - public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound - public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Inches.of(8).in(Meters)), ROBOT_MASS); - public static final double LOOP_TIME = 0.13; //s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(14.5); - public static final double MAX_ANGULAR_SPEED = Math.toRadians(240.0); - public static final double MAX_ACCELERATION = 1.5; - public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(120.0); - - public static final class DrivebaseConstants { - - // Hold time on motor brakes when disabled - public static final double WHEEL_LOCK_TIME = 10; // seconds - } - - public static class OperatorConstants { - - // Joystick Deadband - public static final double DEADBAND = 0.1; - public static final double LEFT_Y_DEADBAND = 0.1; - public static final double RIGHT_X_DEADBAND = 0.1; - public static final double TURN_CONSTANT = 6; - } - - public static class CanConstants { - - // Scoring Motor IDs - public static final int scoreMotor1 = 24; - public static final int scoreMotor2 = 25; - public static final int scoreTiltMotor = 28; - - // Elevator Motor IDs - public static final int elevatorMotorL = 26; - public static final int elevatorMotorR = 27; - - // Climber Motor IDs - public static final int beefyMotor = 29; - } - - public static class DIOConstants { - - // Sensor ports - public static final int coralSensorPort = 1; - public static final int elevatorEncoderL = 2; - public static final int elevatorEncoderR = 3; - public static final int homingTiltClickySwitch = 4; - } - - public static class AutoScoring { - - public static class Processor { - - public static final Transform2d offset = new Transform2d( - Inches.of(24).in(Meters), - Inches.of(0).in(Meters), - Rotation2d.fromDegrees(0) - ); - } - - public static class Reef { - - public static final Transform2d coralOffsetL = new Transform2d( - Inches.of(21.5).in(Meters), - Inches.of(2.5).in(Meters), - Rotation2d.fromDegrees(180) - ); - - public static final Transform2d coralOffsetR = new Transform2d( - Inches.of(21.5).in(Meters), - Inches.of(2.5).in(Meters), - Rotation2d.fromDegrees(180) - ); - - public static final Transform2d algaeOffset = new Transform2d( - Inches.of(18).in(Meters), - Inches.of(0).in(Meters), - Rotation2d.fromDegrees(0) - ); - } - - public static class HumanPlayer { - - public static class Left { - - public static final Transform2d offset = new Transform2d( - Inches.of(24).in(Meters), - Inches.of(0).in(Meters), - Rotation2d.fromDegrees(0) - ); - } - - public static class Right { - - public static final Transform2d offset = new Transform2d( - Inches.of(24).in(Meters), - Inches.of(0).in(Meters), - Rotation2d.fromDegrees(0) - ); - } - } - } + public static final double ROBOT_MASS = (148 - 20.3) * 0.453592; // 32lbs * kg per pound + public static final Matter CHASSIS = + new Matter(new Translation3d(0, 0, Inches.of(8).in(Meters)), ROBOT_MASS); + public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag + public static final double MAX_SPEED = Units.feetToMeters(14.5); + public static final double MAX_ANGULAR_SPEED = Math.toRadians(240.0); + public static final double MAX_ACCELERATION = 1.5; + public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(120.0); + + public static final class DrivebaseConstants { + + // Hold time on motor brakes when disabled + public static final double WHEEL_LOCK_TIME = 10; // seconds + } + + public static class OperatorConstants { + + // Joystick Deadband + public static final double DEADBAND = 0.1; + public static final double LEFT_Y_DEADBAND = 0.1; + public static final double RIGHT_X_DEADBAND = 0.1; + public static final double TURN_CONSTANT = 6; + } + + public static class CanConstants { + + // Scoring Motor IDs + public static final int scoreMotor1 = 24; + public static final int scoreMotor2 = 25; + public static final int scoreTiltMotor = 28; + + // Elevator Motor IDs + public static final int elevatorMotorL = 26; + public static final int elevatorMotorR = 27; + + // Climber Motor IDs + public static final int beefyMotor = 29; + } + + public static class DIOConstants { + + // Sensor ports + public static final int coralSensorPort = 1; + public static final int elevatorEncoderL = 2; + public static final int elevatorEncoderR = 3; + public static final int homingTiltClickySwitch = 4; + } + + public static class AutoScoring { + + public static class Processor { + + public static final Transform2d offset = + new Transform2d( + Inches.of(24).in(Meters), Inches.of(0).in(Meters), Rotation2d.fromDegrees(0)); + } + + public static class Reef { + + public static final Transform2d coralOffsetL = + new Transform2d( + Inches.of(21.5).in(Meters), Inches.of(2.5).in(Meters), Rotation2d.fromDegrees(180)); + + public static final Transform2d coralOffsetR = + new Transform2d( + Inches.of(21.5).in(Meters), Inches.of(2.5).in(Meters), Rotation2d.fromDegrees(180)); + + public static final Transform2d algaeOffset = + new Transform2d( + Inches.of(18).in(Meters), Inches.of(0).in(Meters), Rotation2d.fromDegrees(0)); + } + + public static class HumanPlayer { + + public static class Left { + + public static final Transform2d offset = + new Transform2d( + Inches.of(24).in(Meters), Inches.of(0).in(Meters), Rotation2d.fromDegrees(0)); + } + + public static class Right { + + public static final Transform2d offset = + new Transform2d( + Inches.of(24).in(Meters), Inches.of(0).in(Meters), Rotation2d.fromDegrees(0)); + } + } + } } diff --git a/src/main/java/frc/robot/ControllerActions.java b/src/main/java/frc/robot/ControllerActions.java index 2b1ba45..e5d6127 100644 --- a/src/main/java/frc/robot/ControllerActions.java +++ b/src/main/java/frc/robot/ControllerActions.java @@ -9,14 +9,14 @@ public class ControllerActions { - public CommandXboxController m_XboxController; - public final CommandJoystick m_JoystickL = new CommandJoystick(1); - public final CommandJoystick m_JoystickR = new CommandJoystick(2); + public CommandXboxController m_XboxController; + public final CommandJoystick m_JoystickL = new CommandJoystick(1); + public final CommandJoystick m_JoystickR = new CommandJoystick(2); - public final Trigger scoreButton = new Trigger(m_JoystickL.button(1).or(m_JoystickR.button(1))); - public final Trigger intakeButton = new Trigger(m_JoystickL.button(3).or(m_JoystickR.button(3))); + public final Trigger scoreButton = new Trigger(m_JoystickL.button(1).or(m_JoystickR.button(1))); + public final Trigger intakeButton = new Trigger(m_JoystickL.button(3).or(m_JoystickR.button(3))); - public ControllerActions(CommandXboxController driverXbox) { - this.m_XboxController = driverXbox; - } + public ControllerActions(CommandXboxController driverXbox) { + this.m_XboxController = driverXbox; + } } diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8217ed0..176f152 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -7,19 +7,20 @@ 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. + * 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() {} + 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); - } + /** + * 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/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 65d520b..52763e4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,156 +13,137 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -/** ---------- - * Robot - * --- - * The VM is configured to automatically run this class, and to call the functions corresponding to each mode, as - * described in the TimedRobot documentation. If you change the name of this class or the package after creating this - * project, you must also update the build.gradle file in the project. - * --- +/** + * ---------- Robot --- The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot documentation. If you change + * the name of this class or the package after creating this project, you must also update the + * build.gradle file in the project. --- */ public class Robot extends TimedRobot { - private static Robot instance; - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - private Timer disabledTimer; - - public Robot() { - instance = this; - } - - public static Robot getInstance() { - return instance; - } - - /** ---------- - * RobotInit - * --- - * This function is run when the robot is first started up and should be used for any initialization code. - * --- - */ - @Override - public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - - // create webserve for elastic dashboard layouts - WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); - - // Create a timer to disable motor brake a few seconds after disable. This will let the robot stop - // immediately when disabled, but then also let it be pushed more - disabledTimer = new Timer(); - - if (isSimulation()) { - DriverStation.silenceJoystickConnectionWarning(true); - } - } - - /** ---------- - * RobotPeriodic - * --- - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics that you want ran - * during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - *--- - */ - @Override - public void robotPeriodic() { - /* ---------- - * Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - * commands, running already-scheduled commands, removing finished or interrupted commands, - * and running subsystem periodic() methods. This must be called from the robot's periodic - * block in order for anything in the Command-based framework to work. - */ - CommandScheduler.getInstance().run(); - - // Put the current match time on the dashboard - SmartDashboard.putNumber("misc/Match Time", DriverStation.getMatchTime()); - SmartDashboard.putBoolean("misc/robotRelative", m_robotContainer.robotRelative); - } - - /* ---------- - * DisabledInit - * --- - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when the robot is disabled. - * --- - * */ - @Override - public void disabledInit() { - m_robotContainer.setMotorBrake(true); - disabledTimer.reset(); - disabledTimer.start(); - } - - /** ---------- - * DisabledPeriodic - * --- - * This function is called periodically while the robot is in disabled mode. - * --- - */ - @Override - public void disabledPeriodic() { - if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { - m_robotContainer.setMotorBrake(false); - disabledTimer.stop(); - disabledTimer.reset(); - } - } - - /** ---------- - * AutonomousInit - * --- - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - * --- - */ - @Override - public void autonomousInit() { - m_robotContainer.setMotorBrake(true); - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - /** ---------- - * TeleopInit - * --- - * This function is called at the beginning of operator control. - * --- - */ - @Override - public void teleopInit() { - /* ---------- - * This makes sure that the autonomous stops running when - * teleop starts running. If you want the autonomous to - * continue until interrupted by another command, remove - * this line or comment it out. - */ - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } else { - CommandScheduler.getInstance().cancelAll(); - } - } - - /** ---------- - * TestInit - * --- - * This function is called periodically during test mode. - * --- - */ - @Override - public void testInit() { - // Cancel's all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } + private static Robot instance; + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + private Timer disabledTimer; + + public Robot() { + instance = this; + } + + public static Robot getInstance() { + return instance; + } + + /** + * ---------- RobotInit --- This function is run when the robot is first started up and should be + * used for any initialization code. --- + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + + // create webserve for elastic dashboard layouts + WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); + + // Create a timer to disable motor brake a few seconds after disable. This will let the robot + // stop + // immediately when disabled, but then also let it be pushed more + disabledTimer = new Timer(); + + if (isSimulation()) { + DriverStation.silenceJoystickConnectionWarning(true); + } + } + + /** + * ---------- RobotPeriodic --- This function is called every 20 ms, no matter the mode. Use this + * for items like diagnostics that you want ran during disabled, autonomous, teleoperated and + * test. + * + *

This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. --- + */ + @Override + public void robotPeriodic() { + /* ---------- + * Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + * commands, running already-scheduled commands, removing finished or interrupted commands, + * and running subsystem periodic() methods. This must be called from the robot's periodic + * block in order for anything in the Command-based framework to work. + */ + CommandScheduler.getInstance().run(); + + // Put the current match time on the dashboard + SmartDashboard.putNumber("misc/Match Time", DriverStation.getMatchTime()); + SmartDashboard.putBoolean("misc/robotRelative", m_robotContainer.robotRelative); + } + + /* ---------- + * DisabledInit + * --- + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when the robot is disabled. + * --- + * */ + @Override + public void disabledInit() { + m_robotContainer.setMotorBrake(true); + disabledTimer.reset(); + disabledTimer.start(); + } + + /** + * ---------- DisabledPeriodic --- This function is called periodically while the robot is in + * disabled mode. --- + */ + @Override + public void disabledPeriodic() { + if (disabledTimer.hasElapsed(Constants.DrivebaseConstants.WHEEL_LOCK_TIME)) { + m_robotContainer.setMotorBrake(false); + disabledTimer.stop(); + disabledTimer.reset(); + } + } + + /** + * ---------- AutonomousInit --- This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. --- + */ + @Override + public void autonomousInit() { + m_robotContainer.setMotorBrake(true); + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** ---------- TeleopInit --- This function is called at the beginning of operator control. --- */ + @Override + public void teleopInit() { + /* ---------- + * This makes sure that the autonomous stops running when + * teleop starts running. If you want the autonomous to + * continue until interrupted by another command, remove + * this line or comment it out. + */ + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } else { + CommandScheduler.getInstance().cancelAll(); + } + } + + /** ---------- TestInit --- This function is called periodically during test mode. --- */ + @Override + public void testInit() { + // Cancel's all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3032e3c..ddd08d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,481 +27,444 @@ import java.io.File; import swervelib.SwerveInputStream; -/** ---------- - * RobotContainer Class - * --- - * This class is where the bulk of the robot should be declared. Since - * Command-based is a "declarative" paradigm, very little robot logic - * should actually be handled in the {@link Robot} periodic methods - * (other than the scheduler calls). Instead, the structure of the robot - * (including subsystems, commands, and trigger mappings) should be declared here. - * --- +/** + * ---------- RobotContainer Class --- This class is where the bulk of the robot should be declared. + * Since Command-based is a "declarative" paradigm, very little robot logic should actually be + * handled in the {@link Robot} periodic methods (other than the scheduler calls). Instead, the + * structure of the robot (including subsystems, commands, and trigger mappings) should be declared + * here. --- */ public class RobotContainer { - private final SendableChooser autoChooser; - - public boolean robotRelative = false; - - /** ---------- - * HID Initialization - * ------------ */ - private final CommandXboxController driverXbox = new CommandXboxController(0); - private final CommandJoystick m_JoystickL = new CommandJoystick(1); - private final CommandJoystick m_JoystickR = new CommandJoystick(2); - - /** ---------- - * Subsystems - * ------------ */ - private final SwerveSubsystem drivebase = new SwerveSubsystem( - new File(Filesystem.getDeployDirectory(), "swerve/neo") - ); - private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); - private final ScoringSubsystem scoringSubsystem = new ScoringSubsystem(); - private final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); - private final TargetingSubsystem targetingSubsystem = new TargetingSubsystem(); - - /** ---------- - * Swerve Drive Input Streams - * ------------ */ - - /** - * Converts driver input into a field-relative ChassisSpeeds that is controlled - * by angular velocity. - */ - SwerveInputStream driveAngularVelocity = SwerveInputStream.of( - drivebase.getSwerveDrive(), - () -> driverXbox.getLeftY() * -1, - () -> driverXbox.getLeftX() * -1 - ) - .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - - /** - * Clone's the angular velocity input stream and converts it to a fieldRelative - * input stream. - */ - SwerveInputStream driveDirectAngle = driveAngularVelocity - .copy() - .withControllerHeadingAxis(driverXbox::getRightX, driverXbox::getRightY) - .headingWhile(true); - - /** - * Clone's the angular velocity input stream and converts it to a robotRelative - * input stream. - */ - SwerveInputStream driveRobotOriented = driveAngularVelocity - .copy() - .robotRelative(true) - .allianceRelativeControl(false); - - SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of( - drivebase.getSwerveDrive(), - () -> -driverXbox.getLeftY(), - () -> -driverXbox.getLeftX() - ) - .withControllerRotationAxis(() -> driverXbox.getRawAxis(2)) - .deadband(OperatorConstants.DEADBAND) - .scaleTranslation(0.9) - .allianceRelativeControl(true); - // Derive the heading axis with math! - SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard - .copy() - .withControllerHeadingAxis( - () -> Math.sin(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2), - () -> Math.cos(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2) - ) - .headingWhile(true); - - /** ---------- - * RobotContainer Root Class - * --- - * This is the root class for the robot. It is responsible for configuring the robot, its subsystems, and bindings. - * --- - */ - public RobotContainer() { - NamedCommands.registerCommand("score", score); - NamedCommands.registerCommand("intake", intake); - NamedCommands.registerCommand("driveToLeftBranch", driveToLeftBranch); - NamedCommands.registerCommand("driveToRightBranch", driveToRightBranch); - NamedCommands.registerCommand("driveToSource", driveToSource); - NamedCommands.registerCommand("L1", L1); - NamedCommands.registerCommand("L2", L2); - NamedCommands.registerCommand("L3", L3); - NamedCommands.registerCommand("L4", L4); - configureBindings(); - DriverStation.silenceJoystickConnectionWarning(true); - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("misc/Auto Chooser", autoChooser); - } - - /** ---------- - * Configure the button bindings - * --- - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link CommandButton} with a {@link Command} and then calling the - * various button-press functions on it. - * --- - */ - private void configureBindings() { - // NOTE: Avoid duplicating button bindings in the same mode (teleop, test, etc.) - // Always check for existing bindings before assigning new ones - // Use comments to document button assignments for better clarity - - Command driveFieldOrientedAnglularVelocity = drivebase.robotDriveCommand(driveAngularVelocity, () -> - robotRelative - ); - - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - - /** ------------------------------------- * - * Xbox Swerve and Navigation bindings - * --- - * drivebase locking and fake vision reading - * zero gyro and print mode - * center modules and none - * zero elevator and none - * drive to pose (for sysid) and none - * --- - */ - - driverXbox - .x() - .onTrue( - targetingSubsystem - .autoTargetPairCommand(drivebase::getPose, Side.LEFT) - .andThen( - Commands.either( - targetingSubsystem.driveToCoralTarget(drivebase, 0.6, 1), - Commands.runEnd( - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0) - ).withTimeout(.15), - () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose) - ) - ) - ); - - driverXbox - .b() - .onTrue( - targetingSubsystem - .autoTargetPairCommand(drivebase::getPose, Side.RIGHT) - .andThen( - Commands.either( - targetingSubsystem.driveToCoralTarget(drivebase, 0.6, 1), - Commands.runEnd( - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0) - ).withTimeout(.15), - () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose) - ) - ) - ); - - // this effectively cancels the auto align - driverXbox.y().onTrue(drivebase.driveToPose(drivebase::getPose)); - - driverXbox - .a() - .onTrue( - Commands.runOnce(drivebase::zeroGyro).andThen( - Commands.print(DriverStation.isTest() ? "Test Mode: Reset Gyro" : "Other Mode: Reset Gyro") - ) - ); - - driverXbox - .back() - .whileTrue(Commands.either(drivebase.centerModulesCommand(), Commands.none(), DriverStation::isTest)); - - driverXbox - .rightBumper() - .onTrue( - Commands.runOnce(() -> { - robotRelative = robotRelative ? false : true; - }) - ) - .and(DriverStation::isTeleop); - - /** ------------------------------------- - * Xbox Scoring and Intake bindings - * --- - * duck song and drivebase locking - * intake and duck song - * eject and none - * --- - */ - - driverXbox - .leftBumper() - .whileTrue( - Commands.either( - climberSubsystem.playMusicCommand(), - Commands.runOnce(drivebase::lock, drivebase).repeatedly(), - DriverStation::isTest - ) - ); - - driverXbox.rightBumper().and(DriverStation::isTest).onTrue(scoringSubsystem.runIntakeCommand()); - - driverXbox.rightTrigger().and(DriverStation::isTeleop).onTrue(climberSubsystem.playMusicCommand()); - - /** ------------------------------------- - * Scoring Bindings - * --- - * positions are staggered so it is in an increasing u shape on the joystick - * demo command will be toggled via binding and will continuially run elevator positions - * manual control sets elevator position and speed based on joystick position - * --- - */ - - m_JoystickL - .trigger() - .onTrue( - Commands.either( - scoringSubsystem.runIntakeCommand(), - scoringSubsystem.runEjectCommand(), - scoringSubsystem::getCoralSensor - ) - ); - - m_JoystickL.trigger().and(m_JoystickL.button(7)).whileTrue(scoringSubsystem.runEjectCommand()); - m_JoystickL.trigger().and(m_JoystickL.button(8)).whileTrue(scoringSubsystem.runIntakeCommand()); - - /** ------------------------------------- - * Elevator position bindings - * --- - * positions are staggered so it is in an increasing u shape on the joystick - * demo command will be toggled via binding and will continuially run elevator positions - * manual control sets elevator position and speed based on joystick position - * --- - */ - - m_JoystickL - .trigger() - .and(DriverStation::isTest) - .whileTrue(elevatorSubsystem.setSpeed(() -> m_JoystickL.getY() * -1)); - - m_JoystickL - .button(2) - .and(() -> !DriverStation.isTest()) - .whileTrue(elevatorSubsystem.setSpeed(() -> m_JoystickL.getY() * -1)); - - m_JoystickL.button(5).onTrue(L1); - m_JoystickL.button(3).onTrue(L2); - m_JoystickL.button(4).onTrue(L3); - m_JoystickL.button(6).onTrue(L4); - - m_JoystickL - .button(7) - .toggleOnTrue( - Commands.either( - Commands.repeatingSequence( - elevatorSubsystem - .setPos(() -> FieldConstants.CoralStation.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)), - Commands.waitSeconds(2), - elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L2.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)), - Commands.waitSeconds(2), - elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L3.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)), - Commands.waitSeconds(2), - elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L4.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) - .andThen(scoringSubsystem.tiltCommand(144)), - Commands.waitSeconds(2) - ), - Commands.none(), - DriverStation::isTest - ) - ); - - /** ------------------------------------- - * Climber bindings - * --- - * climb and descend - * --- - */ - - m_JoystickL.button(9).whileTrue(climberSubsystem.descendCommand().alongWith(Commands.print("climber down"))); - m_JoystickL.button(10).whileTrue(climberSubsystem.climbCommand().alongWith(Commands.print("climber up"))); - - /** ------------------------------------- - * Test-mode specific tilt bindings - * --- - * tilt command (button 8) and tilt nudge - * tilt sysid command - * --- - */ - - m_JoystickL.button(8).and(DriverStation::isTest).whileTrue(scoringSubsystem.tiltCommand(0)); - - Command tiltJoyCommand = scoringSubsystem.tiltJoy(() -> m_JoystickL.getY()); - tiltJoyCommand.addRequirements(scoringSubsystem); - scoringSubsystem.setDefaultCommand(tiltJoyCommand); - - m_JoystickL.button(12).onTrue(scoringSubsystem.zeroEncoder()); - - /** ------------------------------------- - * Test-mode specific sysid bindings - * --- - * sysid command for elevator, tilt, and drivebase - * --- - */ - - m_JoystickL - .button(2) - .onTrue( - Commands.either(scoringSubsystem.tiltSysIDCommand(4, 2, 2), Commands.none(), DriverStation::isTest) - ); - - m_JoystickR - .button(9) - .whileTrue( - Commands.either(elevatorSubsystem.sysIDCommand(4, 2, 2), Commands.none(), DriverStation::isTest) - ); - - m_JoystickR - .button(10) - .whileTrue( - Commands.either(scoringSubsystem.tiltSysIDCommand(4, 2, 2), Commands.none(), DriverStation::isTest) - ); - m_JoystickR - .button(11) - .whileTrue(Commands.either(drivebase.sysIdAngleMotorCommand(), Commands.none(), DriverStation::isTest)); - m_JoystickR - .button(12) - .whileTrue(Commands.either(drivebase.sysIdDriveMotorCommand(), Commands.none(), DriverStation::isTest)); - } - - /** ---------- - * Auto named commands - * --- - * named commands for reef align and auto scoring on different levels - * --- - */ - public Command score = Commands.waitUntil( - () -> scoringSubsystem.atDesiredPosition() && elevatorSubsystem.atDesiredPosition() - ) - .withTimeout(1.5) - .andThen(scoringSubsystem.runEjectCommand()); - public Command intake = scoringSubsystem - .tiltCommand(FieldConstants.CoralStation.pitch) - .andThen( - Commands.waitUntil(() -> scoringSubsystem.atDesiredPosition() & elevatorSubsystem.atDesiredPosition()) - .withTimeout(1.5) - .andThen(scoringSubsystem.runIntakeCommand()) - .andThen( - new PrintCommand("Coral sensor active, running intake").until(() -> - !scoringSubsystem.getCoralSensor() - ) - ) - ); - public Command driveToLeftBranch = targetingSubsystem.driveToLeftBranch(drivebase); - public Command driveToRightBranch = targetingSubsystem.driveToRightBranch(drivebase); - public Command driveToSource = targetingSubsystem.driveToSourceCommand(drivebase); - public Command L1 = elevatorSubsystem - .setPos(() -> FieldConstants.CoralStation.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) - .until(elevatorSubsystem::atDesiredPosition); - public Command L2 = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L2.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)) - .until(scoringSubsystem::atDesiredPosition); - public Command L3 = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L3.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)) - .until(scoringSubsystem::atDesiredPosition); - public Command L4 = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L4.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) - .until(scoringSubsystem::atDesiredPosition); - - public Command autoTargetLeftBranchCommand = targetingSubsystem - .autoTargetPairCommand(drivebase::getPose, Side.LEFT) - .andThen( - Commands.either( - targetingSubsystem.driveToCoralTarget(drivebase), - Commands.runEnd( - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0) - ).withTimeout(.2), - () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose) - ) - ) - .withName("autoTargetLeftBranch"); - - public Command autoTargetRightBranchCommand = targetingSubsystem - .autoTargetPairCommand(drivebase::getPose, Side.RIGHT) - .andThen( - Commands.either( - targetingSubsystem.driveToCoralTarget(drivebase), - Commands.runEnd( - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), - () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0) - ).withTimeout(.2), - () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose) - ) - ) - .withName("autoTargetRightBranch"); - - public Command autoLvl4Command = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L4.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.runIntakeCommand()) - .withName("autoLvl4"); - - public Command autoLvl3Command = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L3.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.runIntakeCommand()) - .withName("autoLvl3"); - - public Command autoLvl2Command = elevatorSubsystem - .setPos(() -> FieldConstants.ReefHeight.L2.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.runEjectCommand()) - .withName("autoLvl2"); - - public Command autoIntakeCommand = elevatorSubsystem - .setPos(() -> FieldConstants.CoralStation.height) - .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) - .until(elevatorSubsystem::atDesiredPosition) - .andThen(scoringSubsystem.runIntakeCommand()) - .withName("autoIntake"); - - public Command autoAlignSourceCommand = targetingSubsystem - .driveToSourceCommand(drivebase) - .alongWith(autoIntakeCommand) - .withName("autoAlignSource"); - - /** ---------- - * Use this to pass the autonomous command to the main {@link Robot} class. - * --- - * @return the command to run in autonomous - * --- - */ - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } - - public void setMotorBrake(boolean brake) { - drivebase.setMotorBrake(brake); - } + private final SendableChooser autoChooser; + + public boolean robotRelative = false; + + /** ---------- HID Initialization ------------ */ + private final CommandXboxController driverXbox = new CommandXboxController(0); + + private final CommandJoystick m_JoystickL = new CommandJoystick(1); + private final CommandJoystick m_JoystickR = new CommandJoystick(2); + + /** ---------- Subsystems ------------ */ + private final SwerveSubsystem drivebase = + new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); + + private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + private final ScoringSubsystem scoringSubsystem = new ScoringSubsystem(); + private final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); + private final TargetingSubsystem targetingSubsystem = new TargetingSubsystem(); + + /** ---------- Swerve Drive Input Streams ------------ */ + + /** + * Converts driver input into a field-relative ChassisSpeeds that is controlled by angular + * velocity. + */ + SwerveInputStream driveAngularVelocity = + SwerveInputStream.of( + drivebase.getSwerveDrive(), + () -> driverXbox.getLeftY() * -1, + () -> driverXbox.getLeftX() * -1) + .withControllerRotationAxis(() -> driverXbox.getRightX() * -1) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + + /** Clone's the angular velocity input stream and converts it to a fieldRelative input stream. */ + SwerveInputStream driveDirectAngle = + driveAngularVelocity + .copy() + .withControllerHeadingAxis(driverXbox::getRightX, driverXbox::getRightY) + .headingWhile(true); + + /** Clone's the angular velocity input stream and converts it to a robotRelative input stream. */ + SwerveInputStream driveRobotOriented = + driveAngularVelocity.copy().robotRelative(true).allianceRelativeControl(false); + + SwerveInputStream driveAngularVelocityKeyboard = + SwerveInputStream.of( + drivebase.getSwerveDrive(), + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX()) + .withControllerRotationAxis(() -> driverXbox.getRawAxis(2)) + .deadband(OperatorConstants.DEADBAND) + .scaleTranslation(0.9) + .allianceRelativeControl(true); + // Derive the heading axis with math! + SwerveInputStream driveDirectAngleKeyboard = + driveAngularVelocityKeyboard + .copy() + .withControllerHeadingAxis( + () -> Math.sin(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2), + () -> Math.cos(driverXbox.getRawAxis(2) * Math.PI) * (Math.PI * 2)) + .headingWhile(true); + + /** + * ---------- RobotContainer Root Class --- This is the root class for the robot. It is + * responsible for configuring the robot, its subsystems, and bindings. --- + */ + public RobotContainer() { + NamedCommands.registerCommand("score", score); + NamedCommands.registerCommand("intake", intake); + NamedCommands.registerCommand("driveToLeftBranch", driveToLeftBranch); + NamedCommands.registerCommand("driveToRightBranch", driveToRightBranch); + NamedCommands.registerCommand("driveToSource", driveToSource); + NamedCommands.registerCommand("L1", L1); + NamedCommands.registerCommand("L2", L2); + NamedCommands.registerCommand("L3", L3); + NamedCommands.registerCommand("L4", L4); + configureBindings(); + DriverStation.silenceJoystickConnectionWarning(true); + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("misc/Auto Chooser", autoChooser); + } + + /** + * ---------- Configure the button bindings --- Use this method to define your button->command + * mappings. Buttons can be created by instantiating a {@link CommandButton} with a {@link + * Command} and then calling the various button-press functions on it. --- + */ + private void configureBindings() { + // NOTE: Avoid duplicating button bindings in the same mode (teleop, test, etc.) + // Always check for existing bindings before assigning new ones + // Use comments to document button assignments for better clarity + + Command driveFieldOrientedAnglularVelocity = + drivebase.robotDriveCommand(driveAngularVelocity, () -> robotRelative); + + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + + /** + * ------------------------------------- * Xbox Swerve and Navigation bindings --- drivebase + * locking and fake vision reading zero gyro and print mode center modules and none zero + * elevator and none drive to pose (for sysid) and none --- + */ + driverXbox + .x() + .onTrue( + targetingSubsystem + .autoTargetPairCommand(drivebase::getPose, Side.LEFT) + .andThen( + Commands.either( + targetingSubsystem.driveToCoralTarget(drivebase, 0.6, 1), + Commands.runEnd( + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0)) + .withTimeout(.15), + () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose)))); + + driverXbox + .b() + .onTrue( + targetingSubsystem + .autoTargetPairCommand(drivebase::getPose, Side.RIGHT) + .andThen( + Commands.either( + targetingSubsystem.driveToCoralTarget(drivebase, 0.6, 1), + Commands.runEnd( + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0)) + .withTimeout(.15), + () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose)))); + + // this effectively cancels the auto align + driverXbox.y().onTrue(drivebase.driveToPose(drivebase::getPose)); + + driverXbox + .a() + .onTrue( + Commands.runOnce(drivebase::zeroGyro) + .andThen( + Commands.print( + DriverStation.isTest() + ? "Test Mode: Reset Gyro" + : "Other Mode: Reset Gyro"))); + + driverXbox + .back() + .whileTrue( + Commands.either( + drivebase.centerModulesCommand(), Commands.none(), DriverStation::isTest)); + + driverXbox + .rightBumper() + .onTrue( + Commands.runOnce( + () -> { + robotRelative = robotRelative ? false : true; + })) + .and(DriverStation::isTeleop); + + /** + * ------------------------------------- Xbox Scoring and Intake bindings --- duck song and + * drivebase locking intake and duck song eject and none --- + */ + driverXbox + .leftBumper() + .whileTrue( + Commands.either( + climberSubsystem.playMusicCommand(), + Commands.runOnce(drivebase::lock, drivebase).repeatedly(), + DriverStation::isTest)); + + driverXbox.rightBumper().and(DriverStation::isTest).onTrue(scoringSubsystem.runIntakeCommand()); + + driverXbox + .rightTrigger() + .and(DriverStation::isTeleop) + .onTrue(climberSubsystem.playMusicCommand()); + + /** + * ------------------------------------- Scoring Bindings --- positions are staggered so it is + * in an increasing u shape on the joystick demo command will be toggled via binding and will + * continuially run elevator positions manual control sets elevator position and speed based on + * joystick position --- + */ + m_JoystickL + .trigger() + .onTrue( + Commands.either( + scoringSubsystem.runIntakeCommand(), + scoringSubsystem.runEjectCommand(), + scoringSubsystem::getCoralSensor)); + + m_JoystickL.trigger().and(m_JoystickL.button(7)).whileTrue(scoringSubsystem.runEjectCommand()); + m_JoystickL.trigger().and(m_JoystickL.button(8)).whileTrue(scoringSubsystem.runIntakeCommand()); + + /** + * ------------------------------------- Elevator position bindings --- positions are staggered + * so it is in an increasing u shape on the joystick demo command will be toggled via binding + * and will continuially run elevator positions manual control sets elevator position and speed + * based on joystick position --- + */ + m_JoystickL + .trigger() + .and(DriverStation::isTest) + .whileTrue(elevatorSubsystem.setSpeed(() -> m_JoystickL.getY() * -1)); + + m_JoystickL + .button(2) + .and(() -> !DriverStation.isTest()) + .whileTrue(elevatorSubsystem.setSpeed(() -> m_JoystickL.getY() * -1)); + + m_JoystickL.button(5).onTrue(L1); + m_JoystickL.button(3).onTrue(L2); + m_JoystickL.button(4).onTrue(L3); + m_JoystickL.button(6).onTrue(L4); + + m_JoystickL + .button(7) + .toggleOnTrue( + Commands.either( + Commands.repeatingSequence( + elevatorSubsystem + .setPos(() -> FieldConstants.CoralStation.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)), + Commands.waitSeconds(2), + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L2.height) + .alongWith( + scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)), + Commands.waitSeconds(2), + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L3.height) + .alongWith( + scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)), + Commands.waitSeconds(2), + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L4.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) + .andThen(scoringSubsystem.tiltCommand(144)), + Commands.waitSeconds(2)), + Commands.none(), + DriverStation::isTest)); + + /** ------------------------------------- Climber bindings --- climb and descend --- */ + m_JoystickL + .button(9) + .whileTrue(climberSubsystem.descendCommand().alongWith(Commands.print("climber down"))); + m_JoystickL + .button(10) + .whileTrue(climberSubsystem.climbCommand().alongWith(Commands.print("climber up"))); + + /** + * ------------------------------------- Test-mode specific tilt bindings --- tilt command + * (button 8) and tilt nudge tilt sysid command --- + */ + m_JoystickL.button(8).and(DriverStation::isTest).whileTrue(scoringSubsystem.tiltCommand(0)); + + Command tiltJoyCommand = scoringSubsystem.tiltJoy(() -> m_JoystickL.getY()); + tiltJoyCommand.addRequirements(scoringSubsystem); + scoringSubsystem.setDefaultCommand(tiltJoyCommand); + + m_JoystickL.button(12).onTrue(scoringSubsystem.zeroEncoder()); + + /** + * ------------------------------------- Test-mode specific sysid bindings --- sysid command for + * elevator, tilt, and drivebase --- + */ + m_JoystickL + .button(2) + .onTrue( + Commands.either( + scoringSubsystem.tiltSysIDCommand(4, 2, 2), + Commands.none(), + DriverStation::isTest)); + + m_JoystickR + .button(9) + .whileTrue( + Commands.either( + elevatorSubsystem.sysIDCommand(4, 2, 2), Commands.none(), DriverStation::isTest)); + + m_JoystickR + .button(10) + .whileTrue( + Commands.either( + scoringSubsystem.tiltSysIDCommand(4, 2, 2), + Commands.none(), + DriverStation::isTest)); + m_JoystickR + .button(11) + .whileTrue( + Commands.either( + drivebase.sysIdAngleMotorCommand(), Commands.none(), DriverStation::isTest)); + m_JoystickR + .button(12) + .whileTrue( + Commands.either( + drivebase.sysIdDriveMotorCommand(), Commands.none(), DriverStation::isTest)); + } + + /** + * ---------- Auto named commands --- named commands for reef align and auto scoring on different + * levels --- + */ + public Command score = + Commands.waitUntil( + () -> scoringSubsystem.atDesiredPosition() && elevatorSubsystem.atDesiredPosition()) + .withTimeout(1.5) + .andThen(scoringSubsystem.runEjectCommand()); + + public Command intake = + scoringSubsystem + .tiltCommand(FieldConstants.CoralStation.pitch) + .andThen( + Commands.waitUntil( + () -> + scoringSubsystem.atDesiredPosition() + & elevatorSubsystem.atDesiredPosition()) + .withTimeout(1.5) + .andThen(scoringSubsystem.runIntakeCommand()) + .andThen( + new PrintCommand("Coral sensor active, running intake") + .until(() -> !scoringSubsystem.getCoralSensor()))); + public Command driveToLeftBranch = targetingSubsystem.driveToLeftBranch(drivebase); + public Command driveToRightBranch = targetingSubsystem.driveToRightBranch(drivebase); + public Command driveToSource = targetingSubsystem.driveToSourceCommand(drivebase); + public Command L1 = + elevatorSubsystem + .setPos(() -> FieldConstants.CoralStation.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) + .until(elevatorSubsystem::atDesiredPosition); + public Command L2 = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L2.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)) + .until(scoringSubsystem::atDesiredPosition); + public Command L3 = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L3.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)) + .until(scoringSubsystem::atDesiredPosition); + public Command L4 = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L4.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) + .until(scoringSubsystem::atDesiredPosition); + + public Command autoTargetLeftBranchCommand = + targetingSubsystem + .autoTargetPairCommand(drivebase::getPose, Side.LEFT) + .andThen( + Commands.either( + targetingSubsystem.driveToCoralTarget(drivebase), + Commands.runEnd( + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0)) + .withTimeout(.2), + () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose))) + .withName("autoTargetLeftBranch"); + + public Command autoTargetRightBranchCommand = + targetingSubsystem + .autoTargetPairCommand(drivebase::getPose, Side.RIGHT) + .andThen( + Commands.either( + targetingSubsystem.driveToCoralTarget(drivebase), + Commands.runEnd( + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 1), + () -> driverXbox.getHID().setRumble(RumbleType.kBothRumble, 0)) + .withTimeout(.2), + () -> targetingSubsystem.areWeAllowedToDrive(drivebase::getPose))) + .withName("autoTargetRightBranch"); + + public Command autoLvl4Command = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L4.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L4.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.runIntakeCommand()) + .withName("autoLvl4"); + + public Command autoLvl3Command = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L3.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L3.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.runIntakeCommand()) + .withName("autoLvl3"); + + public Command autoLvl2Command = + elevatorSubsystem + .setPos(() -> FieldConstants.ReefHeight.L2.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.ReefHeight.L2.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.runEjectCommand()) + .withName("autoLvl2"); + + public Command autoIntakeCommand = + elevatorSubsystem + .setPos(() -> FieldConstants.CoralStation.height) + .alongWith(scoringSubsystem.tiltCommand(FieldConstants.CoralStation.pitch)) + .until(elevatorSubsystem::atDesiredPosition) + .andThen(scoringSubsystem.runIntakeCommand()) + .withName("autoIntake"); + + public Command autoAlignSourceCommand = + targetingSubsystem + .driveToSourceCommand(drivebase) + .alongWith(autoIntakeCommand) + .withName("autoAlignSource"); + + /** + * ---------- Use this to pass the autonomous command to the main {@link Robot} class. --- + * + * @return the command to run in autonomous --- + */ + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } + + public void setMotorBrake(boolean brake) { + drivebase.setMotorBrake(brake); + } } diff --git a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java index 038c59f..4c0bfbf 100644 --- a/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java +++ b/src/main/java/frc/robot/commands/swervedrive/auto/AutoBalanceCommand.java @@ -12,73 +12,69 @@ import frc.robot.subsystems.swervedrive.SwerveSubsystem; /** - * Auto Balance command using a simple PID controller. Created by Team 3512 - * ... + * Auto Balance command using a simple PID controller. Created by Team 3512 ... */ public class AutoBalanceCommand extends Command { - private final SwerveSubsystem swerveSubsystem; - private final PIDController controller; + private final SwerveSubsystem swerveSubsystem; + private final PIDController controller; - public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) { - this.swerveSubsystem = swerveSubsystem; - controller = new PIDController(1.0, 0.0, 0.0); - controller.setTolerance(1); - controller.setSetpoint(0.0); - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.swerveSubsystem); - } + public AutoBalanceCommand(SwerveSubsystem swerveSubsystem) { + this.swerveSubsystem = swerveSubsystem; + controller = new PIDController(1.0, 0.0, 0.0); + controller.setTolerance(1); + controller.setSetpoint(0.0); + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.swerveSubsystem); + } - /** - * The initial subroutine of a command. Called once when the command is initially scheduled. - */ - @Override - public void initialize() {} + /** The initial subroutine of a command. Called once when the command is initially scheduled. */ + @Override + public void initialize() {} - /** - * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is called repeatedly - * until {@link #isFinished()}) returns true.) - */ - @Override - public void execute() { - SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); + /** + * The main body of a command. Called repeatedly while the command is scheduled. (That is, it is + * called repeatedly until {@link #isFinished()}) returns true.) + */ + @Override + public void execute() { + SmartDashboard.putBoolean("At Tolerance", controller.atSetpoint()); - double translationVal = MathUtil.clamp( - controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), - -0.5, - 0.5 - ); - swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); - } + double translationVal = + MathUtil.clamp( + controller.calculate(swerveSubsystem.getPitch().getDegrees(), 0.0), -0.5, 0.5); + swerveSubsystem.drive(new Translation2d(translationVal, 0.0), 0.0, true); + } - /** - *

- * Returns whether this command has finished. Once a command finishes -- indicated by this method returning true -- - * the scheduler will call its {@link #end(boolean)} method. - *

- * Returning false will result in the command never ending automatically. It may still be cancelled manually or - * interrupted by another command. Hard coding this command to always return true will result in the command executing - * once and finishing immediately. It is recommended to use * - * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an operation. - *

- * - * @return whether this command has finished. - */ - @Override - public boolean isFinished() { - return controller.atSetpoint(); - } + /** + * Returns whether this command has finished. Once a command finishes -- indicated by this method + * returning true -- the scheduler will call its {@link #end(boolean)} method. + * + *

Returning false will result in the command never ending automatically. It may still be + * cancelled manually or interrupted by another command. Hard coding this command to always return + * true will result in the command executing once and finishing immediately. It is recommended to + * use * {@link edu.wpi.first.wpilibj2.command.InstantCommand InstantCommand} for such an + * operation. + * + * @return whether this command has finished. + */ + @Override + public boolean isFinished() { + return controller.atSetpoint(); + } - /** - * The action to take when the command ends. Called when either the command finishes normally -- that is it is called - * when {@link #isFinished()} returns true -- or when it is interrupted/canceled. This is where you may want to wrap - * up loose ends, like shutting off a motor that was being used in the command. - * - * @param interrupted whether the command was interrupted/canceled - */ - @Override - public void end(boolean interrupted) { - swerveSubsystem.lock(); - } + /** + * The action to take when the command ends. Called when either the command finishes normally -- + * that is it is called when {@link #isFinished()} returns true -- or when it is + * interrupted/canceled. This is where you may want to wrap up loose ends, like shutting off a + * motor that was being used in the command. + * + * @param interrupted whether the command was interrupted/canceled + */ + @Override + public void end(boolean interrupted) { + swerveSubsystem.lock(); + } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java index f0bbc63..8539241 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDrive.java @@ -16,106 +16,104 @@ import swervelib.SwerveController; import swervelib.math.SwerveMath; -/** - * An example command that uses an example subsystem. - */ +/** An example command that uses an example subsystem. */ public class AbsoluteDrive extends Command { - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingHorizontal, headingVertical; - private boolean initRotation = false; + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingHorizontal, headingVertical; + private boolean initRotation = false; - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian - * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot - * will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 - * to 1 with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 - * to 1 with deadband already accounted for. Positive Y is towards the left wall when - * looking through the driver station glass. - * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's heading angle. In the - * robot coordinate system, this is along the same axis as vY. Should range from -1 to 1 with - * no deadband. Positive is towards the left wall when looking through the driver station - * glass. - * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's heading angle. In the - * robot coordinate system, this is along the same axis as vX. Should range from -1 to 1 - * with no deadband. Positive is away from the alliance wall. - */ - public AbsoluteDrive( - SwerveSubsystem swerve, - DoubleSupplier vX, - DoubleSupplier vY, - DoubleSupplier headingHorizontal, - DoubleSupplier headingVertical - ) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingHorizontal = headingHorizontal; - this.headingVertical = headingVertical; + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and + * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— + * they will be converted to a polar angle, which the robot will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingHorizontal DoubleSupplier that supplies the horizontal component of the robot's + * heading angle. In the robot coordinate system, this is along the same axis as vY. Should + * range from -1 to 1 with no deadband. Positive is towards the left wall when looking through + * the driver station glass. + * @param headingVertical DoubleSupplier that supplies the vertical component of the robot's + * heading angle. In the robot coordinate system, this is along the same axis as vX. Should + * range from -1 to 1 with no deadband. Positive is away from the alliance wall. + */ + public AbsoluteDrive( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingHorizontal, + DoubleSupplier headingVertical) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingHorizontal = headingHorizontal; + this.headingVertical = headingVertical; - addRequirements(swerve); - } + addRequirements(swerve); + } - @Override - public void initialize() { - initRotation = true; - } + @Override + public void initialize() { + initRotation = true; + } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds( - vX.getAsDouble(), - vY.getAsDouble(), - headingHorizontal.getAsDouble(), - headingVertical.getAsDouble() - ); + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // Get the desired chassis speeds based on a 2 joystick module. + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds( + vX.getAsDouble(), + vY.getAsDouble(), + headingHorizontal.getAsDouble(), + headingVertical.getAsDouble()); - // Prevent Movement After Auto - if (initRotation) { - if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { - // Get the curretHeading - Rotation2d firstLoopHeading = swerve.getHeading(); + // Prevent Movement After Auto + if (initRotation) { + if (headingHorizontal.getAsDouble() == 0 && headingVertical.getAsDouble() == 0) { + // Get the curretHeading + Rotation2d firstLoopHeading = swerve.getHeading(); - // Set the Current Heading to the desired Heading - desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); - } - //Dont Init Rotation Again - initRotation = false; - } + // Set the Current Heading to the desired Heading + desiredSpeeds = + swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos()); + } + // Dont Init Rotation Again + initRotation = false; + } - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration() - ); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} + // 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; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java index 8c8390b..fa104fd 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java @@ -17,134 +17,136 @@ import swervelib.SwerveController; import swervelib.math.SwerveMath; -/** - * A more advanced Swerve Control System that has 4 buttons for which direction to face - */ +/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ public class AbsoluteDriveAdv extends Command { - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY; - private final DoubleSupplier headingAdjust; - private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; - private boolean resetHeading = false; - - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. Heading Adjust changes the current heading after being - * multipied by a constant. The look booleans are shortcuts to get the robot to face a certian direction. Based off of - * ideas in https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 - * with deadband already accounted for. Positive Y is towards the left wall when looking through - * the driver station glass. - * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle that should be - * adjusted. Should range from -1 to 1 with deadband already accounted for. - * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the driver is - * facing - * @param lookTowards Face the robot towards the driver - * @param lookLeft Face the robot left - * @param lookRight Face the robot right - */ - public AbsoluteDriveAdv( - SwerveSubsystem swerve, - DoubleSupplier vX, - DoubleSupplier vY, - DoubleSupplier headingAdjust, - BooleanSupplier lookAway, - BooleanSupplier lookTowards, - BooleanSupplier lookLeft, - BooleanSupplier lookRight - ) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.headingAdjust = headingAdjust; - this.lookAway = lookAway; - this.lookTowards = lookTowards; - this.lookLeft = lookLeft; - this.lookRight = lookRight; - - addRequirements(swerve); - } - - @Override - public void initialize() { - resetHeading = true; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double headingX = 0; - double headingY = 0; - - // These are written to allow combinations for 45 angles - // Face Away from Drivers - if (lookAway.getAsBoolean()) { - headingY = -1; - } - // Face Right - if (lookRight.getAsBoolean()) { - headingX = 1; - } - // Face Left - if (lookLeft.getAsBoolean()) { - headingX = -1; - } - // Face Towards the Drivers - if (lookTowards.getAsBoolean()) { - headingY = 1; - } - - // Prevent Movement After Auto - if (resetHeading) { - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { - // Get the curret Heading - Rotation2d currentHeading = swerve.getHeading(); - - // Set the Current Heading to the desired Heading - headingX = currentHeading.getSin(); - headingY = currentHeading.getCos(); - } - //Dont reset Heading Again - resetHeading = false; - } - - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); - - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration() - ); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); - - // Make the robot move - if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { - resetHeading = true; - swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true); - } else { - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } - } - - // 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; - } + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; + private boolean resetHeading = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the + * current heading after being multipied by a constant. The look booleans are shortcuts to get the + * robot to face a certian direction. Based off of ideas in + * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle + * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the + * driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingAdjust, + BooleanSupplier lookAway, + BooleanSupplier lookTowards, + BooleanSupplier lookLeft, + BooleanSupplier lookRight) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() { + resetHeading = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double headingX = 0; + double headingY = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if (lookAway.getAsBoolean()) { + headingY = -1; + } + // Face Right + if (lookRight.getAsBoolean()) { + headingX = 1; + } + // Face Left + if (lookLeft.getAsBoolean()) { + headingX = -1; + } + // Face Towards the Drivers + if (lookTowards.getAsBoolean()) { + headingY = 1; + } + + // Prevent Movement After Auto + if (resetHeading) { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) == 0) { + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); + } + // Dont reset Heading Again + resetHeading = false; + } + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + resetHeading = true; + swerve.drive( + translation, + (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), + true); + } else { + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java index ac4eb62..96577aa 100644 --- a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java +++ b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteFieldDrive.java @@ -16,76 +16,73 @@ import swervelib.SwerveController; import swervelib.math.SwerveMath; -/** - * An example command that uses an example subsystem. - */ +/** An example command that uses an example subsystem. */ public class AbsoluteFieldDrive extends Command { - private final SwerveSubsystem swerve; - private final DoubleSupplier vX, vY, heading; + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY, heading; - /** - * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, where x is - * torwards/away from alliance wall and y is left/right. headingHorzontal and headingVertical are the Cartesian - * coordinates from which the robot's angle will be derived— they will be converted to a polar angle, which the robot - * will rotate to. - * - * @param swerve The swerve drivebase subsystem. - * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range -1 to 1 with - * deadband already accounted for. Positive X is away from the alliance wall. - * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range -1 to 1 with - * deadband already accounted for. Positive Y is towards the left wall when looking through the driver - * station glass. - * @param heading DoubleSupplier that supplies the robot's heading angle. - */ - public AbsoluteFieldDrive(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { - this.swerve = swerve; - this.vX = vX; - this.vY = vY; - this.heading = heading; + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. headingHorzontal and + * headingVertical are the Cartesian coordinates from which the robot's angle will be derived— + * they will be converted to a polar angle, which the robot will rotate to. + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param heading DoubleSupplier that supplies the robot's heading angle. + */ + public AbsoluteFieldDrive( + SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplier vY, DoubleSupplier heading) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.heading = heading; - addRequirements(swerve); - } + addRequirements(swerve); + } - @Override - public void initialize() {} + @Override + public void initialize() {} - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // Get the desired chassis speeds based on a 2 joystick module. + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // Get the desired chassis speeds based on a 2 joystick module. - ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds( - vX.getAsDouble(), - vY.getAsDouble(), - new Rotation2d(heading.getAsDouble() * Math.PI) - ); + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds( + vX.getAsDouble(), vY.getAsDouble(), new Rotation2d(heading.getAsDouble() * Math.PI)); - // Limit velocity to prevent tippy - Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); - translation = SwerveMath.limitVelocity( - translation, - swerve.getFieldVelocity(), - swerve.getPose(), - Constants.LOOP_TIME, - Constants.ROBOT_MASS, - List.of(Constants.CHASSIS), - swerve.getSwerveDriveConfiguration() - ); - SmartDashboard.putNumber("LimitedTranslation", translation.getX()); - SmartDashboard.putString("Translation", translation.toString()); + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); - // Make the robot move - swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); - } + // Make the robot move + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} + // 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; - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/libs/AllianceFlipUtil.java b/src/main/java/frc/robot/libs/AllianceFlipUtil.java index 433c2a8..bd6c854 100644 --- a/src/main/java/frc/robot/libs/AllianceFlipUtil.java +++ b/src/main/java/frc/robot/libs/AllianceFlipUtil.java @@ -11,33 +11,34 @@ public class AllianceFlipUtil { - public static double applyX(double x) { - return shouldFlip() ? FieldConstants.fieldLength - x : x; - } - - public static double applyY(double y) { - return shouldFlip() ? FieldConstants.fieldWidth - y : y; - } - - public static Translation2d apply(Translation2d translation) { - return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); - } - - public static Rotation2d apply(Rotation2d rotation) { - return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; - } - - public static Pose2d apply(Pose2d pose) { - return shouldFlip() ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) : pose; - } - - public static Pose2d flip(Pose2d pose) { - return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); - } - - public static boolean shouldFlip() { - return ( - DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red - ); - } + public static double applyX(double x) { + return shouldFlip() ? FieldConstants.fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? FieldConstants.fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Pose2d flip(Pose2d pose) { + return new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static boolean shouldFlip() { + return (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red); + } } diff --git a/src/main/java/frc/robot/libs/FieldConstants.java b/src/main/java/frc/robot/libs/FieldConstants.java index 781e877..079a088 100644 --- a/src/main/java/frc/robot/libs/FieldConstants.java +++ b/src/main/java/frc/robot/libs/FieldConstants.java @@ -18,140 +18,154 @@ import java.util.Map; /** - * Contains various field dimensions and useful reference points. All units are in meters and poses have a blue alliance - * origin. + * Contains various field dimensions and useful reference points. All units are in meters and poses + * have a blue alliance origin. */ public class FieldConstants { - public static final double fieldLength = Units.inchesToMeters(690.876); - public static final double fieldWidth = Units.inchesToMeters(317); - public static final double startingLineX = Units.inchesToMeters(299.438); // Measured from the inside of starting line - - public enum ReefHeight { - L4(1.23, 64), - L3(0.62, 54), - L2(0.19, 61), - L1(0.3, 65); - - public final double height; - public final double pitch; - - ReefHeight(double height, double pitch) { - this.height = height; // in meters of elevator extension - this.pitch = pitch; // in degrees with 0 being facing down and positive being up toward the outside of the robot - } - } - - public static class Processor { - - public static final Pose2d centerFace = new Pose2d( - Units.inchesToMeters(235.726), - 0, - Rotation2d.fromDegrees(90) - ); - } - - public static class Barge { - - public static final Translation2d farCage = new Translation2d( - Units.inchesToMeters(345.428), - Units.inchesToMeters(286.779) - ); - public static final Translation2d middleCage = new Translation2d( - Units.inchesToMeters(345.428), - Units.inchesToMeters(242.855) - ); - public static final Translation2d closeCage = new Translation2d( - Units.inchesToMeters(345.428), - Units.inchesToMeters(199.947) - ); - - // Measured from floor to bottom of cage - public static final double deepHeight = Units.inchesToMeters(3.125); - public static final double shallowHeight = Units.inchesToMeters(30.125); - } - - public static class CoralStation { - - public static final double stationLength = Units.inchesToMeters(79.750); - public static final Pose2d rightCenterFace = new Pose2d( - Units.inchesToMeters(33.526), - Units.inchesToMeters(25.824), - Rotation2d.fromDegrees(144.011 - 90) - ); - public static final Pose2d leftCenterFace = new Pose2d( - rightCenterFace.getX(), - fieldWidth - rightCenterFace.getY(), - Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians()) - ); - - public static final List bothPoses = List.of(rightCenterFace, leftCenterFace); - - public static final double height = 0, pitch = 130; - } - - public static class Reef { - - public static final Translation2d center = new Translation2d( - Units.inchesToMeters(176.746), - Units.inchesToMeters(158.501) - ); - public static final double faceToZoneLine = Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line - - public static final Pose2d[] centerFaces = new Pose2d[] { - new Pose2d(Units.inchesToMeters(144.003), Units.inchesToMeters(158.500), Rotation2d.fromDegrees(180)), - new Pose2d(Units.inchesToMeters(160.373), Units.inchesToMeters(186.857), Rotation2d.fromDegrees(120)), - new Pose2d(Units.inchesToMeters(193.116), Units.inchesToMeters(186.858), Rotation2d.fromDegrees(60)), - new Pose2d(Units.inchesToMeters(209.489), Units.inchesToMeters(158.502), Rotation2d.fromDegrees(0)), - new Pose2d(Units.inchesToMeters(193.118), Units.inchesToMeters(130.145), Rotation2d.fromDegrees(-60)), - new Pose2d(Units.inchesToMeters(160.375), Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)) - }; // Starting facing the driver station in clockwise order - public static final ArrayList> branchPositions = new ArrayList<>(13); // Starting at the right branch facing the driver station in clockwise - - static { - // Initialize branch positions - for (int face = 0; face < 6; face++) { - Map fillRight = new HashMap<>(); - Map fillLeft = new HashMap<>(); - for (var level : ReefHeight.values()) { - Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); - double adjustX = Units.inchesToMeters(17.738); //offsets for the left reef coral thing scoring - double adjustY = Units.inchesToMeters(7.9); - - fillRight.put( - level, - new Pose3d( - new Translation3d( - poseDirection.transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())).getX(), - poseDirection.transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())).getY(), - level.height - ), - new Rotation3d( - 0, - Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians() - ) - ) - ); - fillLeft.put( - level, - new Pose3d( - new Translation3d( - poseDirection.transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())).getX(), - poseDirection.transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())).getY(), - level.height - ), - new Rotation3d( - 0, - Units.degreesToRadians(level.pitch), - poseDirection.getRotation().getRadians() - ) - ) - ); - } - branchPositions.add(fillLeft); - branchPositions.add(fillRight); - } - } - } + public static final double fieldLength = Units.inchesToMeters(690.876); + public static final double fieldWidth = Units.inchesToMeters(317); + public static final double startingLineX = + Units.inchesToMeters(299.438); // Measured from the inside of starting line + + public enum ReefHeight { + L4(1.23, 64), + L3(0.62, 54), + L2(0.19, 61), + L1(0.3, 65); + + public final double height; + public final double pitch; + + ReefHeight(double height, double pitch) { + this.height = height; // in meters of elevator extension + this.pitch = + pitch; // in degrees with 0 being facing down and positive being up toward the outside of + // the robot + } + } + + public static class Processor { + + public static final Pose2d centerFace = + new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); + } + + public static class Barge { + + public static final Translation2d farCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); + public static final Translation2d middleCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); + public static final Translation2d closeCage = + new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); + + // Measured from floor to bottom of cage + public static final double deepHeight = Units.inchesToMeters(3.125); + public static final double shallowHeight = Units.inchesToMeters(30.125); + } + + public static class CoralStation { + + public static final double stationLength = Units.inchesToMeters(79.750); + public static final Pose2d rightCenterFace = + new Pose2d( + Units.inchesToMeters(33.526), + Units.inchesToMeters(25.824), + Rotation2d.fromDegrees(144.011 - 90)); + public static final Pose2d leftCenterFace = + new Pose2d( + rightCenterFace.getX(), + fieldWidth - rightCenterFace.getY(), + Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians())); + + public static final List bothPoses = List.of(rightCenterFace, leftCenterFace); + + public static final double height = 0, pitch = 130; + } + + public static class Reef { + + public static final Translation2d center = + new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); + public static final double faceToZoneLine = + Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line + + public static final Pose2d[] centerFaces = + new Pose2d[] { + new Pose2d( + Units.inchesToMeters(144.003), + Units.inchesToMeters(158.500), + Rotation2d.fromDegrees(180)), + new Pose2d( + Units.inchesToMeters(160.373), + Units.inchesToMeters(186.857), + Rotation2d.fromDegrees(120)), + new Pose2d( + Units.inchesToMeters(193.116), + Units.inchesToMeters(186.858), + Rotation2d.fromDegrees(60)), + new Pose2d( + Units.inchesToMeters(209.489), + Units.inchesToMeters(158.502), + Rotation2d.fromDegrees(0)), + new Pose2d( + Units.inchesToMeters(193.118), + Units.inchesToMeters(130.145), + Rotation2d.fromDegrees(-60)), + new Pose2d( + Units.inchesToMeters(160.375), + Units.inchesToMeters(130.144), + Rotation2d.fromDegrees(-120)) + }; // Starting facing the driver station in clockwise order + public static final ArrayList> branchPositions = + new ArrayList<>(13); // Starting at the right branch facing the driver station in clockwise + + static { + // Initialize branch positions + for (int face = 0; face < 6; face++) { + Map fillRight = new HashMap<>(); + Map fillLeft = new HashMap<>(); + for (var level : ReefHeight.values()) { + Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = + Units.inchesToMeters(17.738); // offsets for the left reef coral thing scoring + double adjustY = Units.inchesToMeters(7.9); + + fillRight.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + fillLeft.put( + level, + new Pose3d( + new Translation3d( + poseDirection + .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getX(), + poseDirection + .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) + .getY(), + level.height), + new Rotation3d( + 0, + Units.degreesToRadians(level.pitch), + poseDirection.getRotation().getRadians()))); + } + branchPositions.add(fillLeft); + branchPositions.add(fillRight); + } + } + } } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index e708a3f..d1234ac 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -23,93 +23,102 @@ public class ClimberSubsystem extends SubsystemBase { - private final TalonFX beefyMotor; - private final Orchestra marioUnderwater; - private final Orchestra duckOrchestra; - private final VoltageOut m_voltage = new VoltageOut(0).withEnableFOC(false); - private final double runVotlts = 25; - - public ClimberSubsystem() { - beefyMotor = new TalonFX(CanConstants.beefyMotor); - - HardwareLimitSwitchConfigs newHardwareLimitSwitch = new HardwareLimitSwitchConfigs() - .withForwardLimitEnable(false) - .withReverseLimitEnable(false) - .withForwardLimitSource(ForwardLimitSourceValue.Disabled) - .withReverseLimitSource(ReverseLimitSourceValue.Disabled); - CurrentLimitsConfigs currentConfigs = new CurrentLimitsConfigs() - .withStatorCurrentLimit(60) - .withStatorCurrentLimitEnable(false); - AudioConfigs audioConfigs = new AudioConfigs().withAllowMusicDurDisable(true); - MotorOutputConfigs outputConfigs = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); - TalonFXConfiguration config = new TalonFXConfiguration() - .withHardwareLimitSwitch(newHardwareLimitSwitch) - .withAudio(audioConfigs) - .withCurrentLimits(currentConfigs) - .withMotorOutput(outputConfigs); - - config.Slot0.kP = 2; - config.Slot0.kI = 0; - config.Slot0.kD = 0; - beefyMotor.getConfigurator().apply(config); - - beefyMotor.setPosition(0); - - marioUnderwater = new Orchestra(); - marioUnderwater.addInstrument(beefyMotor); - marioUnderwater.loadMusic("music/mario-underwater.chrp"); - marioUnderwater.play(); - - duckOrchestra = new Orchestra(); - duckOrchestra.addInstrument(beefyMotor); - duckOrchestra.loadMusic("music/duck.chrp"); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("climber/climber speed", beefyMotor.get()); - SmartDashboard.putNumber("climber/climber current", beefyMotor.getStatorCurrent().getValueAsDouble()); - SmartDashboard.putNumber("climber/climber temp", beefyMotor.getDeviceTemp().getValueAsDouble()); - } - - public Command climbCommand() { - return run(() -> { - duckOrchestra.stop(); - marioUnderwater.stop(); - // beefyMotor.setControl(m_voltage.withOutput(-10).withEnableFOC(false)); - beefyMotor.setVoltage(-runVotlts); - - // System.out.println("climbCommand set to -10"); - }).finallyDo(() -> { - beefyMotor.setVoltage(0); - // beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); - // System.out.println("turn off climb command set to 0"); - }); - } - - public Command descendCommand() { - return run(() -> { - duckOrchestra.stop(); - marioUnderwater.stop(); - beefyMotor.setVoltage(runVotlts); - // beefyMotor.setControl(m_voltage.withOutput(10).withEnableFOC(false)); - // System.out.println("descendCommand set to 10"); - }).finallyDo(() -> { - beefyMotor.setVoltage(0); - // beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); - // System.out.println("turn off descend command set to 0"); - }); - } - - public Command joyCommand(DoubleSupplier pos) { - return run(() -> { - beefyMotor.setControl(m_voltage.withOutput(pos.getAsDouble() * 10).withEnableFOC(false)); - }).finallyDo(() -> { - beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); - }); - } - - public Command playMusicCommand() { - return runOnce(() -> duckOrchestra.play()); - } + private final TalonFX beefyMotor; + private final Orchestra marioUnderwater; + private final Orchestra duckOrchestra; + private final VoltageOut m_voltage = new VoltageOut(0).withEnableFOC(false); + private final double runVotlts = 25; + + public ClimberSubsystem() { + beefyMotor = new TalonFX(CanConstants.beefyMotor); + + HardwareLimitSwitchConfigs newHardwareLimitSwitch = + new HardwareLimitSwitchConfigs() + .withForwardLimitEnable(false) + .withReverseLimitEnable(false) + .withForwardLimitSource(ForwardLimitSourceValue.Disabled) + .withReverseLimitSource(ReverseLimitSourceValue.Disabled); + CurrentLimitsConfigs currentConfigs = + new CurrentLimitsConfigs().withStatorCurrentLimit(60).withStatorCurrentLimitEnable(false); + AudioConfigs audioConfigs = new AudioConfigs().withAllowMusicDurDisable(true); + MotorOutputConfigs outputConfigs = + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + TalonFXConfiguration config = + new TalonFXConfiguration() + .withHardwareLimitSwitch(newHardwareLimitSwitch) + .withAudio(audioConfigs) + .withCurrentLimits(currentConfigs) + .withMotorOutput(outputConfigs); + + config.Slot0.kP = 2; + config.Slot0.kI = 0; + config.Slot0.kD = 0; + beefyMotor.getConfigurator().apply(config); + + beefyMotor.setPosition(0); + + marioUnderwater = new Orchestra(); + marioUnderwater.addInstrument(beefyMotor); + marioUnderwater.loadMusic("music/mario-underwater.chrp"); + marioUnderwater.play(); + + duckOrchestra = new Orchestra(); + duckOrchestra.addInstrument(beefyMotor); + duckOrchestra.loadMusic("music/duck.chrp"); + } + + @Override + public void periodic() { + SmartDashboard.putNumber("climber/climber speed", beefyMotor.get()); + SmartDashboard.putNumber( + "climber/climber current", beefyMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("climber/climber temp", beefyMotor.getDeviceTemp().getValueAsDouble()); + } + + public Command climbCommand() { + return run(() -> { + duckOrchestra.stop(); + marioUnderwater.stop(); + // beefyMotor.setControl(m_voltage.withOutput(-10).withEnableFOC(false)); + beefyMotor.setVoltage(-runVotlts); + + // System.out.println("climbCommand set to -10"); + }) + .finallyDo( + () -> { + beefyMotor.setVoltage(0); + // beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); + // System.out.println("turn off climb command set to 0"); + }); + } + + public Command descendCommand() { + return run(() -> { + duckOrchestra.stop(); + marioUnderwater.stop(); + beefyMotor.setVoltage(runVotlts); + // beefyMotor.setControl(m_voltage.withOutput(10).withEnableFOC(false)); + // System.out.println("descendCommand set to 10"); + }) + .finallyDo( + () -> { + beefyMotor.setVoltage(0); + // beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); + // System.out.println("turn off descend command set to 0"); + }); + } + + public Command joyCommand(DoubleSupplier pos) { + return run(() -> { + beefyMotor.setControl(m_voltage.withOutput(pos.getAsDouble() * 10).withEnableFOC(false)); + }) + .finallyDo( + () -> { + beefyMotor.setControl(m_voltage.withOutput(0).withEnableFOC(false)); + }); + } + + public Command playMusicCommand() { + return runOnce(() -> duckOrchestra.play()); + } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 5c05f3c..18ffe7b 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -32,265 +32,273 @@ public class ElevatorSubsystem extends SubsystemBase { - private final double maxHeight = 1.23; - private final double minHeight = 0; - private long t = System.nanoTime(); - private RelativeEncoder encoderL; - private RelativeEncoder encoderR; - - private enum Limits { - TOP, - NONE, - BOTTOM, - TEST - } - - private SparkMax motorL; - private SparkMax motorR; - private SparkClosedLoopController controllerL; - private SparkClosedLoopController controllerR; - private SparkMaxConfig config = new SparkMaxConfig(); - private double kp = 0.00065323, ki = 0, kd = 0; - private double maxV = 1, maxA = 1; - private double krot = 42.4; // rotations/meter - private static final double upSpeed = 0.5; - private static final double downSpeed = 0.1; - private static final int elevatorCurrentLimit = 30; - private double ks = 0.36656, kg = 0.48642, kv = 4.7049; - - private double currentMaxVel = maxV; - private TrapezoidProfile.Constraints ffc = new TrapezoidProfile.Constraints(maxV, maxA); - private TrapezoidProfile.State ffState = new TrapezoidProfile.State(); - private TrapezoidProfile.State preRenfernce = new TrapezoidProfile.State(); - private TrapezoidProfile Profiler = new TrapezoidProfile(ffc); - private ElevatorFeedforward ff = new ElevatorFeedforward(ks, kg, kv); - - // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage m_appliedVoltage = Volts.mutable(0); - private final MutDistance m_distance = Meters.mutable(0); - private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); - - public ElevatorSubsystem() { - motorL = new SparkMax(CanConstants.elevatorMotorL, MotorType.kBrushless); - motorR = new SparkMax(CanConstants.elevatorMotorR, MotorType.kBrushless); - // the defualt is lsot zero - config.closedLoop.p(kp, ClosedLoopSlot.kSlot0).i(ki, ClosedLoopSlot.kSlot0).d(kd, ClosedLoopSlot.kSlot0); - config.smartCurrentLimit(elevatorCurrentLimit).idleMode(IdleMode.kBrake); - config.signals.primaryEncoderPositionPeriodMs(10); - - motorL.configure(config.inverted(false), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - motorR.configure(config.inverted(true), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - - controllerL = motorL.getClosedLoopController(); - controllerR = motorR.getClosedLoopController(); - - encoderL = motorL.getEncoder(); - encoderR = motorR.getEncoder(); - // System.out.println("TODO: please put reset encodes pos in"); - } - - public boolean atDesiredPosition() { - return MathUtil.isNear(preRenfernce.position, ffState.position, 0.1); - } - - public void setDesiredPosistion(double height, double time) { - ffState.position = height; - ffState.velocity = 0.0; - } - - private Limits checkLimits() { - double height = encoderL.getPosition() / krot; - if (height >= maxHeight) { - return Limits.TOP; - } else if (height <= minHeight) { - return Limits.BOTTOM; - } else { - return Limits.NONE; - } - } - - private void motorBreak() { - ffState.position = preRenfernce.position; - ffState.velocity = 0.0; - - motorL.stopMotor(); - motorR.stopMotor(); - } - - @Override - public void periodic() { - double ffValue = 0.0; - boolean running = false; - double height = encoderL.getPosition() / krot; - - ffState.position = MathUtil.clamp(ffState.position, 0, maxHeight); - ffState.velocity = 0.0; - - preRenfernce.position = MathUtil.isNear(ffState.position, height, 0.1) ? preRenfernce.position : height; - // preRenfernce.velocity = (encoderL.getVelocity() / krot) / 60.0; - - preRenfernce.velocity = MathUtil.clamp(preRenfernce.velocity, -currentMaxVel, currentMaxVel); - preRenfernce.position = MathUtil.clamp(preRenfernce.position, 0, maxHeight); - - preRenfernce = Profiler.calculate((System.nanoTime() - t) / 1e9, preRenfernce, ffState); - - t = System.nanoTime(); - ffValue = ff.calculate(MathUtil.clamp(preRenfernce.velocity, -currentMaxVel, currentMaxVel)); - - switch (checkLimits()) { - case NONE: - running = true; - break; - case BOTTOM: - if (preRenfernce.velocity > 0) { - running = true; - } else { - this.motorBreak(); - } - break; - case TOP: - if (preRenfernce.velocity < 0) { - running = true; - } else { - this.motorBreak(); - } - break; - case TEST: - System.out.println("why did you run this? you forgot to program in the limits"); - running = false; - break; - } - - if (running) { - controllerL.setReference( - preRenfernce.position * krot, - ControlType.kPosition, - ClosedLoopSlot.kSlot0, - ffValue - ); - controllerR.setReference( - preRenfernce.position * krot, - ControlType.kPosition, - ClosedLoopSlot.kSlot0, - ffValue - ); - } - - SmartDashboard.putNumber("elevator/current-position", preRenfernce.position); - SmartDashboard.putNumber("elevator/current-velocity", preRenfernce.velocity); - SmartDashboard.putNumber("elevator/end-position", ffState.position); - SmartDashboard.putNumber("elevator/currentMaxVal", currentMaxVel); - SmartDashboard.putNumber("elevator/encoderL", encoderL.getPosition()); - SmartDashboard.putNumber("elevator/encoderR", encoderR.getPosition()); - SmartDashboard.putNumber("elevator/motorL current", motorL.getOutputCurrent()); - SmartDashboard.putNumber("elevator/motorL voltage", motorL.getBusVoltage()); - SmartDashboard.putNumber("elevator/motorR current", motorR.getOutputCurrent()); - SmartDashboard.putNumber("elevator/motorR voltage", motorR.getBusVoltage()); - } - - public Command setPos(DoubleSupplier height) { - return Commands.runOnce(() -> { - currentMaxVel = maxV; - ffState.position = height.getAsDouble(); - ffState.velocity = 0.0; - }); - } - - public Command setSpeed(DoubleSupplier velo) { - return Commands.run(() -> { - double tol = 0.1; - currentMaxVel = Math.abs(MathUtil.clamp(velo.getAsDouble(), -maxV, maxV)); - if (currentMaxVel > tol) { - if (velo.getAsDouble() > 0) { - ffState.position = maxHeight; - ffState.velocity = 0.0; - } else if (velo.getAsDouble() < 0) { - ffState.position = minHeight; - ffState.velocity = 0.0; - } - } else { - ffState.position = preRenfernce.position; - ffState.velocity = 0.0; - } - }).finallyDo(() -> { - ffState.position = preRenfernce.position; - ffState.velocity = 0.0; - currentMaxVel = maxV; - }); - } - - public Command manualElevatorCommand(double input) { - return this.startEnd( - () -> { - if (input > 0) { - motorL.set(upSpeed * input); - motorR.set(upSpeed * input); - } else { - motorL.set(downSpeed * input); - motorR.set(downSpeed * input); - } - }, - () -> { - motorL.set(0); - motorR.set(0); - } - ); - } - - public Command zeroCommand() { - return this.runOnce(() -> { - preRenfernce.position = 0; - ffState.position = 0; - encoderL.setPosition(0); - encoderR.setPosition(0); - }); - } - - public Command sysIDQuasistatic(Direction dir, double timeout) { - return m_sysIdRoutine.quasistatic(dir).withTimeout(timeout); - } - - public Command sysIDDynamic(Direction dir, double timeout) { - return m_sysIdRoutine.dynamic(dir).withTimeout(timeout); - } - - public Command sysIDCommand(double quasiTimeout, double timeout, double dynamicTimeout) { - return m_sysIdRoutine - .quasistatic(SysIdRoutine.Direction.kForward) - .withTimeout(quasiTimeout) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout)) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)); - } - - private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine( - // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. - new SysIdRoutine.Config(null, Voltage.ofBaseUnits(5, Volts), null, null), - new SysIdRoutine.Mechanism( - // Tell SysId how to plumb the driving voltage to the motors. - voltage -> { - motorL.setVoltage(voltage); - motorR.setVoltage(voltage); - }, - // Tell SysId how to record a frame of data for each motor on the mechanism - // being - // characterized. - log -> { - // Record a frame for the left motors. Since these share an encoder, we consider - // the entire group to be one motor. - log - .motor("elevator") - .voltage(m_appliedVoltage.mut_replace(motorL.getBusVoltage() * motorL.getAppliedOutput(), Volts)) - .linearPosition(m_distance.mut_replace(encoderL.getPosition() / krot, Meters)) - .linearVelocity(m_velocity.mut_replace(encoderL.getVelocity() / 60.0 / krot, MetersPerSecond)); - }, - // Tell SysId to make generated commands require this subsystem, suffix test - // state in - // WPILog with this subsystem's name ("drive") - this - ) - ); + private final double maxHeight = 1.23; + private final double minHeight = 0; + private long t = System.nanoTime(); + private RelativeEncoder encoderL; + private RelativeEncoder encoderR; + + private enum Limits { + TOP, + NONE, + BOTTOM, + TEST + } + + private SparkMax motorL; + private SparkMax motorR; + private SparkClosedLoopController controllerL; + private SparkClosedLoopController controllerR; + private SparkMaxConfig config = new SparkMaxConfig(); + private double kp = 0.00065323, ki = 0, kd = 0; + private double maxV = 1, maxA = 1; + private double krot = 42.4; // rotations/meter + private static final double upSpeed = 0.5; + private static final double downSpeed = 0.1; + private static final int elevatorCurrentLimit = 30; + private double ks = 0.36656, kg = 0.48642, kv = 4.7049; + + private double currentMaxVel = maxV; + private TrapezoidProfile.Constraints ffc = new TrapezoidProfile.Constraints(maxV, maxA); + private TrapezoidProfile.State ffState = new TrapezoidProfile.State(); + private TrapezoidProfile.State preRenfernce = new TrapezoidProfile.State(); + private TrapezoidProfile Profiler = new TrapezoidProfile(ffc); + private ElevatorFeedforward ff = new ElevatorFeedforward(ks, kg, kv); + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + private final MutDistance m_distance = Meters.mutable(0); + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + public ElevatorSubsystem() { + motorL = new SparkMax(CanConstants.elevatorMotorL, MotorType.kBrushless); + motorR = new SparkMax(CanConstants.elevatorMotorR, MotorType.kBrushless); + // the defualt is lsot zero + config + .closedLoop + .p(kp, ClosedLoopSlot.kSlot0) + .i(ki, ClosedLoopSlot.kSlot0) + .d(kd, ClosedLoopSlot.kSlot0); + config.smartCurrentLimit(elevatorCurrentLimit).idleMode(IdleMode.kBrake); + config.signals.primaryEncoderPositionPeriodMs(10); + + motorL.configure( + config.inverted(false), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + motorR.configure( + config.inverted(true), ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + controllerL = motorL.getClosedLoopController(); + controllerR = motorR.getClosedLoopController(); + + encoderL = motorL.getEncoder(); + encoderR = motorR.getEncoder(); + // System.out.println("TODO: please put reset encodes pos in"); + } + + public boolean atDesiredPosition() { + return MathUtil.isNear(preRenfernce.position, ffState.position, 0.1); + } + + public void setDesiredPosistion(double height, double time) { + ffState.position = height; + ffState.velocity = 0.0; + } + + private Limits checkLimits() { + double height = encoderL.getPosition() / krot; + if (height >= maxHeight) { + return Limits.TOP; + } else if (height <= minHeight) { + return Limits.BOTTOM; + } else { + return Limits.NONE; + } + } + + private void motorBreak() { + ffState.position = preRenfernce.position; + ffState.velocity = 0.0; + + motorL.stopMotor(); + motorR.stopMotor(); + } + + @Override + public void periodic() { + double ffValue = 0.0; + boolean running = false; + double height = encoderL.getPosition() / krot; + + ffState.position = MathUtil.clamp(ffState.position, 0, maxHeight); + ffState.velocity = 0.0; + + preRenfernce.position = + MathUtil.isNear(ffState.position, height, 0.1) ? preRenfernce.position : height; + // preRenfernce.velocity = (encoderL.getVelocity() / krot) / 60.0; + + preRenfernce.velocity = MathUtil.clamp(preRenfernce.velocity, -currentMaxVel, currentMaxVel); + preRenfernce.position = MathUtil.clamp(preRenfernce.position, 0, maxHeight); + + preRenfernce = Profiler.calculate((System.nanoTime() - t) / 1e9, preRenfernce, ffState); + + t = System.nanoTime(); + ffValue = ff.calculate(MathUtil.clamp(preRenfernce.velocity, -currentMaxVel, currentMaxVel)); + + switch (checkLimits()) { + case NONE: + running = true; + break; + case BOTTOM: + if (preRenfernce.velocity > 0) { + running = true; + } else { + this.motorBreak(); + } + break; + case TOP: + if (preRenfernce.velocity < 0) { + running = true; + } else { + this.motorBreak(); + } + break; + case TEST: + System.out.println("why did you run this? you forgot to program in the limits"); + running = false; + break; + } + + if (running) { + controllerL.setReference( + preRenfernce.position * krot, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffValue); + controllerR.setReference( + preRenfernce.position * krot, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffValue); + } + + SmartDashboard.putNumber("elevator/current-position", preRenfernce.position); + SmartDashboard.putNumber("elevator/current-velocity", preRenfernce.velocity); + SmartDashboard.putNumber("elevator/end-position", ffState.position); + SmartDashboard.putNumber("elevator/currentMaxVal", currentMaxVel); + SmartDashboard.putNumber("elevator/encoderL", encoderL.getPosition()); + SmartDashboard.putNumber("elevator/encoderR", encoderR.getPosition()); + SmartDashboard.putNumber("elevator/motorL current", motorL.getOutputCurrent()); + SmartDashboard.putNumber("elevator/motorL voltage", motorL.getBusVoltage()); + SmartDashboard.putNumber("elevator/motorR current", motorR.getOutputCurrent()); + SmartDashboard.putNumber("elevator/motorR voltage", motorR.getBusVoltage()); + } + + public Command setPos(DoubleSupplier height) { + return Commands.runOnce( + () -> { + currentMaxVel = maxV; + ffState.position = height.getAsDouble(); + ffState.velocity = 0.0; + }); + } + + public Command setSpeed(DoubleSupplier velo) { + return Commands.run( + () -> { + double tol = 0.1; + currentMaxVel = Math.abs(MathUtil.clamp(velo.getAsDouble(), -maxV, maxV)); + if (currentMaxVel > tol) { + if (velo.getAsDouble() > 0) { + ffState.position = maxHeight; + ffState.velocity = 0.0; + } else if (velo.getAsDouble() < 0) { + ffState.position = minHeight; + ffState.velocity = 0.0; + } + } else { + ffState.position = preRenfernce.position; + ffState.velocity = 0.0; + } + }) + .finallyDo( + () -> { + ffState.position = preRenfernce.position; + ffState.velocity = 0.0; + currentMaxVel = maxV; + }); + } + + public Command manualElevatorCommand(double input) { + return this.startEnd( + () -> { + if (input > 0) { + motorL.set(upSpeed * input); + motorR.set(upSpeed * input); + } else { + motorL.set(downSpeed * input); + motorR.set(downSpeed * input); + } + }, + () -> { + motorL.set(0); + motorR.set(0); + }); + } + + public Command zeroCommand() { + return this.runOnce( + () -> { + preRenfernce.position = 0; + ffState.position = 0; + encoderL.setPosition(0); + encoderR.setPosition(0); + }); + } + + public Command sysIDQuasistatic(Direction dir, double timeout) { + return m_sysIdRoutine.quasistatic(dir).withTimeout(timeout); + } + + public Command sysIDDynamic(Direction dir, double timeout) { + return m_sysIdRoutine.dynamic(dir).withTimeout(timeout); + } + + public Command sysIDCommand(double quasiTimeout, double timeout, double dynamicTimeout) { + return m_sysIdRoutine + .quasistatic(SysIdRoutine.Direction.kForward) + .withTimeout(quasiTimeout) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout)) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)); + } + + private final SysIdRoutine m_sysIdRoutine = + new SysIdRoutine( + // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + new SysIdRoutine.Config(null, Voltage.ofBaseUnits(5, Volts), null, null), + new SysIdRoutine.Mechanism( + // Tell SysId how to plumb the driving voltage to the motors. + (voltage) -> { + motorL.setVoltage(voltage); + motorR.setVoltage(voltage); + }, + // Tell SysId how to record a frame of data for each motor on the mechanism + // being + // characterized. + (log) -> { + // Record a frame for the left motors. Since these share an encoder, we consider + // the entire group to be one motor. + log.motor("elevator") + .voltage( + m_appliedVoltage.mut_replace( + motorL.getBusVoltage() * motorL.getAppliedOutput(), Volts)) + .linearPosition(m_distance.mut_replace(encoderL.getPosition() / krot, Meters)) + .linearVelocity( + m_velocity.mut_replace( + encoderL.getVelocity() / 60.0 / krot, MetersPerSecond)); + }, + // Tell SysId to make generated commands require this subsystem, suffix test + // state in + // WPILog with this subsystem's name ("drive") + this)); } diff --git a/src/main/java/frc/robot/subsystems/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/ScoringSubsystem.java index b3d0050..2c4f8fb 100644 --- a/src/main/java/frc/robot/subsystems/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ScoringSubsystem.java @@ -34,220 +34,243 @@ public class ScoringSubsystem extends SubsystemBase { - private SparkMax motor1; - private SparkMax motor2; - private SparkClosedLoopController spinnyController, spinnyController2; - private final double kp = 0.01, ki = 0, kd = 0, kv = 1 / 917; - private SparkMax canTiltMax; - private SparkMaxConfig tiltConfig = new SparkMaxConfig(); - private EncoderConfig tiltEncoderConfig = new EncoderConfig(); - private RelativeEncoder canTiltEncoder; - private SparkMaxConfig motorConfig = new SparkMaxConfig(); - private SparkMaxConfig motorConfig2 = new SparkMaxConfig(); - private SparkClosedLoopController canTiltController; - public DigitalInput coralSensor; - private double setpoint = 0; - - private static final int scoringCurrent = 25; - - // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage m_appliedVoltage = Volts.mutable(0); - private final MutAngle m_rotation = Rotations.mutable(0); - private final MutAngularVelocity m_velocity = RotationsPerSecond.mutable(0); - - public ScoringSubsystem() { - motorConfig - .smartCurrentLimit(35) - .idleMode(IdleMode.kBrake) - .inverted(true) - .closedLoop.p(kp, ClosedLoopSlot.kSlot0) - .i(ki, ClosedLoopSlot.kSlot0) - .d(kd, ClosedLoopSlot.kSlot0) - .velocityFF(kv, ClosedLoopSlot.kSlot0); - motor1 = new SparkMax(CanConstants.scoreMotor1, MotorType.kBrushless); - motorConfig.signals.absoluteEncoderPositionPeriodMs(10); - motor1.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - - motorConfig2 - .smartCurrentLimit(35) - .idleMode(IdleMode.kBrake) - .closedLoop.p(kp, ClosedLoopSlot.kSlot0) - .i(ki, ClosedLoopSlot.kSlot0) - .d(kd, ClosedLoopSlot.kSlot0) - .velocityFF(kv, ClosedLoopSlot.kSlot0); - motor2 = new SparkMax(CanConstants.scoreMotor2, MotorType.kBrushless); - motor2.configure(motorConfig2, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - spinnyController = motor1.getClosedLoopController(); - - spinnyController2 = motor2.getClosedLoopController(); - coralSensor = new DigitalInput(DIOConstants.coralSensorPort); - - tiltEncoderConfig.countsPerRevolution(8192).inverted(true); - - tiltConfig.inverted(false).smartCurrentLimit(15).apply(tiltEncoderConfig); - tiltConfig.closedLoop.p(4.5, ClosedLoopSlot.kSlot0).i(0, ClosedLoopSlot.kSlot0).d(0.4, ClosedLoopSlot.kSlot0); - canTiltMax = new SparkMax(CanConstants.scoreTiltMotor, MotorType.kBrushed); - canTiltMax.configure(tiltConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - canTiltController = canTiltMax.getClosedLoopController(); - canTiltEncoder = canTiltMax.getEncoder(); - } - - @Override - public void periodic() { - // Only apply limits when not in test mode - if (!DriverStation.isTest()) { - setpoint = MathUtil.clamp(setpoint, 0, 0.4); - } - canTiltController.setReference(setpoint, ControlType.kPosition); - - SmartDashboard.putNumber("scoring/setpoint", setpoint); - SmartDashboard.putNumber("scoring/tilt current", canTiltMax.getOutputCurrent()); - SmartDashboard.putNumber("scoring/tilt voltage", canTiltMax.getBusVoltage() * canTiltMax.getAppliedOutput()); - SmartDashboard.putNumber("scoring/tilt encoder", canTiltMax.getEncoder().getPosition()); - SmartDashboard.putBoolean("scoring/coral sensor", !coralSensor.get()); - SmartDashboard.putNumber("scoring/motor1 current", motor1.getOutputCurrent()); - SmartDashboard.putNumber("scoring/motor1 voltage", motor1.getBusVoltage() * motor1.getAppliedOutput()); - SmartDashboard.putNumber("scoring/motor2 current", motor2.getOutputCurrent()); - SmartDashboard.putNumber("scoring/motor2 voltage", motor2.getBusVoltage() * motor2.getAppliedOutput()); - - SmartDashboard.putNumber("scoring/motor1 temp", motor1.getMotorTemperature()); - SmartDashboard.putNumber("scoring/motor2 temp", motor2.getMotorTemperature()); - } - - public Command runIntakeCommand() { - return this.run(() -> { - // System.out.println("intake command started"); - spinnyController.setReference(scoringCurrent, ControlType.kCurrent); - spinnyController2.setReference(scoringCurrent, ControlType.kCurrent); - }) - .until(() -> !coralSensor.get()) - .withTimeout(4.5) - .andThen( - this.run(() -> { - spinnyController.setReference(scoringCurrent, ControlType.kCurrent); - spinnyController2.setReference(scoringCurrent, ControlType.kCurrent); - }) - .finallyDo(() -> { - // System.out.println("intake command finished"); - spinnyController.setReference(0, ControlType.kCurrent); - spinnyController2.setReference(0, ControlType.kCurrent); - }) - .withTimeout(0.2) - ) - .finallyDo(() -> { - // System.out.println("intake command finished"); - spinnyController.setReference(0, ControlType.kCurrent); - spinnyController2.setReference(0, ControlType.kCurrent); - }); - } - - public Command runEjectCommand() { - return this.run(() -> { - // System.out.println("eject command started"); - spinnyController.setReference(-scoringCurrent, ControlType.kCurrent); - spinnyController2.setReference(-scoringCurrent, ControlType.kCurrent); - }) - .finallyDo(() -> { - // System.out.println("eject command ended"); - spinnyController.setReference(0, ControlType.kCurrent); - spinnyController2.setReference(0, ControlType.kCurrent); - }) - .withTimeout(1.7); - } - - public boolean atDesiredPosition() { - return MathUtil.isNear(setpoint, canTiltEncoder.getPosition(), 0.1); - } - - public boolean getCoralSensor() { - return coralSensor.get(); - } - - public Command tiltCommand(double degrees) { - return this.runOnce(() -> { - double rotations = Units.degreesToRotations(degrees); - if (!DriverStation.isTest()) { - rotations = MathUtil.clamp(rotations, 0, 0.4); - } - setpoint = rotations; - }); - } - - public Command tiltNudge(boolean direction) { - double amount = Units.degreesToRotations(0.4); - return Commands.run(() -> { - if (direction) { - if (DriverStation.isTest()) { - setpoint = setpoint + amount; - } else { - setpoint = MathUtil.clamp(setpoint + amount, 0, 0.4); - } - } else { - if (DriverStation.isTest()) { - setpoint = setpoint - amount; - } else { - setpoint = MathUtil.clamp(setpoint - amount, 0, 0.4); - } - } - }); - } - - public Command tiltJoy(DoubleSupplier joy) { - return Commands.run(() -> { - double joyVal = MathUtil.applyDeadband(joy.getAsDouble(), 0.1); - double adjustment = Units.degreesToRotations(joyVal); - - if (DriverStation.isTest()) { - setpoint = setpoint + adjustment; - } else { - setpoint = MathUtil.clamp(setpoint + adjustment, 0, 0.4); - } - }); - } - - public Command zeroEncoder() { - return this.runOnce(() -> { - canTiltEncoder.setPosition(0); - setpoint = 0; - }); - } - - public Command tiltSysIDCommand(double quasiTimeout, double timeout, double dynamicTimeout) { - return m_sysIdRoutine - .quasistatic(SysIdRoutine.Direction.kForward) - .withTimeout(quasiTimeout) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout)) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) - .andThen(Commands.waitSeconds(timeout)) - .andThen(m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)); - } - - private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine( - // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. - new SysIdRoutine.Config(null, Voltage.ofBaseUnits(5, Volts), null, null), - new SysIdRoutine.Mechanism( - // Tell SysId how to plumb the driving voltage to the motors. - voltage -> { - canTiltMax.setVoltage(voltage); - }, - // Tell SysId how to record a frame of data for each motor on the mechanism - // being - // characterized. - log -> { - log - .motor("tilt") - .voltage( - m_appliedVoltage.mut_replace(canTiltMax.getBusVoltage() * canTiltMax.getAppliedOutput(), Volts) - ) - .angularPosition(m_rotation.mut_replace(canTiltEncoder.getPosition(), Rotations)) - .angularVelocity(m_velocity.mut_replace(canTiltEncoder.getVelocity(), RotationsPerSecond)); - }, - // Tell SysId to make generated commands require this subsystem, suffix test - // state in - // WPILog with this subsystem's name ("drive") - this - ) - ); + private SparkMax motor1; + private SparkMax motor2; + private SparkClosedLoopController spinnyController, spinnyController2; + private final double kp = 0.01, ki = 0, kd = 0, kv = 1 / 917; + private SparkMax canTiltMax; + private SparkMaxConfig tiltConfig = new SparkMaxConfig(); + private EncoderConfig tiltEncoderConfig = new EncoderConfig(); + private RelativeEncoder canTiltEncoder; + private SparkMaxConfig motorConfig = new SparkMaxConfig(); + private SparkMaxConfig motorConfig2 = new SparkMaxConfig(); + private SparkClosedLoopController canTiltController; + public DigitalInput coralSensor; + private double setpoint = 0; + + private static final int scoringCurrent = 25; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + private final MutAngle m_rotation = Rotations.mutable(0); + private final MutAngularVelocity m_velocity = RotationsPerSecond.mutable(0); + + public ScoringSubsystem() { + motorConfig + .smartCurrentLimit(35) + .idleMode(IdleMode.kBrake) + .inverted(true) + .closedLoop + .p(kp, ClosedLoopSlot.kSlot0) + .i(ki, ClosedLoopSlot.kSlot0) + .d(kd, ClosedLoopSlot.kSlot0) + .velocityFF(kv, ClosedLoopSlot.kSlot0); + motor1 = new SparkMax(CanConstants.scoreMotor1, MotorType.kBrushless); + motorConfig.signals.absoluteEncoderPositionPeriodMs(10); + motor1.configure(motorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + + motorConfig2 + .smartCurrentLimit(35) + .idleMode(IdleMode.kBrake) + .closedLoop + .p(kp, ClosedLoopSlot.kSlot0) + .i(ki, ClosedLoopSlot.kSlot0) + .d(kd, ClosedLoopSlot.kSlot0) + .velocityFF(kv, ClosedLoopSlot.kSlot0); + motor2 = new SparkMax(CanConstants.scoreMotor2, MotorType.kBrushless); + motor2.configure( + motorConfig2, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + spinnyController = motor1.getClosedLoopController(); + + spinnyController2 = motor2.getClosedLoopController(); + coralSensor = new DigitalInput(DIOConstants.coralSensorPort); + + tiltEncoderConfig.countsPerRevolution(8192).inverted(true); + + tiltConfig.inverted(false).smartCurrentLimit(15).apply(tiltEncoderConfig); + tiltConfig + .closedLoop + .p(4.5, ClosedLoopSlot.kSlot0) + .i(0, ClosedLoopSlot.kSlot0) + .d(0.4, ClosedLoopSlot.kSlot0); + canTiltMax = new SparkMax(CanConstants.scoreTiltMotor, MotorType.kBrushed); + canTiltMax.configure( + tiltConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + canTiltController = canTiltMax.getClosedLoopController(); + canTiltEncoder = canTiltMax.getEncoder(); + } + + @Override + public void periodic() { + // Only apply limits when not in test mode + if (!DriverStation.isTest()) { + setpoint = MathUtil.clamp(setpoint, 0, 0.4); + } + canTiltController.setReference(setpoint, ControlType.kPosition); + + SmartDashboard.putNumber("scoring/setpoint", setpoint); + SmartDashboard.putNumber("scoring/tilt current", canTiltMax.getOutputCurrent()); + SmartDashboard.putNumber( + "scoring/tilt voltage", canTiltMax.getBusVoltage() * canTiltMax.getAppliedOutput()); + SmartDashboard.putNumber("scoring/tilt encoder", canTiltMax.getEncoder().getPosition()); + SmartDashboard.putBoolean("scoring/coral sensor", !coralSensor.get()); + SmartDashboard.putNumber("scoring/motor1 current", motor1.getOutputCurrent()); + SmartDashboard.putNumber( + "scoring/motor1 voltage", motor1.getBusVoltage() * motor1.getAppliedOutput()); + SmartDashboard.putNumber("scoring/motor2 current", motor2.getOutputCurrent()); + SmartDashboard.putNumber( + "scoring/motor2 voltage", motor2.getBusVoltage() * motor2.getAppliedOutput()); + + SmartDashboard.putNumber("scoring/motor1 temp", motor1.getMotorTemperature()); + SmartDashboard.putNumber("scoring/motor2 temp", motor2.getMotorTemperature()); + } + + public Command runIntakeCommand() { + return this.run( + () -> { + // System.out.println("intake command started"); + spinnyController.setReference(scoringCurrent, ControlType.kCurrent); + spinnyController2.setReference(scoringCurrent, ControlType.kCurrent); + }) + .until(() -> !coralSensor.get()) + .withTimeout(4.5) + .andThen( + this.run( + () -> { + spinnyController.setReference(scoringCurrent, ControlType.kCurrent); + spinnyController2.setReference(scoringCurrent, ControlType.kCurrent); + }) + .finallyDo( + () -> { + // System.out.println("intake command finished"); + spinnyController.setReference(0, ControlType.kCurrent); + spinnyController2.setReference(0, ControlType.kCurrent); + }) + .withTimeout(0.2)) + .finallyDo( + () -> { + // System.out.println("intake command finished"); + spinnyController.setReference(0, ControlType.kCurrent); + spinnyController2.setReference(0, ControlType.kCurrent); + }); + } + + public Command runEjectCommand() { + return this.run( + () -> { + // System.out.println("eject command started"); + spinnyController.setReference(-scoringCurrent, ControlType.kCurrent); + spinnyController2.setReference(-scoringCurrent, ControlType.kCurrent); + }) + .finallyDo( + () -> { + // System.out.println("eject command ended"); + spinnyController.setReference(0, ControlType.kCurrent); + spinnyController2.setReference(0, ControlType.kCurrent); + }) + .withTimeout(1.7); + } + + public boolean atDesiredPosition() { + return MathUtil.isNear(setpoint, canTiltEncoder.getPosition(), 0.1); + } + + public boolean getCoralSensor() { + return coralSensor.get(); + } + + public Command tiltCommand(double degrees) { + return this.runOnce( + () -> { + double rotations = Units.degreesToRotations(degrees); + if (!DriverStation.isTest()) { + rotations = MathUtil.clamp(rotations, 0, 0.4); + } + setpoint = rotations; + }); + } + + public Command tiltNudge(boolean direction) { + double amount = Units.degreesToRotations(0.4); + return Commands.run( + () -> { + if (direction) { + if (DriverStation.isTest()) { + setpoint = setpoint + amount; + } else { + setpoint = MathUtil.clamp(setpoint + amount, 0, 0.4); + } + } else { + if (DriverStation.isTest()) { + setpoint = setpoint - amount; + } else { + setpoint = MathUtil.clamp(setpoint - amount, 0, 0.4); + } + } + }); + } + + public Command tiltJoy(DoubleSupplier joy) { + return Commands.run( + () -> { + double joyVal = MathUtil.applyDeadband(joy.getAsDouble(), 0.1); + double adjustment = Units.degreesToRotations(joyVal); + + if (DriverStation.isTest()) { + setpoint = setpoint + adjustment; + } else { + setpoint = MathUtil.clamp(setpoint + adjustment, 0, 0.4); + } + }); + } + + public Command zeroEncoder() { + return this.runOnce( + () -> { + canTiltEncoder.setPosition(0); + setpoint = 0; + }); + } + + public Command tiltSysIDCommand(double quasiTimeout, double timeout, double dynamicTimeout) { + return m_sysIdRoutine + .quasistatic(SysIdRoutine.Direction.kForward) + .withTimeout(quasiTimeout) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout)) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) + .andThen(Commands.waitSeconds(timeout)) + .andThen( + m_sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)); + } + + private final SysIdRoutine m_sysIdRoutine = + new SysIdRoutine( + // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + new SysIdRoutine.Config(null, Voltage.ofBaseUnits(5, Volts), null, null), + new SysIdRoutine.Mechanism( + // Tell SysId how to plumb the driving voltage to the motors. + (voltage) -> { + canTiltMax.setVoltage(voltage); + }, + // Tell SysId how to record a frame of data for each motor on the mechanism + // being + // characterized. + (log) -> { + log.motor("tilt") + .voltage( + m_appliedVoltage.mut_replace( + canTiltMax.getBusVoltage() * canTiltMax.getAppliedOutput(), Volts)) + .angularPosition( + m_rotation.mut_replace(canTiltEncoder.getPosition(), Rotations)) + .angularVelocity( + m_velocity.mut_replace(canTiltEncoder.getVelocity(), RotationsPerSecond)); + }, + // Tell SysId to make generated commands require this subsystem, suffix test + // state in + // WPILog with this subsystem's name ("drive") + this)); } diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystem.java b/src/main/java/frc/robot/subsystems/TargetingSubsystem.java index 6d2a2c1..fb8b4d8 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystem.java @@ -1,7 +1,5 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Inches; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -40,245 +38,260 @@ */ public class TargetingSubsystem extends SubsystemBase { - private ReefBranch targetBranch; - private Side targetSide; - - private List reefBranches = null; - private List allianceRelativeReefBranches = null; - private Map reefPoseToBranchMap = null; - - private void initializeBranchPoses() { - reefBranches = new ArrayList<>(); - reefPoseToBranchMap = new HashMap<>(); - for (int branchPositionIndex = 0; branchPositionIndex < Reef.branchPositions.size(); branchPositionIndex++) { - Map branchPosition = Reef.branchPositions.get(branchPositionIndex); - Pose2d targetPose = branchPosition.get(ReefHeight.L4).toPose2d(); - reefBranches.add(targetPose); - reefPoseToBranchMap.put(targetPose, ReefBranch.values()[branchPositionIndex]); - reefPoseToBranchMap.put(AllianceFlipUtil.flip(targetPose), ReefBranch.values()[branchPositionIndex]); - } - allianceRelativeReefBranches = reefBranches.stream().map(AllianceFlipUtil::apply).collect(Collectors.toList()); - } - - public TargetingSubsystem() { - new Trigger(() -> DriverStation.getAlliance().isPresent()).toggleOnTrue( - Commands.runOnce(this::initializeBranchPoses) - ); - } - - public Command setBranchCommand(ReefBranch branch) { - return Commands.runOnce(() -> { - targetBranch = branch; - }); - } - - public ReefBranch getTargetBranch() { - return targetBranch; - } - - public Command driveToCoralTarget(SwerveSubsystem swerveDrive) { - return driveToCoralTarget(swerveDrive, 0.6, 1); - } - - public Command driveToCoralTarget(SwerveSubsystem swerveDrive, double speed, double acceleration) { - return Commands.print("GOING TO POSE") - .andThen( - Commands.runOnce(() -> { - System.out.println("drivetoCoralTarget"); - swerveDrive.getSwerveDrive().field.getObject("target").setPose(getCoralTargetPose()); - }) - ) - .andThen(swerveDrive.driveToPose(this::getCoralTargetPose, speed, acceleration)) - .andThen(Commands.print("DONE GOING TO POSE")); - } - - public Pose2d getCoralTargetPose() { - System.out.println("getCoarlTargetPose"); - Pose2d scoringPose = Pose2d.kZero; - if (targetBranch != null) { - Pose2d startingPose = Reef.branchPositions.get(targetBranch.ordinal()).get(ReefHeight.L2).toPose2d(); - SmartDashboard.putString("Targetted Coral Pose without Offset (Meters)", startingPose.toString()); - scoringPose = startingPose.plus( - targetSide == Side.LEFT ? AutoScoring.Reef.coralOffsetL : AutoScoring.Reef.coralOffsetR - ); - scoringPose = scoringPose.plus(getBranchSpecificOffset(targetBranch)); - SmartDashboard.putString("Targetted Coral Pose with Offset (Meters)", scoringPose.toString()); - } else { - System.out.println("targetBranch = null"); - } - return AllianceFlipUtil.apply(scoringPose); - } - - public Pose2d autoTarget(Supplier currentPose) { - if (reefBranches == null) { - initializeBranchPoses(); - } - System.out.println("autoTarget"); - - Pose2d selectedTargetPose = currentPose.get().nearest(allianceRelativeReefBranches); - targetBranch = reefPoseToBranchMap.get(selectedTargetPose); - targetSide = (targetBranch.ordinal() % 2 == 0) ? Side.LEFT : Side.RIGHT; - return selectedTargetPose; - } - - public Command autoTargetCommand(Supplier currentPose) { - return Commands.runOnce(() -> { - autoTarget(currentPose); - System.out.println("Auto-targetting complete - Selected: " + targetBranch.name()); - }); - } - - /* ----------- - * Pair auto targeting - * --- - * getReefFromPairAndSide: gets the reef branch from the pair and side - */ - - private ReefBranch getReefFromPairAndSide(ReefBranch pairBase, Side side) { - int ordinal = pairBase.ordinal(); - // If it's an even number, it's the left side (A, C, E, etc.) - // If it's an odd number, it's the right side (B, D, F, etc.) - if (side == Side.LEFT && (ordinal % 2 == 1)) { - return ReefBranch.values()[ordinal - 1]; - } else if (side == Side.RIGHT && (ordinal % 2 == 0)) { - return ReefBranch.values()[ordinal + 1]; - } - return pairBase; - } - - public Pose2d autoTargetPair(Supplier currentPose, Side preferredSide) { - if (reefBranches == null) { - initializeBranchPoses(); - } - System.out.println("autoTaregtPair"); - - // Find the closest reef position - Pose2d selectedTargetPose = currentPose.get().nearest(allianceRelativeReefBranches); - ReefBranch closestBranch = reefPoseToBranchMap.get(selectedTargetPose); - - // Get the base branch of the pair (always the left one) - ReefBranch pairBase = ReefBranch.values()[closestBranch.ordinal() - (closestBranch.ordinal() % 2)]; - - // Set the target branch based on the preferred side - targetBranch = getReefFromPairAndSide(pairBase, preferredSide); - targetSide = preferredSide; - - // Return the pose for the selected branch - return allianceRelativeReefBranches.get(targetBranch.ordinal()); - } - - public Command autoTargetPairCommand(Supplier currentPose, Side preferredSide) { - return Commands.runOnce(() -> { - autoTargetPair(currentPose, preferredSide); - System.out.println("Auto-targetting pair complete - Selected: " + targetBranch.name()); - }); - } - - /* ----------- - * areWeAllowedToDrive - * --- - * areWeAllowedToDrive: checks if we are allowed to drive - */ - - public boolean areWeAllowedToDrive(Supplier currentPose) { - if (targetBranch == null) { - return false; - } - - Pose2d currentLocation = currentPose.get(); - Pose2d targetLocation = allianceRelativeReefBranches.get(targetBranch.ordinal()); - double distance = currentLocation.getTranslation().getDistance(targetLocation.getTranslation()); - - System.out.println(distance); - - return distance < 2.5; - } - - /* ----------- - * Source Commands - * --- - * driveToSourceCommand: drives to the source - */ - - public Command driveToSourceCommand(SwerveSubsystem swerveDrive) { - return defer(() -> { - Pose2d currentPose = swerveDrive.getPose(); - Pose2d sourcePose = currentPose - .nearest(FieldConstants.CoralStation.bothPoses) - .plus( - new Transform2d( - // positive is backwards; positive is right - new Translation2d(Units.inchesToMeters(12), Units.inchesToMeters(4)), - new Rotation2d(Units.degreesToRadians(180)) - ) - ); - - return Commands.print("GOING TO SOURCE") - .andThen( - Commands.runOnce(() -> { - swerveDrive.getSwerveDrive().field.getObject("target").setPose(sourcePose); - }) - ) - .andThen(swerveDrive.driveToPose(() -> sourcePose, 0.6, 1)) - .andThen(Commands.print("DONE GOING TO SOURCE")); - }); - } - - public Command driveToLeftBranch(SwerveSubsystem swerve) { - return autoTargetPairCommand(swerve::getPose, Side.LEFT) - .andThen(driveToCoralTarget(swerve)) - .andThen(Commands.print("ened the auto routine thing")); - } - - public Command driveToRightBranch(SwerveSubsystem swerve) { - return autoTargetPairCommand(swerve::getPose, Side.RIGHT) - .andThen(driveToCoralTarget(swerve)) - .andThen(Commands.print("ened the auto routine thing")); - } - - public Transform2d getBranchSpecificOffset(ReefBranch branch) { - double x = 0; // in inches; positive is robot forward - double y = 0; // in inches; positive is left - double rot = 0; // in degrees; positive is counter clockwise - - switch (branch) { - case G: - y = 0; - case I: - y = -4; - x = -4; - rot = -5; - break; - default: - x = 0; - y = 0; - rot = 0; - break; - } - - return new Transform2d( - new Translation2d(Units.inchesToMeters(x), Units.inchesToMeters(y)), - new Rotation2d(Units.degreesToRadians(rot)) - ); - } - - public enum Side { - LEFT, - RIGHT - } - - public enum ReefBranch { - A, - B, - K, - L, - I, - J, - G, - H, - E, - F, - C, - D - } + private ReefBranch targetBranch; + private Side targetSide; + + private List reefBranches = null; + private List allianceRelativeReefBranches = null; + private Map reefPoseToBranchMap = null; + + private void initializeBranchPoses() { + reefBranches = new ArrayList<>(); + reefPoseToBranchMap = new HashMap<>(); + for (int branchPositionIndex = 0; + branchPositionIndex < Reef.branchPositions.size(); + branchPositionIndex++) { + Map branchPosition = Reef.branchPositions.get(branchPositionIndex); + Pose2d targetPose = branchPosition.get(ReefHeight.L4).toPose2d(); + reefBranches.add(targetPose); + reefPoseToBranchMap.put(targetPose, ReefBranch.values()[branchPositionIndex]); + reefPoseToBranchMap.put( + AllianceFlipUtil.flip(targetPose), ReefBranch.values()[branchPositionIndex]); + } + allianceRelativeReefBranches = + reefBranches.stream().map(AllianceFlipUtil::apply).collect(Collectors.toList()); + } + + public TargetingSubsystem() { + new Trigger(() -> DriverStation.getAlliance().isPresent()) + .toggleOnTrue(Commands.runOnce(this::initializeBranchPoses)); + } + + public Command setBranchCommand(ReefBranch branch) { + return Commands.runOnce( + () -> { + targetBranch = branch; + }); + } + + public ReefBranch getTargetBranch() { + return targetBranch; + } + + public Command driveToCoralTarget(SwerveSubsystem swerveDrive) { + return driveToCoralTarget(swerveDrive, 0.6, 1); + } + + public Command driveToCoralTarget( + SwerveSubsystem swerveDrive, double speed, double acceleration) { + return Commands.print("GOING TO POSE") + .andThen( + Commands.runOnce( + () -> { + System.out.println("drivetoCoralTarget"); + swerveDrive + .getSwerveDrive() + .field + .getObject("target") + .setPose(getCoralTargetPose()); + })) + .andThen(swerveDrive.driveToPose(this::getCoralTargetPose, speed, acceleration)) + .andThen(Commands.print("DONE GOING TO POSE")); + } + + public Pose2d getCoralTargetPose() { + System.out.println("getCoarlTargetPose"); + Pose2d scoringPose = Pose2d.kZero; + if (targetBranch != null) { + Pose2d startingPose = + Reef.branchPositions.get(targetBranch.ordinal()).get(ReefHeight.L2).toPose2d(); + SmartDashboard.putString( + "Targetted Coral Pose without Offset (Meters)", startingPose.toString()); + scoringPose = + startingPose.plus( + targetSide == Side.LEFT + ? AutoScoring.Reef.coralOffsetL + : AutoScoring.Reef.coralOffsetR); + scoringPose = scoringPose.plus(getBranchSpecificOffset(targetBranch)); + SmartDashboard.putString("Targetted Coral Pose with Offset (Meters)", scoringPose.toString()); + } else { + System.out.println("targetBranch = null"); + } + return AllianceFlipUtil.apply(scoringPose); + } + + public Pose2d autoTarget(Supplier currentPose) { + if (reefBranches == null) { + initializeBranchPoses(); + } + System.out.println("autoTarget"); + + Pose2d selectedTargetPose = currentPose.get().nearest(allianceRelativeReefBranches); + targetBranch = reefPoseToBranchMap.get(selectedTargetPose); + targetSide = (targetBranch.ordinal() % 2 == 0) ? Side.LEFT : Side.RIGHT; + return selectedTargetPose; + } + + public Command autoTargetCommand(Supplier currentPose) { + return Commands.runOnce( + () -> { + autoTarget(currentPose); + System.out.println("Auto-targetting complete - Selected: " + targetBranch.name()); + }); + } + + /* ----------- + * Pair auto targeting + * --- + * getReefFromPairAndSide: gets the reef branch from the pair and side + */ + + private ReefBranch getReefFromPairAndSide(ReefBranch pairBase, Side side) { + int ordinal = pairBase.ordinal(); + // If it's an even number, it's the left side (A, C, E, etc.) + // If it's an odd number, it's the right side (B, D, F, etc.) + if (side == Side.LEFT && (ordinal % 2 == 1)) { + return ReefBranch.values()[ordinal - 1]; + } else if (side == Side.RIGHT && (ordinal % 2 == 0)) { + return ReefBranch.values()[ordinal + 1]; + } + return pairBase; + } + + public Pose2d autoTargetPair(Supplier currentPose, Side preferredSide) { + if (reefBranches == null) { + initializeBranchPoses(); + } + System.out.println("autoTaregtPair"); + + // Find the closest reef position + Pose2d selectedTargetPose = currentPose.get().nearest(allianceRelativeReefBranches); + ReefBranch closestBranch = reefPoseToBranchMap.get(selectedTargetPose); + + // Get the base branch of the pair (always the left one) + ReefBranch pairBase = + ReefBranch.values()[closestBranch.ordinal() - (closestBranch.ordinal() % 2)]; + + // Set the target branch based on the preferred side + targetBranch = getReefFromPairAndSide(pairBase, preferredSide); + targetSide = preferredSide; + + // Return the pose for the selected branch + return allianceRelativeReefBranches.get(targetBranch.ordinal()); + } + + public Command autoTargetPairCommand(Supplier currentPose, Side preferredSide) { + return Commands.runOnce( + () -> { + autoTargetPair(currentPose, preferredSide); + System.out.println("Auto-targetting pair complete - Selected: " + targetBranch.name()); + }); + } + + /* ----------- + * areWeAllowedToDrive + * --- + * areWeAllowedToDrive: checks if we are allowed to drive + */ + + public boolean areWeAllowedToDrive(Supplier currentPose) { + if (targetBranch == null) { + return false; + } + + Pose2d currentLocation = currentPose.get(); + Pose2d targetLocation = allianceRelativeReefBranches.get(targetBranch.ordinal()); + double distance = currentLocation.getTranslation().getDistance(targetLocation.getTranslation()); + + System.out.println(distance); + + return distance < 2.5; + } + + /* ----------- + * Source Commands + * --- + * driveToSourceCommand: drives to the source + */ + + public Command driveToSourceCommand(SwerveSubsystem swerveDrive) { + return defer( + () -> { + Pose2d currentPose = swerveDrive.getPose(); + Pose2d sourcePose = + currentPose + .nearest(FieldConstants.CoralStation.bothPoses) + .plus( + new Transform2d( + // positive is backwards; positive is right + new Translation2d(Units.inchesToMeters(12), Units.inchesToMeters(4)), + new Rotation2d(Units.degreesToRadians(180)))); + + return Commands.print("GOING TO SOURCE") + .andThen( + Commands.runOnce( + () -> { + swerveDrive.getSwerveDrive().field.getObject("target").setPose(sourcePose); + })) + .andThen(swerveDrive.driveToPose(() -> sourcePose, 0.6, 1)) + .andThen(Commands.print("DONE GOING TO SOURCE")); + }); + } + + public Command driveToLeftBranch(SwerveSubsystem swerve) { + return autoTargetPairCommand(swerve::getPose, Side.LEFT) + .andThen(driveToCoralTarget(swerve)) + .andThen(Commands.print("ened the auto routine thing")); + } + + public Command driveToRightBranch(SwerveSubsystem swerve) { + return autoTargetPairCommand(swerve::getPose, Side.RIGHT) + .andThen(driveToCoralTarget(swerve)) + .andThen(Commands.print("ened the auto routine thing")); + } + + public Transform2d getBranchSpecificOffset(ReefBranch branch) { + double x = 0; // in inches; positive is robot forward + double y = 0; // in inches; positive is left + double rot = 0; // in degrees; positive is counter clockwise + + switch (branch) { + case G: + y = 0; + case I: + y = -4; + x = -4; + rot = -5; + break; + default: + x = 0; + y = 0; + rot = 0; + break; + } + + return new Transform2d( + new Translation2d(Units.inchesToMeters(x), Units.inchesToMeters(y)), + new Rotation2d(Units.degreesToRadians(rot))); + } + + public enum Side { + LEFT, + RIGHT + } + + public enum ReefBranch { + A, + B, + K, + L, + I, + J, + G, + H, + E, + F, + C, + D + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index de41d7b..e59d211 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -64,783 +64,749 @@ public class SwerveSubsystem extends SubsystemBase { - /** - * Swerve drive object. - */ - private final SwerveDrive swerveDrive; - /** - * Enable vision odometry updates while driving. - */ - private boolean visionDriveTest = true; - /** - * PhotonVision class to keep an accurate odometry. - */ - private Vision vision; - - // Pose2d startPose; - // double initDis; - - /** - * Initialize {@link SwerveDrive} with the directory provided. - * - * @param directory Directory of swerve drive config files. - */ - public SwerveSubsystem(File directory) { - // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary - // objects being created. - SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; - try { - swerveDrive = new SwerveParser(directory).createSwerveDrive( - MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)) - ); - // Alternative method if you don't want to supply the conversion factor via JSON - // files. - // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, - // angleConversionFactor, driveConversionFactor); - } catch (Exception e) { - throw new RuntimeException(e); - } - this.replaceSwerveModuleFeedforward(0.024309, 2.7435, 2.0788); - - swerveDrive.setMaximumAllowableSpeeds(MAX_SPEED, MAX_ANGULAR_SPEED); - - swerveDrive.swerveController.addSlewRateLimiters( - new SlewRateLimiter(MAX_ACCELERATION), - new SlewRateLimiter(MAX_ACCELERATION), - new SlewRateLimiter(MAX_ANGULAR_ACCELERATION) - ); - - swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via - // angle. - swerveDrive.setCosineCompensator(true); // !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for - // simulations since it causes discrepancies not seen in real life. - swerveDrive.setAngularVelocityCompensation(true, true, 0.1); // Correct for skew that gets worse as angular velocity - // increases. Start with a - // coefficient of 0.1. - swerveDrive.setModuleEncoderAutoSynchronize(false, 1); // Enable if you want to resynchronize your absolute encoders - // anti jitter - Arrays.stream(swerveDrive.getModules()).forEach(m -> m.setAntiJitter(true)); - // and motor encoders - // periodically when they are not moving. - // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used - // over the internal encoder and push the offsets onto it. Throws warning if not - // possible - SmartDashboard.putBoolean("Swerve Drive/vision enabled", visionDriveTest); - if (visionDriveTest) { - setupPhotonVision(); - // Stop the odometry thread if we are using vision that way we can synchronize - // updates better. - swerveDrive.stopOdometryThread(); - } - setupPathPlanner(); - // Epilogue.bind(this); - } - - /** - * Construct the swerve drive. - * - * @param driveCfg SwerveDriveConfiguration for the swerve. - * @param controllerCfg Swerve Controller. - */ - public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { - swerveDrive = new SwerveDrive( - driveCfg, - controllerCfg, - Constants.MAX_SPEED, - new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0)) - ); - // Epilogue.bind(this); - } - - /** - * Setup the photon vision class. - */ - public void setupPhotonVision() { - vision = new Vision(swerveDrive::getPose, swerveDrive.field); - } - - @Override - public void periodic() { - visionDriveTest = SmartDashboard.getBoolean("Swerve Drive/vision enabled", visionDriveTest); - - // When vision is enabled we must manually update odometry in SwerveDrive - swerveDrive.updateOdometry(); - if (visionDriveTest) { - vision.updatePoseEstimation(swerveDrive); - } - } - - @Override - public void simulationPeriodic() {} - - /** - * Setup AutoBuilder for PathPlanner. - */ - public void setupPathPlanner() { - // Load the RobotConfig from the GUI settings. You should probably - // store this in your Constants file - RobotConfig config; - try { - config = RobotConfig.fromGUISettings(); - - final boolean enableFeedforward = true; - // Configure AutoBuilder last - AutoBuilder.configure( - this::getPose, - // Robot pose supplier - this::resetOdometry, - // Method to reset odometry (will be called if your auto has a starting pose) - this::getRobotVelocity, - // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speedsRobotRelative, moduleFeedForwards) -> { - if (enableFeedforward) { - swerveDrive.drive( - speedsRobotRelative, - swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), - moduleFeedForwards.linearForces() - ); - } else { - swerveDrive.setChassisSpeeds(speedsRobotRelative); - } - }, - // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also - // optionally outputs individual module feedforwards - new PPHolonomicDriveController( - // PPHolonomicController is the built in path following controller for holonomic - // drive trains - new PIDConstants(5.0, 0.0, 0.0), - // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) - // Rotation PID constants - ), - config, - // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red - // alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this - // Reference to this subsystem to set requirements - ); - } catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - } - - // Preload PathPlanner Path finding - // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE - PathfindingCommand.warmupCommand().schedule(); - } - - /** - * Aim the robot at the target returned by PhotonVision. - * - * @return A {@link Command} which will run the alignment. - */ - public Command aimAtTarget(Cameras camera) { - return run(() -> { - Optional resultO = camera.getBestResult(); - if (resultO.isPresent()) { - var result = resultO.get(); - if (result.hasTargets()) { - drive(getTargetSpeeds(0, 0, Rotation2d.fromDegrees(result.getBestTarget().getYaw()))); // Not sure if this will work, more math - } - } - }); - } - - /** - * Get the path follower with events. - * - * @param pathName PathPlanner path name. - * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. - */ - public Command getAutonomousCommand(String pathName) { - // Create a path following command using AutoBuilder. This will also trigger - // event markers. - return new PathPlannerAuto(pathName); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Supplier pose) { - return defer(() -> { - // Create the constraints to use while pathfinding - PathConstraints constraints = new PathConstraints( - 1, - 1, - swerveDrive.getMaximumChassisAngularVelocity(), - Units.degreesToRadians(720) - ); - - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose.get(), - constraints, - edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec - ); - }); - } - - /** - * Use PathPlanner Path finding to go to a point on the field. - * - * @param pose Target {@link Pose2d} to go to. - * @return PathFinding command - */ - public Command driveToPose(Supplier pose, double velocity, double acceleration) { - return defer(() -> { - // Create the constraints to use while pathfinding - PathConstraints constraints = new PathConstraints( - velocity, - acceleration, - swerveDrive.getMaximumChassisAngularVelocity(), - Units.degreesToRadians(720) - ); - - // Since AutoBuilder is configured, we can use it to build pathfinding commands - return AutoBuilder.pathfindToPose( - pose.get(), - constraints, - edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec - ); - }); - } - - /** - * Drive with {@link SwerveSetpointGenerator} from 254, implemented by - * PathPlanner. - * - * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to - * achieve. - * @return {@link Command} to run. - * @throws IOException If the PathPlanner GUI settings is invalid - * @throws ParseException If PathPlanner GUI settings is nonexistent. - */ - private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) - throws IOException, ParseException { - SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator( - RobotConfig.fromGUISettings(), - swerveDrive.getMaximumChassisAngularVelocity() - ); - AtomicReference prevSetpoint = new AtomicReference<>( - new SwerveSetpoint( - swerveDrive.getRobotVelocity(), - swerveDrive.getStates(), - DriveFeedforwards.zeros(swerveDrive.getModules().length) - ) - ); - AtomicReference previousTime = new AtomicReference<>(); - - return startRun( - () -> previousTime.set(Timer.getFPGATimestamp()), - () -> { - double newTime = Timer.getFPGATimestamp(); - SwerveSetpoint newSetpoint = setpointGenerator.generateSetpoint( - prevSetpoint.get(), - robotRelativeChassisSpeed.get(), - newTime - previousTime.get() - ); - swerveDrive.drive( - newSetpoint.robotRelativeSpeeds(), - newSetpoint.moduleStates(), - newSetpoint.feedforwards().linearForces() - ); - prevSetpoint.set(newSetpoint); - previousTime.set(newTime); - } - ); - } - - /** - * Drive with 254's Setpoint generator; port written by PathPlanner. - * - * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds} - * @return Command to drive the robot using the setpoint generator. - */ - public Command driveWithSetpointGeneratorFieldRelative(Supplier fieldRelativeSpeeds) { - try { - return driveWithSetpointGenerator(() -> { - return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); - }); - } catch (Exception e) { - DriverStation.reportError(e.toString(), true); - } - return Commands.none(); - } - - /** - * Command to characterize the robot drive motors using SysId - * - * @return SysId Drive Command - */ - public Command sysIdDriveMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setDriveSysIdRoutine( - new Config(null, Voltage.ofBaseUnits(9, Volts), null, null), - this, - swerveDrive, - 9, - false - ), - 3.0, - 5.0, - 2.0 - ); - } - - /** - * Command to characterize the robot angle motors using SysId - * - * @return SysId Angle Command - */ - public Command sysIdAngleMotorCommand() { - return SwerveDriveTest.generateSysIdCommand( - SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), - 3.0, - 5.0, - 3.0 - ); - } - - /** - * Returns a Command that centers the modules of the SwerveDrive subsystem. - * - * @return a Command that centers the modules of the SwerveDrive subsystem - */ - public Command centerModulesCommand() { - return run(() -> Arrays.asList(swerveDrive.getModules()).forEach(it -> it.setAngle(0.0))); - } - - /** - * Returns a Command that drives the swerve drive to a specific distance at a - * given speed. - * - * @param distanceInMeters the distance to drive in meters - * @param speedInMetersPerSecond the speed at which to drive in meters per - * second - * @return a Command that drives the swerve drive to a specific distance at a - * given speed - */ - - public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { - return new Command() { - private Translation2d startTranslation; - - @Override - public void initialize() { - startTranslation = swerveDrive.getPose().getTranslation(); - } - - @Override - public void execute() { - driveFieldOriented(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)); - } - - @Override - public boolean isFinished() { - return (swerveDrive.getPose().getTranslation().getDistance(startTranslation) > distanceInMeters); - } - }; - // return run(() -> { - // startPose = swerveDrive.getPose(); - // initDis = - // swerveDrive.getPose().getTranslation().getDistance(startPose.getTranslation()); - - // drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)); - // System.out.println("got here! :yay:! " + initDis); - // SmartDashboard.putNumber("StartPOse: ", initDis); - // } - - // ).until(() -> - // swerveDrive.getPose().getTranslation().getDistance(startPose.getTranslation()) - // > distanceInMeters); - } - - /** - * Replaces the swerve module feedforward with a new SimpleMotorFeedforward - * object. - * - * @param kS the static gain of the feedforward - * @param kV the velocity gain of the feedforward - * @param kA the acceleration gain of the feedforward - */ - public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { - swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); - } - - /** - * Command to drive the robot using translative values and heading as angular - * velocity. - * - * @param translationX Translation in the X direction. Cubed for smoother - * controls. - * @param translationY Translation in the Y direction. Cubed for smoother - * controls. - * @param angularRotationX Angular velocity of the robot to set. Cubed for - * smoother controls. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier angularRotationX - ) { - return run(() -> { - // Make the robot move - swerveDrive.drive( - SwerveMath.scaleTranslation( - new Translation2d( - translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), - translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity() - ), - 0.8 - ), - Math.pow(angularRotationX.getAsDouble(), 3) * swerveDrive.getMaximumChassisAngularVelocity(), - true, - false - ); - }); - } - - /** - * Command to drive the robot using translative values and heading as a - * setpoint. - * - * @param translationX Translation in the X direction. Cubed for smoother - * controls. - * @param translationY Translation in the Y direction. Cubed for smoother - * controls. - * @param headingX Heading X to calculate angle of the joystick. - * @param headingY Heading Y to calculate angle of the joystick. - * @return Drive command. - */ - public Command driveCommand( - DoubleSupplier translationX, - DoubleSupplier translationY, - DoubleSupplier headingX, - DoubleSupplier headingY - ) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading - // correction for this kind of control. - return run(() -> { - Translation2d scaledInputs = SwerveMath.scaleTranslation( - new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()), - 0.8 - ); - - // Make the robot move - driveFieldOriented( - swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - headingX.getAsDouble(), - headingY.getAsDouble(), - swerveDrive.getOdometryHeading().getRadians(), - swerveDrive.getMaximumChassisVelocity() - ) - ); - }); - } - - /** - * The primary method for controlling the drivebase. Takes a - * {@link Translation2d} and a rotation rate, and - * calculates and commands module states accordingly. Can use either open-loop - * or closed-loop velocity control for - * the wheel velocities. Also has field- and robot-relative modes, which affect - * how the translation vector is used. - * - * @param translation {@link Translation2d} that is the commanded linear - * velocity of the robot, in meters per - * second. In robot-relative mode, positive x is torwards - * the bow (front) and positive y is - * torwards port (left). In field-relative mode, positive x - * is away from the alliance wall - * (field North) and positive y is torwards the left wall - * when looking through the driver station - * glass (field West). - * @param rotation Robot angular rate, in radians per second. CCW positive. - * Unaffected by field/robot - * relativity. - * @param fieldRelative Drive mode. True for field-relative, false for - * robot-relative. - */ - public void drive(Translation2d translation, double rotation, boolean fieldRelative) { - swerveDrive.drive(translation, rotation, fieldRelative, false); // Open loop is disabled since it shouldn't be used - // most of the time. - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public void driveFieldOriented(ChassisSpeeds velocity) { - swerveDrive.driveFieldOriented(velocity); - } - - /** - * Drive the robot given a chassis field oriented velocity. - * - * @param velocity Velocity according to the field. - */ - public Command driveFieldOriented(Supplier velocity) { - return run(() -> { - swerveDrive.driveFieldOriented(velocity.get()); - }); - } - - public Command robotDriveCommand(Supplier velocity, BooleanSupplier robotRelative) { - return run(() -> { - if (robotRelative.getAsBoolean()) { - swerveDrive.driveFieldOrientedAndRobotOriented(new ChassisSpeeds(0, 0, 0), velocity.get()); - } else { - swerveDrive.driveFieldOriented(velocity.get()); - } - }); - } - - /** - * Drive according to the chassis robot oriented velocity. - * - * @param velocity Robot oriented {@link ChassisSpeeds} - */ - public void drive(ChassisSpeeds velocity) { - swerveDrive.drive(velocity); - } - - /** - * Get the swerve drive kinematics object. - * - * @return {@link SwerveDriveKinematics} of the swerve drive. - */ - @NotLogged - public SwerveDriveKinematics getKinematics() { - return swerveDrive.kinematics; - } - - /** - * Resets odometry to the given pose. Gyro angle and module positions do not - * need to be reset when calling this - * method. However, if either gyro angle or module position is reset, this must - * be called in order for odometry to - * keep working. - * - * @param initialHolonomicPose The pose to set the odometry to - */ - public void resetOdometry(Pose2d initialHolonomicPose) { - swerveDrive.resetOdometry(initialHolonomicPose); - } - - /** - * Gets the current pose (position and rotation) of the robot, as reported by - * odometry. - * - * @return The robot's pose - */ - @NotLogged - public Pose2d getPose() { - return swerveDrive.getPose(); - } - - /** - * Set chassis speeds with closed-loop velocity control. - * - * @param chassisSpeeds Chassis Speeds to set. - */ - public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - swerveDrive.setChassisSpeeds(chassisSpeeds); - } - - /** - * Post the trajectory to the field. - * - * @param trajectory The trajectory to post. - */ - public void postTrajectory(Trajectory trajectory) { - swerveDrive.postTrajectory(trajectory); - } - - /** - * Resets the gyro angle to zero and resets odometry to the same position, but - * facing toward 0. - */ - public void zeroGyro() { - swerveDrive.zeroGyro(); - } - - /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * - * @return true if the red alliance, false if blue. Defaults to false if none is - * available. - */ - private boolean isRedAlliance() { - var alliance = DriverStation.getAlliance(); - return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; - } - - /** - * This will zero (calibrate) the robot to assume the current position is facing - * forward - *

- * If red alliance rotate the robot 180 after the drviebase zero command - */ - public void zeroGyroWithAlliance() { - if (isRedAlliance()) { - zeroGyro(); - // Set the pose 180 degrees - resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else { - zeroGyro(); - } - } - - /** - * Sets the drive motors to brake/coast mode. - * - * @param brake True to set motors to brake mode, false for coast. - */ - public void setMotorBrake(boolean brake) { - swerveDrive.setMotorIdleMode(brake); - } - - /** - * Gets the current yaw angle of the robot, as reported by the swerve pose - * estimator in the underlying drivebase. - * Note, this is not the raw gyro reading, this may be corrected from calls to - * resetOdometry(). - * - * @return The yaw angle - */ - public Rotation2d getHeading() { - return getPose().getRotation(); - } - - /** - * Get the chassis speeds based on controller input of 2 joysticks. One for - * speeds in which direction. The other for - * the angle of the robot. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param headingX X joystick which controls the angle of the robot. - * @param headingY Y joystick which controls the angle of the robot. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - return swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - headingX, - headingY, - getHeading().getRadians(), - Constants.MAX_SPEED - ); - } - - /** - * Get the chassis speeds based on controller input of 1 joystick and one angle. - * Control the robot at an offset of - * 90deg. - * - * @param xInput X joystick input for the robot to move in the X direction. - * @param yInput Y joystick input for the robot to move in the Y direction. - * @param angle The angle in as a {@link Rotation2d}. - * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. - */ - public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { - Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); - - return swerveDrive.swerveController.getTargetSpeeds( - scaledInputs.getX(), - scaledInputs.getY(), - angle.getRadians(), - getHeading().getRadians(), - Constants.MAX_SPEED - ); - } - - /** - * Gets the current field-relative velocity (x, y and omega) of the robot - * - * @return A ChassisSpeeds object of the current field-relative velocity - */ - public ChassisSpeeds getFieldVelocity() { - return swerveDrive.getFieldVelocity(); - } - - /** - * Gets the current velocity (x, y and omega) of the robot - * - * @return A {@link ChassisSpeeds} object of the current velocity - */ - public ChassisSpeeds getRobotVelocity() { - return swerveDrive.getRobotVelocity(); - } - - /** - * Get the {@link SwerveController} in the swerve drive. - * - * @return {@link SwerveController} from the {@link SwerveDrive}. - */ - public SwerveController getSwerveController() { - return swerveDrive.swerveController; - } - - /** - * Get the {@link SwerveDriveConfiguration} object. - * - * @return The {@link SwerveDriveConfiguration} fpr the current drive. - */ - public SwerveDriveConfiguration getSwerveDriveConfiguration() { - return swerveDrive.swerveDriveConfiguration; - } - - /** - * Lock the swerve drive to prevent it from moving. - */ - public void lock() { - swerveDrive.lockPose(); - } - - /** - * Gets the current pitch angle of the robot, as reported by the imu. - * - * @return The heading as a {@link Rotation2d} angle - */ - public Rotation2d getPitch() { - return swerveDrive.getPitch(); - } - - /** - * Add a fake vision reading for testing purposes. - */ - public void addFakeVisionReading() { - swerveDrive.addVisionMeasurement(new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); - } - - /** - * Gets the swerve drive object. - * - * @return {@link SwerveDrive} - */ - public SwerveDrive getSwerveDrive() { - return swerveDrive; - } + /** Swerve drive object. */ + private final SwerveDrive swerveDrive; + + /** Enable vision odometry updates while driving. */ + private boolean visionDriveTest = true; + + /** PhotonVision class to keep an accurate odometry. */ + private Vision vision; + + // Pose2d startPose; + // double initDis; + + /** + * Initialize {@link SwerveDrive} with the directory provided. + * + * @param directory Directory of swerve drive config files. + */ + public SwerveSubsystem(File directory) { + // Configure the Telemetry before creating the SwerveDrive to avoid unnecessary + // objects being created. + SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH; + try { + swerveDrive = + new SwerveParser(directory) + .createSwerveDrive( + MAX_SPEED, + new Pose2d( + new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0))); + // Alternative method if you don't want to supply the conversion factor via JSON + // files. + // swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed, + // angleConversionFactor, driveConversionFactor); + } catch (Exception e) { + throw new RuntimeException(e); + } + this.replaceSwerveModuleFeedforward(0.024309, 2.7435, 2.0788); + + swerveDrive.setMaximumAllowableSpeeds(MAX_SPEED, MAX_ANGULAR_SPEED); + + swerveDrive.swerveController.addSlewRateLimiters( + new SlewRateLimiter(MAX_ACCELERATION), + new SlewRateLimiter(MAX_ACCELERATION), + new SlewRateLimiter(MAX_ANGULAR_ACCELERATION)); + + swerveDrive.setHeadingCorrection( + true); // Heading correction should only be used while controlling the robot via + // angle. + swerveDrive.setCosineCompensator( + true); // !SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for + // simulations since it causes discrepancies not seen in real life. + swerveDrive.setAngularVelocityCompensation( + true, true, 0.1); // Correct for skew that gets worse as angular velocity + // increases. Start with a + // coefficient of 0.1. + swerveDrive.setModuleEncoderAutoSynchronize( + false, 1); // Enable if you want to resynchronize your absolute encoders + // anti jitter + Arrays.stream(swerveDrive.getModules()).forEach((m) -> m.setAntiJitter(true)); + // and motor encoders + // periodically when they are not moving. + // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used + // over the internal encoder and push the offsets onto it. Throws warning if not + // possible + SmartDashboard.putBoolean("Swerve Drive/vision enabled", visionDriveTest); + if (visionDriveTest) { + setupPhotonVision(); + // Stop the odometry thread if we are using vision that way we can synchronize + // updates better. + swerveDrive.stopOdometryThread(); + } + setupPathPlanner(); + // Epilogue.bind(this); + } + + /** + * Construct the swerve drive. + * + * @param driveCfg SwerveDriveConfiguration for the swerve. + * @param controllerCfg Swerve Controller. + */ + public SwerveSubsystem( + SwerveDriveConfiguration driveCfg, SwerveControllerConfiguration controllerCfg) { + swerveDrive = + new SwerveDrive( + driveCfg, + controllerCfg, + Constants.MAX_SPEED, + new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0))); + // Epilogue.bind(this); + } + + /** Setup the photon vision class. */ + public void setupPhotonVision() { + vision = new Vision(swerveDrive::getPose, swerveDrive.field); + } + + @Override + public void periodic() { + visionDriveTest = SmartDashboard.getBoolean("Swerve Drive/vision enabled", visionDriveTest); + + // When vision is enabled we must manually update odometry in SwerveDrive + swerveDrive.updateOdometry(); + if (visionDriveTest) { + vision.updatePoseEstimation(swerveDrive); + } + } + + @Override + public void simulationPeriodic() {} + + /** Setup AutoBuilder for PathPlanner. */ + public void setupPathPlanner() { + // Load the RobotConfig from the GUI settings. You should probably + // store this in your Constants file + RobotConfig config; + try { + config = RobotConfig.fromGUISettings(); + + final boolean enableFeedforward = true; + // Configure AutoBuilder last + AutoBuilder.configure( + this::getPose, + // Robot pose supplier + this::resetOdometry, + // Method to reset odometry (will be called if your auto has a starting pose) + this::getRobotVelocity, + // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speedsRobotRelative, moduleFeedForwards) -> { + if (enableFeedforward) { + swerveDrive.drive( + speedsRobotRelative, + swerveDrive.kinematics.toSwerveModuleStates(speedsRobotRelative), + moduleFeedForwards.linearForces()); + } else { + swerveDrive.setChassisSpeeds(speedsRobotRelative); + } + }, + // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also + // optionally outputs individual module feedforwards + new PPHolonomicDriveController( + // PPHolonomicController is the built in path following controller for holonomic + // drive trains + new PIDConstants(5.0, 0.0, 0.0), + // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) + // Rotation PID constants + ), + config, + // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red + // alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this + // Reference to this subsystem to set requirements + ); + } catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + } + + // Preload PathPlanner Path finding + // IF USING CUSTOM PATHFINDER ADD BEFORE THIS LINE + PathfindingCommand.warmupCommand().schedule(); + } + + /** + * Aim the robot at the target returned by PhotonVision. + * + * @return A {@link Command} which will run the alignment. + */ + public Command aimAtTarget(Cameras camera) { + return run( + () -> { + Optional resultO = camera.getBestResult(); + if (resultO.isPresent()) { + var result = resultO.get(); + if (result.hasTargets()) { + drive( + getTargetSpeeds( + 0, + 0, + Rotation2d.fromDegrees( + result + .getBestTarget() + .getYaw()))); // Not sure if this will work, more math + } + } + }); + } + + /** + * Get the path follower with events. + * + * @param pathName PathPlanner path name. + * @return {@link AutoBuilder#followPath(PathPlannerPath)} path command. + */ + public Command getAutonomousCommand(String pathName) { + // Create a path following command using AutoBuilder. This will also trigger + // event markers. + return new PathPlannerAuto(pathName); + } + + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Supplier pose) { + return defer( + () -> { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + 1, + 1, + swerveDrive.getMaximumChassisAngularVelocity(), + Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose.get(), + constraints, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + }); + } + + /** + * Use PathPlanner Path finding to go to a point on the field. + * + * @param pose Target {@link Pose2d} to go to. + * @return PathFinding command + */ + public Command driveToPose(Supplier pose, double velocity, double acceleration) { + return defer( + () -> { + // Create the constraints to use while pathfinding + PathConstraints constraints = + new PathConstraints( + velocity, + acceleration, + swerveDrive.getMaximumChassisAngularVelocity(), + Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + pose.get(), + constraints, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + }); + } + + /** + * Drive with {@link SwerveSetpointGenerator} from 254, implemented by PathPlanner. + * + * @param robotRelativeChassisSpeed Robot relative {@link ChassisSpeeds} to achieve. + * @return {@link Command} to run. + * @throws IOException If the PathPlanner GUI settings is invalid + * @throws ParseException If PathPlanner GUI settings is nonexistent. + */ + private Command driveWithSetpointGenerator(Supplier robotRelativeChassisSpeed) + throws IOException, ParseException { + SwerveSetpointGenerator setpointGenerator = + new SwerveSetpointGenerator( + RobotConfig.fromGUISettings(), swerveDrive.getMaximumChassisAngularVelocity()); + AtomicReference prevSetpoint = + new AtomicReference<>( + new SwerveSetpoint( + swerveDrive.getRobotVelocity(), + swerveDrive.getStates(), + DriveFeedforwards.zeros(swerveDrive.getModules().length))); + AtomicReference previousTime = new AtomicReference<>(); + + return startRun( + () -> previousTime.set(Timer.getFPGATimestamp()), + () -> { + double newTime = Timer.getFPGATimestamp(); + SwerveSetpoint newSetpoint = + setpointGenerator.generateSetpoint( + prevSetpoint.get(), + robotRelativeChassisSpeed.get(), + newTime - previousTime.get()); + swerveDrive.drive( + newSetpoint.robotRelativeSpeeds(), + newSetpoint.moduleStates(), + newSetpoint.feedforwards().linearForces()); + prevSetpoint.set(newSetpoint); + previousTime.set(newTime); + }); + } + + /** + * Drive with 254's Setpoint generator; port written by PathPlanner. + * + * @param fieldRelativeSpeeds Field-Relative {@link ChassisSpeeds} + * @return Command to drive the robot using the setpoint generator. + */ + public Command driveWithSetpointGeneratorFieldRelative( + Supplier fieldRelativeSpeeds) { + try { + return driveWithSetpointGenerator( + () -> { + return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds.get(), getHeading()); + }); + } catch (Exception e) { + DriverStation.reportError(e.toString(), true); + } + return Commands.none(); + } + + /** + * Command to characterize the robot drive motors using SysId + * + * @return SysId Drive Command + */ + public Command sysIdDriveMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setDriveSysIdRoutine( + new Config(null, Voltage.ofBaseUnits(9, Volts), null, null), + this, + swerveDrive, + 9, + false), + 3.0, + 5.0, + 2.0); + } + + /** + * Command to characterize the robot angle motors using SysId + * + * @return SysId Angle Command + */ + public Command sysIdAngleMotorCommand() { + return SwerveDriveTest.generateSysIdCommand( + SwerveDriveTest.setAngleSysIdRoutine(new Config(), this, swerveDrive), 3.0, 5.0, 3.0); + } + + /** + * Returns a Command that centers the modules of the SwerveDrive subsystem. + * + * @return a Command that centers the modules of the SwerveDrive subsystem + */ + public Command centerModulesCommand() { + return run(() -> Arrays.asList(swerveDrive.getModules()).forEach((it) -> it.setAngle(0.0))); + } + + /** + * Returns a Command that drives the swerve drive to a specific distance at a given speed. + * + * @param distanceInMeters the distance to drive in meters + * @param speedInMetersPerSecond the speed at which to drive in meters per second + * @return a Command that drives the swerve drive to a specific distance at a given speed + */ + public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { + return new Command() { + private Translation2d startTranslation; + + @Override + public void initialize() { + startTranslation = swerveDrive.getPose().getTranslation(); + } + + @Override + public void execute() { + driveFieldOriented(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)); + } + + @Override + public boolean isFinished() { + return (swerveDrive.getPose().getTranslation().getDistance(startTranslation) + > distanceInMeters); + } + }; + // return run(() -> { + // startPose = swerveDrive.getPose(); + // initDis = + // swerveDrive.getPose().getTranslation().getDistance(startPose.getTranslation()); + + // drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)); + // System.out.println("got here! :yay:! " + initDis); + // SmartDashboard.putNumber("StartPOse: ", initDis); + // } + + // ).until(() -> + // swerveDrive.getPose().getTranslation().getDistance(startPose.getTranslation()) + // > distanceInMeters); + } + + /** + * Replaces the swerve module feedforward with a new SimpleMotorFeedforward object. + * + * @param kS the static gain of the feedforward + * @param kV the velocity gain of the feedforward + * @param kA the acceleration gain of the feedforward + */ + public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { + swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); + } + + /** + * Command to drive the robot using translative values and heading as angular velocity. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param angularRotationX Angular velocity of the robot to set. Cubed for smoother controls. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier angularRotationX) { + return run( + () -> { + // Make the robot move + swerveDrive.drive( + SwerveMath.scaleTranslation( + new Translation2d( + translationX.getAsDouble() * swerveDrive.getMaximumChassisVelocity(), + translationY.getAsDouble() * swerveDrive.getMaximumChassisVelocity()), + 0.8), + Math.pow(angularRotationX.getAsDouble(), 3) + * swerveDrive.getMaximumChassisAngularVelocity(), + true, + false); + }); + } + + /** + * Command to drive the robot using translative values and heading as a setpoint. + * + * @param translationX Translation in the X direction. Cubed for smoother controls. + * @param translationY Translation in the Y direction. Cubed for smoother controls. + * @param headingX Heading X to calculate angle of the joystick. + * @param headingY Heading Y to calculate angle of the joystick. + * @return Drive command. + */ + public Command driveCommand( + DoubleSupplier translationX, + DoubleSupplier translationY, + DoubleSupplier headingX, + DoubleSupplier headingY) { + // swerveDrive.setHeadingCorrection(true); // Normally you would want heading + // correction for this kind of control. + return run( + () -> { + Translation2d scaledInputs = + SwerveMath.scaleTranslation( + new Translation2d(translationX.getAsDouble(), translationY.getAsDouble()), 0.8); + + // Make the robot move + driveFieldOriented( + swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + headingX.getAsDouble(), + headingY.getAsDouble(), + swerveDrive.getOdometryHeading().getRadians(), + swerveDrive.getMaximumChassisVelocity())); + }); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation + * rate, and calculates and commands module states accordingly. Can use either open-loop or + * closed-loop velocity control for the wheel velocities. Also has field- and robot-relative + * modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in + * meters per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from the + * alliance wall (field North) and positive y is torwards the left wall when looking through + * the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by + * field/robot relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + */ + public void drive(Translation2d translation, double rotation, boolean fieldRelative) { + swerveDrive.drive( + translation, + rotation, + fieldRelative, + false); // Open loop is disabled since it shouldn't be used + // most of the time. + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public void driveFieldOriented(ChassisSpeeds velocity) { + swerveDrive.driveFieldOriented(velocity); + } + + /** + * Drive the robot given a chassis field oriented velocity. + * + * @param velocity Velocity according to the field. + */ + public Command driveFieldOriented(Supplier velocity) { + return run( + () -> { + swerveDrive.driveFieldOriented(velocity.get()); + }); + } + + public Command robotDriveCommand( + Supplier velocity, BooleanSupplier robotRelative) { + return run( + () -> { + if (robotRelative.getAsBoolean()) { + swerveDrive.driveFieldOrientedAndRobotOriented( + new ChassisSpeeds(0, 0, 0), velocity.get()); + } else { + swerveDrive.driveFieldOriented(velocity.get()); + } + }); + } + + /** + * Drive according to the chassis robot oriented velocity. + * + * @param velocity Robot oriented {@link ChassisSpeeds} + */ + public void drive(ChassisSpeeds velocity) { + swerveDrive.drive(velocity); + } + + /** + * Get the swerve drive kinematics object. + * + * @return {@link SwerveDriveKinematics} of the swerve drive. + */ + @NotLogged + public SwerveDriveKinematics getKinematics() { + return swerveDrive.kinematics; + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when + * calling this method. However, if either gyro angle or module position is reset, this must be + * called in order for odometry to keep working. + * + * @param initialHolonomicPose The pose to set the odometry to + */ + public void resetOdometry(Pose2d initialHolonomicPose) { + swerveDrive.resetOdometry(initialHolonomicPose); + } + + /** + * Gets the current pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + @NotLogged + public Pose2d getPose() { + return swerveDrive.getPose(); + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param chassisSpeeds Chassis Speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + swerveDrive.setChassisSpeeds(chassisSpeeds); + } + + /** + * Post the trajectory to the field. + * + * @param trajectory The trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) { + swerveDrive.postTrajectory(trajectory); + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0. + */ + public void zeroGyro() { + swerveDrive.zeroGyro(); + } + + /** + * Checks if the alliance is red, defaults to false if alliance isn't available. + * + * @return true if the red alliance, false if blue. Defaults to false if none is available. + */ + private boolean isRedAlliance() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; + } + + /** + * This will zero (calibrate) the robot to assume the current position is facing forward + * + *

If red alliance rotate the robot 180 after the drviebase zero command + */ + public void zeroGyroWithAlliance() { + if (isRedAlliance()) { + zeroGyro(); + // Set the pose 180 degrees + resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); + } else { + zeroGyro(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorBrake(boolean brake) { + swerveDrive.setMotorIdleMode(brake); + } + + /** + * Gets the current yaw angle of the robot, as reported by the swerve pose estimator in the + * underlying drivebase. Note, this is not the raw gyro reading, this may be corrected from calls + * to resetOdometry(). + * + * @return The yaw angle + */ + public Rotation2d getHeading() { + return getPose().getRotation(); + } + + /** + * Get the chassis speeds based on controller input of 2 joysticks. One for speeds in which + * direction. The other for the angle of the robot. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param headingX X joystick which controls the angle of the robot. + * @param headingY Y joystick which controls the angle of the robot. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds( + double xInput, double yInput, double headingX, double headingY) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + return swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + headingX, + headingY, + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Get the chassis speeds based on controller input of 1 joystick and one angle. Control the robot + * at an offset of 90deg. + * + * @param xInput X joystick input for the robot to move in the X direction. + * @param yInput Y joystick input for the robot to move in the Y direction. + * @param angle The angle in as a {@link Rotation2d}. + * @return {@link ChassisSpeeds} which can be sent to the Swerve Drive. + */ + public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, Rotation2d angle) { + Translation2d scaledInputs = SwerveMath.cubeTranslation(new Translation2d(xInput, yInput)); + + return swerveDrive.swerveController.getTargetSpeeds( + scaledInputs.getX(), + scaledInputs.getY(), + angle.getRadians(), + getHeading().getRadians(), + Constants.MAX_SPEED); + } + + /** + * Gets the current field-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() { + return swerveDrive.getFieldVelocity(); + } + + /** + * Gets the current velocity (x, y and omega) of the robot + * + * @return A {@link ChassisSpeeds} object of the current velocity + */ + public ChassisSpeeds getRobotVelocity() { + return swerveDrive.getRobotVelocity(); + } + + /** + * Get the {@link SwerveController} in the swerve drive. + * + * @return {@link SwerveController} from the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() { + return swerveDrive.swerveController; + } + + /** + * Get the {@link SwerveDriveConfiguration} object. + * + * @return The {@link SwerveDriveConfiguration} fpr the current drive. + */ + public SwerveDriveConfiguration getSwerveDriveConfiguration() { + return swerveDrive.swerveDriveConfiguration; + } + + /** Lock the swerve drive to prevent it from moving. */ + public void lock() { + swerveDrive.lockPose(); + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() { + return swerveDrive.getPitch(); + } + + /** Add a fake vision reading for testing purposes. */ + public void addFakeVisionReading() { + swerveDrive.addVisionMeasurement( + new Pose2d(3, 3, Rotation2d.fromDegrees(65)), Timer.getFPGATimestamp()); + } + + /** + * Gets the swerve drive object. + * + * @return {@link SwerveDrive} + */ + public SwerveDrive getSwerveDrive() { + return swerveDrive; + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index cc86b66..f1a15fb 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -46,517 +46,472 @@ import swervelib.telemetry.SwerveDriveTelemetry; /** - * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken - * from + * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads */ public class Vision { - /** - * April Tag Field Layout of the year. - */ - public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField( - AprilTagFields.k2025ReefscapeWelded - ); - /** - * Photon Vision Simulation - */ - public VisionSystemSim visionSim; - /** - * Current pose from the pose estimator using wheel odometry. - */ - private Supplier currentPose; - /** - * Field from {@link swervelib.SwerveDrive#field} - */ - private Field2d field2d; - - /** - * Alert to show when no PhotonVision clients are found - */ - private final Alert noPhotonVisionAlert = new Alert("No PhotonVision clients found", AlertType.kWarning); - - /** - * Constructor for the Vision class. - * - * @param currentPose Current pose supplier, should reference - * {@link SwerveDrive#getPose()} - * @param field Current field, should be {@link SwerveDrive#field} - */ - public Vision(Supplier currentPose, Field2d field) { - this.currentPose = currentPose; - this.field2d = field; - - if (Robot.isSimulation()) { - visionSim = new VisionSystemSim("Vision"); - visionSim.addAprilTags(fieldLayout); - - for (Cameras c : Cameras.values()) { - c.addToVisionSim(visionSim); - } - - openSimCameraViews(); - } - - // Check if any cameras are connected - boolean hasCamera = false; - for (Cameras c : Cameras.values()) { - if (c.camera.isConnected()) { - hasCamera = true; - break; - } - } - noPhotonVisionAlert.set(!hasCamera); - } - - /** - * Calculates a target pose relative to an AprilTag on the field. - * - * @param aprilTag The ID of the AprilTag. - * @param robotOffset The offset {@link Transform2d} of the robot to apply to - * the pose for the robot to position - * itself correctly. - * @return The target pose of the AprilTag. - */ - public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { - Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); - if (aprilTagPose3d.isPresent()) { - return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); - } else { - throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); - } - } - - /** - * Update the pose estimation inside of {@link SwerveDrive} with all of the - * given poses. - * - * @param swerveDrive {@link SwerveDrive} instance. - */ - public void updatePoseEstimation(SwerveDrive swerveDrive) { - if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) { - /* - * In the maple-sim, odometry is simulated using encoder values, accounting for - * factors like skidding and drifting. - * As a result, the odometry may not always be 100% accurate. - * However, the vision system should be able to provide a reasonably accurate - * pose estimation, even when odometry is incorrect. - * (This is why teams implement vision system to correct odometry.) - * Therefore, we must ensure that the actual robot pose is provided in the - * simulator when updating the vision simulation during the simulation. - */ - visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); - } - for (Cameras camera : Cameras.values()) { - if (!camera.camera.isConnected()) { - continue; - } - Optional poseEst = getEstimatedGlobalPose(camera); - if (poseEst.isPresent()) { - var pose = poseEst.get(); - swerveDrive.addVisionMeasurement( - pose.estimatedPose.toPose2d(), - pose.timestampSeconds, - camera.curStdDevs - ); - } - } - } - - /** - * Generates the estimated robot pose. Returns empty if: - *

    - *
  • No Pose Estimates could be generated
    • - *
  • The generated pose estimate was considered not accurate
    • - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and - * targets used to create the estimate - */ - public Optional getEstimatedGlobalPose(Cameras camera) { - Optional poseEst = camera.getEstimatedGlobalPose(); - if (Robot.isSimulation()) { - Field2d debugField = visionSim.getDebugField(); - // Uncomment to enable outputting of vision targets in sim. - poseEst.ifPresentOrElse( - est -> debugField.getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), - () -> { - debugField.getObject("VisionEstimation").setPoses(); - } - ); - } - return poseEst; - } - - /** - * Get distance of the robot from the AprilTag pose. - * - * @param id AprilTag ID - * @return Distance - */ - public double getDistanceFromAprilTag(int id) { - Optional tag = fieldLayout.getTagPose(id); - return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0); - } - - /** - * Get tracked target from a camera of AprilTagID - * - * @param id AprilTag ID - * @param camera Camera to check. - * @return Tracked target. - */ - public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { - PhotonTrackedTarget target = null; - for (PhotonPipelineResult result : camera.resultsList) { - if (result.hasTargets()) { - for (PhotonTrackedTarget i : result.getTargets()) { - if (i.getFiducialId() == id) { - return i; - } - } - } - } - return target; - } - - /** - * Vision simulation. - * - * @return Vision Simulation - */ - public VisionSystemSim getVisionSim() { - return visionSim; - } - - /** - * Open up the photon vision camera streams on the localhost, assumes running - * photon vision on localhost. - */ - private void openSimCameraViews() { - if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { - // try - // { - // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); - // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); - // } catch (IOException | URISyntaxException e) - // { - // e.printStackTrace(); - // } - } - } - - /** - * Update the {@link Field2d} to include tracked targets/ - */ - public void updateVisionField() { - List targets = new ArrayList(); - for (Cameras c : Cameras.values()) { - if (!c.resultsList.isEmpty()) { - PhotonPipelineResult latest = c.resultsList.get(0); - if (latest.hasTargets()) { - targets.addAll(latest.targets); - } - } - } - - List poses = new ArrayList<>(); - for (PhotonTrackedTarget target : targets) { - if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { - Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); - poses.add(targetPose); - } - } - - field2d.getObject("tracked targets").setPoses(poses); - } - - /** - * Camera Enum to select each camera - */ - enum Cameras { - /** - * Center Camera - */ - CENTER_CAM( - "PEBBLE", - new Rotation3d(0, Units.degreesToRadians(15.0), 0), - new Translation3d(0.32, 0.32, 0.30), - VecBuilder.fill(4, 4, 8), - VecBuilder.fill(0.5, 0.5, 1) - ); - - /** - * Latency alert to use when high latency is detected. - */ - public final Alert latencyAlert; - /** - * Camera instance for comms. - */ - public final PhotonCamera camera; - /** - * Pose estimator for camera. - */ - public final PhotonPoseEstimator poseEstimator; - /** - * Standard Deviation for single tag readings for pose estimation. - */ - private final Matrix singleTagStdDevs; - /** - * Standard deviation for multi-tag readings for pose estimation. - */ - private final Matrix multiTagStdDevs; - /** - * Transform of the camera rotation and translation relative to the center of - * the robot - */ - private final Transform3d robotToCamTransform; - /** - * Current standard deviations used. - */ - public Matrix curStdDevs; - /** - * Estimated robot pose. - */ - public Optional estimatedRobotPose; - /** - * Simulated camera instance which only exists during simulations. - */ - public PhotonCameraSim cameraSim; - /** - * Results list to be updated periodically and cached to avoid unnecessary - * queries. - */ - public List resultsList = new ArrayList<>(); - /** - * Last read from the camera timestamp to prevent lag due to slow data fetches. - */ - private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); - - /** - * Construct a Photon Camera class with help. Standard deviations are fake - * values, experiment and determine - * estimation noise on an actual robot. - * - * @param name Name of the PhotonVision camera found in the PV - * UI. - * @param robotToCamRotation {@link Rotation3d} of the camera. - * @param robotToCamTranslation {@link Translation3d} relative to the center of - * the robot. - * @param singleTagStdDevs Single AprilTag standard deviations of estimated - * poses from the camera. - * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated - * poses from the camera. - */ - Cameras( - String name, - Rotation3d robotToCamRotation, - Translation3d robotToCamTranslation, - Matrix singleTagStdDevs, - Matrix multiTagStdDevsMatrix - ) { - latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); - - camera = new PhotonCamera(name); - - // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html - robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); - - poseEstimator = new PhotonPoseEstimator( - Vision.fieldLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCamTransform - ); - poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - - this.singleTagStdDevs = singleTagStdDevs; - this.multiTagStdDevs = multiTagStdDevsMatrix; - - if (Robot.isSimulation()) { - SimCameraProperties cameraProp = new SimCameraProperties(); - // A 640 x 480 camera with a 100 degree diagonal FOV. - cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); - // Approximate detection noise with average and standard deviation error in - // pixels. - cameraProp.setCalibError(0.25, 0.08); - // Set the camera image capture framerate (Note: this is limited by robot loop - // rate). - cameraProp.setFPS(30); - // The average and standard deviation in milliseconds of image data latency. - cameraProp.setAvgLatencyMs(35); - cameraProp.setLatencyStdDevMs(5); - - cameraSim = new PhotonCameraSim(camera, cameraProp); - cameraSim.enableDrawWireframe(true); - } - } - - /** - * Add camera to {@link VisionSystemSim} for simulated photon vision. - * - * @param systemSim {@link VisionSystemSim} to use. - */ - public void addToVisionSim(VisionSystemSim systemSim) { - if (Robot.isSimulation()) { - systemSim.addCamera(cameraSim, robotToCamTransform); - } - } - - /** - * Get the result with the least ambiguity from the best tracked target within - * the Cache. This may not be the most - * recent result! - * - * @return The result in the cache with the least ambiguous best tracked target. - * This is not the most recent result! - */ - public Optional getBestResult() { - if (resultsList.isEmpty()) { - return Optional.empty(); - } - - PhotonPipelineResult bestResult = resultsList.get(0); - double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); - double currentAmbiguity = 0; - for (PhotonPipelineResult result : resultsList) { - currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); - if (currentAmbiguity < amiguity && currentAmbiguity > 0) { - bestResult = result; - amiguity = currentAmbiguity; - } - } - return Optional.of(bestResult); - } - - /** - * Get the latest result from the current cache. - * - * @return Empty optional if nothing is found. Latest result if something is - * there. - */ - public Optional getLatestResult() { - return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); - } - - /** - * Get the estimated robot pose. Updates the current robot pose estimation, - * standard deviations, and flushes the - * cache of results. - * - * @return Estimated pose. - */ - public Optional getEstimatedGlobalPose() { - updateUnreadResults(); - return estimatedRobotPose; - } - - /** - * Update the latest results, cached with a maximum refresh rate of 1req/15ms. - * Sorts the list by timestamp. - */ - private void updateUnreadResults() { - double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); - double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); - double debounceTime = Milliseconds.of(15).in(Seconds); - for (PhotonPipelineResult result : resultsList) { - mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); - } - if ( - (resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) && - (currentTimestamp - lastReadTimestamp) >= debounceTime - ) { - resultsList = Robot.isReal() - ? camera.getAllUnreadResults() - : cameraSim.getCamera().getAllUnreadResults(); - lastReadTimestamp = currentTimestamp; - resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> { - return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; - }); - if (!resultsList.isEmpty()) { - updateEstimatedGlobalPose(); - } - } - } - - /** - * The latest estimated robot pose on the field from vision data. This may be - * empty. This should only be called once - * per loop. - * - *

- * Also includes updates for the standard deviations, which can (optionally) be - * retrieved with - * {@link Cameras#updateEstimationStdDevs} - * - * @return An {@link EstimatedRobotPose} with an estimated pose, estimate - * timestamp, and targets used for - * estimation. - */ - private void updateEstimatedGlobalPose() { - Optional visionEst = Optional.empty(); - for (var change : resultsList) { - visionEst = poseEstimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets()); - } - estimatedRobotPose = visionEst; - } - - /** - * Calculates new standard deviations This algorithm is a heuristic that creates - * dynamic standard deviations based - * on number of tags, estimation strategy, and distance from the tags. - * - * @param estimatedPose The estimated pose to guess standard deviations for. - * @param targets All targets in this camera frame - */ - private void updateEstimationStdDevs( - Optional estimatedPose, - List targets - ) { - if (estimatedPose.isEmpty()) { - // No pose input. Default to single-tag std devs - curStdDevs = singleTagStdDevs; - } else { - // Pose present. Start running Heuristic - var estStdDevs = singleTagStdDevs; - int numTags = 0; - double avgDist = 0; - - // Precalculation - see how many tags we found, and calculate an - // average-distance metric - for (var tgt : targets) { - var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) { - continue; - } - numTags++; - avgDist += tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - } - - if (numTags == 0) { - // No tags visible. Default to single-tag std devs - curStdDevs = singleTagStdDevs; - } else { - // One or more tags visible, run the full heuristic. - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) { - estStdDevs = multiTagStdDevs; - } - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) { - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - } else { - estStdDevs = estStdDevs.times(1 + ((avgDist * avgDist) / 30)); - } - curStdDevs = estStdDevs; - } - } - } - } + /** April Tag Field Layout of the year. */ + public static final AprilTagFieldLayout fieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + /** Photon Vision Simulation */ + public VisionSystemSim visionSim; + + /** Current pose from the pose estimator using wheel odometry. */ + private Supplier currentPose; + + /** Field from {@link swervelib.SwerveDrive#field} */ + private Field2d field2d; + + /** Alert to show when no PhotonVision clients are found */ + private final Alert noPhotonVisionAlert = + new Alert("No PhotonVision clients found", AlertType.kWarning); + + /** + * Constructor for the Vision class. + * + * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()} + * @param field Current field, should be {@link SwerveDrive#field} + */ + public Vision(Supplier currentPose, Field2d field) { + this.currentPose = currentPose; + this.field2d = field; + + if (Robot.isSimulation()) { + visionSim = new VisionSystemSim("Vision"); + visionSim.addAprilTags(fieldLayout); + + for (Cameras c : Cameras.values()) { + c.addToVisionSim(visionSim); + } + + openSimCameraViews(); + } + + // Check if any cameras are connected + boolean hasCamera = false; + for (Cameras c : Cameras.values()) { + if (c.camera.isConnected()) { + hasCamera = true; + break; + } + } + noPhotonVisionAlert.set(!hasCamera); + } + + /** + * Calculates a target pose relative to an AprilTag on the field. + * + * @param aprilTag The ID of the AprilTag. + * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the + * robot to position itself correctly. + * @return The target pose of the AprilTag. + */ + public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) { + Optional aprilTagPose3d = fieldLayout.getTagPose(aprilTag); + if (aprilTagPose3d.isPresent()) { + return aprilTagPose3d.get().toPose2d().transformBy(robotOffset); + } else { + throw new RuntimeException( + "Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString()); + } + } + + /** + * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses. + * + * @param swerveDrive {@link SwerveDrive} instance. + */ + public void updatePoseEstimation(SwerveDrive swerveDrive) { + if (SwerveDriveTelemetry.isSimulation + && swerveDrive.getSimulationDriveTrainPose().isPresent()) { + /* + * In the maple-sim, odometry is simulated using encoder values, accounting for + * factors like skidding and drifting. + * As a result, the odometry may not always be 100% accurate. + * However, the vision system should be able to provide a reasonably accurate + * pose estimation, even when odometry is incorrect. + * (This is why teams implement vision system to correct odometry.) + * Therefore, we must ensure that the actual robot pose is provided in the + * simulator when updating the vision simulation during the simulation. + */ + visionSim.update(swerveDrive.getSimulationDriveTrainPose().get()); + } + for (Cameras camera : Cameras.values()) { + if (!camera.camera.isConnected()) { + continue; + } + Optional poseEst = getEstimatedGlobalPose(camera); + if (poseEst.isPresent()) { + var pose = poseEst.get(); + swerveDrive.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds, camera.curStdDevs); + } + } + } + + /** + * Generates the estimated robot pose. Returns empty if: + * + *

    + *
  • No Pose Estimates could be generated + *
  • The generated pose estimate was considered not accurate + *
+ * + * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to + * create the estimate + */ + public Optional getEstimatedGlobalPose(Cameras camera) { + Optional poseEst = camera.getEstimatedGlobalPose(); + if (Robot.isSimulation()) { + Field2d debugField = visionSim.getDebugField(); + // Uncomment to enable outputting of vision targets in sim. + poseEst.ifPresentOrElse( + (est) -> debugField.getObject("VisionEstimation").setPose(est.estimatedPose.toPose2d()), + () -> { + debugField.getObject("VisionEstimation").setPoses(); + }); + } + return poseEst; + } + + /** + * Get distance of the robot from the AprilTag pose. + * + * @param id AprilTag ID + * @return Distance + */ + public double getDistanceFromAprilTag(int id) { + Optional tag = fieldLayout.getTagPose(id); + return tag.map((pose3d) -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())) + .orElse(-1.0); + } + + /** + * Get tracked target from a camera of AprilTagID + * + * @param id AprilTag ID + * @param camera Camera to check. + * @return Tracked target. + */ + public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) { + PhotonTrackedTarget target = null; + for (PhotonPipelineResult result : camera.resultsList) { + if (result.hasTargets()) { + for (PhotonTrackedTarget i : result.getTargets()) { + if (i.getFiducialId() == id) { + return i; + } + } + } + } + return target; + } + + /** + * Vision simulation. + * + * @return Vision Simulation + */ + public VisionSystemSim getVisionSim() { + return visionSim; + } + + /** + * Open up the photon vision camera streams on the localhost, assumes running photon vision on + * localhost. + */ + private void openSimCameraViews() { + if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) { + // try + // { + // Desktop.getDesktop().browse(new URI("http://localhost:1182/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1184/")); + // Desktop.getDesktop().browse(new URI("http://localhost:1186/")); + // } catch (IOException | URISyntaxException e) + // { + // e.printStackTrace(); + // } + } + } + + /** Update the {@link Field2d} to include tracked targets/ */ + public void updateVisionField() { + List targets = new ArrayList(); + for (Cameras c : Cameras.values()) { + if (!c.resultsList.isEmpty()) { + PhotonPipelineResult latest = c.resultsList.get(0); + if (latest.hasTargets()) { + targets.addAll(latest.targets); + } + } + } + + List poses = new ArrayList<>(); + for (PhotonTrackedTarget target : targets) { + if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) { + Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d(); + poses.add(targetPose); + } + } + + field2d.getObject("tracked targets").setPoses(poses); + } + + /** Camera Enum to select each camera */ + enum Cameras { + /** Center Camera */ + CENTER_CAM( + "PEBBLE", + new Rotation3d(0, Units.degreesToRadians(15.0), 0), + new Translation3d(0.32, 0.32, 0.30), + VecBuilder.fill(4, 4, 8), + VecBuilder.fill(0.5, 0.5, 1)); + + /** Latency alert to use when high latency is detected. */ + public final Alert latencyAlert; + + /** Camera instance for comms. */ + public final PhotonCamera camera; + + /** Pose estimator for camera. */ + public final PhotonPoseEstimator poseEstimator; + + /** Standard Deviation for single tag readings for pose estimation. */ + private final Matrix singleTagStdDevs; + + /** Standard deviation for multi-tag readings for pose estimation. */ + private final Matrix multiTagStdDevs; + + /** Transform of the camera rotation and translation relative to the center of the robot */ + private final Transform3d robotToCamTransform; + + /** Current standard deviations used. */ + public Matrix curStdDevs; + + /** Estimated robot pose. */ + public Optional estimatedRobotPose; + + /** Simulated camera instance which only exists during simulations. */ + public PhotonCameraSim cameraSim; + + /** Results list to be updated periodically and cached to avoid unnecessary queries. */ + public List resultsList = new ArrayList<>(); + + /** Last read from the camera timestamp to prevent lag due to slow data fetches. */ + private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + + /** + * Construct a Photon Camera class with help. Standard deviations are fake values, experiment + * and determine estimation noise on an actual robot. + * + * @param name Name of the PhotonVision camera found in the PV UI. + * @param robotToCamRotation {@link Rotation3d} of the camera. + * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot. + * @param singleTagStdDevs Single AprilTag standard deviations of estimated poses from the + * camera. + * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the + * camera. + */ + Cameras( + String name, + Rotation3d robotToCamRotation, + Translation3d robotToCamTranslation, + Matrix singleTagStdDevs, + Matrix multiTagStdDevsMatrix) { + latencyAlert = + new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning); + + camera = new PhotonCamera(name); + + // https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html + robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation); + + poseEstimator = + new PhotonPoseEstimator( + Vision.fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamTransform); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + this.singleTagStdDevs = singleTagStdDevs; + this.multiTagStdDevs = multiTagStdDevsMatrix; + + if (Robot.isSimulation()) { + SimCameraProperties cameraProp = new SimCameraProperties(); + // A 640 x 480 camera with a 100 degree diagonal FOV. + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100)); + // Approximate detection noise with average and standard deviation error in + // pixels. + cameraProp.setCalibError(0.25, 0.08); + // Set the camera image capture framerate (Note: this is limited by robot loop + // rate). + cameraProp.setFPS(30); + // The average and standard deviation in milliseconds of image data latency. + cameraProp.setAvgLatencyMs(35); + cameraProp.setLatencyStdDevMs(5); + + cameraSim = new PhotonCameraSim(camera, cameraProp); + cameraSim.enableDrawWireframe(true); + } + } + + /** + * Add camera to {@link VisionSystemSim} for simulated photon vision. + * + * @param systemSim {@link VisionSystemSim} to use. + */ + public void addToVisionSim(VisionSystemSim systemSim) { + if (Robot.isSimulation()) { + systemSim.addCamera(cameraSim, robotToCamTransform); + } + } + + /** + * Get the result with the least ambiguity from the best tracked target within the Cache. This + * may not be the most recent result! + * + * @return The result in the cache with the least ambiguous best tracked target. This is not the + * most recent result! + */ + public Optional getBestResult() { + if (resultsList.isEmpty()) { + return Optional.empty(); + } + + PhotonPipelineResult bestResult = resultsList.get(0); + double amiguity = bestResult.getBestTarget().getPoseAmbiguity(); + double currentAmbiguity = 0; + for (PhotonPipelineResult result : resultsList) { + currentAmbiguity = result.getBestTarget().getPoseAmbiguity(); + if (currentAmbiguity < amiguity && currentAmbiguity > 0) { + bestResult = result; + amiguity = currentAmbiguity; + } + } + return Optional.of(bestResult); + } + + /** + * Get the latest result from the current cache. + * + * @return Empty optional if nothing is found. Latest result if something is there. + */ + public Optional getLatestResult() { + return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0)); + } + + /** + * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, + * and flushes the cache of results. + * + * @return Estimated pose. + */ + public Optional getEstimatedGlobalPose() { + updateUnreadResults(); + return estimatedRobotPose; + } + + /** + * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by + * timestamp. + */ + private void updateUnreadResults() { + double mostRecentTimestamp = + resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds(); + double currentTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds); + double debounceTime = Milliseconds.of(15).in(Seconds); + for (PhotonPipelineResult result : resultsList) { + mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds()); + } + if ((resultsList.isEmpty() || (currentTimestamp - mostRecentTimestamp >= debounceTime)) + && (currentTimestamp - lastReadTimestamp) >= debounceTime) { + resultsList = + Robot.isReal() + ? camera.getAllUnreadResults() + : cameraSim.getCamera().getAllUnreadResults(); + lastReadTimestamp = currentTimestamp; + resultsList.sort( + (PhotonPipelineResult a, PhotonPipelineResult b) -> { + return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1; + }); + if (!resultsList.isEmpty()) { + updateEstimatedGlobalPose(); + } + } + } + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved + * with {@link Cameras#updateEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + private void updateEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : resultsList) { + visionEst = poseEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + } + estimatedRobotPose = visionEst; + } + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic + * standard deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = singleTagStdDevs; + } else { + // Pose present. Start running Heuristic + var estStdDevs = singleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) { + continue; + } + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = singleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) { + estStdDevs = multiTagStdDevs; + } + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) { + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + } else { + estStdDevs = estStdDevs.times(1 + ((avgDist * avgDist) / 30)); + } + curStdDevs = estStdDevs; + } + } + } + } } From 7b4e6a3b4ca7f01ecc94b62dd1cfbd854a08e27f Mon Sep 17 00:00:00 2001 From: Kieran Klukas Date: Mon, 11 Aug 2025 22:14:32 -0400 Subject: [PATCH 4/5] chore: fix ci --- .github/workflows/ci.yml | 56 +++++++++++++++++++--------------------- .mill-version | 1 + 2 files changed, 28 insertions(+), 29 deletions(-) create mode 100644 .mill-version diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8f1d260..cfd1b7a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,34 +1,32 @@ name: Code Quality on: - push: - branches: [ main, develop ] - pull_request: - branches: [ main ] + push: + branches: [main, develop] + pull_request: + branches: [main] jobs: - format-check: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - - name: Set up JDK 17 - uses: actions/setup-java@v4 - with: - java-version: '17' - distribution: 'temurin' - - - name: Setup Mill - uses: jodersky/setup-mill@v0.2.3 - with: - mill-version: 0.12.11 - - - name: Check Java formatting - run: mill checkJavaFormat - - - name: Compile code - run: mill compile - - - name: Run tests - run: mill test \ No newline at end of file + format-check: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + java-version: "17" + distribution: "temurin" + + - uses: zhutmost/setup-mill@main + with: + mill-version: 0.12.11 + + - name: Check Java formatting + run: mill checkJavaFormat + # the server process is kept alive across steps, + # so separate invocations of mill remain extremely fast! + + - name: Compile + run: mill compile diff --git a/.mill-version b/.mill-version new file mode 100644 index 0000000..0801881 --- /dev/null +++ b/.mill-version @@ -0,0 +1 @@ +0.12.11 From fffdc610380d94f336cf67cea810ef313c9c0aff Mon Sep 17 00:00:00 2001 From: Your Name Date: Tue, 12 Aug 2025 21:15:12 -0400 Subject: [PATCH 5/5] add mill's windows script --- mill.bat | 299 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 299 insertions(+) create mode 100644 mill.bat diff --git a/mill.bat b/mill.bat new file mode 100644 index 0000000..f36d812 --- /dev/null +++ b/mill.bat @@ -0,0 +1,299 @@ +@echo off + +rem This is a wrapper script, that automatically selects or downloads Mill from Maven Central or GitHub release pages. +rem +rem This script determines the Mill version to use by trying these sources +rem - env-variable `MILL_VERSION` +rem - local file `.mill-version` +rem - local file `.config/mill-version` +rem - `mill-version` from YAML fronmatter of current buildfile +rem - if accessible, find the latest stable version available on Maven Central (https://repo1.maven.org/maven2) +rem - env-variable `DEFAULT_MILL_VERSION` +rem +rem If a version has the suffix '-native' a native binary will be used. +rem If a version has the suffix '-jvm' an executable jar file will be used, requiring an already installed Java runtime. +rem If no such suffix is found, the script will pick a default based on version and platform. +rem +rem Once a version was determined, it tries to use either +rem - a system-installed mill, if found and it's version matches +rem - an already downloaded version under %USERPROFILE%\.mill\download +rem +rem If no working mill version was found on the system, +rem this script downloads a binary file from Maven Central or Github Pages (this is version dependent) +rem into a cache location (%USERPROFILE%\.mill\download). +rem +rem Mill Project URL: https://github.com/com-lihaoyi/mill +rem Script Version: 1.0.0-M1-21-7b6fae-DIRTY892b63e8 +rem +rem If you want to improve this script, please also contribute your changes back! +rem This script was generated from: dist/scripts/src/mill.bat +rem +rem Licensed under the Apache License, Version 2.0 + +rem setlocal seems to be unavailable on Windows 95/98/ME +rem but I don't think we need to support them in 2019 +setlocal enabledelayedexpansion + +if [!DEFAULT_MILL_VERSION!]==[] ( set "DEFAULT_MILL_VERSION=0.12.10" ) + +if [!MILL_GITHUB_RELEASE_CDN!]==[] ( set "MILL_GITHUB_RELEASE_CDN=" ) + +if [!MILL_MAIN_CLI!]==[] ( set "MILL_MAIN_CLI=%~f0" ) + +set "MILL_REPO_URL=https://github.com/com-lihaoyi/mill" + +SET MILL_BUILD_SCRIPT= + +if exist "build.mill" ( + set MILL_BUILD_SCRIPT=build.mill +) else ( + if exist "build.mill.scala" ( + set MILL_BUILD_SCRIPT=build.mill.scala + ) else ( + if exist "build.sc" ( + set MILL_BUILD_SCRIPT=build.sc + ) else ( + rem no-op + ) + ) +) + +if [!MILL_VERSION!]==[] ( + if exist .mill-version ( + set /p MILL_VERSION=<.mill-version + ) else ( + if exist .config\mill-version ( + set /p MILL_VERSION=<.config\mill-version + ) else ( + if not "%MILL_BUILD_SCRIPT%"=="" ( + for /f "tokens=1-2*" %%a in ('findstr /C:"//| mill-version:" %MILL_BUILD_SCRIPT%') do ( + set "MILL_VERSION=%%c" + ) + ) else ( + rem no-op + ) + ) + ) +) + +if [!MILL_VERSION!]==[] set MILL_VERSION=%DEFAULT_MILL_VERSION% + +if [!MILL_DOWNLOAD_PATH!]==[] set MILL_DOWNLOAD_PATH=%USERPROFILE%\.mill\download + +rem without bat file extension, cmd doesn't seem to be able to run it + +set "MILL_NATIVE_SUFFIX=-native" +set "MILL_JVM_SUFFIX=-jvm" +set "FULL_MILL_VERSION=%MILL_VERSION%" +set "MILL_EXT=.bat" +set "ARTIFACT_SUFFIX=" +REM Check if MILL_VERSION contains MILL_NATIVE_SUFFIX +echo !MILL_VERSION! | findstr /C:"%MILL_NATIVE_SUFFIX%" >nul +if !errorlevel! equ 0 ( + set "MILL_VERSION=%MILL_VERSION:-native=%" + REM -native images compiled with graal do not support windows-arm + REM https://github.com/oracle/graal/issues/9215 + IF /I NOT "%PROCESSOR_ARCHITECTURE%"=="ARM64" ( + set "ARTIFACT_SUFFIX=-native-windows-amd64" + set "MILL_EXT=.exe" + ) else ( + rem no-op + ) +) else ( + echo !MILL_VERSION! | findstr /C:"%MILL_JVM_SUFFIX%" >nul + if !errorlevel! equ 0 ( + set "MILL_VERSION=%MILL_VERSION:-jvm=%" + ) else ( + set "SKIP_VERSION=false" + set "MILL_PREFIX=%MILL_VERSION:~0,4%" + if "!MILL_PREFIX!"=="0.1." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.2." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.3." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.4." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.5." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.6." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.7." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.8." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.9." set "SKIP_VERSION=true" + set "MILL_PREFIX=%MILL_VERSION:~0,5%" + if "!MILL_PREFIX!"=="0.10." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.11." set "SKIP_VERSION=true" + if "!MILL_PREFIX!"=="0.12." set "SKIP_VERSION=true" + + if "!SKIP_VERSION!"=="false" ( + IF /I NOT "%PROCESSOR_ARCHITECTURE%"=="ARM64" ( + set "ARTIFACT_SUFFIX=-native-windows-amd64" + set "MILL_EXT=.exe" + ) + ) else ( + rem no-op + ) + ) +) + +set MILL=%MILL_DOWNLOAD_PATH%\!FULL_MILL_VERSION!!MILL_EXT! + +set MILL_RESOLVE_DOWNLOAD= + +if not exist "%MILL%" ( + set MILL_RESOLVE_DOWNLOAD=true +) else ( + if defined MILL_TEST_DRY_RUN_LAUNCHER_SCRIPT ( + set MILL_RESOLVE_DOWNLOAD=true + ) else ( + rem no-op + ) +) + + +if [!MILL_RESOLVE_DOWNLOAD!]==[true] ( + set MILL_VERSION_PREFIX=%MILL_VERSION:~0,4% + set MILL_SHORT_VERSION_PREFIX=%MILL_VERSION:~0,2% + rem Since 0.5.0 + set MILL_DOWNLOAD_SUFFIX=-assembly + rem Since 0.11.0 + set MILL_DOWNLOAD_FROM_MAVEN=1 + if [!MILL_VERSION_PREFIX!]==[0.0.] ( + set MILL_DOWNLOAD_SUFFIX= + set MILL_DOWNLOAD_FROM_MAVEN=0 + ) + if [!MILL_VERSION_PREFIX!]==[0.1.] ( + set MILL_DOWNLOAD_SUFFIX= + set MILL_DOWNLOAD_FROM_MAVEN=0 + ) + if [!MILL_VERSION_PREFIX!]==[0.2.] ( + set MILL_DOWNLOAD_SUFFIX= + set MILL_DOWNLOAD_FROM_MAVEN=0 + ) + if [!MILL_VERSION_PREFIX!]==[0.3.] ( + set MILL_DOWNLOAD_SUFFIX= + set MILL_DOWNLOAD_FROM_MAVEN=0 + ) + if [!MILL_VERSION_PREFIX!]==[0.4.] ( + set MILL_DOWNLOAD_SUFFIX= + set MILL_DOWNLOAD_FROM_MAVEN=0 + ) + if [!MILL_VERSION_PREFIX!]==[0.5.] set MILL_DOWNLOAD_FROM_MAVEN=0 + if [!MILL_VERSION_PREFIX!]==[0.6.] set MILL_DOWNLOAD_FROM_MAVEN=0 + if [!MILL_VERSION_PREFIX!]==[0.7.] set MILL_DOWNLOAD_FROM_MAVEN=0 + if [!MILL_VERSION_PREFIX!]==[0.8.] set MILL_DOWNLOAD_FROM_MAVEN=0 + if [!MILL_VERSION_PREFIX!]==[0.9.] set MILL_DOWNLOAD_FROM_MAVEN=0 + + set MILL_VERSION_PREFIX=%MILL_VERSION:~0,5% + if [!MILL_VERSION_PREFIX!]==[0.10.] set MILL_DOWNLOAD_FROM_MAVEN=0 + + set MILL_VERSION_PREFIX=%MILL_VERSION:~0,8% + if [!MILL_VERSION_PREFIX!]==[0.11.0-M] set MILL_DOWNLOAD_FROM_MAVEN=0 + + set MILL_VERSION_PREFIX=%MILL_VERSION:~0,5% + set DOWNLOAD_EXT=exe + if [!MILL_SHORT_VERSION_PREFIX!]==[0.] set DOWNLOAD_EXT=jar + if [!MILL_VERSION_PREFIX!]==[0.12.] set DOWNLOAD_EXT=exe + if [!MILL_VERSION!]==[0.12.0] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.1] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.2] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.3] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.4] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.5] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.6] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.7] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.8] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.9] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.10] set DOWNLOAD_EXT=jar + if [!MILL_VERSION!]==[0.12.11] set DOWNLOAD_EXT=jar + + set MILL_VERSION_PREFIX= + set MILL_SHORT_VERSION_PREFIX= + + for /F "delims=- tokens=1" %%A in ("!MILL_VERSION!") do set MILL_VERSION_BASE=%%A + set MILL_VERSION_MILESTONE= + for /F "delims=- tokens=2" %%A in ("!MILL_VERSION!") do set MILL_VERSION_MILESTONE=%%A + set MILL_VERSION_MILESTONE_START=!MILL_VERSION_MILESTONE:~0,1! + if [!MILL_VERSION_MILESTONE_START!]==[M] ( + set MILL_VERSION_TAG=!MILL_VERSION_BASE!-!MILL_VERSION_MILESTONE! + ) else ( + set MILL_VERSION_TAG=!MILL_VERSION_BASE! + ) + if [!MILL_DOWNLOAD_FROM_MAVEN!]==[1] ( + set MILL_DOWNLOAD_URL=https://repo1.maven.org/maven2/com/lihaoyi/mill-dist!ARTIFACT_SUFFIX!/!MILL_VERSION!/mill-dist!ARTIFACT_SUFFIX!-!MILL_VERSION!.!DOWNLOAD_EXT! + ) else ( + set MILL_DOWNLOAD_URL=!MILL_GITHUB_RELEASE_CDN!%MILL_REPO_URL%/releases/download/!MILL_VERSION_TAG!/!MILL_VERSION!!MILL_DOWNLOAD_SUFFIX! + ) + + if defined MILL_TEST_DRY_RUN_LAUNCHER_SCRIPT ( + echo !MILL_DOWNLOAD_URL! + echo !MILL! + exit /b 0 + ) + + rem there seems to be no way to generate a unique temporary file path (on native Windows) + set MILL_DOWNLOAD_FILE=%MILL%.tmp + + echo Downloading mill !MILL_VERSION! from !MILL_DOWNLOAD_URL! ... 1>&2 + + if not exist "%MILL_DOWNLOAD_PATH%" mkdir "%MILL_DOWNLOAD_PATH%" + rem curl is bundled with recent Windows 10 + rem but I don't think we can expect all the users to have it in 2019 + where /Q curl + if !ERRORLEVEL! EQU 0 ( + curl -f -L "!MILL_DOWNLOAD_URL!" -o "!MILL_DOWNLOAD_FILE!" + ) else ( + rem bitsadmin seems to be available on Windows 7 + rem without /dynamic, github returns 403 + rem bitsadmin is sometimes needlessly slow but it looks better with /priority foreground + bitsadmin /transfer millDownloadJob /dynamic /priority foreground "!MILL_DOWNLOAD_URL!" "!MILL_DOWNLOAD_FILE!" + ) + if not exist "!MILL_DOWNLOAD_FILE!" ( + echo Could not download mill !MILL_VERSION! 1>&2 + exit /b 1 + ) + + move /y "!MILL_DOWNLOAD_FILE!" "%MILL%" + + set MILL_DOWNLOAD_FILE= + set MILL_DOWNLOAD_SUFFIX= +) + +set MILL_DOWNLOAD_PATH= +set MILL_VERSION= +set MILL_REPO_URL= + +rem Need to preserve the first position of those listed options +set MILL_FIRST_ARG= +if [%~1%]==[--bsp] ( + set MILL_FIRST_ARG=%1% +) else ( + if [%~1%]==[-i] ( + set MILL_FIRST_ARG=%1% + ) else ( + if [%~1%]==[--interactive] ( + set MILL_FIRST_ARG=%1% + ) else ( + if [%~1%]==[--no-server] ( + set MILL_FIRST_ARG=%1% + ) else ( + if [%~1%]==[--no-daemon] ( + set MILL_FIRST_ARG=%1% + ) else ( + if [%~1%]==[--repl] ( + set MILL_FIRST_ARG=%1% + ) else ( + if [%~1%]==[--help] ( + set MILL_FIRST_ARG=%1% + ) + ) + ) + ) + ) + ) +) +set "MILL_PARAMS=%*%" + +if not [!MILL_FIRST_ARG!]==[] ( + for /f "tokens=1*" %%a in ("%*") do ( + set "MILL_PARAMS=%%b" + ) +) + +rem -D mill.main.cli is for compatibility with Mill 0.10.9 - 0.13.0-M2 +"%MILL%" %MILL_FIRST_ARG% -D "mill.main.cli=%MILL_MAIN_CLI%" %MILL_PARAMS%