diff --git a/.vscode/launch.json b/.vscode/launch.json index 77bee667..13c2122a 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -9,13 +9,35 @@ "name": "Main", "request": "launch", "mainClass": "frc.robot.Main", - "projectName": "roomba" + "projectName": "roomba", + "env": { + "GLFW_PLATFORM": "x11", + "LIBGL_ALWAYS_SOFTWARE": "1", + "LIBGL_DRI3_DISABLE": "1", + "__GLX_VENDOR_LIBRARY_NAME": "mesa", + "MESA_LOADER_DRIVER_OVERRIDE": "llvmpipe", + "GALLIUM_DRIVER": "llvmpipe", + "HALSIM_EXTENSIONS": "${workspaceFolder}/build/jni/release/libhalsim_gui.so", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true + "desktop": true, + "env": { + "GLFW_PLATFORM": "x11", + "LIBGL_ALWAYS_SOFTWARE": "1", + "LIBGL_DRI3_DISABLE": "1", + "__GLX_VENDOR_LIBRARY_NAME": "mesa", + "MESA_LOADER_DRIVER_OVERRIDE": "llvmpipe", + "GALLIUM_DRIVER": "llvmpipe", + "HALSIM_EXTENSIONS": "${workspaceFolder}/build/jni/release/libhalsim_gui.so", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } }, { "type": "wpilib", diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede86..babf34e4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,6 +27,8 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "java.import.gradle.annotationProcessing.enabled": false, + "wpilib.skipSelectSimulateExtension": true, + "wpilib.selectDefaultSimulateExtension": true, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", "org.junit.Assume.*", @@ -57,5 +59,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false + "java.dependency.enableDependencyCheckup": false, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/README.md b/README.md index a1eac699..ad882e84 100644 --- a/README.md +++ b/README.md @@ -30,3 +30,13 @@ git fetch --prune && git branch -vv | grep ': gone]' | awk '{print $1}' | xargs ![GitHub Release](https://img.shields.io/github/v/release/robohornets/WhatTime) [https://github.com/robohornets/WhatTime](https://github.com/robohornets/WhatTime) + +# QuestNav +![GitHub Release](https://img.shields.io/github/v/release/QuestNav/QuestNav) + +[https://github.com/QuestNav/QuestNav/releases](https://github.com/QuestNav/QuestNav/releases) + +# AdvantageKit +![GitHub Release](https://img.shields.io/github/v/release/Mechanical-Advantage/AdvantageKit) + +[https://github.com/Mechanical-Advantage/AdvantageKit/releases](https://github.com/Mechanical-Advantage/AdvantageKit/releases) \ No newline at end of file diff --git a/build.gradle b/build.gradle index 732788e6..cf55bbd7 100644 --- a/build.gradle +++ b/build.gradle @@ -88,6 +88,106 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() +def simSoftwareGl = (project.findProperty("simSoftwareGl") ?: System.getenv("SIM_SOFTWARE_GL"))?.toString()?.toBoolean() +def simGlxVendor = (project.findProperty("simGlxVendor") ?: System.getenv("SIM_GLX_VENDOR"))?.toString() +def waylandSession = System.getenv("WAYLAND_DISPLAY") != null + +tasks.withType(JavaExec).configureEach { + if (!name.startsWith("simulate")) { + return + } + + doFirst { + // Hyprland/Wayland sessions are more reliable when GLFW targets Xwayland. + if (waylandSession && !environment.containsKey("GLFW_PLATFORM")) { + environment("GLFW_PLATFORM", "x11") + } + + if (simSoftwareGl) { + environment("LIBGL_ALWAYS_SOFTWARE", "1") + } + + environment("LIBGL_DRI3_DISABLE", "1") + environment("__GLX_VENDOR_LIBRARY_NAME", "mesa") + environment("MESA_LOADER_DRIVER_OVERRIDE", "llvmpipe") + environment("GALLIUM_DRIVER", "llvmpipe") + + if (simGlxVendor) { + environment("__GLX_VENDOR_LIBRARY_NAME", simGlxVendor) + } + + // Remove empty path entries (e.g. trailing ':') that can break extension loading. + def rawExtensions = (environment["HALSIM_EXTENSIONS"] ?: System.getenv("HALSIM_EXTENSIONS") ?: "").toString() + def cleanedExtensions = rawExtensions.split(":").findAll { !it.trim().isEmpty() }.join(":") + if (rawExtensions != cleanedExtensions) { + environment("HALSIM_EXTENSIONS", cleanedExtensions) + } + } +} + +def patchWpilibJavaSimJson = { File jsonFile -> + if (!jsonFile.exists()) { + return + } + + def parsed = new groovy.json.JsonSlurper().parseText(jsonFile.text) + if (!(parsed instanceof List)) { + return + } + + parsed.each { entry -> + if (!(entry instanceof Map)) { + return + } + + if (!(entry.environment instanceof Map)) { + entry.environment = [:] + } + + // Force X11-backed GLFW on Hyprland/Wayland. + if (waylandSession) { + entry.environment["GLFW_PLATFORM"] = "x11" + } + + // Software rendering avoids broken GLX context creation on this Hyprland setup. + entry.environment["LIBGL_ALWAYS_SOFTWARE"] = "1" + entry.environment["LIBGL_DRI3_DISABLE"] = "1" + entry.environment["__GLX_VENDOR_LIBRARY_NAME"] = "mesa" + entry.environment["MESA_LOADER_DRIVER_OVERRIDE"] = "llvmpipe" + entry.environment["GALLIUM_DRIVER"] = "llvmpipe" + + if (simGlxVendor) { + entry.environment["__GLX_VENDOR_LIBRARY_NAME"] = simGlxVendor + } + + def enabledExtensions = [] + if (entry.extensions instanceof List) { + enabledExtensions = entry.extensions + .findAll { it instanceof Map && it.defaultEnabled && it.libName } + .collect { it.libName.toString() } + } + + if (!enabledExtensions.isEmpty()) { + // Override WPILib extension launcher value to avoid trailing path delimiters. + entry.environment["HALSIM_EXTENSIONS"] = enabledExtensions.join(":") + } + } + + jsonFile.text = groovy.json.JsonOutput.prettyPrint(groovy.json.JsonOutput.toJson(parsed)) + System.lineSeparator() +} + +tasks.matching { it.name == "simulateExternalJavaRelease" }.configureEach { + doLast { + patchWpilibJavaSimJson(file("$buildDir/sim/release_java.json")) + } +} + +tasks.matching { it.name == "simulateExternalJavaDebug" }.configureEach { + doLast { + patchWpilibJavaSimJson(file("$buildDir/sim/debug_java.json")) + } +} + // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. @@ -119,4 +219,4 @@ gversion { dateFormat = "yyyy-MM-dd HH:mm:ss z" timeZone = "America/Chicago" indent = " " -} \ No newline at end of file +} diff --git a/simgui-ds.json b/simgui-ds.json index b65e6fb3..1a01ae9b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -102,5 +102,6 @@ { "guid": "Keyboard2" } - ] + ], + "zeroDisconnectedJoysticks": false } diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto new file mode 100644 index 00000000..36c5b6a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto @@ -0,0 +1,62 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeDown" + } + }, + { + "type": "path", + "data": { + "pathName": "Neutral Auto 1 - Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Neutral Auto 1 - Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": null + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootAutoFuel" + } + }, + { + "type": "named", + "data": { + "name": "AgitateAutoFuel" + } + }, + { + "type": "named", + "data": { + "name": "RunAllFeederIn" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto new file mode 100644 index 00000000..440a1ea5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Tests.auto b/src/main/deploy/pathplanner/autos/Tests.auto deleted file mode 100644 index 15fbbbda..00000000 --- a/src/main/deploy/pathplanner/autos/Tests.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SimplePath" - } - }, - { - "type": "named", - "data": { - "name": "shoot" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path deleted file mode 100644 index efd5399d..00000000 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 - }, - "prevControl": null, - "nextControl": { - "x": 2.495586630003213, - "y": 1.8166481481481478 - }, - "isLocked": false, - "linkedName": "HDH Point 1" - }, - { - "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 - }, - "prevControl": { - "x": 2.3721977719907414, - "y": 0.6346029369212967 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HDH Point 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path deleted file mode 100644 index b349155b..00000000 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 - }, - "prevControl": null, - "nextControl": { - "x": 1.3721977719907414, - "y": 0.6346029369212962 - }, - "isLocked": false, - "linkedName": "HDH Point 2" - }, - { - "anchor": { - "x": 2.4603841869212966, - "y": 2.5775276331018517 - }, - "prevControl": { - "x": 2.4603841869212966, - "y": 1.654830760080282 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 34.63187389435466 - }, - "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path new file mode 100644 index 00000000..757a5f23 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.455928530092593, + "y": 7.19860865162037 + }, + "prevControl": null, + "nextControl": { + "x": 4.793412840783701, + "y": 7.126602132123037 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.160853949652778, + "y": 7.029202256944444 + }, + "prevControl": { + "x": 6.6386748822686785, + "y": 7.591245387179791 + }, + "nextControl": { + "x": 7.80660430720271, + "y": 6.334154188230901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": { + "x": 7.389464795846691, + "y": 3.8473959916429026 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1729029605263157, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.0609580592105259, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Neutral Auto 1", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path new file mode 100644 index 00000000..d40f8182 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": null, + "nextControl": { + "x": 7.875992484880923, + "y": 2.2896624619841495 + }, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + }, + { + "anchor": { + "x": 7.160853949652778, + "y": 7.029202256944444 + }, + "prevControl": { + "x": 6.6386748822686785, + "y": 7.591245387179791 + }, + "nextControl": { + "x": 7.80660430720271, + "y": 6.334154188230901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": { + "x": 7.389464795846691, + "y": 3.8473959916429026 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1729029605263157, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.0609580592105259, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Neutral Auto 1", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path similarity index 71% rename from src/main/deploy/pathplanner/paths/HDH - Part 1.path rename to src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path index e40a25de..5fdaa0f4 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6584026786452846, - "y": 3.816648148148148 + "x": 3.61808203125, + "y": 3.84925658275463 }, "prevControl": null, "nextControl": { - "x": 2.6584026786452846, - "y": 3.816648148148148 + "x": 3.36808203125, + "y": 3.84925658275463 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 + "x": 2.89557884837963, + "y": 3.84925658275463 }, "prevControl": { - "x": 3.495586630003213, - "y": 3.816648148148148 + "x": 3.14557884837963, + "y": 3.84925658275463 }, "nextControl": null, "isLocked": false, - "linkedName": "HDH Point 1" + "linkedName": "Neutral Auto 2 - Shoot Point" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", + "folder": "Neutral Auto 2", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path similarity index 52% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path index a4f1409a..832f518a 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path @@ -3,57 +3,41 @@ "waypoints": [ { "anchor": { - "x": 3.646, - "y": 3.839 + "x": 2.89557884837963, + "y": 3.84925658275463 }, "prevControl": null, "nextControl": { - "x": 3.6659141248056653, - "y": 2.7709942982324054 + "x": 2.83915379050926, + "y": 2.2673210358796303 }, "isLocked": false, - "linkedName": null + "linkedName": "Neutral Auto 2 - Shoot Point" }, { "anchor": { - "x": 3.646, - "y": 2.5083085214120375 + "x": 6.091139829282408, + "y": 2.286807364004629 }, "prevControl": { - "x": 3.5675651685926173, - "y": 2.745685810178456 + "x": 5.3060533457174754, + "y": 2.3267179618763683 }, "nextControl": { - "x": 3.757652405960003, - "y": 2.1704006693077176 + "x": 6.766140986689816, + "y": 2.2524930555555556 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.874690682870371, - "y": 2.273750868055556 + "x": 7.710210937499999, + "y": 2.6393983651620374 }, "prevControl": { - "x": 5.628975714164271, - "y": 2.2276623211728372 - }, - "nextControl": { - "x": 6.12040565157647, - "y": 2.319839414938275 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.42375535300926, - "y": 2.273750868055556 - }, - "prevControl": { - "x": 6.274127604166668, - "y": 2.0338131510416666 + "x": 7.147535011574075, + "y": 2.417831597222222 }, "nextControl": null, "isLocked": false, @@ -62,8 +46,12 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.2, - "rotationDegrees": 45.0 + "waypointRelativePos": 0.4914268092105266, + "rotationDegrees": 54.84095812282095 + }, + { + "waypointRelativePos": 0.8793585526315789, + "rotationDegrees": 179.17582565489303 } ], "constraintZones": [], @@ -79,10 +67,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 45.0 + "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Neutral Auto 2", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/SimplePath.path b/src/main/deploy/pathplanner/paths/SimplePath.path deleted file mode 100644 index 27e551fa..00000000 --- a/src/main/deploy/pathplanner/paths/SimplePath.path +++ /dev/null @@ -1,139 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6078573019462716, - "y": 1.9760754180372806 - }, - "prevControl": null, - "nextControl": { - "x": 3.542609194557422, - "y": 2.2174106298945087 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.632999802974233, - "y": 2.532426800644188 - }, - "prevControl": { - "x": 3.637278791529922, - "y": 1.5328750011405277 - }, - "nextControl": { - "x": 5.499249744303021, - "y": 3.4020094224060635 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.001672106291119, - "y": 3.4112223307291663 - }, - "prevControl": { - "x": 6.599926789143243, - "y": 3.405055521175869 - }, - "nextControl": { - "x": 7.542175118963073, - "y": 3.4195190774486917 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.71500089946546, - "y": 2.532426800644188 - }, - "prevControl": { - "x": 7.8853493729799276, - "y": 3.1124078786697877 - }, - "nextControl": { - "x": 7.462741885480951, - "y": 1.6735670066350252 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.9750896038925445, - "y": 2.2326333778782894 - }, - "prevControl": { - "x": 5.216321610053111, - "y": 2.16700473166531 - }, - "nextControl": { - "x": 3.946295606997631, - "y": 2.5125230920288746 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.483472322162829, - "y": 2.9627905701754376 - }, - "prevControl": { - "x": 3.2772546423557425, - "y": 2.5587835655112747 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1, - "rotationDegrees": -46.32240446147237 - }, - { - "waypointRelativePos": 1.5918582940251573, - "rotationDegrees": -18.50084926673444 - }, - { - "waypointRelativePos": 2.777736831761006, - "rotationDegrees": -89.11098055349674 - }, - { - "waypointRelativePos": 3.2713615369496862, - "rotationDegrees": -179.9430155787815 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3, - "maxAcceleration": 3, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 31.038981760511703 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -0.18733647533512643 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Testing Pose Path.path b/src/main/deploy/pathplanner/paths/Testing Pose Path.path index c9ccdf50..c82bea20 100644 --- a/src/main/deploy/pathplanner/paths/Testing Pose Path.path +++ b/src/main/deploy/pathplanner/paths/Testing Pose Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5680509348110263, - "y": 3.836822198234916 + "x": 3.643546856465007, + "y": 0.3392170818505338 }, "prevControl": null, "nextControl": { - "x": 2.8144249335901694, - "y": 3.836822198234916 + "x": 3.393546856465007, + "y": 0.33921708185053384 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.799621182152162, - "y": 3.836822198234916 + "x": 3.008754448398578, + "y": 0.3392170818505338 }, "prevControl": { - "x": 3.799621182152162, - "y": 3.836822198234916 + "x": 3.258754448398578, + "y": 0.3392170818505338 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 045f8583..073c4134 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,9 +1,10 @@ { - "robotWidth": 0.762, - "robotLength": 0.762, + "robotWidth": 0.7874, + "robotLength": 0.7874, "holonomicMode": true, "pathFolders": [ - "1 - Hub, Depot, Hub (HDH)" + "Neutral Auto 1", + "Neutral Auto 2" ], "autoFolders": [], "defaultMaxVel": 3.0, @@ -30,5 +31,7 @@ "brModuleY": -0.267, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.425,\"y\":0.0},\"size\":{\"width\":0.6,\"length\":0.15},\"borderRadius\":0.02,\"strokeWidth\":0.01,\"filled\":false}}" + ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8ae32ba3..90cd7f56 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,15 +5,15 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 122; - public static final String GIT_SHA = "1bf40e3766827d608ce15c35ab7e1da9d04e127b"; - public static final String GIT_DATE = "2026-02-25 19:11:15 CST"; - public static final String GIT_BRANCH = "feature/driving-locked-to-hub"; - public static final String BUILD_DATE = "2026-02-25 19:14:38 CST"; - public static final long BUILD_UNIX_TIME = 1772068478558L; - public static final int DIRTY = 0; + public static final int GIT_REVISION = 228; + public static final String GIT_SHA = "f4ba87c2d6f7a5b07ba77b458e5dbf8cc4f11f48"; + public static final String GIT_DATE = "2026-03-19 18:10:24 CDT"; + public static final String GIT_BRANCH = "Whattime-refactor"; + public static final String BUILD_DATE = "2026-03-20 08:18:59 CDT"; + public static final long BUILD_UNIX_TIME = 1774012739222L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 33f8dfae..11c4aff6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import java.util.Arrays; import java.util.Optional; import org.littletonrobotics.junction.LogFileUtil; @@ -15,16 +14,12 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import com.btwrobotics.WhatTime.frc.DriverStation.MatchTimeManager; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorBulkActions; import com.btwrobotics.WhatTime.frc.YearlyMethods.Rebuilt.RebuiltHubManager; import com.ctre.phoenix6.HootAutoReplay; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.AdvantageKit.AdvantageKitConstants; @@ -52,13 +47,6 @@ public class Robot extends LoggedRobot { // The current alliance for the robot public Optional currentAlliance; - // Manages rumble for Xbox controller - // Start high so it doesn't trigger randomly - public double nextRumbleStartTime = 1000; - public MotorBulkActions motorBulkActions = new MotorBulkActions(); - - - // MARK: Hub Manager public MatchTimeManager matchTimeManager = new MatchTimeManager(); public RebuiltHubManager rebuiltHubManager = new RebuiltHubManager(matchTimeManager); @@ -67,6 +55,7 @@ public Robot() { robotContainer = new RobotContainer(); } + // MARK: Robot Init @Override public void robotInit() { // Configure logging for AdvantageKit @@ -99,53 +88,62 @@ public void robotInit() { // Start AdvantageKit logging Logger.start(); - currentAlliance = DriverStation.getAlliance(); - - motorBulkActions.setNeutralModeBulk(Arrays.asList( - robotContainer.shooterSubsystem.shooterPitchMotor, - robotContainer.shooterSubsystem.shooterMotor, - robotContainer.climberSubsystem.climberLeft, - robotContainer.climberSubsystem.climberRight - ), NeutralModeValue.Brake); - - - Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); + DriverStation.silenceJoystickConnectionWarning(true); + + // Enable motors (WhatTime Motor.isEnabled defaults to false — drive/goTo do nothing until this is called) + robotContainer.shooterSubsystem.shooterPitchMotor.toggleEnabled(true); + robotContainer.shooterSubsystem.leftShooterMotor.toggleEnabled(true); + robotContainer.shooterSubsystem.rightShooterMotor.toggleEnabled(true); + // robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); + robotContainer.intakeSubsystem.intakeWheelsMotor.toggleEnabled(true); + robotContainer.feederSubsystem.feederBedMotor.toggleEnabled(true); + robotContainer.feederSubsystem.feederFeederMotor.toggleEnabled(true); + robotContainer.feederSubsystem.shooterFeederMotor.toggleEnabled(true); + + // Apply full motor config here (after CAN bus is stable — constructor-time apply() silently fails) + // StatusCodes are logged so you can verify success in AdvantageScope under MotorConfig/ + Logger.recordOutput("MotorConfig/ShooterPitch", robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/LeftShooter", robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/RightShooter", robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + // Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/IntakeWheels", robotContainer.intakeSubsystem.intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/FeederBed", robotContainer.feederSubsystem.feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/FeederFeeder", robotContainer.feederSubsystem.feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/ShooterFeeder", robotContainer.feederSubsystem.shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); } + // MARK: Robot Periodic @Override public void robotPeriodic() { - pdp.clearStickyFaults(); - m_timeAndJoystickReplay.update(); CommandScheduler.getInstance().run(); - matchTimeRemainingSeconds = DriverStation.getMatchTime(); - matchTimeElapsedSeconds = 160 - matchTimeRemainingSeconds; - - if (matchTimeElapsedSeconds - nextRumbleStartTime >= 0 && matchTimeElapsedSeconds - nextRumbleStartTime <= 2) { - robotContainer.driverJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - robotContainer.operatorJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - robotContainer.debugJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - } - else { - robotContainer.driverJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - robotContainer.operatorJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - robotContainer.debugJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - } + logDriveStationValues(); - updateNetworkTablesValues(); + // if (!currentAlliance.equals(DriverStation.getAlliance())) { + // currentAlliance = DriverStation.getAlliance(); + // Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); + // } } + // MARK: Disabled Init @Override - public void disabledInit() {} + public void disabledInit() { + } + // MARK: Disabled Periodic @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + pdp.clearStickyFaults(); + } + // MARK: Disabled Exit @Override - public void disabledExit() {} + public void disabledExit() { + } + // MARK: Autonomous Init @Override public void autonomousInit() { m_autonomousCommand = robotContainer.getAutonomousCommand(); @@ -155,12 +153,15 @@ public void autonomousInit() { } } + // MARK: Autonomous Periodic @Override public void autonomousPeriodic() {} + // MARK: Autonomous Exit @Override public void autonomousExit() {} + // MARK: Teleop Init @Override public void teleopInit() { if (m_autonomousCommand != null) { @@ -168,6 +169,7 @@ public void teleopInit() { } } + // MARK: Teleop Periodic @Override public void teleopPeriodic() { // NetworkTablesUtil.put("Hub is Active", rebuiltHubManager.hubIsActive()); @@ -176,37 +178,38 @@ public void teleopPeriodic() { Logger.recordOutput("RebuiltHubManager/InactiveFirst", rebuiltHubManager.getInactiveFirstAlliance().toString()); } + // MARK: Teleop Exit @Override public void teleopExit() {} + // MARK: Test Init @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); } + // MARK: Test Periodic @Override public void testPeriodic() {} + // MARK: Test Exit @Override public void testExit() {} + // MARK: Simulation Periodic @Override public void simulationPeriodic() {} - Field2d robotField2d = new Field2d(); - - Field2d limelight4Field2d = new Field2d(); - Field2d limelight2Field2d = new Field2d(); - - public void updateNetworkTablesValues() { - // MARK: use limelight to calculate this - // double[] robotPose = NetworkTableInstance.getDefault().getTable("Pose").getEntry("robotPose").getDoubleArray(new double[]{0.0,0.0,0.0}); - // NetworkTableInstance.getDefault().getTable("CustomDashboard").getEntry("Pose").setDoubleArray(robotPose); - - Logger.recordOutput("MatchInfo/TimeRemaining", DriverStation.getMatchTime()); - Logger.recordOutput("ShooterSubsystem/Pitch", robotContainer.shooterSubsystem.getShooterMotorPitchDeg()); - //robotField2d.setRobotPose(robotContainer.drivetrain.getState().Pose); - // NetworkTablesUtil.put("Main Robot Pose", robotField2d); + // MARK: Log DriverStation + private void logDriveStationValues() { + Logger.recordOutput("DriverStation/GameSpecificMessage", DriverStation.getGameSpecificMessage()); + Logger.recordOutput("DriverStation/MatchTime", DriverStation.getMatchTime()); + Logger.recordOutput("DriverStation/MatchType", DriverStation.getMatchType()); + Logger.recordOutput("DriverStation/MatchNumber", DriverStation.getMatchNumber()); + Logger.recordOutput("DriverStation/IsAutonomous", DriverStation.isAutonomous()); + Logger.recordOutput("DriverStation/AutonomousEnabled", DriverStation.isAutonomousEnabled()); + Logger.recordOutput("DriverStation/IsEnabled", DriverStation.isEnabled()); + Logger.recordOutput("DriverStation/IsEStopped", DriverStation.isEStopped()); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb7bde6d..48a0cc3e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,16 +4,24 @@ package frc.robot; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import static edu.wpi.first.units.Units.Amps; +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -24,66 +32,70 @@ import frc.robot.joysticks.OperatorJoystick; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; -public class RobotContainer { - // public static double MAX_SPEED = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - // public static double MAX_ANGULAR_RATE = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity +public class RobotContainer { // MARK: Drivetrain // Create the swerve drivetrain subsystem for the robot public final Drive drivetrain = new Drive(TunerConstants.createDrivetrain()); - - // private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(DriveConstants.MAX_SPEED); - // MARK: Vision - // Uses the Quest to periodically add vision measurements - // public QuestNavSubsystem questNavSubsystem = new QuestNavSubsystem(drivetrain); - - // LimelightSubsystem limelight2Subsystem = new LimelightSubsystem(drivetrain, "limelight-two"); - // MARK: Subsystems - public final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); - public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); - public final MotorSubsystem motorSubsystem = new MotorSubsystem(); public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(drivetrain); + public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + public final FeederSubsystem feederSubsystem = new FeederSubsystem(); // MARK: Xbox Controllers - public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain); - public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain); - - - - + public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); + public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); + public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); // MARK: Register Commands - public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); + public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem, feederSubsystem); + + // MARK: Motor Config + public static final TalonFXConfiguration mechanismsMotorConfiguration = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(100)) + .withSupplyCurrentLimit(Amps.of(40)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) + ) + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(0.04) + ); - // MARK: Tests - public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); + // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); - - // private final SendableChooser autoChooser; - private final LoggedDashboardChooser autoChooser; + private final SendableChooser autoChooser; public RobotContainer() { - DriverStation.silenceJoystickConnectionWarning(true); - registerCommands.registerCommands(); - autoChooser = new LoggedDashboardChooser<>("Autonomous Mode", AutoBuilder.buildAutoChooser()); + // MARK: Auto Chooser + autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.onChange( + command -> { + Commands.runOnce( + () -> { + Logger.recordOutput("Autonomous/SelectedAuto", autoChooser.getSelected().getName()); + } + ); + } + ); + SmartDashboard.putData("Auto Chooser", autoChooser); // MARK: Run Tests /* Disable tests on actual code */ - tests.runTests(); + // tests.runTests(); configureBindings(); @@ -96,16 +108,7 @@ private void configureBindings() { driverJoystick.configureBindings(); operatorJoystick.configureBindings(); debugJoystick.configureBindings(); - - // Positive X is forward, Positive Y is left according to WPILib - // drivetrain.setDefaultCommand( - // drivetrain.applyRequest(() -> - // Drive.drive - // .withVelocityX(-driverJoystick.joystick.getLeftY() * DriveConstants.MAX_SPEED) // Drive forward with negative Y (forward) - // .withVelocityY(-driverJoystick.joystick.getLeftX() * DriveConstants.MAX_SPEED) // Drive left with negative X (left) - // .withRotationalRate(-driverJoystick.joystick.getRightX() * DriveConstants.MAX_ANGULAR_RATE) // Drive counterclockwise with negative X (left) - // ) - // ); + drivetrain.setDefaultCommand(drivetrain.joysticksDefaultCommand(driverJoystick.joystick)); // Idle while the robot is disabled. This ensures the configured @@ -129,8 +132,10 @@ private void configureBindings() { drivetrain.registerTelemetry(logger::telemeterize); } + // MARK: Get Autonomous public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ - return autoChooser.get(); + // return autoChooser.get(); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/Tests.java b/src/main/java/frc/robot/Tests.java index 7fc6b71e..4ca7513e 100644 --- a/src/main/java/frc/robot/Tests.java +++ b/src/main/java/frc/robot/Tests.java @@ -1,21 +1,18 @@ package frc.robot; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; +import frc.robot.subsystems.math.MathSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; public class Tests { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; - ClimberSubsystem climberSubsystem; - MotorSubsystem motorSubsystem; + MathSubsystem motorSubsystem; - public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MotorSubsystem motorSubsystem) { + public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, MathSubsystem motorSubsystem) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; - this.climberSubsystem = climberSubsystem; this.motorSubsystem = motorSubsystem; } // Run tests diff --git a/src/main/java/frc/robot/commands/NamedCommands.java b/src/main/java/frc/robot/commands/NamedCommands.java deleted file mode 100644 index 657d0286..00000000 --- a/src/main/java/frc/robot/commands/NamedCommands.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.commands; - -public class NamedCommands { - -} diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index a5cf3484..34312c1d 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -2,26 +2,92 @@ import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.mechanisms.feeder.FeederState; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; +import frc.robot.subsystems.mechanisms.intake.IntakeConstants; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; public class RegisterCommands { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; - ClimberSubsystem climberSubsystem; - MotorSubsystem motorSubsystem; + FeederSubsystem feederSubsystem; - public RegisterCommands(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MotorSubsystem motorSubsystem) { + public RegisterCommands( + IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, + FeederSubsystem feederSubsystem + ) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; - this.climberSubsystem = climberSubsystem; - this.motorSubsystem = motorSubsystem; + this.feederSubsystem = feederSubsystem; } public void registerCommands(){ - NamedCommands.registerCommand("shoot", shooterSubsystem.aimAtHub()); + // MARK: ShootFullSpeed + NamedCommands.registerCommand("ShootFullSpeed", + Commands.run( + () -> { + shooterSubsystem.shooterMotors.drive(); + } + ).withTimeout(5) + ); + // MARK: IntakeDown + NamedCommands.registerCommand("IntakeDown", + Commands.run( + () -> { + intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE); + }, intakeSubsystem + ) + ); + + // MARK: IntakeUp + NamedCommands.registerCommand("IntakeUp", + Commands.run( + () -> { + intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE); + }, intakeSubsystem + ) + ); + + // MARK: IntakeAgitate + NamedCommands.registerCommand("IntakeAgitate", + Commands.sequence( + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE / 2)), + Commands.waitSeconds(0.75), + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE)) + ) + ); + + // MARK: AgitateAutoFuel + NamedCommands.registerCommand("AgitateAutoFuel", + Commands.repeatingSequence( + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE / 2)), + Commands.waitSeconds(2), + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE)), + Commands.waitSeconds(2) + ) + ); + + // MARK: RunIntakeIn + NamedCommands.registerCommand("IntakeIn", + Commands.runOnce( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + } + ) + ); + + // MARK: + NamedCommands.registerCommand("RunAllFeederIn", + Commands.runOnce( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); + } + ) + ); } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b9c7c2fc..06e17620 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -25,7 +25,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) + .withKS(0.1).withKV(2.33).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -55,14 +55,25 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) ); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs @@ -74,14 +85,14 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.58); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 18.75; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; @@ -129,7 +140,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 2; private static final int kFrontLeftSteerMotorId = 1; private static final int kFrontLeftEncoderId = 30; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.406005859375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.320556640625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -140,7 +151,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 3; private static final int kFrontRightEncoderId = 31; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.355224609375); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.132568359375); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -151,7 +162,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 8; private static final int kBackLeftSteerMotorId = 7; private static final int kBackLeftEncoderId = 33; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.009765625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114501953125); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -162,7 +173,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 6; private static final int kBackRightSteerMotorId = 5; private static final int kBackRightEncoderId = 32; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.1044921875); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.062744140625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 3ba896cd..e39ad1e5 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -2,52 +2,142 @@ import org.littletonrobotics.junction.Logger; +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederState; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; +import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; +import frc.robot.subsystems.vision.limelight.LimelightConstants; import frc.robot.subsystems.vision.limelight.LimelightHelpers; public class DebugJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final FeederSubsystem feederSubsystem; + public DebugJoystick( CommandXboxController joystick, - Drive drivetrain + Drive drivetrain, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; + + + // intakeSubsystem.setDefaultCommand( + // Commands.run( + // () -> { + // double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); + // intakeSubsystem.intakeWheelsMotor.getMotor().set(triggerSpeed); + + // Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); + // }, intakeSubsystem + // ) + // ); } public void configureBindings() { - joystick.a(); - - joystick.b(); + // Continuously adjust flywheel speed (left Y) and pitch target (right Y). + // No subsystem requirement — just updates the shared values; the subsystem's + // default command picks them up and calls the motors. + Commands.run(() -> { + double changeAmountPerTick = 0.0025; + double leftY = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY() : 0.0; + double rightY = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY() : 0.0; + + // shooterSubsystem.shooterMotors.drive(leftY); + + + // shooterSubsystem.shooterMotors.drive(shooterSubsystem.leftShooterMotor.getCurrentValue() + Math.signum(leftY) * changeAmountPerTick); + // shooterSubsystem.shooterPitchMotor.goTo(shooterSubsystem.shooterPitchMotor.getCurrentValue() + + Math.signum(rightY) * changeAmountPerTick); + // shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); + // shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); + }).schedule(); + + + joystick.a().onTrue( + NamedCommands.getCommand("IntakeDown") + ); - joystick.x(); + joystick.b().onTrue( + NamedCommands.getCommand("IntakeUp") + ); - joystick.y(); + joystick.x().onTrue( + Commands.runOnce( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); + } + ) + ); - joystick.rightTrigger(); + joystick.y().onTrue( + Commands.runOnce( + () -> { + feederSubsystem.setFeederState(FeederState.OFF); + } + ) + ); - joystick.leftTrigger(); + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( + () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), + () -> intakeSubsystem.setIntake(IntakeStates.OFF), + intakeSubsystem + ) + ); - joystick.rightBumper(); + joystick.leftBumper().onTrue( + Commands.runOnce( + () -> { + shooterSubsystem.setPitchTarget(50); + } + ) + ); - joystick.leftBumper(); + // Reset pose to limelight output + joystick.povUp().onTrue( + Commands.runOnce( + () -> { + Logger.recordOutput("SwerveDrive/SetSwervePoseLimelight", true); - joystick.povUp(); + drivetrain.resetPose(LimelightHelpers.getBotPose2d("limelight-four")); + } + ) + ); // Reset Field Centric Heading - joystick.povDown().onTrue(drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric)); + joystick.povDown().onTrue( + drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric) + ); joystick.povLeft().onTrue( Commands.runOnce( () -> { Logger.recordOutput("QuestNav/SetQuestPose", true); // Reset QuestNav pose to Limelight position - drivetrain.questNavSubsystem.setQuestPose(LimelightHelpers.getBotPose3d_wpiBlue("limelight-four")); - + drivetrain.questNavSubsystem.setQuestPose( + LimelightHelpers.getBotPose3d_wpiBlue("limelight-four") + .transformBy( + new Transform3d(LimelightConstants.LIMELIGHT_4_TRANSFORM_FROM_CENTRE).inverse() + ) + ); + Logger.recordOutput("QuestNav/SetQuestPose", false); } ) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 7e8d2743..a7797b72 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -2,23 +2,37 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.vision.limelight.LimelightConstants; -import frc.robot.subsystems.vision.limelight.LimelightHelpers; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; +import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterConstants; +import frc.robot.subsystems.mechanisms.shooter.ShooterDataPoint; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class DriverJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final FeederSubsystem feederSubsystem; public DriverJoystick( CommandXboxController joystick, - Drive drivetrain + Drive drivetrain, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; + + Logger.recordOutput("DriverJoystick/ShooterSpeed", 0.0); } public void configureBindings() { @@ -38,46 +52,88 @@ public void configureBindings() { joystick.y(); - joystick.rightTrigger(); - - joystick.leftTrigger(); + final ShooterDataPoint[] saveShooterDataPoint = {new ShooterDataPoint(0.0, 0.0, 0.0)}; + + // MARK: RT - Shooter shoot + joystick.rightTrigger() + .whileTrue( + Commands.startRun( + () -> { + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; + + shooterDataPoint = shooterSubsystem.calculateShooterValues( + shooterSubsystem.shooterUpperLower(), + drivetrain.getDistanceToHub() + ); + + double rpm = shooterSubsystem.getRequiredRPM(shooterDataPoint); + + double maxRPM = 200; // MARK: Populate max rpm + + shooterDataPoint.speed = rpm / maxRPM; + shooterDataPoint.angle = shooterDataPoint.angle / 180; // angle (0.5 = 180deg) + + saveShooterDataPoint[0] = shooterDataPoint; + }, + () -> { + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterDataPoint.speed); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooterDataPoint.speed); + + // maintain motor speed + shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); + feederSubsystem.shooterFeederMotor.drive(shooterDataPoint.speed); + shooterSubsystem.shooterPitchMotor.goTo(shooterDataPoint.angle); + + saveShooterDataPoint[0] = shooterDataPoint; + }, + shooterSubsystem, drivetrain + ) + ) + .onFalse( + Commands.run( + () -> { + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; + + shooterDataPoint.speed = Math.max(0.0, shooterDataPoint.speed - 0.05); + shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); + feederSubsystem.shooterFeederMotor.drive(shooterDataPoint.speed); + + shooterSubsystem.shooterPitchMotor.goTo(ShooterConstants.SHOOTER_MAX_ANGLE); + + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterDataPoint.speed); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooterDataPoint.angle); + + saveShooterDataPoint[0] = shooterDataPoint; + }, + shooterSubsystem + ) + .until(() -> saveShooterDataPoint[0].speed <= 0.0) + ); + + // MARK: LT - Intake + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + }, + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + intakeSubsystem + ) + ); joystick.rightBumper(); joystick.leftBumper(); - // Reset pose to limelight output - joystick.povUp().onTrue( - Commands.runOnce( - () -> { - Logger.recordOutput("SwerveDrive/SetSwervePoseLimelight", true); - - drivetrain.resetPose(LimelightHelpers.getBotPose2d("limelight-four")); - } - ) - ); - - // Reset Field Centric Heading - joystick.povDown().onTrue(drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric)); + joystick.povUp(); - //joystick.povDown(); + joystick.povDown(); - joystick.povLeft().onTrue( - Commands.runOnce( - () -> { - Logger.recordOutput("QuestNav/SetQuestPose", true); - // Reset QuestNav pose to Limelight position - drivetrain.questNavSubsystem.setQuestPose( - LimelightHelpers.getBotPose3d_wpiBlue("limelight-four") - .transformBy( - new Transform3d(LimelightConstants.LIMELIGHT_4_TRANSFORM_FROM_CENTRE).inverse() - ) - ); - - Logger.recordOutput("QuestNav/SetQuestPose", false); - } - ) - ); + joystick.povLeft(); joystick.povRight(); } diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index cdd27950..c6f50c5a 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -1,62 +1,116 @@ package frc.robot.joysticks; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederState; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterConstants; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class OperatorJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; - + private final FeederSubsystem feederSubsystem; + public OperatorJoystick( CommandXboxController joystick, - Drive drivetrain, - IntakeSubsystem intakeSubsystem + Drive drivetrain, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; + + } public void configureBindings() { - joystick.a(); - - joystick.b(); + // MARK: Intake Down - A + joystick.a().onTrue( + NamedCommands.getCommand("IntakeDown") + ); - joystick.x(); + // MARK: Intake Up - B + joystick.b().onTrue( + NamedCommands.getCommand("IntakeUp") + ); - joystick.y(); + // MARK: Jostle Fuel - X + joystick.x().onTrue( + NamedCommands.getCommand("IntakeAgitate") + ); - // MARK: Intake out - joystick.rightTrigger().onTrue( - Commands.runOnce( + // MARK: nothing - Y + joystick.y().whileTrue( + Commands.run( () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_OUT); + shooterSubsystem.shooterPitchMotor.goTo(ShooterConstants.SHOOTER_MAX_ANGLE); } ) ); - // MARK: Intake in - joystick.leftTrigger().onTrue( - Commands.runOnce( + // MARK: Intake In - LT + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + }, + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + intakeSubsystem + ) + ); + + + // MARK: Intake Out - LB + joystick.leftBumper().whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + // Reset intake to last state. + intakeSubsystem.setIntake(intakeSubsystem.lastIntakeState); } ) ); - joystick.rightBumper().onTrue( - Commands.runOnce( + // MARK: Shoot Feed In - RT + joystick.rightTrigger().whileTrue( + Commands.runEnd( () -> { - + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); + }, + () -> { + feederSubsystem.setFeederState(FeederState.OFF); } ) ); - joystick.leftBumper(); + // MARK: Shoot Feed Out - RB + joystick.rightBumper().whileTrue( + Commands.runEnd( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_OUT); + }, + () -> { + feederSubsystem.setFeederState(FeederState.OFF); + } + ) + ); joystick.povUp(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 840b678b..950daccb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.drive; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import java.util.function.Consumer; import java.util.function.Supplier; @@ -26,14 +22,12 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.vision.limelight.LimelightSubsystem; import frc.robot.subsystems.vision.questnav.QuestNavSubsystem; @@ -43,41 +37,37 @@ public class Drive extends SubsystemBase { public QuestNavSubsystem questNavSubsystem; - LimelightSubsystem limelightSubsystem; + public LimelightSubsystem limelightSubsystem; + // MARK: Constructor public Drive(CommandSwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; this.questNavSubsystem = new QuestNavSubsystem(this); - this.limelightSubsystem = new LimelightSubsystem(this, questNavSubsystem, "limelight-four"); configureAutoBuilder(); } - public static final double ODOMETRY_FREQUENCY = 250.0; - public static double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - public static double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - // MARK: Field Centric Drive + // MARK: Field Centric public static final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - // MARK: Field Centric Drive with Heading Control + // MARK: Heading Control public final SwerveRequest.FieldCentricFacingAngle driveFacingHub = new SwerveRequest.FieldCentricFacingAngle() .withHeadingPID(5, 0, 0) - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - // MARK: Robot Centric Drive + // MARK: Robot Centric public static final SwerveRequest.RobotCentric driveRobotCentric = new SwerveRequest.RobotCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); @@ -90,15 +80,8 @@ public Drive(CommandSwerveDrivetrain drivetrain) { public void periodic() { drivetrain.periodic(); - Logger.recordOutput("SwerveDrive/Pose", getPose2d()); - - // Log module states for AdvantageScope swerve visualization - Logger.recordOutput("SwerveDrive/ModuleStates", drivetrain.getState().ModuleStates); - Logger.recordOutput("SwerveDrive/ModuleTargets", drivetrain.getState().ModuleTargets); - Logger.recordOutput("SwerveDrive/ChassisSpeeds", drivetrain.getState().Speeds); - Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); - - Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + logValues(); + logMotorInformation(); } public Command applyRequest(Supplier request) { @@ -188,7 +171,7 @@ private void configureAutoBuilder() { config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - drivetrain // Subsystem for requirements + this // Subsystem for requirements ); } catch (Exception ex) { DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); @@ -224,6 +207,7 @@ public Command joysticksDefaultCommand(CommandXboxController joystick) { ); } + // MARK: Get Angle to Hub public Rotation2d getAngleToHub() { Pose2d robotPose = getPose2d(); @@ -246,6 +230,25 @@ public Rotation2d getAngleToHub() { return Rotation2d.fromDegrees(rotationAngleDegrees); } + // MARK: Get Distance to Hub + public double getDistanceToHub() { + Pose2d robotPose = getPose2d(); + + Translation2d targetHub = DriverStation.getAlliance() + .orElse(Alliance.Blue) + .equals(Alliance.Blue) + ? DriveConstants.HUB_BLUE_POSITION: + DriveConstants.HUB_RED_POSITION; + + // Get X distance + double xDistance = targetHub.getX() - robotPose.getX(); + // Get Y distance + double yDistance = targetHub.getY() - robotPose.getY(); + + return Math.sqrt((xDistance * xDistance) + (yDistance * yDistance)); + } + + // MARK: Flip Alliance public static Pose2d flipAlliance(Pose2d pose) { if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { return new Pose2d( @@ -256,4 +259,59 @@ public static Pose2d flipAlliance(Pose2d pose) { } return pose; } + + // MARK: Logging + private void logValues() { + Logger.recordOutput("SwerveDrive/Pose", getPose2d()); + + // Log module states for AdvantageScope swerve visualization + Logger.recordOutput("SwerveDrive/ModuleStates", drivetrain.getState().ModuleStates); + Logger.recordOutput("SwerveDrive/ModuleTargets", drivetrain.getState().ModuleTargets); + Logger.recordOutput("SwerveDrive/ChassisSpeeds", drivetrain.getState().Speeds); + Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); + + Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + Logger.recordOutput("SwerveDrive/DistanceToHub", getDistanceToHub()); + } + + // MARK: Motor Logging + public void logMotorInformation() { + for (int i = 0; i <= 3; i++) { + // Logs the current readings to AdvantageKit + Logger.recordOutput( + "SwerveDrive/Motors/Current/Stator/SteerMotor" + i, + drivetrain.getModule(i).getSteerMotor().getStatorCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Stator/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Supply/SteerMotor" + i, + drivetrain.getModule(i).getSteerMotor().getSupplyCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Supply/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble() + ); + + // Logs the voltage to AdvantageKit + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Output/SteerMotor" + i, + drivetrain.getModule(i).getDriveMotor().getMotorVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Output/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getMotorVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Supply/SteerMotor" + i, + drivetrain.getModule(i).getDriveMotor().getSupplyVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Supply/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble() + ); + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 49908538..236c1b7c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -14,6 +14,8 @@ public class DriveConstants { // 3/4 of a rotation per second max angular velocity public static double MAX_ANGULAR_RATE = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + public static final double ODOMETRY_FREQUENCY = 250.0; + // MARK: Coordinates of Hub public static Translation2d HUB_RED_POSITION = new Translation2d(11.915, 4.034); diff --git a/src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java similarity index 76% rename from src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java rename to src/main/java/frc/robot/subsystems/math/MathSubsystem.java index becdccbb..79fe6fe2 100644 --- a/src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java @@ -1,7 +1,5 @@ -package frc.robot.subsystems.motor; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; +package frc.robot.subsystems.math; /** * MotorSubsystem provides utility methods for motion-related calculations used by @@ -14,7 +12,7 @@ * * In the future this should be moved to WhatTime */ -public class MotorSubsystem extends SubsystemBase { +public class MathSubsystem { /** * Calculates the signed angular difference (in degrees) required to rotate from a * reference heading to face a target described by a 2D position vector. @@ -77,4 +75,28 @@ public double[] calculateTrajectory(double distanceX, double distanceY, double e } + /** + * Calculates trajectory values when the shooter exit angle is provided as input. + * + * The method returns an array: + * [launchSpeed (m/s), enterAngle (degrees)]. + * + * @param distanceX Horizontal distance to the target (meters) + * @param distanceY Vertical distance to the target (meters) + * @param exitAngle Shooter exit angle in degrees (0 = horizontal) + * @return double[] where index 0 is launch speed (m/s), index 1 is enter angle (degrees) + */ + public double[] calculateTrajectoryFromExitAngle(double distanceX, double distanceY, double exitAngle){ + exitAngle = exitAngle * Math.PI / 180; + double enterAngle = Math.atan(2 * distanceY / distanceX - Math.tan(exitAngle)); + + double launchSpeed = Math.sqrt( + ( 9.81 * distanceX * distanceY ) / + ( 2 * Math.cos(exitAngle) * Math.cos(exitAngle) + * ( distanceX * Math.tan(exitAngle) - distanceY ) + )); + + return new double[]{launchSpeed, enterAngle * 180 / Math.PI}; + } + } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java deleted file mode 100644 index 7af5ad81..00000000 --- a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java +++ /dev/null @@ -1,121 +0,0 @@ -package frc.robot.subsystems.mechanisms.climber; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.btwrobotics.WhatTime.frc.FlywheelPair; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ClimberSubsystem extends SubsystemBase { - - // TODO: Recalculate all these values - public final double climberUpDownSpeed = 0.5; - public final double climberUpDownSpeedSlow = 0.1; - - public static final double minHeight = 0.0; - public static final double maxHeight = 65.0; - - - - - // TODO: VERY IMPORTANT!!!! Put the correct device ids - public final MotorWrapper climberLeft = new MotorWrapper( - new TalonFX(12), - true - ); - public final MotorWrapper climberRight = new MotorWrapper( - new TalonFX(13), - false - ); - public final FlywheelPair climberPair = new FlywheelPair(climberLeft, climberRight, climberUpDownSpeed); - - - - public double elevatorEncoderOffset = 0.0; - - public void resetClimberEncoder() { - elevatorEncoderOffset = climberLeft.getPosition(); - } - - public double getClimberHeight() { - return climberLeft.getPosition(); - } - - - // TODO: Caedmon mentioned something about this being implemented differently, needs to be done (like we dont have to make the motors rotate opposite) - public Command climberUp() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) <= 65.0) { - climberPair.runForward(); - CommandScheduler.getInstance().cancelAll(); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberDown() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) >= 5.0) { - climberPair.runBackward(); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberUpManual() { - return Commands.run( - () -> { - climberPair.runForward(); - } - ); - } - - public Command climberDownManual() { - return Commands.run( - () -> { - climberPair.runBackward(); - } - ); - } - - public Command climberUpSlow() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) <= 65.0) { - climberPair.runForward(climberUpDownSpeedSlow); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberDownSlow() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) >= 5.0) { - climberPair.runBackward(-climberUpDownSpeedSlow); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command stopClimber() { - return Commands.run( - () -> { - climberPair.runForward(0.015); - } - ); - } -} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java new file mode 100644 index 00000000..27a30bc6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.mechanisms.feeder; + +public class FeederConstants { + public static final double FEEDER_BED_SPEED = 0.2; + public static final double FEEDER_FEEDER_SPEED = 0.5; + public static final double SHOOTER_FEEDER_SPEED = 0.75; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java new file mode 100644 index 00000000..8a65897e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java @@ -0,0 +1,15 @@ +package frc.robot.subsystems.mechanisms.feeder; + + +public enum FeederState { + /** Runs all motors in the feeder subsystem in towards the shooter. */ + ALL_FEEDER_IN, + /** Runs all motors in the feeder subsystem out away from the shooter. */ + ALL_FEEDER_OUT, + /** Runs the shooter feed and feeder feeder motors to control feeding to the shooter. */ + SHOOTER_FEED_IN, + /** Runs the shooter feed and feeder feeder motors out to remove balls from the shooter in the event they get stuck. */ + SHOOTER_FEED_OUT, + /** Stops all motors in the feeder subsystem. */ + OFF; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java new file mode 100644 index 00000000..37e054e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -0,0 +1,105 @@ +package frc.robot.subsystems.mechanisms.feeder; + +import org.littletonrobotics.junction.Logger; +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; + + +public class FeederSubsystem extends SubsystemBase { + // MARK: Feeder Bed + public Motor feederBedMotor = new Motor(15, "Mechanisms", true).setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); + + // MARK: Feeder Feeder + public Motor feederFeederMotor = new Motor(16, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.FEEDER_FEEDER_SPEED); + + // MARK: Shooter Feeder + public Motor shooterFeederMotor = new Motor(12, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.SHOOTER_FEEDER_SPEED); + + // MARK: Feeder State + private FeederState feederState = FeederState.OFF; + + // MARK: Constructor + public FeederSubsystem() { + feederBedMotor.toggleEnabled(true); + feederFeederMotor.toggleEnabled(true); + shooterFeederMotor.toggleEnabled(true); + + feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + + feederBedMotor.setDefaultCommand(Commands.run(() -> {}, feederBedMotor)); + feederFeederMotor.setDefaultCommand(Commands.run(() -> {}, feederFeederMotor)); + shooterFeederMotor.setDefaultCommand(Commands.run(() -> {}, shooterFeederMotor)); + } + + // MARK: Periodic Loop + @Override + public void periodic() { + logValues(); + + switch (feederState) { + case ALL_FEEDER_IN: + runSpecifiedMotors(true, false, true, false, true, true); + break; + + case ALL_FEEDER_OUT: + runSpecifiedMotors(true, true, true, true, true, false); + break; + + case SHOOTER_FEED_IN: + runSpecifiedMotors(true, false, true, false, false, false); + break; + + case SHOOTER_FEED_OUT: + runSpecifiedMotors(true, true, true, true, false, false); + break; + + case OFF: + runSpecifiedMotors(false, false, false, false, false, false); + break; + + default: + runSpecifiedMotors(false, false, false, false, false, false); + break; + } + } + + private void runSpecifiedMotors( + boolean runFeederFeeder, + boolean feederFeederInverted, + boolean runShooterFeeder, + boolean shooterFeederInverted, + boolean runBed, + boolean bedInverted + ) { + double feederSpeed = runFeederFeeder ? (feederFeederInverted ? -FeederConstants.FEEDER_FEEDER_SPEED : FeederConstants.FEEDER_FEEDER_SPEED) : 0.0; + double shooterSpeed = runShooterFeeder ? (shooterFeederInverted ? -FeederConstants.SHOOTER_FEEDER_SPEED : FeederConstants.SHOOTER_FEEDER_SPEED) : 0.0; + double bedSpeed = runBed ? (bedInverted ? -FeederConstants.FEEDER_BED_SPEED : FeederConstants.FEEDER_BED_SPEED) : 0.0; + + feederFeederMotor.getMotor().set(feederSpeed); + shooterFeederMotor.getMotor().set(shooterSpeed); + feederBedMotor.getMotor().set(bedSpeed); + + Logger.recordOutput("FeederSubsystem/FeederFeederSpeed", feederSpeed); + Logger.recordOutput("FeederSubsystem/ShooterFeederSpeed", shooterSpeed); + Logger.recordOutput("FeederSubsystem/BedSpeed", bedSpeed); + Logger.recordOutput("FeederSubsystem/FeederState", feederState.toString()); + } + + public void setFeederState(FeederState feederState) { + this.feederState = feederState; + } + + // MARK: Logging + public void logValues() { + Logger.recordOutput("FeederSubsystem/MotorConnections/FeederBedConnected", feederBedMotor.getMotor().isConnected()); + Logger.recordOutput("FeederSubsystem/MotorConnections/FeederFeederConnected", feederFeederMotor.getMotor().isConnected()); + Logger.recordOutput("FeederSubsystem/MotorConnections/ShooterFeederConnected", shooterFeederMotor.getMotor().isConnected()); + + Logger.recordOutput("FeederSubsystem/FeederState", feederState.toString()); + } +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java new file mode 100644 index 00000000..481dd3ee --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.mechanisms.intake; + +public class IntakeConstants { + public static final double INTAKE_MIN_VALUE = 0.0; + public static final double INTAKE_MAX_VALUE = 2.34; + public static final double INTAKE_THRESHOLD = 0.2; + + public static final double INTAKE_WHEELS_SPEED = 0.75; + public static final double INTAKE_DOWN_SPEED = 0.3; + public static final double INTAKE_UP_SPEED = 0.2; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index c5fd9e42..1ec3d7f7 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -1,68 +1,112 @@ package frc.robot.subsystems.mechanisms.intake; -import java.util.List; - import org.littletonrobotics.junction.Logger; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; -import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; -import com.ctre.phoenix6.hardware.CANcoder; +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; public class IntakeSubsystem extends SubsystemBase { - private double minValue = 0.0; - private double maxValue = 1.0; + // MARK: Intake Angle + public Motor angleMotor = new Motor(9, "Mechanisms") + .setFree(false) + .setMotorSpeed(1.0) + .setMinValue(0.0) + .setMaxValue(2.34) + .setThreshold(0.2); + // public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); - private List angleMotors = List.of( - new MotorWrapper( - new TalonFX(9), false - ) - ); - @Override - public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleEncoder.getAbsolutePosition().getValueAsDouble()); - } + // MARK: Intake Wheels + public Motor intakeWheelsMotor = new Motor(10, "Mechanisms"); - private MotorWrapper intakeWheelsMotor = new MotorWrapper( - new TalonFX(10), false - ); + public IntakeSubsystem() { + angleMotor.toggleEnabled(true); + intakeWheelsMotor.toggleEnabled(true); - private CANcoder angleEncoder = new CANcoder(0); + intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); - private PositionManager intakePositionManager = new PositionManager( - minValue, - maxValue, - angleMotors, - 0.2, - 0.0, - 0.02, - () -> angleEncoder.getAbsolutePosition().getValueAsDouble() - ); + angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + } - public void setPosition(double targetPosition) { - Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - intakePositionManager.move(targetPosition); + // MARK: Intake Wheel State + private IntakeStates intakeState = IntakeStates.OFF; + public IntakeStates lastIntakeState = IntakeStates.OFF; + + // MARK: Periodic Loop + @Override + public void periodic() { + // runAngleControl(); + runIntakeWheels(); + + logValues(); } - private double intakeSpeed = 0.5; + // MARK: Angle Control + // private void runAngleControl() { + // if (Double.isNaN(angleTarget)) { + // angleMotor.set(0.0); + // return; + // } - public void setIntake(IntakeStates intakeState) { - Logger.recordOutput("IntakeSubsystem/State", intakeState.toString()); + // double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); + // double error = angleTarget - currentPos; + + // if (Math.abs(error) <= IntakeConstants.INTAKE_THRESHOLD) { + // angleMotor.set(0.0); + // } else if (error < 0) { + // angleMotor.set(Math.copySign(IntakeConstants.INTAKE_DOWN_SPEED, error)); + // } + // else { + // angleMotor.set(Math.copySign(IntakeConstants.INTAKE_UP_SPEED, error)); + // } + // } + + // MARK: Intake Wheels + private void runIntakeWheels() { switch (intakeState) { case INTAKE_IN: - intakeWheelsMotor.set(intakeSpeed); + intakeWheelsMotor.getMotor().set(IntakeConstants.INTAKE_WHEELS_SPEED); break; case INTAKE_OUT: - intakeWheelsMotor.set(-intakeSpeed); + intakeWheelsMotor.getMotor().set(-IntakeConstants.INTAKE_WHEELS_SPEED); break; case OFF: - intakeWheelsMotor.set(0); + intakeWheelsMotor.getMotor().set(0.0); break; default: + intakeWheelsMotor.getMotor().set(0.0); break; } } + + public void setPosition(double targetPosition) { + Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); + angleMotor.goTo(targetPosition); + } + + public IntakeStates getIntakeState() { + return intakeState; + } + + public void setIntake(IntakeStates intakeState) { + if (!intakeState.equals(this.intakeState)) { + lastIntakeState = this.intakeState; + } + + this.intakeState = intakeState; + } + + // MARK: Logging + private void logValues() { + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + // Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); + Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); + Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); + Logger.recordOutput("IntakeSubsystem/Current/AngleMotor", angleMotor.getMotor().getStatorCurrent().getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java new file mode 100644 index 00000000..e5abf7ed --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.mechanisms.shooter; + +import java.util.List; + +public class ShooterConstants { + public static final List shooterDataPoints = List.of( + new ShooterDataPoint(1.4986, 69.6, 0.65), + new ShooterDataPoint(2.775, 45.5, 0.74) + ); + + // --- Shooter configuration and tuning fields --- + + /** Entry angle to the hub in degrees (TODO: calculate actual value). */ + // public double hubEnterAngle = -70; + + /** Speed for pitching the shooter (open-loop, 0..1). */ + // public double shooterPitchSpeed = 0.1; + /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ + public static final double SHOOTER_PITCH_HOLD_SPEED = 0.0; + + /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ + public static final double SHOOTER_PITCH_MAX_SPEED = 0.6; + /** Minimum allowed shooter pitch. */ + public static final double SHOOTER_PITCH_MIN_SPEED = 0.0; + + /** Threshold for position manager to consider the shooter "at position". */ + public static final double POSITION_THRESHOLD = 1.0; + + public static final double FEEDER_IN_SPEED = 0.2; + public static final double FEEDER_OUT_SPEED = -0.05; + + public static final double SHOOTER_MIN_ANGLE = 40; + + public static final double SHOOTER_MAX_ANGLE = 65; + + + + /** Height of the hub (target) in meters. */ + public static final double HUB_HEIGHT_METRES = 6 / 3.281; // 6 feet to meters + /** Aim above hub in meters */ + public static final double AIM_ABOVE = 1 / 3.281; // 1 foot to meters + /** Height of the shooter in meters. */ + public static final double SHOOTER_HEIGHT = 20 / 12 / 3.281; // 26 inches to meters +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java new file mode 100644 index 00000000..124d1d14 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.mechanisms.shooter; + +public class ShooterDataPoint { + public double distance; + public double angle; + public double speed; + + public ShooterDataPoint( + double distance, + double angle, + double speed + ) { + this.distance = distance; + this.angle = angle; + this.speed = speed; + } +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 54276f6c..9d24ec4e 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,17 +1,24 @@ package frc.robot.subsystems.mechanisms.shooter; -import java.util.List; +import java.util.Arrays; +import java.util.Map; +import java.util.TreeMap; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; -import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; +import org.littletonrobotics.junction.Logger; + +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import com.btwrobotics.WhatTime.frc.MotorManagers.MotorGroup; + +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.motor.MotorSubsystem; +import frc.robot.subsystems.math.MathSubsystem; @@ -32,117 +39,179 @@ public class ShooterSubsystem extends SubsystemBase { /** Reference to the drivetrain for coordinated aiming. */ Drive drivetrain; + TreeMap dataPoints = new TreeMap<>(); + + // public ShooterConstants shooterConstants = new ShooterConstants(); + + // MARK: Motors + /** Motor controlling the shooter angle */ + public final Motor shooterPitchMotor = new Motor(11, "Mechanisms") + .setFree(false) + .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) + .setMotorSpeed(0.1) + .setMinSpeed(0.0) + .setHoldSpeed(0.0) + .setPG(0.02) + .setThreshold(ShooterConstants.POSITION_THRESHOLD) + .setPositionSupplier(() -> getPigeonPosition()); + + public final Motor leftShooterMotor = new Motor(13, "Mechanisms"); + public final Motor rightShooterMotor = new Motor(14, "Mechanisms", true); + + public final MotorGroup shooterMotors = new MotorGroup( + Arrays.asList(leftShooterMotor, rightShooterMotor) + ).setMotorSpeed(0.4) + .setPG(0.01); + + /** IMU sensor for shooter orientation feedback. */ + public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms"); + + // MARK: Constructor /** * Constructs the ShooterSubsystem. * @param drivetrain the swerve drivetrain subsystem (for aiming/coordination) */ public ShooterSubsystem(Drive drivetrain) { this.drivetrain = drivetrain; + + shooterPitchMotor.toggleEnabled(true); + leftShooterMotor.toggleEnabled(true); + rightShooterMotor.toggleEnabled(true); + + for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { + dataPoints.put(point.distance, point); + } + + setDefaultCommand( + Commands.run(() -> { + if (pitchTarget != lastSentPitchTarget) { + shooterPitchMotor.goTo(pitchTarget); + lastSentPitchTarget = pitchTarget; + } + if (flywheelSpeed != lastSentFlywheelSpeed) { + shooterMotors.drive(flywheelSpeed); + lastSentFlywheelSpeed = flywheelSpeed; + } + }, this) + ); } - /** Utility for motion calculations (e.g., trajectory, angles). */ - MotorSubsystem motorSubsystem = new MotorSubsystem(); - - /** Motor controlling the shooter flywheel. */ - public final MotorWrapper shooterMotor = new MotorWrapper( - new TalonFX(10), - false - ); - /** Motor controlling the shooter pitch (angle). */ - public final MotorWrapper shooterPitchMotor = new MotorWrapper( - new TalonFX(9), - false - ); + public double shooterAngleTarget = 65; - /** IMU sensor for shooter orientation feedback. */ - public final Pigeon2 shooterPigeon = new Pigeon2(34); + /** Shared pitch target used by manual joystick control and button bindings. */ + public double pitchTarget = ShooterConstants.SHOOTER_MAX_ANGLE; + private double lastSentPitchTarget = Double.NaN; - // --- Shooter configuration and tuning fields --- + // MARK: Set Pitch Target + public void setPitchTarget(double pitch) { + // pitchTarget = MathUtil.clamp(pitch, ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE); + shooterPitchMotor.goTo(pitchTarget / 180); + } - /** Entry angle to the hub in degrees (TODO: calculate actual value). */ - public double hubEnterAngle = -70; + /** Shared flywheel speed target used by manual joystick control and button bindings. */ + public double flywheelSpeed = 0.0; + private double lastSentFlywheelSpeed = Double.NaN; - /** Speed for pitching the shooter (open-loop, 0..1). */ - public double shooterPitchSpeed = 0.1; - /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ - public double shooterPitchHoldSpeed = 0.02; + // MARK: Set Flywheel + public void setFlywheelSpeed(double speed) { + shooterMotors.drive(speed); + } - /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ - public double shooterPitchMax = 0.3; - /** Minimum allowed shooter pitch. */ - public double shooterPitchMin = 0.0; + // MARK: Periodic Loop + @Override + public void periodic() { + if (drivetrain.isLockedToHub()) { + double currentDistance = drivetrain.getDistanceToHub(); + ShooterDataPoint values = calculateShooterValues(shooterUpperLower(), currentDistance); + setPitchTarget(values.angle); + setFlywheelSpeed(values.speed); + } + + logValues(); + } - /** Threshold for position manager to consider the shooter "at position". */ - public double positionThreshold = 1.0; + // MARK: Get Pigeon + public double getPigeonPosition() { + return shooterPigeon.getRoll().refresh().getValueAsDouble() * -1; + } - /** Height of the hub (target) in meters. */ - public double hubHeight = 2; - /** Height of the shooter in meters. */ - public double shooterHeight = 1; + // MARK: Increment Shooter + /** For testing shooter angle manually */ + public void incrementShooterAngle(double incrementValue) { + shooterAngleTarget += incrementValue; + + shooterPitchMotor.goTo(shooterAngleTarget); + } - /** - * PositionManager for controlling the shooter pitch motor to a target angle. - * Uses feedback from the shooter IMU. - */ - public PositionManager shooterPositionManager = new PositionManager( - shooterPitchMin, - shooterPitchMax, - List.of(shooterPitchMotor), - 0.2, - 0.0, - positionThreshold, - () -> getShooterPitchDeg() - ); - - // --- Commands --- + // MARK: UpperLowerPoint + public UpperLowerPoint shooterUpperLower() { + // MARK: NEEDS REFACTORING + // if (dataPoints.isEmpty()) { - /** - * Returns a command to pitch the shooter to the specified angle (degrees). - * @param angle target pitch angle in degrees - * @return a command that moves the shooter pitch to the given angle - */ - public Command pitchToAngleDeg(double angle) { - return shooterPositionManager.move(angle); - } - - /** - * Returns a command to aim the shooter at the hub. - * Currently a placeholder: should calculate robot position, distance to hub, - * required rotation, and pitch, then command the shooter and drivetrain. - * @return a command that aims the shooter at the hub - */ - public Command aimAtHub() { - return Commands.run( - () -> { - // Get robot position (Should be implemented into WhatTime) - // Calculate distance to hub (Should be implemented into WhatTime) + // double currentDistance = drivetrain.getDistanceToHub(); + // double aimHeight = (6 - 20 / 12) / 3.281; - // Calculate required rotations to face hub - // Rotate to face hub + // double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); - // Calculate required shooter pitch angle to hit hub - // Pitch shooter to required angle - } - ); + + // return new UpperLowerPoint( + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) + // ); + // } + + double currentDistance = drivetrain.getDistanceToHub(); + + Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); + Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); + + // Handle out-of-range cases by clamping to the nearest point + ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); + ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); + + return new UpperLowerPoint(upper, lower); } - // --- Sensor feedback --- + public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, double currentDistance) { + double lowerDist = upperLowerPoint.getLowerDistance(); + double upperDist = upperLowerPoint.getUpperDistance(); + double valueRange = upperDist - lowerDist; - /** - * Gets the shooter pitch motor's position in degrees. - * @return shooter pitch (motor) position in degrees - */ - public double getShooterMotorPitchDeg() { - return shooterPitchMotor.getPosition() * 360; + if (valueRange == 0) { + return new ShooterDataPoint(currentDistance, upperLowerPoint.getLowerAngle(), upperLowerPoint.getLowerSpeed()); + } + + double interpolationFactor = (currentDistance - lowerDist) / valueRange; + + double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); + double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); + + return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); } - /** - * Gets the current shooter pitch in degrees from the Pigeon2 IMU. - * @return shooter pitch in degrees (roll axis) - */ - public double getShooterPitchDeg() { - // MARK: Should be roll? - return shooterPigeon.getRoll().getValueAsDouble(); + public Command accelerateToSpeed(double targetSpeed) { + Timer timer = new Timer(); + return Commands.startRun( + () -> timer.restart(), + () -> { + double rampedSpeed = Math.min(timer.get() / 5.0, 1.0) * targetSpeed; + setFlywheelSpeed(rampedSpeed); + }, + this + ).until(() -> timer.hasElapsed(5.0)); } + public double getRequiredRPM(ShooterDataPoint shooterDataPoint){ + double vWheel = 2 * shooterDataPoint.speed; + return vWheel * 60 / (Math.PI * 4 * 0.0254); // get rpm required for wheel with diameter of 4 inches + } + + // MARK: Logging + private void logValues() { + Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); + Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); + Logger.recordOutput("ShooterSubsystem/PitchMotorOutput", shooterPitchMotor.getMotor().get()); + Logger.recordOutput("ShooterSubsystem/ShooterSpeed", flywheelSpeed); + Logger.recordOutput("ShooterSubsystem/TargetAngle", shooterAngleTarget); + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java new file mode 100644 index 00000000..db28d3c6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.mechanisms.shooter; + +public class UpperLowerPoint { + public ShooterDataPoint upper; + public ShooterDataPoint lower; + + public UpperLowerPoint( + ShooterDataPoint upper, + ShooterDataPoint lower + ) { + this.upper = upper; + this.lower = lower; + } + + public double getUpperDistance() { + return upper.distance; + } + + public double getLowerDistance() { + return lower.distance; + } + + public double getUpperAngle() { + return upper.angle; + } + + public double getLowerAngle() { + return lower.angle; + } + + public double getUpperSpeed() { + return upper.speed; + } + + public double getLowerSpeed() { + return lower.speed; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java index 80ab19e8..bfebbee7 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision.limelight; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation2d; @@ -23,8 +25,8 @@ public class LimelightConstants { // Position of Limelight relative to the centre of the robot in metres public static final Transform2d LIMELIGHT_4_TRANSFORM_FROM_CENTRE = new Transform2d( - new Translation2d(0.3429, 0.0), - new Rotation2d(0) + new Translation2d(0.0762, -0.26035), + new Rotation2d(-0.174532925199433) ); /** Calculate dynamic standard deviations for Quest */ diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java index 70d3189c..e269a97e 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java @@ -69,6 +69,9 @@ public LimelightSubsystem( Field2d limelightField2d = new Field2d(); + private int totalLimelightEstimates = 0; + private int estimatesAddedToQuest = 0; + /** * Periodic update called by the scheduler. Adds a vision odometry measurement each cycle. *

Delegates to {@link #addOdometryMeasurement()} to perform the actual read/filter/submit @@ -130,11 +133,16 @@ public void addOdometryMeasurement() { Logger.recordOutput("Limelight/" + limelightName + "/Pose", transformedPose); // Add measurement to drivetrain pose estimator - drivetrain.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.VISION_STD_DEVS); + drivetrain.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate)); + + totalLimelightEstimates++; + Logger.recordOutput("Limelight/TotalEstimates", totalLimelightEstimates); // Add measurement to QuestNav pose estimator if enabled if (QuestNavConstants.USE_LIMELIGHT_FOR_VISION_MEASUREMENTS) { - questNavSubsystem.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.VISION_STD_DEVS, estimate); + estimatesAddedToQuest++; + Logger.recordOutput("QuestNav/LimelightEstimates", estimatesAddedToQuest); + questNavSubsystem.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate), estimate); } } diff --git a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java index b74a8cd6..a69f2cbb 100644 --- a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; @@ -12,8 +11,8 @@ public class QuestNavConstants { // 🏳️‍⚧️ TRANS-form 3d public static final Transform3d ROBOT_TO_QUEST = new Transform3d( - new Translation3d(0.0, -0.4, 0.45), - new Rotation3d(new Rotation2d(-0.5 * Math.PI)) + new Translation3d(0.0762, 0.254, 0.3556), + new Rotation3d(0.5 * Math.PI, 0.0, 0.0) ); /** @@ -22,9 +21,9 @@ public class QuestNavConstants { */ public static final Matrix ODOMETRY_STD_DEVS = VecBuilder.fill( - 0.05, // 5cm standard deviation in X - 0.05, // 5cm standard deviation in Y - 0.01 // ~0.57 degrees standard deviation in rotation + 0.15, + 0.15, + 0.15 ); /** @@ -33,9 +32,9 @@ public class QuestNavConstants { */ public static final Matrix QUESTNAV_STD_DEVS = VecBuilder.fill( - 0.02, // 2cm standard deviation in X - 0.02, // 2cm standard deviation in Y - 0.035 // 2deg/0.035rad standard deviation in rotation + 0.05, + 0.05, + 0.1 ); public static final boolean USE_LIMELIGHT_FOR_VISION_MEASUREMENTS = true; diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 9e9152b0..868ae9d3 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "26.0.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2026", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "26.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "26.0.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] } \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-frc2026-latest.json index d6cf160b..97ea239c 100644 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ b/vendordeps/Phoenix6-frc2026-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2026-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.1", + "version": "26.1.2", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.1" + "version": "26.1.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 7f68d20d..ebdd6a9c 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.1", + "version": "v2026.2.1-beta.3", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.1" + "version": "v2026.2.1-beta.3" } ], "jniDependencies": [],