diff --git a/.gitignore b/.gitignore index 9535c83..6f3e25f 100644 --- a/.gitignore +++ b/.gitignore @@ -158,5 +158,19 @@ gradle-app.setting .settings/ bin/ +# IntelliJ +*.iml +*.ipr +*.iws +.idea/ +out/ + +# Fleet +.fleet + # Simulation GUI and other tools window save file *-window.json + +ctre_sim/ + +src/main/java/frc/robot/BuildConstants.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json deleted file mode 100644 index 943159a..0000000 --- a/.pathplanner/settings.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "robotWidth": 0.756, - "robotLength": 0.908, - "holonomicMode": true, - "generateJSON": false, - "generateCSV": false -} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index c9c9713..0000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index fd12831..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,33 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "automatic", - "java.server.launchMode": "Standard", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - ], - "java.test.defaultConfig": "WPIlibUnitTests", - "editor.tabSize": 4, - "editor.detectIndentation": false, - "editor.insertSpaces": true, - "editor.inlayHints.enabled": "offUnlessPressed" -} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 04eafde..d5baaaf 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2023", - "teamNumber": 401 + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024beta", + "teamNumber": 401 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 3d5a824..43b62ec 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors +Copyright (c) 2009-2023 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index b233ef1..e40d3f0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,10 +1,13 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.3" + id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" + id "com.peterabeles.gversion" version "1.10" } -sourceCompatibility = JavaVersion.VERSION_11 -targetCompatibility = JavaVersion.VERSION_11 +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} def ROBOT_MAIN_CLASS = "frc.robot.Main" @@ -43,7 +46,38 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true + +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/New_York" // Use preferred time zone + indent = " " +} // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -68,6 +102,10 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2' testImplementation 'org.junit.jupiter:junit-jupiter-params:5.8.2' testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' + + // ... + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } test { @@ -84,6 +122,7 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar index 249e583..7f93135 100644 Binary files a/gradle/wrapper/gradle-wrapper.jar and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index c23a1b3..1058752 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,7 @@ distributionBase=GRADLE_USER_HOME distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.5.1-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip +networkTimeout=10000 +validateDistributionUrl=true zipStoreBase=GRADLE_USER_HOME zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew index a69d9cb..1aa94a4 100755 --- a/gradlew +++ b/gradlew @@ -55,7 +55,7 @@ # Darwin, MinGW, and NonStop. # # (3) This script is generated from the Groovy template -# https://github.com/gradle/gradle/blob/master/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt +# https://github.com/gradle/gradle/blob/HEAD/subprojects/plugins/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt # within the Gradle project. # # You can find Gradle at https://github.com/gradle/gradle/. @@ -80,13 +80,11 @@ do esac done -APP_HOME=$( cd "${APP_HOME:-./}" && pwd -P ) || exit - -APP_NAME="Gradle" +# This is normally unused +# shellcheck disable=SC2034 APP_BASE_NAME=${0##*/} - -# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. -DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' +# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036) +APP_HOME=$( cd "${APP_HOME:-./}" > /dev/null && pwd -P ) || exit # Use the maximum available, or set MAX_FD != -1 to use that value. MAX_FD=maximum @@ -133,22 +131,29 @@ location of your Java installation." fi else JAVACMD=java - which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + if ! command -v java >/dev/null 2>&1 + then + die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. Please set the JAVA_HOME variable in your environment to match the location of your Java installation." + fi fi # Increase the maximum file descriptors if we can. if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then case $MAX_FD in #( max*) + # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 MAX_FD=$( ulimit -H -n ) || warn "Could not query maximum file descriptor limit" esac case $MAX_FD in #( '' | soft) :;; #( *) + # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked. + # shellcheck disable=SC2039,SC3045 ulimit -n "$MAX_FD" || warn "Could not set maximum file descriptor limit to $MAX_FD" esac @@ -193,11 +198,15 @@ if "$cygwin" || "$msys" ; then done fi -# Collect all arguments for the java command; -# * $DEFAULT_JVM_OPTS, $JAVA_OPTS, and $GRADLE_OPTS can contain fragments of -# shell script including quotes and variable substitutions, so put them in -# double quotes to make sure that they get re-expanded; and -# * put everything else in single quotes, so that it's not re-expanded. + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Collect all arguments for the java command: +# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments, +# and any embedded shellness will be escaped. +# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be +# treated as '${Hostname}' itself on the command line. set -- \ "-Dorg.gradle.appname=$APP_BASE_NAME" \ diff --git a/gradlew.bat b/gradlew.bat index f127cfd..93e3f59 100644 --- a/gradlew.bat +++ b/gradlew.bat @@ -26,6 +26,7 @@ if "%OS%"=="Windows_NT" setlocal set DIRNAME=%~dp0 if "%DIRNAME%"=="" set DIRNAME=. +@rem This is normally unused set APP_BASE_NAME=%~n0 set APP_HOME=%DIRNAME% diff --git a/settings.gradle b/settings.gradle index 48c039e..d94f73c 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2023' + String frcYear = '2024' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') @@ -25,3 +25,6 @@ pluginManagement { } } } + +Properties props = System.getProperties(); +props.setProperty("org.gradle.internal.native.headers.unresolved.dependencies.ignore", "true"); diff --git a/simgui-ds.json b/simgui-ds.json index 690a4a2..4dcea39 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -93,8 +93,7 @@ {}, {}, { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/simgui.json b/simgui.json index c6bcf50..10331c3 100644 --- a/simgui.json +++ b/simgui.json @@ -2,41 +2,11 @@ "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/LiveWindow/Ungrouped/DigitalInput[0]": "Digital Input", - "/LiveWindow/Ungrouped/DigitalInput[1]": "Digital Input", - "/LiveWindow/Ungrouped/DigitalInput[2]": "Digital Input", - "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[3]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[4]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[5]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[6]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[7]": "PIDController", - "/LiveWindow/Ungrouped/PIDController[8]": "PIDController", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/SmartDashboard/Arm Mechanism": "Mechanism2d", - "/SmartDashboard/Auto Mode": "String Chooser", - "/SmartDashboard/Field": "Field2d" - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "Arm Mechanism": { - "arm": { - "pivot": { - "open": false, - "telescope": { - "open": false, - "wrist": { - "open": false - } - } - } - } - }, - "open": true - } + "/Pose": "Field2d", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d" } } } diff --git a/src/main/deploy/pathplanner/B-1-1.path b/src/main/deploy/pathplanner/B-1-1.path deleted file mode 100644 index 299b664..0000000 --- a/src/main/deploy/pathplanner/B-1-1.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.8801201338666413, - "y": 5.030549841319464 - }, - "prevControl": null, - "nextControl": { - "x": 4.271451180264998, - "y": 4.712463051299729 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.945053971250429, - "y": 4.601623645433787 - }, - "prevControl": { - "x": 4.233499038426649, - "y": 4.952901048659241 - }, - "nextControl": { - "x": 4.233499038426649, - "y": 4.952901048659241 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 1.97176965782503, - "y": 4.365996589136035 - }, - "prevControl": { - "x": 3.997407406289746, - "y": 5.057488683508226 - }, - "nextControl": { - "x": 3.997407406289746, - "y": 5.057488683508226 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.935428046680269, - "y": 3.496917103724318 - }, - "prevControl": { - "x": 5.407307957473058, - "y": 5.210566421132842 - }, - "nextControl": { - "x": 5.407307957473058, - "y": 5.210566421132842 - }, - "holonomicAngle": -40.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 1.9574549149098301, - "y": 4.351681846220836 - }, - "prevControl": { - "x": 3.567210183101908, - "y": 4.896397012730276 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B-1-2.path b/src/main/deploy/pathplanner/B-1-2.path deleted file mode 100644 index 8ef7701..0000000 --- a/src/main/deploy/pathplanner/B-1-2.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.8801201338666413, - "y": 5.030549841319464 - }, - "prevControl": null, - "nextControl": { - "x": 4.271451180264998, - "y": 4.712463051299729 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.945053971250429, - "y": 4.601623645433787 - }, - "prevControl": { - "x": 4.276443267172249, - "y": 4.96721579157444 - }, - "nextControl": { - "x": 4.276443267172249, - "y": 4.96721579157444 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.0108242840743094, - "y": 4.38683245388138 - }, - "prevControl": { - "x": 3.878999860471825, - "y": 5.178527748659971 - }, - "nextControl": { - "x": 3.878999860471825, - "y": 5.178527748659971 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.935428046680269, - "y": 3.496917103724318 - }, - "prevControl": { - "x": 4.971722388144382, - "y": 4.689138033137517 - }, - "nextControl": { - "x": 7.143231981829282, - "y": 3.3707534907499612 - }, - "holonomicAngle": -24.28406794282433, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.555985377832267, - "y": 2.8658516418519056 - }, - "prevControl": { - "x": 5.881188462315516, - "y": 2.8414614105156613 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B-1-3.path b/src/main/deploy/pathplanner/B-1-3.path deleted file mode 100644 index e1d3ebd..0000000 --- a/src/main/deploy/pathplanner/B-1-3.path +++ /dev/null @@ -1,227 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.8801201338666413, - "y": 5.030549841319464 - }, - "prevControl": null, - "nextControl": { - "x": 4.027205966499845, - "y": 4.911267295062064 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.8331858641739265, - "y": 4.5818202625416244 - }, - "prevControl": { - "x": 4.430494575274865, - "y": 4.587500383791976 - }, - "nextControl": { - "x": 4.430494575274865, - "y": 4.587500383791976 - }, - "holonomicAngle": 0.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.3174894701437756, - "y": 4.371655776278586 - }, - "prevControl": { - "x": 4.157848755257947, - "y": 5.422478207593778 - }, - "nextControl": { - "x": 4.157848755257947, - "y": 5.422478207593778 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.861586470425689, - "y": 3.2299514049577565 - }, - "prevControl": { - "x": 5.77668331160838, - "y": 4.627261232544445 - }, - "nextControl": { - "x": 5.77668331160838, - "y": 4.627261232544445 - }, - "holonomicAngle": -40.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.2038870451367276, - "y": 4.0478888650085 - }, - "prevControl": { - "x": 3.2944703252043865, - "y": 5.9052885138737325 - }, - "nextControl": { - "x": 3.2944703252043865, - "y": 5.9052885138737325 - }, - "holonomicAngle": 10.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.168611677882696, - "y": 4.042208743758148 - }, - "prevControl": { - "x": 5.782363432858734, - "y": 4.632941353794797 - }, - "nextControl": { - "x": 6.337729246685521, - "y": 3.783558344412651 - }, - "holonomicAngle": -70.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.889987076677449, - "y": 2.2472904286467923 - }, - "prevControl": { - "x": 6.6741424691640585, - "y": 2.9970664336933077 - }, - "nextControl": { - "x": 6.6741424691640585, - "y": 2.9970664336933077 - }, - "holonomicAngle": -70.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 5.4699567640893525, - "y": 4.5136588075373965 - }, - "prevControl": { - "x": 6.111810465379174, - "y": 4.155811168765196 - }, - "nextControl": { - "x": 5.230532030725171, - "y": 4.647143393395303 - }, - "holonomicAngle": -20.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.4424521376515282, - "y": 4.365975655028234 - }, - "prevControl": { - "x": 3.743199903982226, - "y": 5.348636631339198 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.0, - "maxAcceleration": 4.5, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B-3-1.path b/src/main/deploy/pathplanner/B-3-1.path deleted file mode 100644 index ab52099..0000000 --- a/src/main/deploy/pathplanner/B-3-1.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.88, - "y": 0.5203249351731978 - }, - "prevControl": null, - "nextControl": { - "x": 2.3153234877898328, - "y": 0.9590877753184078 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.053503392721072, - "y": 0.9304582894880083 - }, - "prevControl": { - "x": 4.276443267172249, - "y": 0.37218331579520314 - }, - "nextControl": { - "x": 4.276443267172249, - "y": 0.37218331579520314 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.091397935331368, - "y": 1.1056904872430453 - }, - "prevControl": { - "x": 2.787710003991437, - "y": 0.6727929170144054 - }, - "nextControl": { - "x": 2.787710003991437, - "y": 0.6727929170144054 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.082132878551472, - "y": 2.1615261801952177 - }, - "prevControl": { - "x": 5.507511157879459, - "y": 0.028629485830400595 - }, - "nextControl": { - "x": 5.507511157879459, - "y": 0.028629485830400595 - }, - "holonomicAngle": 40.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.1, - "y": 1.0894303330188817 - }, - "prevControl": { - "x": 2.558674117348235, - "y": 0.6727929170144054 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B-3-2.path b/src/main/deploy/pathplanner/B-3-2.path deleted file mode 100644 index 6b8f7d3..0000000 --- a/src/main/deploy/pathplanner/B-3-2.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 1.88, - "y": 0.5203249351731978 - }, - "prevControl": null, - "nextControl": { - "x": 2.3153234877898328, - "y": 0.9590877753184078 - }, - "holonomicAngle": 0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.053503392721072, - "y": 0.9304582894880083 - }, - "prevControl": { - "x": 4.47684966798505, - "y": 0.501016002032004 - }, - "nextControl": { - "x": 4.47684966798505, - "y": 0.501016002032004 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 2.091397935331368, - "y": 1.1056904872430453 - }, - "prevControl": { - "x": 2.787710003991437, - "y": 0.6727929170144054 - }, - "nextControl": { - "x": 2.787710003991437, - "y": 0.6727929170144054 - }, - "holonomicAngle": 0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 7.082132878551472, - "y": 2.1615261801952177 - }, - "prevControl": { - "x": 5.65065858703146, - "y": 0.05725897166080119 - }, - "nextControl": { - "x": 7.176028994677598, - "y": 2.2995534709006225 - }, - "holonomicAngle": 40.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.552487390689067, - "y": 2.605283210566422 - }, - "prevControl": { - "x": 6.810152763162669, - "y": 2.41919155266882 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/B-2-1.path b/src/main/deploy/pathplanner/New Path.path similarity index 56% rename from src/main/deploy/pathplanner/B-2-1.path rename to src/main/deploy/pathplanner/New Path.path index b60130e..74c48a8 100644 --- a/src/main/deploy/pathplanner/B-2-1.path +++ b/src/main/deploy/pathplanner/New Path.path @@ -2,13 +2,13 @@ "waypoints": [ { "anchorPoint": { - "x": 1.9001959432490299, - "y": 2.7484306397184226 + "x": 1.0, + "y": 3.0 }, "prevControl": null, "nextControl": { - "x": 2.1662536957326566, - "y": 2.7484306397184226 + "x": 2.0, + "y": 3.0 }, "holonomicAngle": 0, "isReversal": false, @@ -24,12 +24,37 @@ }, { "anchorPoint": { - "x": 5.908323959505061, - "y": 2.75 + "x": 3.0, + "y": 5.0 }, "prevControl": { - "x": 5.404193617681592, - "y": 2.75 + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 }, "nextControl": null, "holonomicAngle": 0, @@ -45,8 +70,5 @@ } } ], - "maxVelocity": 3.0, - "maxAcceleration": 2.5, - "isReversed": null, "markers": [] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-1-1.path b/src/main/deploy/pathplanner/R-1-1.path deleted file mode 100644 index 85a7d81..0000000 --- a/src/main/deploy/pathplanner/R-1-1.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.636106919272661, - "y": 5.008127501042021 - }, - "prevControl": null, - "nextControl": { - "x": 12.260362896999657, - "y": 4.589219730658439 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.668629803791045, - "y": 4.59349356832588 - }, - "prevControl": { - "x": 12.235440328023513, - "y": 5.010160020320041 - }, - "nextControl": { - "x": 12.235440328023513, - "y": 5.010160020320041 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.55442868028593, - "y": 4.380311332051236 - }, - "prevControl": { - "x": 12.45797778019472, - "y": 4.749972189467863 - }, - "nextControl": { - "x": 12.45797778019472, - "y": 4.749972189467863 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.587329032670235, - "y": 3.4552827726345123 - }, - "prevControl": { - "x": 10.918483979825103, - "y": 5.253510649878443 - }, - "nextControl": { - "x": 10.918483979825103, - "y": 5.253510649878443 - }, - "holonomicAngle": -140.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.54011393737073, - "y": 4.394626074966436 - }, - "prevControl": { - "x": 12.943181973845082, - "y": 4.975861775838053 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-1-2.path b/src/main/deploy/pathplanner/R-1-2.path deleted file mode 100644 index 9bd7ea3..0000000 --- a/src/main/deploy/pathplanner/R-1-2.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.636106919272661, - "y": 5.008127501042021 - }, - "prevControl": null, - "nextControl": { - "x": 12.260362896999657, - "y": 4.589219730658439 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.668629803791045, - "y": 4.59349356832588 - }, - "prevControl": { - "x": 12.321328785514712, - "y": 4.89564207699844 - }, - "nextControl": { - "x": 12.321328785514712, - "y": 4.89564207699844 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.56, - "y": 4.25 - }, - "prevControl": { - "x": 12.427625953449533, - "y": 4.951916014669467 - }, - "nextControl": { - "x": 12.427625953449533, - "y": 4.951916014669467 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.587329032670235, - "y": 3.4552827726345123 - }, - "prevControl": { - "x": 11.183256544159157, - "y": 5.0512102841234405 - }, - "nextControl": { - "x": 9.453121182960595, - "y": 3.3210749229248724 - }, - "holonomicAngle": -140.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.961312579825972, - "y": 2.943087914573397 - }, - "prevControl": { - "x": 9.717410266463535, - "y": 2.918697683237153 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-1-3.path b/src/main/deploy/pathplanner/R-1-3.path deleted file mode 100644 index 94d660b..0000000 --- a/src/main/deploy/pathplanner/R-1-3.path +++ /dev/null @@ -1,227 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.644236996384741, - "y": 4.991867346817859 - }, - "prevControl": null, - "nextControl": { - "x": 12.493840312389963, - "y": 4.991867346817859 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.660499726678966, - "y": 4.59349356832588 - }, - "prevControl": { - "x": 12.063197729659949, - "y": 4.59349356832588 - }, - "nextControl": { - "x": 12.063197729659949, - "y": 4.59349356832588 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.213342909444439, - "y": 4.398371717635931 - }, - "prevControl": { - "x": 12.378032973671782, - "y": 5.457988403100672 - }, - "nextControl": { - "x": 12.378032973671782, - "y": 5.457988403100672 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.587329032670235, - "y": 3.4796730039707557 - }, - "prevControl": { - "x": 10.724443681472163, - "y": 4.834833471871946 - }, - "nextControl": { - "x": 10.724443681472163, - "y": 4.834833471871946 - }, - "holonomicAngle": -140.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.36781437457398, - "y": 4.170729558497658 - }, - "prevControl": { - "x": 13.237733655994694, - "y": 5.593493053111867 - }, - "nextControl": { - "x": 13.237733655994694, - "y": 5.593493053111867 - }, - "holonomicAngle": 170.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.733669905473683, - "y": 4.422761948972175 - }, - "prevControl": { - "x": 11.138499781784915, - "y": 5.0009189298957235 - }, - "nextControl": { - "x": 10.556416428357542, - "y": 4.169617748940649 - }, - "holonomicAngle": -110.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.652369649566884, - "y": 2.2032508973740073 - }, - "prevControl": { - "x": 9.982107497875479, - "y": 2.9103759950257175 - }, - "nextControl": { - "x": 9.982107497875479, - "y": 2.9103759950257175 - }, - "holonomicAngle": -110.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 11.09952337551734, - "y": 4.780485341903748 - }, - "prevControl": { - "x": 10.463108943071223, - "y": 4.413051298014821 - }, - "nextControl": { - "x": 11.336919148870876, - "y": 4.917545855553892 - }, - "holonomicAngle": -160.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.3596842974619, - "y": 4.3902416405238505 - }, - "prevControl": { - "x": 13.110875050091618, - "y": 5.43811701940303 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 4.0, - "maxAcceleration": 4.5, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-2-1.path b/src/main/deploy/pathplanner/R-2-1.path deleted file mode 100644 index 4ff7590..0000000 --- a/src/main/deploy/pathplanner/R-2-1.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.62600239486193, - "y": 2.75 - }, - "prevControl": null, - "nextControl": { - "x": 14.892060147345557, - "y": 2.75 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.5749301498603, - "y": 2.75 - }, - "prevControl": { - "x": 10.070799808036831, - "y": 2.75 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.5, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-3-1.path b/src/main/deploy/pathplanner/R-3-1.path deleted file mode 100644 index 350a0f8..0000000 --- a/src/main/deploy/pathplanner/R-3-1.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.64, - "y": 0.528455012285279 - }, - "prevControl": null, - "nextControl": { - "x": 14.210874850321128, - "y": 0.9304582894880083 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.487009688305092, - "y": 0.9018288036576078 - }, - "prevControl": { - "x": 12.192496099277914, - "y": 0.386498058710404 - }, - "nextControl": { - "x": 12.192496099277914, - "y": 0.386498058710404 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.37, - "y": 1.0406498703463944 - }, - "prevControl": { - "x": 12.264069813853915, - "y": 0.4294422874560043 - }, - "nextControl": { - "x": 12.264069813853915, - "y": 0.4294422874560043 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.487009688305092, - "y": 2.147211437280018 - }, - "prevControl": { - "x": 11.104575637722704, - "y": 0.5439602307776044 - }, - "nextControl": { - "x": 11.104575637722704, - "y": 0.5439602307776044 - }, - "holonomicAngle": 140.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.37, - "y": 1.048779947458477 - }, - "prevControl": { - "x": 12.99412170252912, - "y": 0.44375703037120406 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/R-3-2.path b/src/main/deploy/pathplanner/R-3-2.path deleted file mode 100644 index b138dab..0000000 --- a/src/main/deploy/pathplanner/R-3-2.path +++ /dev/null @@ -1,127 +0,0 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 14.64, - "y": 0.528455012285279 - }, - "prevControl": null, - "nextControl": { - "x": 12.364273014260315, - "y": 0.5296454878624047 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.487009688305092, - "y": 0.9447730324032081 - }, - "prevControl": { - "x": 12.13866147800365, - "y": 0.9447730324032081 - }, - "nextControl": { - "x": 12.13866147800365, - "y": 0.9447730324032081 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 14.37, - "y": 1.0406498703463944 - }, - "prevControl": { - "x": 12.192496099277914, - "y": 0.45807177328640375 - }, - "nextControl": { - "x": 12.192496099277914, - "y": 0.45807177328640375 - }, - "holonomicAngle": 180.0, - "isReversal": true, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 9.487009688305092, - "y": 2.1758409231104174 - }, - "prevControl": { - "x": 11.161834609383506, - "y": 0.3006096012192034 - }, - "nextControl": { - "x": 9.334406645385007, - "y": 2.346704159200426 - }, - "holonomicAngle": 140.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 10.270255510085056, - "y": 2.49593367340893 - }, - "prevControl": { - "x": 10.042613350946782, - "y": 2.4471532107364427 - }, - "nextControl": null, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": false, - "stopEvent": { - "names": [], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - } - ], - "maxVelocity": 3.0, - "maxAcceleration": 2.75, - "isReversed": null, - "markers": [] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto new file mode 100644 index 0000000..c195398 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test Auto.auto @@ -0,0 +1,24 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test Path" + } + } + ] + } + }, + "folder": null +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..7026fb9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"height":8.02},"nodeSizeMeters":0.2,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test Path.path b/src/main/deploy/pathplanner/paths/Test Path.path new file mode 100644 index 0000000..0ff2166 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Test Path.path @@ -0,0 +1,45 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.186490801553032, + "y": 2.2474146376864184 + }, + "prevControl": null, + "nextControl": { + "x": 2.930857433143438, + "y": 2.276044123516819 + }, + "isLocked": false + }, + { + "anchor": { + "x": 3.245781777277841, + "y": 3.1206139555136256 + }, + "prevControl": { + "x": 2.1149170869770324, + "y": 3.077669726768025 + }, + "nextControl": null, + "isLocked": false + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0 + }, + "reversed": false, + "folder": null, + "previewStartingState": null +} \ No newline at end of file diff --git a/src/main/deploy/rick.chrp b/src/main/deploy/rick.chrp deleted file mode 100644 index 8f06997..0000000 Binary files a/src/main/deploy/rick.chrp and /dev/null differ diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/CommandSwerveDrivetrain.java new file mode 100644 index 0000000..6aa51da --- /dev/null +++ b/src/main/java/frc/robot/CommandSwerveDrivetrain.java @@ -0,0 +1,35 @@ +package frc.robot; + +import java.util.function.Supplier; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; + +/** + * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem + * so it can be used in command-based projects easily. + */ +public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + } + public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + } + + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } + + @Override + public void periodic() { + if(Constants.currentMode == Constants.Mode.SIM) { + updateSimState(0.02, 12); + } + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index da6afab..c44eabf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,15 @@ import edu.wpi.first.wpilibj.util.Color; public final class Constants { + + public static final Mode currentMode = Mode.SIM; + + public static enum Mode { + REAL, + SIM, + REPLAY + } + public static final class CANDevices { public static final String canivoreName = "Canivore"; diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..fe215d7 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -6,19 +6,9 @@ import edu.wpi.first.wpilibj.RobotBase; -/** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot - * call. - */ public final class Main { private Main() {} - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7e830df..476f5c7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,130 +4,149 @@ package frc.robot; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import com.pathplanner.lib.commands.PathPlannerAuto; + import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -/** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the - * project. - */ -public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - private RobotContainer m_robotContainer; - - private PowerDistribution pdh; - - private final Timer loopTimer = new Timer(); - - public Robot() { -} - -/** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ -@Override -public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - pdh = new PowerDistribution(1, ModuleType.kRev); - pdh.setSwitchableChannel(false); - - LiveWindow.disableAllTelemetry(); - } - - /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - - SmartDashboard.putNumber("Loop Time", loopTimer.get() * 1000); - loopTimer.reset(); - loopTimer.start(); - } - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() { - pdh.setSwitchableChannel(false); - } - - @Override - public void disabledPeriodic() { - m_robotContainer.disabledPeriodic(); - } - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - - m_robotContainer.enabledInit(); - pdh.setSwitchableChannel(true); - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - - m_robotContainer.enabledInit(); - pdh.setSwitchableChannel(true); - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} - - /** This function is called once when the robot is first started up. */ - @Override - public void simulationInit() {} - - /** This function is called periodically whilst in simulation. */ - @Override - public void simulationPeriodic() {} +public class Robot extends LoggedRobot { + private Command autonomousCommand; + + private RobotContainer robotContainer; + + private PowerDistribution pdh; + + @Override + public void robotInit() { + // Record metadata + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncomitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + Logger.recordMetadata("ProjectName", "2024-Mushussu"); + + switch (Constants.currentMode) { + case REAL: + // Running on a real robot, log to a USB stick + Logger.addDataReceiver(new WPILOGWriter("/U")); + Logger.addDataReceiver(new NT4Publisher()); + pdh = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging + pdh.setSwitchableChannel(false); + break; + + case SIM: + // Running a physics simulator, log to NT + Logger.addDataReceiver(new NT4Publisher()); + break; + + case REPLAY: + // Replaying a log, set up replay source + setUseTiming(false); // Run as fast as possible + String logPath = LogFileUtil.findReplayLog(); + Logger.setReplaySource(new WPILOGReader(logPath)); + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim"))); + break; + } + + Logger.start(); + + robotContainer = new RobotContainer(); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void disabledInit() { + if(Constants.currentMode == Constants.Mode.REAL) { + pdh.setSwitchableChannel(false); + } + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void disabledExit() { + } + + @Override + public void teleopInit() { + if (autonomousCommand != null) { + autonomousCommand.cancel(); + } + + robotContainer.enabledInit(); + if(Constants.currentMode == Constants.Mode.REAL) { + pdh.setSwitchableChannel(true); + } + } + + @Override + public void autonomousInit() { + // m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + autonomousCommand = new PathPlannerAuto("Test Auto"); + + if (autonomousCommand != null) { + autonomousCommand.schedule(); + } + } + + @Override + public void autonomousPeriodic() { + } + + @Override + public void autonomousExit() { + } + + @Override + public void teleopPeriodic() { + } + + @Override + public void teleopExit() { + } + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() { + } + + @Override + public void testExit() { + } + + @Override + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6fe2b75..33d6be8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,346 +1,113 @@ -// // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. package frc.robot; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -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.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.Constants.ArmPositions; -import frc.robot.Constants.DIOPorts; -import frc.robot.Constants.GamePieceMode; -import frc.robot.Constants.Position; -import frc.robot.commands.DriveWithJoysticks; -import frc.robot.commands.FeedForward.TuneArmS; -import frc.robot.commands.auto.Align; -import frc.robot.commands.auto.AutoRoutines; -import frc.robot.commands.auto.SnapHeading; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.LEDManager; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.TelescopeSubsystem; -import frc.robot.subsystems.WristSubsystem; -import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.vision.Vision; -import frc.robot.util.PositionHelper; -import frc.robot.commands.pivot.HoldPivot; -import frc.robot.commands.pivot.MovePivot; -import frc.robot.commands.telescope.HoldTelescope; -import frc.robot.commands.telescope.HomeTelescope; -import frc.robot.commands.telescope.MoveTelescope; -import frc.robot.commands.wrist.HoldWrist; -import frc.robot.commands.wrist.HomeWrist; -import frc.robot.commands.wrist.MoveWrist; -import frc.robot.oi.ButtonMasher; -import frc.robot.oi.Thrustmaster; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.generated.TunerConstants; public class RobotContainer { - - private final Drive drive = new Drive(); - private final PivotSubsystem pivot = new PivotSubsystem(); - private final TelescopeSubsystem telescope = new TelescopeSubsystem(); - private final WristSubsystem wrist = new WristSubsystem(); - private final IntakeSubsystem intake = new IntakeSubsystem(); + final double MaxSpeedMetPerSec = 6; + final double MaxAngularRateRadiansPerSec = Math.PI * 2; // 2 PI is one full rotation per second + final double deadbandPercent = 0.16; + + /* Setting up bindings for necessary control of the swerve drive platform */ + // Add a keyboard joystick for testing + CommandJoystick keyboardJoystick = new CommandJoystick(2); // Keyboard joystick for sim/debugging + CommandJoystick leftJoystick = new CommandJoystick(0); // Left joystick + CommandJoystick rightJoystick = new CommandJoystick(1); // Right joystick + CommandXboxController controller = new CommandXboxController(2); // Button Masher + CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; + SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric().withIsOpenLoop(true); + SwerveRequest.RobotCentric driveRobot = new SwerveRequest.RobotCentric().withIsOpenLoop(true); + SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + Telemetry logger = new Telemetry(MaxSpeedMetPerSec); + + // Grabs the left joystick x-axis with deadband applied @SuppressWarnings("unused") - private final Vision vision = new Vision(); - private final LEDManager ledManager = new LEDManager(); - - private final Thrustmaster leftStick = new Thrustmaster(0); - private final Thrustmaster rightStick = new Thrustmaster(1); - private final ButtonMasher masher = new ButtonMasher(2); - - SendableChooser autoChooser = new SendableChooser(); - private Command activeAutoCommand = null; - private String activeAutoName = null; - - private DigitalInput brakeSwitch = new DigitalInput(DIOPorts.switch1); - private DigitalInput ledsSwitch = new DigitalInput(DIOPorts.switch2); - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - - configureSubsystems(); - configureCompBindings(); - // configureTestBindings(); - configureAutos(); + private double leftStickGetX() { + double rawInput = leftJoystick.getX(); + return Math.abs(rawInput) > deadbandPercent ? rawInput : 0; } - private void configureSubsystems() { - drive.setDefaultCommand(new DriveWithJoysticks( - drive, - () -> -leftStick.getRawAxis(1), - () -> -leftStick.getRawAxis(0), - () -> -rightStick.getRawAxis(0), - true - )); - - pivot.setDefaultCommand(new HoldPivot(pivot, telescope)); - telescope.setDefaultCommand(new HoldTelescope(telescope, pivot)); - wrist.setDefaultCommand(new HoldWrist(wrist, pivot)); + // Grabs the left joystick y-axis with deadband applied + @SuppressWarnings("unused") + private double leftStickGetY() { + double rawInput = leftJoystick.getY(); + return Math.abs(rawInput) > deadbandPercent ? rawInput : 0; } + // Grabs the right joystick x-axis with deadband applied @SuppressWarnings("unused") - private void configureTestBindings() { - // masher.back() - // .onTrue(new MovePivot(pivot, 0.8)) - // .onTrue(new MoveTelescope(telescope, pivot, 0.74)) - // .onTrue(new MoveWrist(wrist, pivot, -0.43)); - - // masher.start() - // .onTrue(new MovePivot(pivot, 0.69)) - // .onTrue(new MoveTelescope(telescope, pivot, 0.78)) - // .onTrue(new MoveWrist(wrist, pivot, -0.379)); - - // masher.a() - // .onTrue(new InstantCommand(() -> RobotState.getInstance().invertBack())); - - drive.setDefaultCommand(new DriveWithJoysticks( - drive, - () -> -masher.getLeftY(), - () -> -masher.getLeftX(), - () -> -masher.getRightX(), - true - )); - - masher.a() - .onTrue(new InstantCommand(() -> { - drive.resetHeading(); - drive.setFieldToVehicle( - new Pose2d( - RobotState.getInstance().getFieldToVehicle().getTranslation(), - new Rotation2d(0))); - })); - - masher.b().onTrue(new TuneArmS(wrist)); - } - - private void configureCompBindings() { - // Reset heading - rightStick.top() - .onTrue(new InstantCommand(() -> { - drive.resetHeading(); - drive.setFieldToVehicle( - new Pose2d( - RobotState.getInstance().getFieldToVehicle().getTranslation(), - new Rotation2d(0))); - })); - - // Slow down drive - leftStick.trigger() - .onTrue(new InstantCommand(() -> drive.setBabyMode(true))) - .onFalse(new InstantCommand(() -> drive.setBabyMode(false))); - - // Robot Relative Drive - rightStick.trigger() - .whileTrue( - new DriveWithJoysticks( - drive, - () -> -leftStick.getRawAxis(1), - () -> -leftStick.getRawAxis(0), - () -> -rightStick.getRawAxis(0), - false - ) - ); - - // Snap robot heading to y-axis - // leftStick.top().whileTrue(new SnapHeading(drive)); - - // Auto-align - leftStick.topLeft().whileTrue(new Align(drive, true)); - leftStick.topRight().whileTrue(new Align(drive, false)); - - // Home - rightStick.lowerButton(12).onTrue(new HomeWrist(wrist)); // Home wrist - rightStick.lowerButton(13).onTrue(new HomeTelescope(telescope)); // Home telescope - - // Manually disable (kill) subsystems - leftStick.lowerButton(7) - .onTrue(new InstantCommand(pivot::toggleKill, pivot)); - leftStick.lowerButton(6) - .onTrue(new InstantCommand(telescope::toggleKill, telescope)); - leftStick.lowerButton(5) - .onTrue(new InstantCommand(wrist::toggleKill, wrist)); - leftStick.lowerButton(8) - .onTrue(new InstantCommand(pivot::toggleKill)) - .onTrue(new InstantCommand(telescope::toggleKill)) - .onTrue(new InstantCommand(wrist::toggleKill)); - - // Set game piece mode - masher.leftBumper().onTrue(new InstantCommand(() -> - RobotState.getInstance().setMode(GamePieceMode.Cube))); - masher.rightBumper().onTrue(new InstantCommand(() -> - RobotState.getInstance().setMode(GamePieceMode.ConeDown))); - masher.y().onTrue(new InstantCommand(() -> - RobotState.getInstance().setMode(GamePieceMode.ConeUp))); - - // Move arm - masher.pov(0).onTrue(getMoveArmCommand(Position.High)); // move to high - masher.pov(90).onTrue(getMoveArmCommand(Position.Mid)); // move to mid - masher.pov(180).onTrue(getMoveArmCommand(Position.Ground)); // move to ground - masher.pov(270).onTrue(getMoveArmCommand(Position.Stow)); // move to stow - masher.x().onTrue(getMoveArmCommand(Position.Shelf)); // move to shelf - - masher.a().onTrue(new MoveWrist(wrist, pivot, ArmPositions.wristConePlace)); // Dunk cone - masher.b().onTrue(new MoveWrist(wrist, pivot, ArmPositions.placeConeDownHigh[2])); // Un-dunk cone - - // TODO: let masher flip sides - // masher.??????().onTrue( - // new InstantCommand(() -> RobotState.getInstance().invertBack())); // flip side - - // Manually jog arm - masher.leftStickLeft() - .onTrue(new InstantCommand(pivot::jogSetpointForward, pivot)); // jog pivot left - masher.leftStickRight() - .onTrue(new InstantCommand(pivot::jogSetpointBack, pivot)); // jog pivot right - - masher.rightStickRight() - .onTrue(new InstantCommand(telescope::jogSetpointForward, telescope)); // jog telescope up - masher.rightStickLeft() - .onTrue(new InstantCommand(telescope::jogSetpointBackward, telescope)); // jog telescope down - - masher.rightStickUp() - .onTrue(new InstantCommand(wrist::jogSetpointForward, wrist)); // jog wrist right - masher.rightStickDown() - .onTrue(new InstantCommand(wrist::jogSetpointBack, wrist)); // jog wrist left - - // Intake - masher.leftTrigger() - .onTrue(new InstantCommand(intake::toggleIntake)); // toggle intake - - masher.rightTrigger() - .onTrue(new InstantCommand(intake::place)) // start place - .onFalse(new InstantCommand(intake::stop)); // stop place - - masher.back() - .onTrue(new HomeWrist(wrist)); - - masher.start() - .onTrue(new InstantCommand(() -> RobotState.getInstance().invertBack())); + private double rightStickGetX() { + double rawInput = rightJoystick.getX(); + return Math.abs(rawInput) > deadbandPercent ? rawInput : 0; } - private void configureAutos() { - autoChooser.setDefaultOption("Blue clear 3", "B-1-1"); - autoChooser.addOption("Blue clear 2.5", "B-1-2"); - // autoChooser.addOption("B-1-3", "B-1-3"); - - autoChooser.addOption("Blue middle balance", "B-2-1"); - - // autoChooser.addOption("B-3-1", "B-3-1"); - // autoChooser.addOption("B-3-2", "B-3-2"); - - autoChooser.addOption("Red clear 3", "R-1-1"); - autoChooser.addOption("Red clear 2.5", "R-1-2"); - // autoChooser.addOption("R-1-3", "R-1-3"); - - autoChooser.addOption("Red middle balance", "R-2-1"); + // Grabs the right joystick y-axis with deadband applied + @SuppressWarnings("unused") + private double rightStickGetY() { + double rawInput = rightJoystick.getY(); + return Math.abs(rawInput) > deadbandPercent ? rawInput : 0; + } - // autoChooser.addOption("R-3-1", "R-3-1"); - // autoChooser.addOption("R-3-2", "R-3-2"); + private void configureBindings() { + drivetrain.setDefaultCommand( + drivetrain.applyRequest(() -> drive.withVelocityX(-leftStickGetY() * MaxSpeedMetPerSec) + .withVelocityY(-leftStickGetX() * MaxSpeedMetPerSec) + .withRotationalRate(-rightStickGetX() * MaxAngularRateRadiansPerSec))); + + rightJoystick.trigger() + .whileTrue( + drivetrain.applyRequest(() -> driveRobot.withVelocityX(-leftStickGetY() * MaxSpeedMetPerSec) + .withVelocityY(-leftStickGetX() * MaxSpeedMetPerSec) + .withRotationalRate(-rightStickGetX() * MaxAngularRateRadiansPerSec))); + + leftJoystick.trigger() + .whileTrue(drivetrain + .applyRequest(() -> drive.withVelocityX(-leftStickGetY() * MaxSpeedMetPerSec * 0.5) + .withVelocityY(-leftStickGetX() * MaxSpeedMetPerSec * 0.5) + .withRotationalRate(-rightStickGetX() * MaxAngularRateRadiansPerSec * 0.5))); + + rightJoystick.button(2) + .whileTrue(new InstantCommand(() -> drivetrain.seedFieldRelative())); + + controller.a().whileTrue(drivetrain.applyRequest(() -> brake)); + controller.b().whileTrue(drivetrain + .applyRequest( + () -> point + .withModuleDirection(new Rotation2d(-controller.getLeftY(), -controller.getLeftX())))); + + controller.x().whileTrue(drivetrain.applyRequest(() -> drive.withIsOpenLoop(false))); + + if (Utils.isSimulation()) { + drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); + } + drivetrain.registerTelemetry(logger::telemeterize); + } - SmartDashboard.putData("Auto Mode", autoChooser); + public void configureSubsystems() { } - public Command getAutonomousCommand() { - return activeAutoCommand; + public RobotContainer() { + configureBindings(); + configureSubsystems(); } public void disabledPeriodic() { - if (activeAutoCommand == null || !activeAutoName.equals(autoChooser.getSelected())) { - activeAutoCommand = new AutoRoutines(autoChooser.getSelected(), drive, pivot, telescope, wrist, intake); - activeAutoName = autoChooser.getSelected(); - } - - drive.setBrakeMode(!brakeSwitch.get()); - pivot.setBrakeMode(!brakeSwitch.get()); - telescope.setBrakeMode(!brakeSwitch.get()); - wrist.setBrakeMode(!brakeSwitch.get()); - - ledManager.setOff(ledsSwitch.get()); } public void enabledInit() { - drive.setBrakeMode(true); - pivot.setBrakeMode(true); - telescope.setBrakeMode(true); - wrist.setBrakeMode(true); - - ledManager.setOff(false); - - if (DriverStation.isTeleop()) { - new HomeTelescope(telescope).schedule(); - if (!RobotState.getInstance().hasIntaked()) { - new HomeWrist(wrist).schedule(); - } - pivot.normalConstrain(); - } - - pivot.setDesiredSetpointRad(new State(ArmPositions.stow[0], 0)); - telescope.setDesiredSetpoint(new State(ArmPositions.stow[1], 0)); - wrist.updateDesiredSetpointRad(new State(ArmPositions.stow[2], 0)); - } - - private Command getMoveArmCommand(Position position) { - return new InstantCommand(() -> { - - RobotState.getInstance().setStow(position == Position.Stow); - double[] positions = PositionHelper.getDouble(position, RobotState.getInstance().getMode()); - - if (position == Position.High || position == Position.Mid) { - boolean atBack = Math.abs(drive.getRotation().getRadians()) < Math.PI / 2; - RobotState.getInstance().setBack(atBack); - } - else if (position == Position.Shelf) { - boolean atBack = Math.abs(drive.getRotation().getRadians()) > Math.PI / 2; - RobotState.getInstance().setBack(atBack); - } - - Timer timer = new Timer(); - timer.reset(); - timer.start(); - - if (telescope.getPositionM() > 0.15 || positions[1] > 0.15) { - // move telescope to 0.05, then move pivot and wrist, then move telescope to goal - new SequentialCommandGroup( - new ParallelRaceGroup( - new HoldPivot(pivot, telescope), - new MoveTelescope(telescope, pivot, 0.05, true), - new HoldWrist(wrist, pivot) - ), - new ParallelRaceGroup( - new MovePivot(pivot, positions[0], true).andThen(new HoldPivot(pivot, telescope)), - new HoldTelescope(telescope, pivot), - new MoveWrist(wrist, pivot, positions[2], true).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && wrist.atGoal)) - ), - new ParallelRaceGroup( - new MovePivot(pivot, positions[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, positions[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, positions[2], false).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (telescope.atGoal && pivot.atGoal && wrist.atGoal)) - ) - ).schedule(); - } - else { - // move - new MovePivot(pivot, positions[0]).schedule(); - new MoveTelescope(telescope, pivot, positions[1]).schedule(); - new MoveWrist(wrist, pivot, positions[2]).schedule(); - } - }); } - } -// diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java deleted file mode 100644 index 9dc8723..0000000 --- a/src/main/java/frc/robot/RobotState.java +++ /dev/null @@ -1,196 +0,0 @@ -package frc.robot; - -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.GamePieceMode; -import frc.robot.Constants.PivotConstants; -import frc.robot.Constants.WristConstants; - -public class RobotState { - - private static RobotState instance; - - public static RobotState getInstance() { - if (instance == null) - instance = new RobotState(); - return instance; - } - - private SwerveDrivePoseEstimator poseEstimator; - - private boolean atBack = false; - - private boolean atStow = true; - - private boolean hasIntaked = false; - - private Mechanism2d displayMechanism = - new Mechanism2d(5, 5, new Color8Bit(Color.kWhite)); - private MechanismRoot2d root = displayMechanism.getRoot("arm", 2.5, 0.43); - - private MechanismLigament2d pivotLigament = root.append( - new MechanismLigament2d( - "pivot", - PivotConstants.lengthWOTeleM, - 0, - 4, - new Color8Bit(Color.kPurple))); - - private MechanismLigament2d telescopeLigament = pivotLigament.append( - new MechanismLigament2d( - "telescope", - 0, - 0, - 3, - new Color8Bit(Color.kBlue))); - - private MechanismLigament2d wrisLigament = telescopeLigament.append( - new MechanismLigament2d( - "wrist", - WristConstants.intakeLengthM, - 0, - 3, - new Color8Bit(Color.kCoral))); - - private GamePieceMode gamePieceMode = GamePieceMode.ConeDown; - - private final Field2d mainField = new Field2d(); - private final Field2d targetField = new Field2d(); - private final Field2d odometryField = new Field2d(); - - private SwerveDriveOdometry driveOdometry; - - private boolean isIntaking = false; - - public void initializePoseEstimator(Rotation2d rotation, SwerveModulePosition[] modulePositions) { - poseEstimator = new SwerveDrivePoseEstimator( - DriveConstants.kinematics, - rotation, - modulePositions, - new Pose2d(), - VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), - VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)) // doesn't matter - ); - - mainField.setRobotPose(new Pose2d(1.9, 4.99, Rotation2d.fromDegrees(0))); - SmartDashboard.putData("Field Pose", mainField); - SmartDashboard.putData("Target Pose", targetField); - driveOdometry = new SwerveDriveOdometry(DriveConstants.kinematics, rotation, modulePositions); - } - - public void recordDriveObservations(Rotation2d rotation, SwerveModulePosition[] modulePositions) { - poseEstimator.update(rotation, modulePositions); - driveOdometry.update(rotation, modulePositions); - } - - public void recordVisionObservations(Pose2d pose, Matrix stdDevs, double timestamp) { - - poseEstimator.addVisionMeasurement(pose, timestamp, stdDevs); - mainField.setRobotPose(poseEstimator.getEstimatedPosition()); - - } - - public void setFieldToVehicle(Rotation2d rotation, SwerveModulePosition[] modulePositions, Pose2d fieldToVehicle) { - poseEstimator.resetPosition(rotation, modulePositions, fieldToVehicle); - driveOdometry.resetPosition(rotation, modulePositions, fieldToVehicle); - } - - public void setGoalPose(Pose2d pose) { - targetField.setRobotPose(pose); - SmartDashboard.putData("Target Pose", targetField); - } - - public Pose2d getFieldToVehicle() { - // SmartDashboard.putNumber("OdometryX", driveOdometry.getPoseMeters().getX()); - // SmartDashboard.putNumber("OdometryY", driveOdometry.getPoseMeters().getY()); - // SmartDashboard.putNumber("OdometryTheta", driveOdometry.getPoseMeters().getRotation().getDegrees()); - - mainField.setRobotPose(poseEstimator.getEstimatedPosition()); - SmartDashboard.putData("Field Pose", mainField); - - odometryField.setRobotPose(driveOdometry.getPoseMeters()); - SmartDashboard.putData("Odometry Pose", odometryField); - - return poseEstimator.getEstimatedPosition(); - } - - public Pose2d getOdometryFieldToVehicle() { - return driveOdometry.getPoseMeters(); - } - - public void invertBack() { - atBack = !atBack; - } - - public void setBack(boolean back) { - atBack = back; - } - - - public void putPivotDisplay(double posRad) { - pivotLigament.setAngle(Units.radiansToDegrees(posRad)); - SmartDashboard.putData("Arm Mechanism", displayMechanism); - } - - public void putTelescopeDisplay(double posM) { - telescopeLigament.setLength(posM); - SmartDashboard.putData("Arm Mechanism", displayMechanism); - } - - public void putWristDisplay(double posRad) { - wrisLigament.setAngle(Units.radiansToDegrees(posRad)); - SmartDashboard.putData("Arm Mechanism", displayMechanism); - } - - public void setStow(boolean stowed) { - atStow = stowed; - } - - public boolean atStow() { - return atStow; - } - - public boolean atBack() { - return atBack; - } - - public GamePieceMode getMode() { - return gamePieceMode; - } - - public void setMode(Constants.GamePieceMode mode) { - gamePieceMode = mode; - } - - public boolean hasIntaked() { - return hasIntaked; - } - - public void setIntaked(boolean i) { - hasIntaked = i; - } - - public boolean isIntaking() { - return isIntaking; - } - - public void setIntaking(boolean i) { - isIntaking = i; - } -} diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..e1fc9ce --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,124 @@ +package frc.robot; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + } + + /* What to publish over networktables for telemetry */ + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot pose for field positioning */ + NetworkTable table = inst.getTable("Pose"); + DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + double robotRotation = 0; + + /* Robot speeds for general checking */ + NetworkTable driveStats = inst.getTable("Drive"); + DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + + /* Keep a reference of the last pose to calculate the speeds */ + Pose2d m_lastPose = new Pose2d(); + double lastTime = Utils.getCurrentTimeSeconds(); + + /* Mechanisms to represent the swerve module states */ + Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + /* Accept the swerve drive state and telemeterize it to smartdashboard */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the pose */ + Pose2d pose = state.Pose; + fieldTypePub.set("Field2d"); + fieldPub.set(new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() + }); + + Logger.recordOutput("pose/Pose2d", pose); + Logger.recordOutput("pose/fieldPose", table.getDoubleArrayTopic("/Pose/robotPose")); + + robotRotation = pose.getRotation().getRadians(); + + /* Telemeterize the robot's general speeds */ + double currentTime = Utils.getCurrentTimeSeconds(); + double diffTime = currentTime - lastTime; + lastTime = currentTime; + Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation(); + m_lastPose = pose; + + Translation2d velocities = distanceDiff.div(diffTime); + + speed.set(velocities.getNorm()); + velocityX.set(velocities.getX()); + velocityY.set(velocities.getY()); + odomPeriod.set(state.OdometryPeriod); + + /* Telemeterize the module's states */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } + + public double getRotationRadians() { + return robotRotation; + } +} diff --git a/src/main/java/frc/robot/commands/CharacterizeMechanism.java b/src/main/java/frc/robot/commands/CharacterizeMechanism.java deleted file mode 100644 index afe5bf8..0000000 --- a/src/main/java/frc/robot/commands/CharacterizeMechanism.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot.commands; - -import java.util.function.Consumer; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/* -Set as default command of subsystem to be characterized -Use Y/A buttons to change value by 0.01 -Use X/B buttons to change value by 0.1 -Use right bumper to run value - -To help characterize the telescope, you could use: -telescope.setDefaultCommand(new CharacterizeMechanism(telescope, masher.getGamepad(), (v) -> telescope.overrideVolts(v), 0)); - -If you wanted to tune the proportional of the telescope controller: -telescope.setDefaultCommand(new CharacterizeMechanism(telescope, masher.getGamepad(), (p) -> telescope.setP(p), 0)); -*/ - -@Deprecated -public class CharacterizeMechanism extends CommandBase { - - private final XboxController controller; - private final Consumer setValue; - - private double value; - - public CharacterizeMechanism(SubsystemBase subsystem, XboxController controller, Consumer setValue, double initialValue) { - - this.controller = controller; - this.setValue = setValue; - - value = initialValue; - - addRequirements(subsystem); - - } - - @Override - public void execute() { - - if (controller.getRightBumperPressed()) { - setValue.accept(value); - } - else if (controller.getRightBumperReleased()) { - setValue.accept(0.0); - } - - if (controller.getLeftBumperPressed()) { - setValue.accept(-value); - } - else if (controller.getLeftBumperReleased()) { - setValue.accept(0.0); - } - - if (controller.getYButtonPressed()) { - value += 0.01; - } - if (controller.getAButtonPressed()) { - value -= 0.01; - } - - if (controller.getXButtonPressed()) { - value += 0.1; - } - if (controller.getBButtonPressed()) { - value -= 0.1; - } - - SmartDashboard.putNumber("CharacterizationValue", value); - - } - -} diff --git a/src/main/java/frc/robot/commands/DriveWithJoysticks.java b/src/main/java/frc/robot/commands/DriveWithJoysticks.java deleted file mode 100644 index cabbac4..0000000 --- a/src/main/java/frc/robot/commands/DriveWithJoysticks.java +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import java.util.function.DoubleSupplier; - -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants.DriveConstants; -import frc.robot.subsystems.drive.Drive; - -public class DriveWithJoysticks extends CommandBase { - private final Drive drive; - private final DoubleSupplier xPercent; - private final DoubleSupplier yPercent; - private final DoubleSupplier omegaPercent; - private final boolean fieldRelative; - - /** Creates a new DriveWithJoysticks. */ - public DriveWithJoysticks(Drive drive, DoubleSupplier xPercent, DoubleSupplier yPercent, DoubleSupplier omegaPercent, boolean fieldRelative) { - this.drive = drive; - this.xPercent = xPercent; - this.yPercent = yPercent; - this.omegaPercent = omegaPercent; - this.fieldRelative = fieldRelative; - - addRequirements(drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - double xMPerS = processJoystickInputs(xPercent.getAsDouble(), false) * DriveConstants.maxDriveSpeed; - double yMPerS = processJoystickInputs(yPercent.getAsDouble(), false) * DriveConstants.maxDriveSpeed; - double omegaRadPerS = processJoystickInputs(omegaPercent.getAsDouble(), true) * DriveConstants.maxTurnRate; - - //Convert to field relative speeds - ChassisSpeeds targetSpeeds = fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xMPerS, yMPerS, omegaRadPerS, drive.getRotation()) - : new ChassisSpeeds(xMPerS, yMPerS, omegaRadPerS); - - drive.setGoalChassisSpeeds(targetSpeeds); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } - - private double processJoystickInputs(double value, boolean square) { - double scaledValue = 0.0; - double deadband = DriveConstants.driveJoystickDeadbandPercent; - if (Math.abs(value) > deadband) { - scaledValue = (Math.abs(value) - deadband) / (1 - deadband); - if (square) { - scaledValue = Math.copySign(scaledValue * scaledValue, value); - } else { - scaledValue = Math.copySign(scaledValue, value); - } - } - return scaledValue; - } - -} diff --git a/src/main/java/frc/robot/commands/FeedForward/TuneArmG.java b/src/main/java/frc/robot/commands/FeedForward/TuneArmG.java deleted file mode 100644 index 146da26..0000000 --- a/src/main/java/frc/robot/commands/FeedForward/TuneArmG.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.commands.FeedForward; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ArmSubsystem; - -public class TuneArmG extends CommandBase { - private ArmSubsystem arm; - - double startPosition; - - double kG; - double kS; - - public TuneArmG(ArmSubsystem arm, double kS) { - this.arm = arm; - this.kS = kS; - } - - @Override - public void initialize() { - startPosition = arm.getPositionRad(); - kG = kS; - } - - @Override - public void execute() { - arm.setVolts(-kG); - kG += 0.001; - } - - @Override - public void end(boolean interrupted) { - arm.stop(); - SmartDashboard.putNumber("kG", kG - kS); - } - - @Override - public boolean isFinished() { - return arm.getPositionRad() > Math.abs(startPosition - 0.1); - } -} diff --git a/src/main/java/frc/robot/commands/FeedForward/TuneArmS.java b/src/main/java/frc/robot/commands/FeedForward/TuneArmS.java deleted file mode 100644 index d7dda96..0000000 --- a/src/main/java/frc/robot/commands/FeedForward/TuneArmS.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.commands.FeedForward; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ArmSubsystem; - -public class TuneArmS extends CommandBase { - private ArmSubsystem arm; - - double startPosition; - - double appliedVolts; - - public TuneArmS(ArmSubsystem arm) { - this.arm = arm; - } - - @Override - public void initialize() { - startPosition = arm.getPositionRad(); - appliedVolts = 0; - } - - @Override - public void execute() { - arm.setVolts(appliedVolts); - appliedVolts += 0.001; - } - - @Override - public void end(boolean interrupted) { - arm.stop(); - SmartDashboard.putNumber("kS", appliedVolts); - } - - @Override - public boolean isFinished() { - return arm.getPositionRad() > Math.abs(startPosition - 20); - } -} diff --git a/src/main/java/frc/robot/commands/FeedForward/TuneArmV.java b/src/main/java/frc/robot/commands/FeedForward/TuneArmV.java deleted file mode 100644 index 8f868bc..0000000 --- a/src/main/java/frc/robot/commands/FeedForward/TuneArmV.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.robot.commands.FeedForward; - -import java.util.ArrayList; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ArmSubsystem; - -public class TuneArmV extends CommandBase { - private ArmSubsystem arm; - - private double volts; - - private ArrayList velocities; - - double startPosition; // TODO - - double kS; - double pastkV; - double average = 0; - double vel = 0; - - public TuneArmV(ArmSubsystem arm, double volts, double kS, double past) { - this.arm = arm; - this.volts = volts; - this.kS = kS; - this.pastkV = past; - } - - @Override - public void initialize() { - SmartDashboard.putBoolean("Ended", false); - arm.setVolts(volts); - velocities = new ArrayList(); - } - - @Override - public void execute() { - vel = arm.getVelRadS(); - SmartDashboard.putNumber("Velocity", vel); - if(Math.abs(arm.getPositionRad()) < 0.6) { - velocities.add(vel); - } - } - - @Override - public void end(boolean interrupted) { - SmartDashboard.putBoolean("Ended", true); - arm.stop(); - - for(double v : velocities) { - average += v; - } - - average /= velocities.size(); - - SmartDashboard.putNumber("kV", ((volts - kS) / average) + pastkV); - } - - @Override - public boolean isFinished() { - return Math.abs(arm.getPositionRad()) > 1; - } -} diff --git a/src/main/java/frc/robot/commands/auto/Align.java b/src/main/java/frc/robot/commands/auto/Align.java deleted file mode 100644 index 295c62d..0000000 --- a/src/main/java/frc/robot/commands/auto/Align.java +++ /dev/null @@ -1,109 +0,0 @@ -package frc.robot.commands.auto; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.RobotState; -import frc.robot.Constants.DriveConstants; -import frc.robot.Constants.GamePieceMode; -import frc.robot.subsystems.drive.Drive; -import frc.robot.util.CustomHolonomicDrive; -import frc.robot.util.ExtendedPathPoint; - -public class Align extends CommandBase { - - private final Drive drive; - private final boolean left; - - private final CustomHolonomicDrive holonomicDrive; - - private ExtendedPathPoint targetPose; - - public Align(Drive drive, boolean left) { - - this.drive = drive; - this.left = left; - - holonomicDrive = new CustomHolonomicDrive( - new PIDController(DriveConstants.poseMoveTranslationkP, 0, 0), - new PIDController(DriveConstants.poseMoveRotationkP, 0, 0), - new SlewRateLimiter(DriveConstants.poseMoveTranslationMaxAccel), - new SlewRateLimiter(DriveConstants.poseMoveTranslationMaxAccel), - new SlewRateLimiter(DriveConstants.poseMoveRotationMaxAccel) - ); - - addRequirements(drive); - - } - - @Override - public void initialize() { - - Pose2d goalPose = getGoalPose(left); - SmartDashboard.putNumberArray("Target Pose", new Double[]{goalPose.getX(), goalPose.getY(), goalPose.getRotation().getDegrees()}); - targetPose = new ExtendedPathPoint(goalPose.getTranslation(), goalPose.getRotation(), goalPose.getRotation()); - - } - - @Override - public void execute() { - - ChassisSpeeds speeds = holonomicDrive.calculate(RobotState.getInstance().getFieldToVehicle(), targetPose.getPose2d()); - - Transform2d distance = RobotState.getInstance().getFieldToVehicle().minus(targetPose.getPose2d()); - if (Math.abs(distance.getX()) < 0.01) speeds.vxMetersPerSecond = 0; - if (Math.abs(distance.getY()) < 0.01) speeds.vyMetersPerSecond = 0; - if (Math.abs(distance.getRotation().getRadians()) < 0.07) speeds.omegaRadiansPerSecond = 0; - - drive.setGoalChassisSpeeds(speeds); - - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - - drive.setGoalChassisSpeeds(new ChassisSpeeds()); - - } - - private Pose2d getGoalPose(boolean left) { - - boolean cube = RobotState.getInstance().getMode() == GamePieceMode.Cube; - Pose2d currentPose = RobotState.getInstance().getFieldToVehicle(); - Rotation2d rotation = new Rotation2d(Math.abs(drive.getRotation().getRadians()) > Math.PI / 2 ? Math.PI - 0.01 : 0); - - if (DriverStation.getAlliance() == Alliance.Blue) { - if (currentPose.getX() > 8) - return left ? new Pose2d(15.5, 7.55, rotation) : new Pose2d(15.5, 5.95, rotation); - double x = 1.95; - double yCenter = currentPose.getY() > 3.58 ? 4.43 : currentPose.getY() > 1.91 ? 2.75 : 1.07; - if (cube) return new Pose2d(x, yCenter, rotation); - if (left) return new Pose2d(x, yCenter+0.56, rotation); - return new Pose2d(x, yCenter-0.56, rotation); - } - if (DriverStation.getAlliance() == Alliance.Red) { - if (currentPose.getX() < 8) - return left ? new Pose2d(1, 5.95, rotation) : new Pose2d(1, 7.55, rotation); - double x = 14.6; - double yCenter = currentPose.getY() > 3.58 ? 4.43 : currentPose.getY() > 1.91 ? 2.75 : 1.07; - if (cube) return new Pose2d(x, yCenter, rotation); - if (left) return new Pose2d(x, yCenter-0.56, rotation); - return new Pose2d(x, yCenter+0.56, rotation); - } - return currentPose; - - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/AutoRoutines.java b/src/main/java/frc/robot/commands/auto/AutoRoutines.java deleted file mode 100644 index fb5fe15..0000000 --- a/src/main/java/frc/robot/commands/auto/AutoRoutines.java +++ /dev/null @@ -1,446 +0,0 @@ -package frc.robot.commands.auto; - -import java.util.List; - -import com.pathplanner.lib.PathConstraints; -import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import frc.robot.RobotState; -import frc.robot.Constants.ArmPositions; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.GamePieceMode; -import frc.robot.commands.pivot.HoldPivot; -import frc.robot.commands.pivot.MovePivot; -import frc.robot.commands.telescope.HoldTelescope; -import frc.robot.commands.telescope.HomeTelescope; -import frc.robot.commands.telescope.MoveTelescope; -import frc.robot.commands.wrist.HoldWrist; -import frc.robot.commands.wrist.HomeWrist; -import frc.robot.commands.wrist.MoveWrist; -import frc.robot.commands.wrist.MoveWristAbsolute; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.TelescopeSubsystem; -import frc.robot.subsystems.WristSubsystem; -import frc.robot.subsystems.drive.Drive; - -public class AutoRoutines extends SequentialCommandGroup { - - private final Drive drive; - private final PivotSubsystem pivot; - private final TelescopeSubsystem telescope; - private final WristSubsystem wrist; - private final IntakeSubsystem intake; - - public AutoRoutines(String pathName, Drive driveSubsystem, PivotSubsystem pivotSubsystem, TelescopeSubsystem telescopeSubsystem, WristSubsystem wristSubsystem, IntakeSubsystem intakeSubsystem) { - - drive = driveSubsystem; - pivot = pivotSubsystem; - telescope = telescopeSubsystem; - wrist = wristSubsystem; - intake = intakeSubsystem; - - /* - 1-1: 3 - 1-2: 2.5 + balance - 1-3: 4 - 2-1: 2 + balance left - 2-2: 2 + balance right - 3-1: 3 - 3-2: 2.5 + balance - */ - - // To transfrom blue path to red path on the x-axis do 16.53-x - // To rotate blue path to red path adjust rotation 180 and set the heading to (180-abs(heading))*sign(heading) - // Load path group - PathConstraints constraints = new PathConstraints(AutoConstants.maxVel, AutoConstants.maxAccel); - if (pathName.endsWith("1-3")) { - constraints = new PathConstraints(AutoConstants.maxVelFast, AutoConstants.maxAccelFast); - } - else if (pathName.contains("-2-")) { - constraints = new PathConstraints(AutoConstants.maxVelSlow, AutoConstants.maxAccelSlow); - } - List pathGroup = PathPlanner.loadPathGroup(pathName, constraints); - - if (!pathName.endsWith("1-3") && !pathName.contains("-2-")) { - addCommands( - new InstantCommand(pivot::startAutoTimer), - resetOdometry(pathGroup), - fakeHome(), - placeCone() - // new InstantCommand(pivot::autoConstrain) - ); - } - if (pathName.contains("-2-")) { - addCommands( - new InstantCommand(pivot::startAutoTimer), - resetOdometry(pathGroup), - fakeHomeCube(), - placeCubeInitial(), - new ParallelRaceGroup( - drive(pathGroup.get(0), true).andThen(new Balance(drive)), - holdStow() - ) - ); - } - if (pathName.contains("1-1") || pathName.contains("1-2")) { - addCommands( - new ParallelRaceGroup( - new WaitCommand(0.7).andThen(drive(pathGroup.get(0))), - intakeCube() - ), - new ParallelRaceGroup( - drive(pathGroup.get(1)), - preparePlaceCubeHigh() - ), - placeCube(), - new ParallelRaceGroup( - new WaitCommand(0.7).andThen(drive(pathGroup.get(2))), - intakeCube() - ) - ); - } - if (pathName.contains("3-1") || pathName.contains("3-2")) { - addCommands( - new ParallelRaceGroup( - new WaitCommand(0.7).andThen(drive(pathGroup.get(0))), - intakeCubeBump() - ), - new ParallelRaceGroup( - drive(pathGroup.get(1)), - preparePlaceCubeHigh() - ), - placeCube(), - new ParallelRaceGroup( - new WaitCommand(0.7).andThen(drive(pathGroup.get(2))), - intakeCubeBump() - ) - ); - } - if (pathName.endsWith("1-1") || pathName.endsWith("3-1")) { - addCommands( - new ParallelRaceGroup( - drive(pathGroup.get(3)), - preparePlaceCubeMid() - ), - placeCube(), - new InstantCommand(pivot::stopAutoTimer), - holdStow() - ); - } - if (pathName.endsWith("1-2") || pathName.endsWith("3-2")) { - addCommands( - new ParallelCommandGroup( - holdStow(), - drive(pathGroup.get(3)) - .andThen(new Balance(drive)) - ) - ); - } - if (pathName.endsWith("1-3")) { - addCommands( - new InstantCommand(pivot::startAutoTimer), - resetOdometry(pathGroup), - fakeHome(), - spitCone(), - new ParallelRaceGroup( - drive(pathGroup.get(0)), - intakeCube() - ), - new ParallelRaceGroup( - drive(pathGroup.get(1)), - preparePlaceCubeMid() - ), - placeCube(), - new ParallelRaceGroup( - drive(pathGroup.get(2)), - intakeCube() - ), - new ParallelRaceGroup( - drive(pathGroup.get(3)), - preparePlaceCubeLow() - ), - placeCube(), - new ParallelRaceGroup( - drive(pathGroup.get(4)), - intakeCube() - ), - new ParallelRaceGroup( - drive(pathGroup.get(5)), - preparePlaceCubeLow() - ), - placeCube(), - new InstantCommand(pivot::stopAutoTimer), - holdStow() - ); - } - } - - private Command resetOdometry(List pathGroup) { - return new InstantCommand( () -> { - PathPlannerState initialState = pathGroup.get(0).getInitialState(); - drive.setHeading(0); - drive.setFieldToVehicle(new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation)); - }); - } - - private Command drive(PathPlannerTrajectory trajectory) { - return new FollowTrajectory(drive, trajectory, false); - } - - private Command drive(PathPlannerTrajectory trajectory, boolean slow) { - return new FollowTrajectory(drive, trajectory, slow); - } - - private Command fakeHome() { - return new InstantCommand(() -> { - telescope.resetOffset(); - telescope.homed = true; - wrist.resetOffset(); - wrist.homed = true; - }); - } - - private Command fakeHomeCube() { - return new InstantCommand(() -> { - telescope.resetOffset(); - telescope.homed = true; - wrist.resetOffsetCube(); - wrist.homed = true; - }); - } - - @SuppressWarnings("unused") - private Command home() { - return new ParallelCommandGroup( - new HomeTelescope(telescope), - new HomeWrist(wrist) - ); - } - - private Command placeCone() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setMode(GamePieceMode.ConeUp)), - new InstantCommand(() -> intake.setIntake(true)), - new InstantCommand(() -> RobotState.getInstance().setBack(true)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.placeConeUpHighPrepare[0], true).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeConeUpHighPrepare[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeConeUpHighPrepare[2], false).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.placeConeUpHighAuto[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeConeUpHighAuto[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeConeUpHighAuto[2], false).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelRaceGroup( - hold(), - new SequentialCommandGroup( - new InstantCommand(intake::place), - new WaitCommand(0.3), - new InstantCommand(intake::stop) - ) - ) - ); - } - - private Command placeCubeInitial() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setMode(GamePieceMode.Cube)), - new InstantCommand(() -> intake.setIntake(true)), - new InstantCommand(() -> RobotState.getInstance().setBack(true)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.placeCubeAuto[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeCubeAuto[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeCubeAuto[2], false).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelRaceGroup( - hold(), - new SequentialCommandGroup( - new InstantCommand(intake::place), - new WaitCommand(0.5), - new InstantCommand(intake::stop) - ) - ) - ); - } - - private Command intakeCube() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setMode(GamePieceMode.Cube)), - new InstantCommand(() -> RobotState.getInstance().setBack(false)), - new ParallelRaceGroup( - new HoldPivot(pivot, telescope), - new MoveTelescope(telescope, pivot, 0.05, true), - new HoldWrist(wrist, pivot), - new WaitUntilCommand(() -> telescope.getPositionM() < 0.25) - ), - new InstantCommand(() -> intake.setIntake(true)), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.intakeCubeGround[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.intakeCubeGround[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.intakeCubeGround[2], false).andThen(new HoldWrist(wrist, pivot)).withTimeout(1.75).andThen(pivot::printAutoTimer).andThen(new HoldWrist(wrist, pivot)) - ) - ); - } - - private Command intakeCubeBump() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setMode(GamePieceMode.Cube)), - new InstantCommand(() -> RobotState.getInstance().setBack(false)), - new ParallelRaceGroup( - new HoldPivot(pivot, telescope), - new MoveTelescope(telescope, pivot, 0.05, true), - new HoldWrist(wrist, pivot), - new WaitUntilCommand(() -> telescope.getPositionM() < 0.25) - ), - new InstantCommand(() -> intake.setIntake(true)), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.intakeCubeGroundBump[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.intakeCubeGroundBump[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.intakeCubeGroundBump[2], false).andThen(new HoldWrist(wrist, pivot)).withTimeout(1.75).andThen(pivot::printAutoTimer).andThen(new HoldWrist(wrist, pivot)) - ) - ); - } - - private Command preparePlaceCubeLow() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setBack(true)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.stow[0], true).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.stow[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.stow[2], true).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.placeCubeLow[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeCubeLow[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeCubeLow[2], false).andThen(new HoldWrist(wrist, pivot)) - ) - ); - } - - private Command preparePlaceCubeMid() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setBack(true)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.stow[0], true).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.stow[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.stow[2], true).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.placeCubeMid[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeCubeMid[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeCubeMid[2], false).andThen(new HoldWrist(wrist, pivot)) - ) - ); - } - - private Command preparePlaceCubeHigh() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setBack(true)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.stow[0], true).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.stow[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.stow[2], true).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.placeCubeHigh[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.placeCubeHigh[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.placeCubeHigh[2], false).andThen(new HoldWrist(wrist, pivot)) - ) - ); - } - - private Command placeCube() { - return new ParallelRaceGroup( - hold(), - new SequentialCommandGroup( - new InstantCommand(intake::shoot), - new WaitCommand(0.1), - new InstantCommand(intake::stop) - ) - ); - } - - @SuppressWarnings("unused") - @Deprecated - private Command flingCube() { - return new SequentialCommandGroup( - new ParallelRaceGroup( - new WaitCommand(3), - holdStow() - ), - new InstantCommand(() -> RobotState.getInstance().setBack(false)), - new ParallelRaceGroup( - new MovePivot(pivot, ArmPositions.preFlingCube[0], true).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.preFlingCube[1], true).andThen(new HoldTelescope(telescope, pivot)), - new MoveWrist(wrist, pivot, ArmPositions.preFlingCube[2], true).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new ParallelCommandGroup( - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.postFlingCube[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveTelescope(telescope, pivot, ArmPositions.postFlingCube[1], false).andThen(new HoldTelescope(telescope, pivot)), - new MoveWristAbsolute(wrist, pivot, ArmPositions.postFlingCube[2], ArmPositions.postFlingCube[0], false).andThen(new HoldWrist(wrist, pivot)), - new WaitUntilCommand(() -> (pivot.atGoal && telescope.atGoal && wrist.atGoal)) - ), - new SequentialCommandGroup( - new WaitCommand(0.4), - new InstantCommand(intake::shootBackwards), - new WaitCommand(0.4), - new InstantCommand(intake::stop) - ) - ) - ); - } - - private Command hold() { - return new ParallelCommandGroup( - new HoldPivot(pivot, telescope), - new HoldTelescope(telescope, pivot), - new HoldWrist(wrist, pivot) - ); - } - - private Command holdStow() { - return new ParallelCommandGroup( - new MoveTelescope(telescope, pivot, ArmPositions.stow[1], true).andThen(new HoldTelescope(telescope, pivot)), - new SequentialCommandGroup( - new WaitUntilCommand(() -> telescope.atGoal), - new ParallelCommandGroup( - new MovePivot(pivot, ArmPositions.stow[0], false).andThen(new HoldPivot(pivot, telescope)), - new MoveWrist(wrist, pivot, ArmPositions.stow[2], false).andThen(new HoldWrist(wrist, pivot)) - ) - ) - ); - } - - private Command spitCone() { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotState.getInstance().setMode(GamePieceMode.ConeUp)), - new InstantCommand(intake::shoot), - new WaitCommand(0.2), - new InstantCommand(intake::stop) - ); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/Balance.java b/src/main/java/frc/robot/commands/auto/Balance.java deleted file mode 100644 index d95cfbe..0000000 --- a/src/main/java/frc/robot/commands/auto/Balance.java +++ /dev/null @@ -1,102 +0,0 @@ -package frc.robot.commands.auto; - -import com.ctre.phoenix.sensors.Pigeon2; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.RobotState; -import frc.robot.Constants.AutoConstants; -import frc.robot.Constants.DriveConstants; -import frc.robot.subsystems.drive.Drive; - -public class Balance extends CommandBase { - - private enum Steps { - DRIVE, - BALANCE, - DONE, - } - - private final Drive drive; - - private Steps step = Steps.DRIVE; - - private Alliance alliance; - - private PIDController xController = - new PIDController(2.3, 0.01, 0); - - private PIDController thetaController = - new PIDController(DriveConstants.poseMoveRotationkP, 0, 0); - - - private Timer timer = new Timer(); - - private double xOffset = 0.0; - - public Balance(Drive drive) { - this.drive = drive; - addRequirements(drive); - } - - @Override - public void initialize() { - alliance = DriverStation.getAlliance(); - - step = Steps.DRIVE; - - timer.start(); - timer.reset(); - - xOffset = 0.0; - } - - @Override - public void execute() { - if (step == Steps.DRIVE) { - drive.setGoalChassisSpeeds(new ChassisSpeeds(-1.25, 0, 0)); - - if (timer.get() > 2.2) { - step = Steps.BALANCE; - } - } - if (step == Steps.BALANCE) { - timer.reset(); - - double pitchFeedforward = 0.0; - - if (Math.abs(drive.getRoll()) > 0.1) { - pitchFeedforward = 0.3; - } else { - pitchFeedforward = 0.0; - } - - ChassisSpeeds speeds = new ChassisSpeeds( - xController.calculate(RobotState.getInstance().getFieldToVehicle().getX(), 3.9) - + pitchFeedforward, - 0, - thetaController.calculate(RobotState.getInstance().getFieldToVehicle().getRotation().getRadians(), 0.0) - ); - - drive.setGoalChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, drive.getRotation())); - } - } - - @Override - public boolean isFinished() { - return step == Steps.DONE; - } - - @Override - public void end(boolean interrupted) { - drive.stop(); - } - -} diff --git a/src/main/java/frc/robot/commands/auto/FollowTrajectory.java b/src/main/java/frc/robot/commands/auto/FollowTrajectory.java deleted file mode 100644 index bce14c4..0000000 --- a/src/main/java/frc/robot/commands/auto/FollowTrajectory.java +++ /dev/null @@ -1,92 +0,0 @@ -package frc.robot.commands.auto; - -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; - -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.RobotState; -import frc.robot.Constants.AutoConstants; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.drive.Drive; - -public class FollowTrajectory extends CommandBase { - - private final Drive drive; - private Pose2d latestFieldToVehicle; - private PathPlannerTrajectory trajectory; - private PIDController xController; - private PIDController yController; - private ProfiledPIDController thetaController; - - private boolean slow; - - private HolonomicDriveController controller; - - private final Timer timer = new Timer(); - - public FollowTrajectory(Drive drive, PathPlannerTrajectory trajectory, boolean slow) { - - this.drive = drive; - this.trajectory = trajectory; - this.slow = slow; - - addRequirements(drive); - - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - - if (!slow) { - xController = new PIDController(AutoConstants.autoTranslationXKp, AutoConstants.autoTranslationXKi, AutoConstants.autoTranslationXKd); - yController = new PIDController(AutoConstants.autoTranslationYKp, AutoConstants.autoTranslationYKi, AutoConstants.autoTranslationYKd); - } - else { - xController = new PIDController(AutoConstants.autoTranslationSlowXKp, AutoConstants.autoTranslationSlowXKi, AutoConstants.autoTranslationSlowXKd); - yController = new PIDController(AutoConstants.autoTranslationSlowYKp, AutoConstants.autoTranslationSlowYKi, AutoConstants.autoTranslationSlowYKd); - } - thetaController = - new ProfiledPIDController( - AutoConstants.autoRotationKp, AutoConstants.autoRotationKi, AutoConstants.autoRotationKd, - new TrapezoidProfile.Constraints(Math.PI, Math.PI) - ); - - thetaController.enableContinuousInput(-Math.PI, Math.PI); - controller = new HolonomicDriveController(xController, yController, thetaController); - } - - @Override - public void execute() { - - latestFieldToVehicle = RobotState.getInstance().getFieldToVehicle(); - - PathPlannerState desiredState = (PathPlannerState) trajectory.sample(timer.get()); - - ChassisSpeeds adjustedSpeeds = new ChassisSpeeds(); - adjustedSpeeds = controller.calculate( - latestFieldToVehicle, desiredState, desiredState.holonomicRotation); - - RobotState.getInstance().setGoalPose(new Pose2d(desiredState.poseMeters.getTranslation(), desiredState.holonomicRotation)); - - drive.setGoalChassisSpeeds(adjustedSpeeds); - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(trajectory.getTotalTimeSeconds()); - } - - @Override - public void end(boolean interrupted) { - drive.setGoalChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/SnapHeading.java b/src/main/java/frc/robot/commands/auto/SnapHeading.java deleted file mode 100644 index 3fdf403..0000000 --- a/src/main/java/frc/robot/commands/auto/SnapHeading.java +++ /dev/null @@ -1,73 +0,0 @@ -package frc.robot.commands.auto; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.RobotState; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.Constants.DriveConstants; -import frc.robot.subsystems.drive.Drive; -import frc.robot.util.CustomHolonomicDrive; -import frc.robot.util.ExtendedPathPoint; - -public class SnapHeading extends CommandBase { - private final Drive drive; - - private final CustomHolonomicDrive holonomicDrive; - - private ExtendedPathPoint targetPose; - - public SnapHeading(Drive drive) { - - this.drive = drive; - - holonomicDrive = new CustomHolonomicDrive( - new PIDController(0, 0, 0), - new PIDController(DriveConstants.poseMoveRotationkP, 0, 0), - new SlewRateLimiter(0), - new SlewRateLimiter(0), - new SlewRateLimiter(DriveConstants.poseMoveRotationMaxAccel) - ); - - addRequirements(drive); - - } - - @Override - public void initialize() { - - Pose2d currentPose = RobotState.getInstance().getFieldToVehicle(); - targetPose = new ExtendedPathPoint(new Translation2d(currentPose.getX(), currentPose.getY()), new Rotation2d(Math.abs(drive.getRotation().getRadians()) > Math.PI / 2 ? Math.PI - 0.01 : 0), new Rotation2d(Math.abs(drive.getRotation().getRadians()) > Math.PI / 2 ? Math.PI - 0.01 : 0)); - - } - - @Override - public void execute() { - - ChassisSpeeds speeds = holonomicDrive.calculate(RobotState.getInstance().getFieldToVehicle(), targetPose.getPose2d()); - - Transform2d distance = RobotState.getInstance().getFieldToVehicle().minus(targetPose.getPose2d()); - if (Math.abs(distance.getX()) < 0.01) speeds.vxMetersPerSecond = 0; - if (Math.abs(distance.getY()) < 0.01) speeds.vyMetersPerSecond = 0; - if (Math.abs(distance.getRotation().getRadians()) < 0.07) speeds.omegaRadiansPerSecond = 0; - - drive.setGoalChassisSpeeds(speeds); - - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - - drive.setGoalChassisSpeeds(new ChassisSpeeds()); - - } -} diff --git a/src/main/java/frc/robot/commands/pivot/HoldPivot.java b/src/main/java/frc/robot/commands/pivot/HoldPivot.java deleted file mode 100644 index bb58c63..0000000 --- a/src/main/java/frc/robot/commands/pivot/HoldPivot.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.TelescopeSubsystem; - -/** - * Command that holds the pivot at the position of its desiredSetpoint - * variable. Should be set as a default command for the pivot and only stopped - * through interruption. - */ -public class HoldPivot extends CommandBase { - private PivotSubsystem pivot; - private TelescopeSubsystem telescope; - - private State goalState; - private TrapezoidProfile profile; - - private final Timer timer = new Timer(); - - public HoldPivot(PivotSubsystem pivot, TelescopeSubsystem telescope) { - this.pivot = pivot; - this.telescope = telescope; - - addRequirements(this.pivot); - } - - @Override - public void initialize() { - goalState = new State(pivot.getDesiredSetpointRad().position, 0); - - profile = new TrapezoidProfile(pivot.getConstraintsRad(), goalState, - new State(pivot.getPositionRad(), pivot.getVelRadS())); - - timer.reset(); - timer.start(); - - pivot.setVolts(0); - } - - @Override - public void execute() { - - State setpoint = profile.calculate(timer.get()); - - SmartDashboard.putNumber("Pivot/hold setpoint", setpoint.position); - - // Calculate output from feedforward & PID - double pivotOut = MathUtil.clamp(pivot.calculateControl(setpoint, telescope.getPositionM()), -4, 4); - - pivot.setVolts(pivotOut); - pivot.setSimPos(goalState.position); - } -} diff --git a/src/main/java/frc/robot/commands/pivot/MovePivot.java b/src/main/java/frc/robot/commands/pivot/MovePivot.java deleted file mode 100644 index 62e75fb..0000000 --- a/src/main/java/frc/robot/commands/pivot/MovePivot.java +++ /dev/null @@ -1,104 +0,0 @@ -package frc.robot.commands.pivot; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.RobotState; -import frc.robot.subsystems.PivotSubsystem; - -/** - * Command that moves the pivot from its current location to a new setpoint. - * Uses a trapizoidal motion profile, and ends when the profile completes. - */ -public class MovePivot extends CommandBase{ - private PivotSubsystem pivot; - - private TrapezoidProfile profile; - private State goalState; - private double posRad; - - private final Timer timer = new Timer(); - - private final Timer finishedTimer = new Timer(); - - private final boolean ignoreValidation; - - /** - * Constructs an instance of this command. The position should be field- - * relative to the front of the robot. - * @param pivot the pivot subsystem - * @param posRad the setpoint position of the pivot in radians. Again, - * this should be field-relative to the front. - * @param velRadS the setpoint velocity. - */ - public MovePivot(PivotSubsystem pivot, double posRad, boolean ignoreValidation) { - this.pivot = pivot; - this.posRad = posRad; - this.ignoreValidation = ignoreValidation; - - addRequirements(this.pivot); - } - - public MovePivot(PivotSubsystem pivot, double posRad) { - this(pivot, posRad, false); - } - - @Override - public void initialize() { - finishedTimer.reset(); - finishedTimer.start(); - - goalState = new TrapezoidProfile.State(posRad, 0); - - // Shift the setpoint to the back of the robot if the pivot is flagged - // as such. - if (RobotState.getInstance().atBack()) - goalState.position = Math.PI - goalState.position; - - // Create the trapezoid motion based on max vel and accel, - // as well as the current starting state - timer.reset(); - timer.start(); - // Constraints constraints = DriverStation.isAutonomous() ? new TrapezoidProfile.Constraints(720, 180) : pivot.getConstraintsRad(); - profile = new TrapezoidProfile(pivot.getConstraintsRad(), goalState, - new State(pivot.getPositionRad(), pivot.getVelRadS())); - - // Marks setpoint in pivot subsystem for the hold command - pivot.setDesiredSetpointRad(goalState); - - pivot.resetPID(); - - pivot.atGoal = false; - } - - @Override - public void execute() { - State setpoint = profile.calculate(timer.get()); - - SmartDashboard.putNumber("Pivot/profiled setpoint", setpoint.position); - - SmartDashboard.putNumber("Pivot/final setpoint", setpoint.position); - - double pivotOut = pivot.calculateControl(setpoint, 0); - pivot.setVolts(pivotOut); - pivot.setSimPos(setpoint.position); - - if (Math.abs(pivot.getPositionRad()-goalState.position) > Units.degreesToRadians(4)) { - finishedTimer.reset(); - } - } - - @Override - public boolean isFinished() { - return finishedTimer.hasElapsed(0.2) || (ignoreValidation && profile.isFinished(timer.get())); - } - - @Override - public void end(boolean interrupted) { - pivot.stop(); - pivot.atGoal = true; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/telescope/HoldTelescope.java b/src/main/java/frc/robot/commands/telescope/HoldTelescope.java deleted file mode 100644 index f9a73ba..0000000 --- a/src/main/java/frc/robot/commands/telescope/HoldTelescope.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.commands.telescope; - -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.TelescopeSubsystem; - -public class HoldTelescope extends CommandBase { - private TelescopeSubsystem telescope; - private PivotSubsystem pivot; - - private State goalState; - - public HoldTelescope(TelescopeSubsystem telescope, PivotSubsystem pivot) { - this.telescope = telescope; - this.pivot = pivot; - - addRequirements(telescope); - } - - - @Override - public void initialize() { - goalState = new State(telescope.getDesiredSetpoint().position, 0); - } - - @Override - public void execute() { - SmartDashboard.putNumber("Pivot/hold setpoint", goalState.position); - - double output = telescope.calculateControl(goalState, pivot.getPositionRad()); - - telescope.setVolts(output); - telescope.setSimPos(goalState.position); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/telescope/HomeTelescope.java b/src/main/java/frc/robot/commands/telescope/HomeTelescope.java deleted file mode 100644 index f7e0bfb..0000000 --- a/src/main/java/frc/robot/commands/telescope/HomeTelescope.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands.telescope; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.TelescopeSubsystem; - -public class HomeTelescope extends CommandBase{ - private TelescopeSubsystem telescope; - - private Timer timer; - - public HomeTelescope(TelescopeSubsystem telescope) { - this.telescope = telescope; - - timer = new Timer(); - - addRequirements(telescope); - } - - public void initialize() { - telescope.setVolts(-1); - timer.start(); - timer.reset(); - } - - public void execute() { - if (telescope.getAmps() < 30) { - timer.reset(); - } - } - - public boolean isFinished() { - return timer.hasElapsed(0.3); - } - - public void end(boolean interrupted) { - if (!interrupted) { - telescope.resetOffset(); - telescope.homed = true; - } - - telescope.stop(); - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/telescope/MoveTelescope.java b/src/main/java/frc/robot/commands/telescope/MoveTelescope.java deleted file mode 100644 index 7fc0349..0000000 --- a/src/main/java/frc/robot/commands/telescope/MoveTelescope.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.robot.commands.telescope; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.TelescopeSubsystem; - -public class MoveTelescope extends CommandBase { - private TelescopeSubsystem telescope; - private PivotSubsystem pivot; - - private TrapezoidProfile profile; - - private double goalM; - - private Timer timer = new Timer(); - - private Timer finishedTimer = new Timer(); - - private final boolean ignoreValidation; - - public MoveTelescope( - TelescopeSubsystem telescope, - PivotSubsystem pivot, - double goalM, - boolean ignoreValidation) { - - this.goalM = goalM; - this.telescope = telescope; - this.pivot = pivot; - this.ignoreValidation = ignoreValidation; - - addRequirements(telescope); - } - - public MoveTelescope(TelescopeSubsystem telescope, - PivotSubsystem pivot, - double goalM) { - this(telescope, pivot, goalM, false); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - - finishedTimer.reset(); - finishedTimer.start(); - - profile = new TrapezoidProfile(telescope.getConstraints(), new State(goalM, 0), new State(telescope.getPositionM(), telescope.getVel())); - - telescope.setDesiredSetpoint(new State(goalM, 0)); - telescope.resetPID(); - - telescope.atGoal = false; - } - - @Override - public void execute() { - - State setpoint = profile.calculate(timer.get()); - - double output = telescope.calculateControl(setpoint, pivot.getPositionRad()); - - SmartDashboard.putNumber("Telescope/profiled setpoint", setpoint.position); - SmartDashboard.putNumber("Telescope/final setpoint", goalM); - - telescope.setVolts(output); - - telescope.setSimPos(setpoint.position); - - if (Math.abs(telescope.getPositionM()-goalM) > 0.02) { - finishedTimer.reset(); - } - } - - @Override - public boolean isFinished() { - return finishedTimer.hasElapsed(0.2) || (ignoreValidation && profile.isFinished(timer.get())); - } - - @Override - public void end(boolean interrupted) { - telescope.stop(); - telescope.atGoal = true; - } -} diff --git a/src/main/java/frc/robot/commands/wrist/HoldWrist.java b/src/main/java/frc/robot/commands/wrist/HoldWrist.java deleted file mode 100644 index 051f8ad..0000000 --- a/src/main/java/frc/robot/commands/wrist/HoldWrist.java +++ /dev/null @@ -1,54 +0,0 @@ -package frc.robot.commands.wrist; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.WristSubsystem; - -public class HoldWrist extends CommandBase { - private final WristSubsystem wrist; - private final PivotSubsystem pivot; - - private final Timer timer = new Timer(); - private TrapezoidProfile profile; - private State goalState; - - public HoldWrist(WristSubsystem wrist, PivotSubsystem pivot) { - this.wrist = wrist; - this.pivot = pivot; - - addRequirements(wrist); - } - - @Override - public void initialize() { - goalState = new State(wrist.getDesiredSetpointRad().position, 0); - // wrist.resetPID(); - - timer.reset(); - timer.start(); - profile = new TrapezoidProfile(wrist.getConstraintsRad(), goalState, - new State(getAdjustedAngle(), wrist.getVelRadS())); - - } - - @Override - public void execute() { - State setpoint = profile.calculate(timer.get()); - - double output = wrist.calculateControl(setpoint, getAdjustedAngle(), true); - - wrist.setVolts(output); - wrist.setSimPosRad(goalState.position - pivot.getPositionRad()); - - SmartDashboard.putNumber("Wrist/hold setpoint", goalState.position); - SmartDashboard.putNumber("Wrist/adjusted position", getAdjustedAngle()); - } - - private double getAdjustedAngle() { - return wrist.getPositionRad() + pivot.getPositionRad(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/wrist/HomeWrist.java b/src/main/java/frc/robot/commands/wrist/HomeWrist.java deleted file mode 100644 index 9b9d53f..0000000 --- a/src/main/java/frc/robot/commands/wrist/HomeWrist.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.commands.wrist; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.WristSubsystem; - -public class HomeWrist extends CommandBase { - private WristSubsystem wrist; - - private Timer timer; - private Timer otherTimer; - private boolean otherTimerStarted = false; - - public HomeWrist(WristSubsystem wrist) { - this.wrist = wrist; - - timer = new Timer(); - otherTimer = new Timer(); - - addRequirements(wrist); - } - - public void initialize() { - - wrist.setVolts(2); - - timer.start(); - timer.reset(); - - otherTimer.reset(); - otherTimer.stop(); - - otherTimerStarted = false; - - // wrist.setCurrentLimit(70, 80, 0.5); - } - - public void execute() { - // SmartDashboard.putNumber("wRISTaMPS", Math.abs(wrist.getAmps())); - - if (Math.abs(wrist.getAmps()) < 60) { - timer.reset(); - } - if (timer.hasElapsed(0.2) && !otherTimerStarted) { - otherTimer.reset(); - otherTimer.start(); - otherTimerStarted = true; - wrist.stop(); - } - } - - public boolean isFinished() { - return otherTimer.hasElapsed(0.2); - } - - public void end(boolean interrupted) { - if (!interrupted) { - wrist.resetOffset(); - wrist.homed = true; - } - - // wrist.setCurrentLimit(70, 80, 0.5); - - // wrist.updateDesiredSetpointRad( - // new TrapezoidProfile.State( - // wrist.getPositionRad() + pivotPosRad.getAsDouble() - 0.3, 0)); - - wrist.setVolts(0); - wrist.updateDesiredSetpointRad(new TrapezoidProfile.State(Math.PI / 2, 0)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/wrist/MoveWrist.java b/src/main/java/frc/robot/commands/wrist/MoveWrist.java deleted file mode 100644 index aa2bc81..0000000 --- a/src/main/java/frc/robot/commands/wrist/MoveWrist.java +++ /dev/null @@ -1,101 +0,0 @@ -package frc.robot.commands.wrist; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.RobotState; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.WristSubsystem; - -public class MoveWrist extends CommandBase { - private WristSubsystem wrist; - private PivotSubsystem pivot; - - private TrapezoidProfile profile; - private State goalState; - - private double goalRad; - - private Timer timer = new Timer(); - - private Timer finishedTimer = new Timer(); - - private final boolean ignoreValidation; - - public MoveWrist(WristSubsystem wrist, PivotSubsystem pivot, double goalRad, boolean ignoreValidation) { - this.wrist = wrist; - this.pivot = pivot; - this.goalRad = goalRad; - this.ignoreValidation = ignoreValidation; - - addRequirements(wrist); - } - - public MoveWrist(WristSubsystem wrist, PivotSubsystem pivot, double goalRad) { - this(wrist, pivot, goalRad, false); - } - - @Override - public void initialize() { - - finishedTimer.reset(); - finishedTimer.start(); - - goalState = new TrapezoidProfile.State(goalRad, 0); - - if (RobotState.getInstance().atBack()) - goalState.position = Math.PI - goalState.position; - - - timer.reset(); - timer.start(); - profile = new TrapezoidProfile(wrist.getConstraintsRad(), goalState, - new State(getAdjustedAngle(), wrist.getVelRadS())); - - wrist.updateDesiredSetpointRad(goalState); - - wrist.resetPID(); - - wrist.atGoal = false; - } - - @Override - public void execute() { - State setpoint = profile.calculate(timer.get()); - - double output = wrist.calculateControl(setpoint, getAdjustedAngle(), false); - - SmartDashboard.putNumber("Wrist/profiled setpoint", setpoint.position); - SmartDashboard.putNumber("Wrist/final setpoint", goalState.position); - SmartDashboard.putNumber("Wrist/adjusted position", getAdjustedAngle()); - - wrist.setVolts(output); - wrist.setSimPosRad(setpoint.position - pivot.getPositionRad()); - - if (Math.abs(getAdjustedAngle()-goalState.position) > Units.degreesToRadians(4)) { - finishedTimer.reset(); - } - } - - @Override - public boolean isFinished() { - return finishedTimer.hasElapsed(0.2) || (ignoreValidation && profile.isFinished(timer.get())); - // return false; - } - - private double getAdjustedAngle() { - return wrist.getPositionRad() + pivot.getPositionRad(); - } - - @Override - public void end(boolean interrupted) { - wrist.stop(); - - wrist.atGoal = true; - - // SmartDashboard.putBoolean("MovingWrist", false); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/wrist/MoveWristAbsolute.java b/src/main/java/frc/robot/commands/wrist/MoveWristAbsolute.java deleted file mode 100644 index e7e7a33..0000000 --- a/src/main/java/frc/robot/commands/wrist/MoveWristAbsolute.java +++ /dev/null @@ -1,98 +0,0 @@ -package frc.robot.commands.wrist; - -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.TrapezoidProfile.State; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.RobotState; -import frc.robot.subsystems.PivotSubsystem; -import frc.robot.subsystems.WristSubsystem; - -public class MoveWristAbsolute extends CommandBase { - private WristSubsystem wrist; - private PivotSubsystem pivot; - - private TrapezoidProfile profile; - private State goalState; - - private double goalRad; - private double pivotGoalRad; - - private Timer timer = new Timer(); - - private Timer finishedTimer = new Timer(); - - private final boolean ignoreValidation; - - public MoveWristAbsolute(WristSubsystem wrist, PivotSubsystem pivot, double goalRad, double pivotGoalRad, boolean ignoreValidation) { - this.wrist = wrist; - this.pivot = pivot; - this.goalRad = goalRad; - this.pivotGoalRad = pivotGoalRad; - this.ignoreValidation = ignoreValidation; - - addRequirements(this.wrist); - } - - @Override - public void initialize() { - - finishedTimer.reset(); - finishedTimer.start(); - - goalState = new TrapezoidProfile.State(goalRad-pivotGoalRad, 0); - - if (RobotState.getInstance().atBack()) { - goalState.position = -goalState.position; - wrist.updateDesiredSetpointRad(new TrapezoidProfile.State(Math.PI-goalRad, 0)); - } - else { - wrist.updateDesiredSetpointRad(new TrapezoidProfile.State(goalRad, 0)); - } - - timer.reset(); - timer.start(); - profile = new TrapezoidProfile(wrist.getConstraintsRad(), goalState, - new State(wrist.getPositionRad(), wrist.getVelRadS())); - - - wrist.resetPID(); - - // SmartDashboard.putBoolean("MovingWrist", true); - - wrist.atGoal = false; - } - - @Override - public void execute() { - State setpoint = profile.calculate(timer.get()); - - double output = wrist.calculateControl(setpoint, wrist.getPositionRad(), false, pivot.getVelRadS()); - - // SmartDashboard.putNumber("Wrist Setpoint", setpoint.position); - // SmartDashboard.putNumber("Wrist real pos", wrist.getPositionRad()); - - wrist.setVolts(output); - wrist.setSimPosRad(setpoint.position - pivot.getPositionRad()); - - if (Math.abs(wrist.getPositionRad()-goalState.position) > Units.degreesToRadians(4)) { - finishedTimer.reset(); - } - } - - @Override - public boolean isFinished() { - return finishedTimer.hasElapsed(0.2) || (ignoreValidation && profile.isFinished(timer.get())); - // return false; - } - - @Override - public void end(boolean interrupted) { - wrist.stop(); - - wrist.atGoal = true; - - // SmartDashboard.putBoolean("MovingWrist", false); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java new file mode 100644 index 0000000..648a631 --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -0,0 +1,121 @@ +package frc.robot.generated; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SwerveModuleSteerFeedbackType; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; + +import edu.wpi.first.math.util.Units; +import frc.robot.CommandSwerveDrivetrain; + +public class TunerConstants { + // Both sets of gains need to be tuned to your individual robot + // The steer motor uses MotionMagicVoltage control + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(7).withKI(0).withKD(0) + .withKS(0).withKV(1.5).withKA(0); + // When using closed-loop control, the drive motor uses: + // - VelocityVoltage, if DrivetrainConstants.SupportsPro is false (default) + // - VelocityTorqueCurrentFOC, if DrivetrainConstants.SupportsPro is true + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.7).withKI(0).withKD(3.5) + .withKS(0).withKV(0).withKA(0); + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final double kSlipCurrentA = 300.0; + + // Theoretical free speed (m/s) at 12v applied output; + // This needs to be tuned to your individual robot + private static final double kSpeedAt12VoltsMps = 6.0; + + // 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 kDriveGearRatio = 6.746031746031747; + private static final double kSteerGearRatio = 21.428571428571427; + private static final double kWheelRadiusInches = 2.02; + + private static final boolean kSteerMotorReversed = true; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final String kCANbusName = "Canivore"; + private static final int kPigeonId = 20; + + // These are only used for simulation + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + + private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withPigeon2Id(kPigeonId) + .withCANbusName(kCANbusName); + + private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(kSlipCurrentA) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSpeedAt12VoltsMps(kSpeedAt12VoltsMps) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder) + .withCouplingGearRatio(kCoupleRatio) + .withSteerMotorInverted(kSteerMotorReversed); + + // Front Left + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 1; + private static final int kFrontLeftEncoderId = 9; + private static final double kFrontLeftEncoderOffset = -0.4609375; + + private static final double kFrontLeftXPosInches = 11.875; + private static final double kFrontLeftYPosInches = 8.875; + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 3; + private static final int kFrontRightEncoderId = 10; + private static final double kFrontRightEncoderOffset = -0.70751953125; + + private static final double kFrontRightXPosInches = 11.875; + private static final double kFrontRightYPosInches = -8.875; + + // Back Left + private static final int kBackLeftDriveMotorId = 8; + private static final int kBackLeftSteerMotorId = 7; + private static final int kBackLeftEncoderId = 12; + private static final double kBackLeftEncoderOffset = -0.265380859375; + + private static final double kBackLeftXPosInches = -11.875; + private static final double kBackLeftYPosInches = 8.875; + + // Back Right + private static final int kBackRightDriveMotorId = 6; + private static final int kBackRightSteerMotorId = 5; + private static final int kBackRightEncoderId = 11; + private static final double kBackRightEncoderOffset = -0.17724609375; + + private static final double kBackRightXPosInches = -11.875; + private static final double kBackRightYPosInches = -8.875; + + private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide); + private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide); + private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide); + private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide); + + public static final CommandSwerveDrivetrain DriveTrain = new CommandSwerveDrivetrain(DrivetrainConstants, FrontLeft, + FrontRight, BackLeft, BackRight); +} diff --git a/src/main/java/frc/robot/oi/ButtonMasher.java b/src/main/java/frc/robot/oi/ButtonMasher.java deleted file mode 100644 index 01f3085..0000000 --- a/src/main/java/frc/robot/oi/ButtonMasher.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class ButtonMasher extends CommandXboxController { - public ButtonMasher(int port) { - super(port); - } - - public Trigger leftStickLeft() { - return new Trigger(() -> super.getLeftX() < -0.5); - } - - public Trigger leftStickRight() { - return new Trigger(() -> super.getLeftX() > 0.5); - } - - public Trigger rightStickLeft() { - return new Trigger(() -> super.getRightX() < -0.5); - } - - public Trigger rightStickRight() { - return new Trigger(() -> super.getRightX() > 0.5); - } - - public Trigger rightStickUp() { - return new Trigger(() -> super.getRightY() > 0.5); - } - - public Trigger rightStickDown() { - return new Trigger(() -> super.getRightX() < -0.5); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/oi/Thrustmaster.java b/src/main/java/frc/robot/oi/Thrustmaster.java deleted file mode 100644 index 247b796..0000000 --- a/src/main/java/frc/robot/oi/Thrustmaster.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.robot.oi; - -import edu.wpi.first.wpilibj2.command.button.CommandJoystick; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * CommandJoystick child class that wraps around our Thrustmaster joysticks. - * Use instead of `new JoystickButton()` - */ -public class Thrustmaster extends CommandJoystick { - - public Thrustmaster(int id) { - super(id); - // TODO: set x, y, twist, and slider channels - } - - public Trigger topCenter() { - return super.button(2); - } - - public Trigger topLeft() { - return super.button(3); - } - - public Trigger topRight() { - return super.button(4); - } - - /** - * For use with bottom 12 buttons - * - *

- * WARNING: The button ids change depending on if the controller is in - * 'left' or 'right' mode. Check the switch on the bottom. - *

- * RIGHT:

- * 5 6 7 | 13 12 11

- * 10 9 8 | 14 15 16

- * - * LEFT:

- * 11 12 13 | 7 6 5

- * 16 15 14 | 8 9 10 - */ - public Trigger lowerButton(int button) { - return super.button(button); - } -} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java deleted file mode 100644 index e76f532..0000000 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ /dev/null @@ -1,15 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ArmSubsystem extends SubsystemBase { - public void zero() {} - - public double getPositionRad() { return 0.0; } - - public void stop() {} - - public void setVolts(double volts) {} - - public double getVelRadS() { return 0.0; } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 8b0e99d..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,186 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.GamePieceMode; - -public class IntakeSubsystem extends SubsystemBase { - private static enum IntakeMode { - None, Intake, Place - } - private CANSparkMax leftMotor = new CANSparkMax( - CANDevices.leftIntakeMotorID, MotorType.kBrushed); - private CANSparkMax rightMotor = new CANSparkMax( - CANDevices.rightIntakeMotorID, MotorType.kBrushed); - - private IntakeMode intakeMode = IntakeMode.None; - private boolean exceededCurrentDraw = false; - - private final Timer intakeTimer = new Timer(); - - public IntakeSubsystem() { - - leftMotor.restoreFactoryDefaults(); - rightMotor.restoreFactoryDefaults(); - - leftMotor.setSmartCurrentLimit(40); - rightMotor.setSmartCurrentLimit(40); - - leftMotor.setInverted(true); - rightMotor.setInverted(false); - - leftMotor.burnFlash(); - rightMotor.burnFlash(); - - intakeTimer.reset(); - intakeTimer.start(); - - } - - public void toggleIntake() { - intakeMode = intakeMode == IntakeMode.Intake ? IntakeMode.None : IntakeMode.Intake; - exceededCurrentDraw = false; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(intakeMode == IntakeMode.Intake); - intakeTimer.reset(); - intakeTimer.start(); - - if (intakeMode == IntakeMode.Intake) { - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - leftMotor.set(-1); - rightMotor.set(-1); - } - else { - leftMotor.set(1); - rightMotor.set(1); - } - } - else { - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } - } - - public void setIntake(boolean intake) { - intakeMode = intake ? IntakeMode.Intake : IntakeMode.None; - exceededCurrentDraw = false; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(intakeMode == IntakeMode.Intake); - intakeTimer.reset(); - intakeTimer.start(); - - if (intakeMode == IntakeMode.Intake) { - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - leftMotor.set(-1); - rightMotor.set(-1); - } - else { - leftMotor.set(1); - rightMotor.set(1); - } - } - else { - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } - } - - public void place() { - intakeMode = IntakeMode.Place; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(false); - - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - boolean back = RobotState.getInstance().atBack(); - leftMotor.set(back ? -0.75 : 0.75); - rightMotor.set(back ? 0.75 : -0.75); - } - else { - leftMotor.set(-0.6); - rightMotor.set(-0.6); - } - } - - public void shoot() { - intakeMode = IntakeMode.Place; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(false); - - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - boolean back = RobotState.getInstance().atBack(); - leftMotor.set(back ? -1 : 1); - rightMotor.set(back ? 1 : -1); - } - else { - leftMotor.set(-0.8); - rightMotor.set(-0.8); - } - } - - public void shootBackwards() { - intakeMode = IntakeMode.Place; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(false); - - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - boolean back = RobotState.getInstance().atBack(); - leftMotor.set(!back ? -1 : 1); - rightMotor.set(!back ? 1 : -1); - } - else { - leftMotor.set(-0.8); - rightMotor.set(-0.8); - } - } - - public void slowPlace() { - intakeMode = IntakeMode.Place; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(false); - - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - boolean back = RobotState.getInstance().atBack(); - leftMotor.set(back ? -0.75 : 0.75); - rightMotor.set(back ? 0.75 : -0.75); - } - else { - leftMotor.set(-0.1); - rightMotor.set(-0.1); - } - } - - public void stop() { - intakeMode = IntakeMode.None; - RobotState.getInstance().setIntaked(false); - RobotState.getInstance().setIntaking(false); - leftMotor.stopMotor(); - rightMotor.stopMotor(); - } - - @Override - public void periodic() { - - double currentDraw = Math.max(leftMotor.getOutputCurrent(), rightMotor.getOutputCurrent()); - if (intakeMode == IntakeMode.Intake && !exceededCurrentDraw) { - if (currentDraw > 20 && intakeTimer.hasElapsed(1)) { - exceededCurrentDraw = true; - RobotState.getInstance().setIntaked(true); - if (RobotState.getInstance().getMode() == GamePieceMode.Cube) { - leftMotor.set(-0.35); - rightMotor.set(-0.35); - } - else { - leftMotor.set(0.35); - rightMotor.set(0.35); - } - } - } - - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LEDManager.java b/src/main/java/frc/robot/subsystems/LEDManager.java deleted file mode 100644 index 7dc8e58..0000000 --- a/src/main/java/frc/robot/subsystems/LEDManager.java +++ /dev/null @@ -1,211 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.AddressableLED; -import edu.wpi.first.wpilibj.AddressableLEDBuffer; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; -import frc.robot.Constants.GamePieceMode; -import frc.robot.Constants.LEDConstants; - -public class LEDManager extends SubsystemBase { - - // LEDs - private final AddressableLED led; - private final AddressableLEDBuffer ledBuffer; - - // Rainbow - private int rainbowFirstPixelHue = 0; - - // Other - private final Timer flashTimer = new Timer(); - private boolean off = false; - - public LEDManager() { - - led = new AddressableLED(LEDConstants.ledPort); - ledBuffer = new AddressableLEDBuffer(LEDConstants.baseLedCount + LEDConstants.armLedCount); - led.setLength(ledBuffer.getLength()); - led.setData(ledBuffer); - led.start(); - - flashTimer.reset(); - flashTimer.start(); - - } - - @Override - public void periodic() { - - clear(); - - if (!off) { - if (DriverStation.isDisabled()){ - rainbow(); - } - else { - setSideIndicator(); - setIntakeModeIndicator(); - flashOnWhistle(); - flashActiveSide(); - flashOnIntake(); - } - } - - led.setData(ledBuffer); - - } - - private void setArmRGB(int i, int r, int g, int b) { - ledBuffer.setRGB(i+LEDConstants.baseLedCount, r, g, b); - } - - private void setArmHSV(int i, int h, int s, int v) { - ledBuffer.setHSV(i+LEDConstants.baseLedCount, h, s, v); - } - - private void setArmLED(int i, Color color) { - ledBuffer.setLED(i+LEDConstants.baseLedCount, color); - } - - private void setBaseRGB(int i, int r, int g, int b) { - ledBuffer.setRGB(i, r, g, b); - } - - private void setBaseHSV(int i, int h, int s, int v) { - ledBuffer.setHSV(i, h, s, v); - } - - private void setBaseLED(int i, Color color) { - ledBuffer.setLED(i, color); - } - - private void clear() { - for (int i = 0; i < LEDConstants.armLedCount; i++) { - setArmRGB(i, 0, 0, 0); - } - - for (int i = 0; i < LEDConstants.baseLedCount; i++) { - setBaseRGB(i, 0, 0, 0); - } - } - - private void rainbow() { - for (int i = 0; i < LEDConstants.armLedCount; i++) { - int hue = (rainbowFirstPixelHue + 90 + (i * 180 / LEDConstants.armLedCount)) % 180; - setArmHSV(i, hue, 255, 127); - } - - for (int i = 0; i < LEDConstants.baseLedCount; i++) { - int hue = (rainbowFirstPixelHue + 90 + (i * 180 / LEDConstants.baseLedCount)) % 180; - setBaseHSV(i, hue, 255, 127); - } - - if (LEDConstants.dynamicRainbow) { - rainbowFirstPixelHue = (rainbowFirstPixelHue+LEDConstants.dynamicRainbowSpeed)%180; - } - - } - - @SuppressWarnings("unused") - private void epilepsy() { - - for (int i = 0; i < LEDConstants.armLedCount; i++) { - setArmRGB(i, (int) (Math.random()*255), (int)(Math.random()*255), (int)(Math.random()*255)); - } - - for (int i = 0; i < LEDConstants.baseLedCount; i++) { - setBaseRGB(i, (int)(Math.random()*255), (int)(Math.random()*255), (int)(Math.random()*255)); - } - - } - - private void setSideIndicator() { - - for (int i = LEDConstants.baseLedCount/2+1; i < 3*LEDConstants.baseLedCount/4; i++) { - setBaseRGB(i, 0, 127, 0); - } - for (int i = LEDConstants.baseLedCount/4; i <= LEDConstants.baseLedCount/2; i++) { - setBaseRGB(i, 127, 0, 0); - } - - } - - private void flashActiveSide() { - if ((flashTimer.get()*10%10)%5 < 3) { - return; - } - - if (!RobotState.getInstance().atBack()) { - for (int i = LEDConstants.baseLedCount/2; i < 3*LEDConstants.baseLedCount/4; i++) { - setBaseLED(i, LEDConstants.activeSideFlashColor); - } - } - else { - for (int i = LEDConstants.baseLedCount/4; i < LEDConstants.baseLedCount/2; i++) { - setBaseLED(i, LEDConstants.activeSideFlashColor); - } - } - } - - private void setIntakeModeIndicator() { - - Color color = RobotState.getInstance().getMode() != GamePieceMode.Cube ? new Color(127, 127, 0) : new Color(127, 0, 127); - for (int i = 0; i < LEDConstants.baseLedCount/4; i++) { - setBaseLED(i, color); - } - for (int i = 3*LEDConstants.baseLedCount/4; i < LEDConstants.baseLedCount; i++) { - setBaseLED(i, color); - } - - if (RobotState.getInstance().getMode() != GamePieceMode.ConeUp) { - for (int i = 0; i < LEDConstants.armLedCount; i++) { - setArmLED(i, color); - } - } - else { - Color yellow = new Color(127, 127, 0); - Color black = new Color(0, 0, 0); - for (int i = 0; i < LEDConstants.armLedCount; i++) { - setArmLED(i, i/10%2 == 0 ? yellow : black); - } - for (int i = 0; i < LEDConstants.baseLedCount/4; i++) { - setBaseLED(i, i/6%2 == 0 ? yellow : black); - } - for (int i = 3*LEDConstants.baseLedCount/4; i < LEDConstants.baseLedCount; i++) { - setBaseLED(i, i/6%2 == 0 ? yellow : black); - } - } - - } - - private void flashOnWhistle() { - - if (DriverStation.isTeleopEnabled() && Math.abs(DriverStation.getMatchTime()-30) < 1.5) { - Color color = (flashTimer.get()*10%10)%5 < 2.5 ? Color.kBlack : LEDConstants.whistleFlashColor; - for (int i = 0; i < LEDConstants.baseLedCount; i++) { - setBaseLED(i, color); - } - } - - } - - private void flashOnIntake() { - if (RobotState.getInstance().isIntaking()) { - Color color = (RobotState.getInstance().hasIntaked() && (flashTimer.get()*10%10)%5 < 2.5) ? Color.kBlack : LEDConstants.intakeFlashColor; - for (int i = 0; i < LEDConstants.baseLedCount/4; i++) { - setBaseLED(i, color); - } - for (int i = 3*LEDConstants.baseLedCount/4; i < LEDConstants.baseLedCount; i++) { - setBaseLED(i, color); - } - } - } - - public void setOff(boolean offf) { - off = offf; - } - -} diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java deleted file mode 100644 index 274f71a..0000000 --- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java +++ /dev/null @@ -1,298 +0,0 @@ -package frc.robot.subsystems; - - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFX; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DutyCycleEncoder; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.RobotState; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.PivotConstants; -import frc.robot.Constants.TelescopeConstants; - -public class PivotSubsystem extends ArmSubsystem { - /**Primary Motor/ Leader */ - private TalonFX rightMotor = new TalonFX(CANDevices.rightPivotMotorID, CANDevices.canivoreName); - private DutyCycleEncoder encoder = new DutyCycleEncoder(CANDevices.pivotEncoderID); - private TalonFX leftMotor = new TalonFX(CANDevices.leftPivotMotorID, CANDevices.canivoreName); - - private double lastEncoderPos; - private double velocityRadS; - - private boolean dead = false; - - public boolean atGoal = false; - - private double simPos; - - // The subsystem holds its own PID and feedforward controllers and provides calculations from - // them, but cannot actually set its own motor output, as accurate feedforward calculations - // require information from the telescope subsytem. - private TrapezoidProfile.Constraints constraintsRad = - new TrapezoidProfile.Constraints( - Units.degreesToRadians(360), - Units.degreesToRadians(600)); - public final PIDController controller = new PIDController( - PivotConstants.kP, - 0, - PivotConstants.kD); - private final ArmFeedforward feedforward = new ArmFeedforward( - PivotConstants.kS, - PivotConstants.kG, - PivotConstants.kV, - PivotConstants.kA); - - - // Stores the most recent setpoint to allow the Hold command to hold it in place - private TrapezoidProfile.State currentSetpointRad = - new TrapezoidProfile.State(getPositionRad(), 0); - - - public PivotSubsystem() { - rightMotor.setInverted(InvertType.None); - - leftMotor.follow(rightMotor); - leftMotor.setInverted(InvertType.OpposeMaster); - - leftMotor.configNeutralDeadband(0.004); - rightMotor.configNeutralDeadband(0.004); - - leftMotor.setNeutralMode(NeutralMode.Brake); - rightMotor.setNeutralMode(NeutralMode.Brake); - - rightMotor.configStatorCurrentLimit( - new StatorCurrentLimitConfiguration( - true, - 70, - 80, - 1) - ); - rightMotor.configStatorCurrentLimit( - new StatorCurrentLimitConfiguration( - true, - 70, - 80, - 1) - ); - } - - public double getPositionRad() { - if (Robot.isReal()) { - return encoder.getAbsolutePosition() * 2 * Math.PI + PivotConstants.encoderOffsetRad; - } - return simPos; - } - - public double getVelRadS() { - return velocityRadS; - } - - public void toggleKill() { - rightMotor.set(ControlMode.PercentOutput, 0); - dead = !dead; - } - - public void autoConstrain() { - constraintsRad = - new TrapezoidProfile.Constraints( - Units.degreesToRadians(360), - Units.degreesToRadians(450)); - } - - public void printAutoTimer() { - // I don't know why this is here, but it is used in AutoRoutines, so I've - // deemed it to dangerous to remove. - SmartDashboard.putNumber("autoTmp", autoTimer.get()); - } - - public void normalConstrain() { - constraintsRad = - new TrapezoidProfile.Constraints( - Units.degreesToRadians(360), - Units.degreesToRadians(600)); - } - - /** - * @return Most recent setpoint set by a move command. This setpoint only exists for utility - * purposes and is not used by the subsystem - */ - public TrapezoidProfile.State getDesiredSetpointRad() { - return currentSetpointRad; - } - - public TrapezoidProfile.Constraints getConstraintsRad() { - return constraintsRad; - } - - /** - * - * @param setpointRad - * @param telescopePosM - * @return - */ - public double calculateControl(TrapezoidProfile.State setpointRad, double telescopePosM) { - - return controller.calculate(getPositionRad(), setpointRad.position) - + feedforward.calculate(setpointRad.position, setpointRad.velocity) - // Compensates for telescope extention - // Gravity constant * Telescope Extention (proportion) * Cosine of Angle - + PivotConstants.extraKg * telescopePosM / TelescopeConstants.maxPosM - * Math.cos(getPositionRad()); - } - - /** - * Resets the PID controller stored in this subsystem. - */ - public void resetPID() { - controller.reset(); - } - - /** - * @param state The setpoint state the telescope should be driven to. - * Has no effect on the function of this subsytem. - */ - public void setDesiredSetpointRad(TrapezoidProfile.State stateRad) { - currentSetpointRad = stateRad; - } - - public void jogSetpointForward() { - currentSetpointRad.position = - MathUtil.clamp( - currentSetpointRad.position + Math.PI/64, - PivotConstants.maxFwdRotationRad, - PivotConstants.maxBackRotationRad); - } - - public void jogSetpointBack() { - currentSetpointRad.position = - MathUtil.clamp( - currentSetpointRad.position - Math.PI/64, - PivotConstants.maxFwdRotationRad, - PivotConstants.maxBackRotationRad); - } - - public void setSimPos(double pos) { - simPos = pos; - } - - /** - * Sets the output power of the motors in volts if the boundry conditions - * are not exceeded. If the pivot is out of bounds or dead, the voltage - * will be set to 0. - * @param input The voltage to set - */ - public void setVolts(double input) { - if (!dead && withinSoftLimits(input)) { - rightMotor.set(ControlMode.PercentOutput, input / 12); - SmartDashboard.putNumber("Pivot/commanded voltage", input); - } else { - rightMotor.set(ControlMode.PercentOutput, 0); - } - } - - public void stop() { - rightMotor.set(ControlMode.PercentOutput, 0); - } - - /** - * Sets the output power of the motors in volts, even if it is past the max - * rotations or dead. Voltage is divided in half.

- * - * ONLY USED BY HUMANS. Do not have PID or feedforward use this method. - * @param input The voltage to set - */ - public void overrideVolts(double input) { - rightMotor.set(ControlMode.PercentOutput, input / 12); - } - - - /** - * 'Kills' the subsystem. The motor will be stopped and no loger respond to input - */ - public void die() { - setVolts(0); - dead = true; - } - - public void setBrakeMode(boolean braked) { - leftMotor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - rightMotor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * 'Revives' the subsytem. If it is dead, the motor will start responding.

- * DO NOT have regular code call this method. Only a human button should do this. - */ - public void revive() { - dead = false; - } - - /** - * @return a new InstantCommand that stops the motor and requires this subsystem - */ - public Command killCommand() { - return new InstantCommand(this::die, this); - } - - - - private Timer autoTimer = new Timer(); - - @Override - public void periodic() { - - // Again, I don't know what this is for, but it will go unremoved for now. - SmartDashboard.putNumber("AutoTimer", autoTimer.get()); - - SmartDashboard.putNumber("Pivot/position", getPositionRad()); - SmartDashboard.putNumber("Pivot/velocity", getVelRadS()); - SmartDashboard.putNumber("Pivot/right motor current", rightMotor.getStatorCurrent()); - SmartDashboard.putNumber("Pivot/left motor current", leftMotor.getStatorCurrent()); - - SmartDashboard.putBoolean("Pivot/dead", dead); - - RobotState.getInstance().putPivotDisplay(getPositionRad()); - - findVel(); - - //if (DriverStation.isEnabled()) checkIfDead(); - } - - public void startAutoTimer() { - autoTimer.reset(); - autoTimer.start(); - } - - public void stopAutoTimer() { - autoTimer.stop(); - } - - private boolean withinSoftLimits(double input) { - if (input > 0 && getPositionRad() > PivotConstants.maxBackRotationRad) { - return false; - } - if (input < 0 && getPositionRad() < PivotConstants.maxFwdRotationRad) { - return false; - } - return true; - } - - private void findVel() { - velocityRadS = (getPositionRad() - lastEncoderPos) / 0.02; - lastEncoderPos = getPositionRad(); - } -} diff --git a/src/main/java/frc/robot/subsystems/TelescopeSubsystem.java b/src/main/java/frc/robot/subsystems/TelescopeSubsystem.java deleted file mode 100644 index fa61a7a..0000000 --- a/src/main/java/frc/robot/subsystems/TelescopeSubsystem.java +++ /dev/null @@ -1,185 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFX; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.RobotState; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.TelescopeConstants; - -public class TelescopeSubsystem extends ArmSubsystem{ - private TalonFX motor = new TalonFX(CANDevices.telescopeMotorID); - - public boolean homed = false; - - private boolean dead = false; - - public boolean atGoal = false; - - private double simPos; - - // The subsystem holds its own PID and feedforward controllers and provides calculations from - // them, but cannot actually set its own motor output, as accurate feedforward calculations - // require information from the pivot subsytem. - private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward( - TelescopeConstants.kS, - TelescopeConstants.kV); - private final TrapezoidProfile.Constraints constraints = - new TrapezoidProfile.Constraints(3, 3); - private final PIDController controller = - new PIDController(TelescopeConstants.kP, 0, 0); - - // Stores the most recent setpoint to allow the Hold command to hold it in place - private TrapezoidProfile.State currentSetpoint = new TrapezoidProfile.State(0.06, 0); - - public TelescopeSubsystem() { - motor.setInverted(InvertType.None); - motor.setNeutralMode(NeutralMode.Brake); - - motor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - motor.setSensorPhase(false); - - motor.configNeutralDeadband(0.004); - - motor.configStatorCurrentLimit( - new StatorCurrentLimitConfiguration(true, 40, 50, 0.5)); - - // SmartDashboard.putNumber("Telescope test setpoint", 0); - } - - public double getPositionM() { - // 4096 units per rotation, multiply rotations by diameter - if (Robot.isReal()) { - return motor.getSelectedSensorPosition() / 4096 - * 2 * Math.PI * TelescopeConstants.conversionM; - } - - return simPos; - } - - public double getVel() { - return motor.getSelectedSensorVelocity() / 4096 - * 2 * Math.PI * TelescopeConstants.conversionM * 10; - } - - public double getAmps() { - return Math.abs(motor.getStatorCurrent()); - } - - /** - * @return Most recent setpoint set by a move command. This setpoint only exists for utility - * purposes and is not used by the subsystem - */ - public TrapezoidProfile.State getDesiredSetpoint() { - return currentSetpoint; - } - - public TrapezoidProfile.Constraints getConstraints() { - return constraints; - } - - public void toggleKill() { - motor.set(ControlMode.PercentOutput, 0); - dead = !dead; - } - - /** - * Does control calculations from its ArmFeedforward and PID controllers. - * Does not command any motors. - * @param setpoint The given setpoint - * @param pivotAngleRad The position of the pivot, required for accurate feedforward - * @return The result of the calculation. Add kG * sine of the pivot angle - */ - public double calculateControl(TrapezoidProfile.State setpoint, double pivotAngleRad) { - // SmartDashboard.putNumber("Telescope calculated", controller.calculate(getPositionM(), setpoint.position)); - return controller.calculate(getPositionM(), setpoint.position) - + feedforward.calculate(setpoint.velocity) - // Compensates for weight of telescope as the pivot goes up - // Gravity Constant * Sine of Angle - + TelescopeConstants.kG * Math.sin(pivotAngleRad); - } - - /** - * Resets the PID controller stored in this subsystem. - */ - public void resetPID() { - controller.reset(); - } - - /** - * @param state The setpoint state the telescope should be driven to. - * Has no effect on the function of this subsytem. - */ - public void setDesiredSetpoint(TrapezoidProfile.State state) { - currentSetpoint = state; - } - - public void jogSetpointForward() { - currentSetpoint.position = - MathUtil.clamp( - currentSetpoint.position + 0.05, - TelescopeConstants.minPosM, - TelescopeConstants.maxPosM); - } - - public void jogSetpointBackward() { - currentSetpoint.position = - MathUtil.clamp( - currentSetpoint.position - 0.05, - TelescopeConstants.minPosM, - TelescopeConstants.maxPosM); - } - - public void setSimPos(double pos) { - simPos = pos; - } - - public void setVolts(double input) { - if (!dead) - motor.set(ControlMode.PercentOutput, input / 12); - } - - public void overrideVolts(double input) { - motor.set(ControlMode.PercentOutput, input / 12); - } - - public void stop() { - motor.set(ControlMode.PercentOutput, 0); - } - - public void setBrakeMode(boolean braked) { - motor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - } - - public void resetOffset() { - motor.setSelectedSensorPosition(0); - } - - public void setP(double p) { - controller.setP(p); - controller.reset(); - } - - @Override - public void periodic() { - SmartDashboard.putNumber("Telescope/position", getPositionM()); - SmartDashboard.putNumber("Telescope/velocity", getVel()); - - SmartDashboard.putBoolean("Telescope/dead", dead); - SmartDashboard.putNumber("Telescope/applied voltage", motor.getMotorOutputVoltage()); - SmartDashboard.putNumber("Telescope/motor current", getAmps()); - - RobotState.getInstance().putTelescopeDisplay(getPositionM()); - } -} diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java deleted file mode 100644 index a01a51c..0000000 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ /dev/null @@ -1,206 +0,0 @@ -package frc.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.can.TalonFX; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Robot; -import frc.robot.RobotState; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.WristConstants; - -public class WristSubsystem extends ArmSubsystem { - private TalonFX motor = new TalonFX(CANDevices.wristMotorID); - - public boolean homed = false; - - public boolean atGoal = false; - - private boolean dead = false; - - // The subsystem holds its own PID and feedforward controllers and provides calculations from - // them, but cannot actually set its own motor output, as accurate feedforward calculations - // require information from the pivot subsytem. - private final TrapezoidProfile.Constraints constraintsRad = new TrapezoidProfile.Constraints( - Units.degreesToRadians(630), - Units.degreesToRadians(810)); - private final PIDController controller = new PIDController(WristConstants.kP, WristConstants.kI, WristConstants.kD); - private final PIDController controllerHold = new PIDController(WristConstants.kPHold, WristConstants.kIHold, WristConstants.kDHold); - private final ArmFeedforward feedforward = new ArmFeedforward( - WristConstants.kS, - WristConstants.kG, - WristConstants.kV, - WristConstants.kA); - - // Stores the most recent setpoint to allow the Hold command to hold it in place - private TrapezoidProfile.State currentSetpointRad = new TrapezoidProfile.State(); - - private double simPos = 0.0; - - public WristSubsystem() { - motor.setInverted(false); - motor.setInverted(InvertType.None); - - motor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - - motor.setNeutralMode(NeutralMode.Brake); - - motor.configNeutralDeadband(0.004); - - motor.configStatorCurrentLimit( - new StatorCurrentLimitConfiguration(true, 70, 80, 0.5)); - - controller.setTolerance(0.05); - controllerHold.setTolerance(0.05); - } - - public double getPositionRad() { - if (Robot.isReal()) { - return motor.getSelectedSensorPosition() / 2048 * 2 * Math.PI * WristConstants.gearRatio; - } - return simPos; - } - - public double getVelRadS() { - return motor.getSelectedSensorVelocity() / 2048 * 2 * Math.PI * 10 * WristConstants.gearRatio; - } - - public double getAmps() { - return Math.abs(motor.getStatorCurrent()); - } - - public void setBrakeMode(boolean braked) { - motor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - } - - /** - * @return Most recent setpoint set by a move command. This setpoint only exists for utility - * purposes and is not used by the subsystem - */ - public TrapezoidProfile.State getDesiredSetpointRad() { - return currentSetpointRad; - } - - public void jogSetpointForward() { - currentSetpointRad.position += Math.PI / 24; - } - - public void jogSetpointBack() { - currentSetpointRad.position -= Math.PI / 24; - } - - public TrapezoidProfile.Constraints getConstraintsRad() { - return constraintsRad; - } - - public void toggleKill() { - motor.set(ControlMode.PercentOutput, 0); - dead = !dead; - } - - public void setCurrentLimit(double currentLimit, double triggerThresholdCurrent, double triggerThresholdTime) { - motor.configStatorCurrentLimit( - new StatorCurrentLimitConfiguration(true, currentLimit, triggerThresholdCurrent, triggerThresholdTime)); - - } - - /** - * Does control calculations from its ArmFeedforward and PID controllers. - * Does not command any motors. - * @param setpoint The given setpoint, field-relative - * @param angleDeg The angle of the wrist, also field-relative. Do not give - * direct sensor values - * @return The result of the calculation - */ - public double calculateControl(TrapezoidProfile.State setpointRad, double angleRad, boolean holding) { - - double fb = holding ? controllerHold.calculate(angleRad, setpointRad.position) : controller.calculate(angleRad, setpointRad.position); - double ff = feedforward.calculate(setpointRad.position, setpointRad.velocity); - - return fb + ff; - - } - - public double calculateControl(TrapezoidProfile.State setpointRad, double angleRad, boolean holding, double pivotVel) { - - double fb = holding ? controllerHold.calculate(angleRad, setpointRad.position) : controller.calculate(angleRad, setpointRad.position); - double ff = feedforward.calculate(setpointRad.position, setpointRad.velocity + pivotVel); - - return fb + ff; - - } - - /** - * Resets the PID controller stored in this subsystem. - */ - public void resetPID() { - controller.reset(); - controllerHold.reset(); - } - - /** - * @param state The setpoint state the wrist should be driven to. - * Has no effect on the function of this subsytem. - */ - public void updateDesiredSetpointRad(TrapezoidProfile.State state) { - currentSetpointRad = state; - } - - public void setSimPosRad(double pos) { - simPos = pos; - } - - /** - * For homing. Resets the encoder offset to the position of 141 degrees, - * the position the wrist should be at when it goes all the way to the arm. - */ - public void resetOffset() { - motor.setSelectedSensorPosition(2.81 / (2 * Math.PI) * 2048 / WristConstants.gearRatio); - } - - public void resetOffsetCube() { - motor.setSelectedSensorPosition(1.5 / (2 * Math.PI) * 2048 / WristConstants.gearRatio); - } - - public void zeroOffset() { - motor.setSelectedSensorPosition(0); - } - - public void setVolts(double input) { - if (!dead) { - motor.set(ControlMode.PercentOutput, input / 12); - return; - } - motor.set(ControlMode.PercentOutput, 0); - } - - public void overrideVolts(double input) { - System.out.println(input / 12); - motor.set(ControlMode.PercentOutput, input / 12); - } - - public void stop() { - motor.set(ControlMode.PercentOutput, 0); - } - - public void periodic() { - SmartDashboard.putNumber("Wrist/position", getPositionRad()); - SmartDashboard.putNumber("Wrist/motor current", getAmps()); - - SmartDashboard.putBoolean("Wrist/dead", dead); - SmartDashboard.putNumber("Wrist/applied voltage", motor.getMotorOutputVoltage()); - SmartDashboard.putNumber("Wrist/velocity", getVelRadS()); - - RobotState.getInstance().putWristDisplay(getPositionRad()); - - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java deleted file mode 100644 index f6ee381..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ /dev/null @@ -1,243 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.DriveConstants; -import frc.robot.RobotState; -import frc.robot.Constants.CANDevices; - -/** - * Subsystem that manages all things related to robot driving - */ -public class Drive extends SubsystemBase { - - /** - * Used to set module states and get data for RobotState - */ - private final DriveModule[] driveModules = new DriveModule[4]; - - /** - * Used to get the rotation of the robot - */ - private final DriveAngle driveAngle = new DriveAngle(); - - /** - * The positions of each module used to update RobotState - */ - private SwerveModulePosition modulePositions[] = new SwerveModulePosition[4]; - - /** - * The desired state for each swerve module - */ - private SwerveModuleState[] goalModuleStates = new SwerveModuleState[4]; - - /** - * The controllers used to calculate the output volts for the rotation motors - */ - private PIDController[] rotationPIDs = new PIDController[4]; - - /** - * True if drive should be slow, false if drive should be going at max speed - */ - private boolean babyMode = false; - - /** - * Initialize all the modules, data arrays, and RobotState - */ - public Drive() { - - driveModules[0] = new DriveModule(CANDevices.frontLeftDriveMotorID, CANDevices.frontLeftRotationMotorID, - CANDevices.frontLeftRotationEncoderID, DriveConstants.frontLeftAngleOffset, false); - driveModules[1] = new DriveModule(CANDevices.frontRightDriveMotorID, CANDevices.frontRightRotationMotorID, - CANDevices.frontRightRotationEncoderID, DriveConstants.frontRightAngleOffset, true); - driveModules[2] = new DriveModule(CANDevices.backLeftDriveMotorID, CANDevices.backLeftRotationMotorID, - CANDevices.backLeftRotationEncoderID, DriveConstants.backLeftAngleOffset, false); - driveModules[3] = new DriveModule(CANDevices.backRightDriveMotorID, CANDevices.backRightRotationMotorID, - CANDevices.backRightRotationEncoderID, DriveConstants.backRightAngleOffset, true); - - for (int i = 0; i < 4; i++) { - driveModules[i].zeroEncoders(); - modulePositions[i] = new SwerveModulePosition(); - goalModuleStates[i] = new SwerveModuleState(); - - rotationPIDs[i] = new PIDController(DriveConstants.rotationKps[i], 0, DriveConstants.rotationKds[i]); - rotationPIDs[i].enableContinuousInput(-Math.PI, Math.PI); - driveModules[i].setDrivePD(DriveConstants.driveKps[i], DriveConstants.driveKds[i]); - - modulePositions[i].distanceMeters = driveModules[i].getDrivePosition() * DriveConstants.wheelRadiusM; - modulePositions[i].angle = new Rotation2d(driveModules[i].getRotationPosition()); - } - - setGoalChassisSpeeds(new ChassisSpeeds(0, 0, 0)); - - RobotState.getInstance().initializePoseEstimator(getRotation(), modulePositions); - - } - - @Override - public void periodic() { - - // SmartDashboard.putNumber("Angle", driveAngle.getHeading()); - - // Driving - for (int i = 0; i < 4; i++) { - - SmartDashboard.putBoolean("Drive/modules/"+i+"/status", driveModules[i].getDeadBoolean()); - - // Get encoder value - Rotation2d moduleRotation = new Rotation2d(driveModules[i].getRotationPosition()); - - // Optimize each module state - SwerveModuleState optimizedState = SwerveModuleState.optimize(goalModuleStates[i], moduleRotation); - double rotationSetpointRadians = optimizedState.angle.getRadians(); - double speedSetpointMPerS = optimizedState.speedMetersPerSecond; - - // Set module speed - double speedRadPerS = speedSetpointMPerS / DriveConstants.wheelRadiusM; - double ffVolts = DriveConstants.driveFF.calculate(speedRadPerS); - // SmartDashboard.putNumber("DesiredSpeed"+i, speedSetpointMPerS); - // SmartDashboard.putNumber("ActualSpeed"+i, driveModules[i].getDriveVelocityMPerS()); - // SmartDashboard.putNumber("ErrorSpeed"+i, Math.abs(driveModules[i].getDriveVelocityMPerS()-speedSetpointMPerS)); - // SmartDashboard.putNumber("DriveOutput"+i, speedRadPerS); - driveModules[i].setDriveVelocity(speedRadPerS, ffVolts); - - // Set module rotation - double rotationVoltage = rotationPIDs[i].calculate(moduleRotation.getRadians(), rotationSetpointRadians); - driveModules[i].setRotationVoltage(rotationVoltage); - // SmartDashboard.putNumber("DesiredRot"+i, rotationSetpointRadians); - // SmartDashboard.putNumber("ActualRot"+i, moduleRotation.getRadians()); - // SmartDashboard.putNumber("DriveRotError"+i, MathUtil.angleModulus(rotationSetpointRadians-moduleRotation.getRadians())); - - // SmartDashboard.putNumber("DesiredDriveAngle"+i, rotationSetpointRadians); - } - - // Pose estimation - for (int i = 0; i < 4; i++) { - modulePositions[i].distanceMeters = driveModules[i].getDrivePosition() * DriveConstants.wheelRadiusM; - modulePositions[i].angle = new Rotation2d(driveModules[i].getRotationPosition()); - - SmartDashboard.putNumber("Drive/modules/"+i+"/angle", modulePositions[i].angle.getRadians()); - SmartDashboard.putNumber("Drive/modules/"+i+"/drive stator current", driveModules[i].getDriveStatorCurrent()); - SmartDashboard.putNumber("Drive/modules/"+i+"/rotation stator current", driveModules[i].getRotationStatorCurrent()); - } - RobotState.getInstance().recordDriveObservations(getRotation(), modulePositions); - - SmartDashboard.putNumber("Drive/velocity magnitude", getChassisSpeeds().vxMetersPerSecond); - } - - /** - * Set the desired state for each module (velocity and angle) - * @param states an array of SwerveModuleStates representing the desired state for each swerve module [frontLeft, frontRight, backLeft, backRight] - */ - public void setGoalModuleStates(SwerveModuleState[] states) { - for (int i = 0; i < 4; i++) { - goalModuleStates[i] = states[i]; - } - } - - /** - * Set the desired velocities of the robot drive (xVelocity, yVelocity, omegaVelocity) - * @param speeds the desired speed of the robot - */ - public void setGoalChassisSpeeds(ChassisSpeeds speeds) { - speeds = new ChassisSpeeds(speeds.vxMetersPerSecond * (babyMode ? 0.2 : 1), speeds.vyMetersPerSecond * (babyMode ? 0.2 : 1), speeds.omegaRadiansPerSecond * (babyMode ? 0.1 : 1)); - SwerveModuleState[] goalModuleStates = DriveConstants.kinematics.toSwerveModuleStates(speeds); - SwerveDriveKinematics.desaturateWheelSpeeds(goalModuleStates, DriveConstants.maxDriveSpeed); - if (speeds.vxMetersPerSecond == 0 && speeds.vyMetersPerSecond == 0 && speeds.omegaRadiansPerSecond == 0) { - goalModuleStates = new SwerveModuleState[] { - new SwerveModuleState(0, modulePositions[0].angle), - new SwerveModuleState(0, modulePositions[1].angle), - new SwerveModuleState(0, modulePositions[2].angle), - new SwerveModuleState(0, modulePositions[3].angle) - }; - } - setGoalModuleStates(goalModuleStates); - } - - public void stop() { - setGoalModuleStates(new SwerveModuleState[] { - new SwerveModuleState(0, new Rotation2d()), - new SwerveModuleState(0, new Rotation2d()), - new SwerveModuleState(0, new Rotation2d()), - new SwerveModuleState(0, new Rotation2d()), - }); - } - - /** - * @return the rotation of the robot in radians from the gyro - */ - public Rotation2d getRotation() { - return new Rotation2d(MathUtil.angleModulus(driveAngle.getHeading())); - } - - /** - * Sets the heading (apparent rotation) of the robot to zero - */ - public void resetHeading() { - driveAngle.resetHeading(); - } - - public void setHeading(double heading) { - driveAngle.setHeading(heading); - } - - /** - * @return the roll of the gyro in radians - */ - public double getRoll() { - return driveAngle.getRoll(); - } - - /** - * @param baby true to make the drive slow, false to make it go max speeds - */ - public void setBabyMode(boolean baby) { - babyMode = baby; - } - - /** - * Used to pass the current field to vehicle to RobotState, needs to pass through drive because driveModulePositions and rotation are needed to set fieldToVehicle - * @param fieldToVehicle the current fieldToVehicle pose - */ - public void setFieldToVehicle(Pose2d fieldToVehicle) { - RobotState.getInstance().setFieldToVehicle(getRotation(), modulePositions, fieldToVehicle); - } - - public void setVolts(double v) { - driveModules[0].setDriveVoltage(v); - driveModules[1].setDriveVoltage(v); - driveModules[2].setDriveVoltage(v); - driveModules[3].setDriveVoltage(v); - } - - public SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = driveModules[i].getModuleState(); - } - return states; - } - - public ChassisSpeeds getChassisSpeeds() { - return DriveConstants.kinematics.toChassisSpeeds(getModuleStates()); - } - - public void setBrakeMode(boolean braked) { - for (int i = 0; i < 4; i++) { - driveModules[i].setBrake(braked); - } - } - - public void toggleKill(int i) { - driveModules[i].toggleKill(); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveAngle.java b/src/main/java/frc/robot/subsystems/drive/DriveAngle.java deleted file mode 100644 index 5a42aa3..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DriveAngle.java +++ /dev/null @@ -1,90 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix.sensors.Pigeon2; - -import edu.wpi.first.math.util.Units; -import frc.robot.Constants.CANDevices; - -/** - * Class used to interface with the gyro - */ -public class DriveAngle { - - /** - * The gyroscope - */ - private final Pigeon2 pigeon; - - /** - * Offset used to the value of the heading - */ - private double degHeadingOffset = 0; - - /** - * Offset used to the value of the heading - */ - private double degPitchOffset = 0; - - /** - * Offset used to the value of the heading - */ - private double degRollOffset = 0; - - /** - * Initializes the gyro and offsets - */ - public DriveAngle() { - pigeon = new Pigeon2(CANDevices.pigeonIMU, CANDevices.canivoreName); - degHeadingOffset = pigeon.getYaw(); - degPitchOffset = pigeon.getPitch(); - degRollOffset = pigeon.getRoll(); - } - - /** - * @return the heading reported by the gyro minus the offset (radians) - */ - public double getHeading() { - return Units.degreesToRadians(pigeon.getYaw() - degHeadingOffset); - } - - /** - * Set the heading offset to make the gyro report a heading of zero - */ - public void resetHeading() { - degHeadingOffset = pigeon.getYaw(); - } - - public void setHeading(double headingRad) { - degHeadingOffset = pigeon.getYaw() - Units.radiansToDegrees(headingRad); - } - - /**a - * @return the pitch reported by the gyro minus the offset (radians) - */ - public double getPitch() { - return Units.degreesToRadians(pigeon.getPitch() - degPitchOffset); - } - - /** - * Set the pitch offset to make the gyro report a heading of zero - */ - public void resetPitch() { - degPitchOffset = pigeon.getPitch(); - } - - /** - * @return the roll reported by the gyro minus the offset (radians) - */ - public double getRoll() { - return Units.degreesToRadians(pigeon.getRoll() - degRollOffset); - } - - /** - * Set the roll offset to make the gyro report a heading of zero - */ - public void resetRoll() { - degRollOffset = pigeon.getRoll(); - } - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/DriveModule.java b/src/main/java/frc/robot/subsystems/drive/DriveModule.java deleted file mode 100644 index 4e087ea..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DriveModule.java +++ /dev/null @@ -1,176 +0,0 @@ -package frc.robot.subsystems.drive; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.DemandType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; -import com.ctre.phoenix.motorcontrol.can.TalonFX; -import com.ctre.phoenix.sensors.CANCoder; -import com.ctre.phoenix.sensors.CANCoderStatusFrame; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.CANDevices; -import frc.robot.Constants.DriveConstants; - -public class DriveModule { - - private final TalonFX driveMotor; - private final TalonFX rotationMotor; - private final CANCoder rotationEncoder; - private final double initialOffsetRadians; - - private boolean killed = false; - private final Timer deadTimer; - private double lastRotationPosition = 0; - - private static void setFramePeriods(TalonFX talon, boolean needMotorSensor) { - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 255, 1000); - if (!needMotorSensor) { - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 255, 1000); - } - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_4_AinTempVbat, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_6_Misc, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_7_CommStatus, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_9_MotProfBuffer, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_11_UartGadgeteer, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 255, 1000); - talon.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 255, 1000); - } - - public DriveModule(int driveMotorID, int rotationMotorID, int cancoderID, double measuredOffsetsRadians, boolean driveInverted) { - - driveMotor = new TalonFX(driveMotorID, CANDevices.canivoreName); - rotationMotor = new TalonFX(rotationMotorID, CANDevices.canivoreName); - rotationEncoder = new CANCoder(cancoderID, CANDevices.canivoreName); - - driveMotor.configFactoryDefault(1000); - rotationMotor.configFactoryDefault(1000); - setFramePeriods(driveMotor, true); - setFramePeriods(rotationMotor, false); - - driveMotor.setNeutralMode(NeutralMode.Brake); - rotationMotor.setNeutralMode(NeutralMode.Brake); - - driveMotor.setInverted(driveInverted); - rotationMotor.setInverted(true); - - driveMotor.configVoltageCompSaturation(12, 1000); - driveMotor.enableVoltageCompensation(true); - rotationMotor.configVoltageCompSaturation(12, 1000); - rotationMotor.enableVoltageCompensation(true); - - driveMotor.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 70, 80, 1)); - rotationMotor.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 70, 80, 1)); - - driveMotor.configNeutralDeadband(0, 1000); - rotationMotor.configNeutralDeadband(0, 1000); - - rotationEncoder.configFactoryDefault(1000); - rotationEncoder.setStatusFramePeriod(CANCoderStatusFrame.SensorData, 20, 1000); - rotationEncoder.setStatusFramePeriod(CANCoderStatusFrame.VbatAndFaults, 255, 1000); - - initialOffsetRadians = measuredOffsetsRadians; - - deadTimer = new Timer(); - deadTimer.reset(); - deadTimer.start(); - - } - - public boolean getDeadBoolean() { - if (getRotationPosition() != lastRotationPosition) { - deadTimer.reset(); - deadTimer.start(); - lastRotationPosition = getRotationPosition(); - } - - if (deadTimer.hasElapsed(5)) { - return true; - } - return false; - } - - public double getDrivePosition() { - return driveMotor.getSelectedSensorPosition() / 2048.0 * 2.0 * Math.PI / DriveConstants.driveWheelGearReduction; - } - - public double getRotationPosition() { - return MathUtil.angleModulus(Units.degreesToRadians(rotationEncoder.getPosition())) - initialOffsetRadians; - } - - public double getRotationVelocity() { - return Units.degreesToRadians(rotationEncoder.getVelocity()); - } - - public double getDriveVelocityMPerS() { - return driveMotor.getSelectedSensorVelocity() / 2048.0 * 10.0 * 2 * Math.PI * DriveConstants.wheelRadiusM / DriveConstants.driveWheelGearReduction; - } - - public void zeroEncoders() { - rotationEncoder.setPositionToAbsolute(1000); - driveMotor.setSelectedSensorPosition(0, 0, 1000); - } - - public SwerveModuleState getModuleState() { - return new SwerveModuleState(getDriveVelocityMPerS(), new Rotation2d(getRotationPosition())); - } - - public void setRotationVoltage(double volts) { - if (!killed) - rotationMotor.set(ControlMode.PercentOutput, volts/12); - } - - public void setDriveVoltage(double volts) { - if (!killed) - driveMotor.set(ControlMode.PercentOutput, volts/12); - } - - public void setDriveVelocity(double velocityRadPerS, double ffVolts) { - double velocityTicksPer100ms = velocityRadPerS * 2048.0 / 10.0 / 2.0 / Math.PI * DriveConstants.driveWheelGearReduction; - - if (!killed) - driveMotor.set(ControlMode.Velocity, velocityTicksPer100ms, DemandType.ArbitraryFeedForward, ffVolts / 12.0); - } - - public void setDrivePD(double p, double d) { - driveMotor.config_kP(0, p); - driveMotor.config_kD(0, d); - } - - public double getDriveStatorCurrent() { - return driveMotor.getStatorCurrent(); - } - - public double getRotationStatorCurrent() { - return rotationMotor.getStatorCurrent(); - } - - public void setBrake(boolean braked) { - driveMotor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - rotationMotor.setNeutralMode(braked ? NeutralMode.Brake : NeutralMode.Coast); - } - - public void toggleKill() { - killed = !killed; - if (killed) { - driveMotor.set(ControlMode.PercentOutput, 0); - rotationMotor.set(ControlMode.PercentOutput, 0); - driveMotor.setNeutralMode(NeutralMode.Coast); - rotationMotor.setNeutralMode(NeutralMode.Coast); - } - else { - driveMotor.setNeutralMode(NeutralMode.Brake); - rotationMotor.setNeutralMode(NeutralMode.Brake); - } - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java deleted file mode 100644 index cfacef5..0000000 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ /dev/null @@ -1,111 +0,0 @@ -package frc.robot.subsystems.vision; - -import java.util.Optional; - -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.RobotState; -import frc.robot.Constants.VisionConstants; - -public class Camera { - - private static final AprilTagFieldLayout tagLayout = new AprilTagFieldLayout(VisionConstants.tags, VisionConstants.fieldLength, VisionConstants.fieldWidth); - - private final PhotonCamera camera; - private final PhotonPoseEstimator poseEstimator; - - private volatile boolean hasNewPose = false; - private volatile Pose3d calculatedPose = new Pose3d(); - private volatile Matrix stdDevs = VecBuilder.fill(1000, 1000, 1000); - private volatile double timestamp = 1; - - private String name; - - - public Camera(String cameraName, Transform3d vehicleToCamera) { - camera = new PhotonCamera(cameraName); - poseEstimator = new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP, camera, vehicleToCamera); - poseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); - - name = cameraName; - } - - public void periodic() { - - if (!camera.isConnected()) return; - - PhotonPipelineResult result = camera.getLatestResult(); - Optional estimatedPose = poseEstimator.update(result); - if (estimatedPose.isEmpty()) return; - EstimatedRobotPose estimation = estimatedPose.get(); - if (estimation.timestampSeconds == timestamp) return; - if (estimation.targetsUsed.size() == 1 && - (estimation.targetsUsed.get(0).getPoseAmbiguity() > VisionConstants.singleTagAmbiguityCutoff || estimation.targetsUsed.get(0).getPoseAmbiguity() == -1)) - return; - - double distance = 0; - for (PhotonTrackedTarget target : estimation.targetsUsed) { - distance += target.getBestCameraToTarget().getTranslation().getDistance(new Translation3d()); - } - - distance /= estimation.targetsUsed.size(); - stdDevs = computeStdDevs(distance); - - calculatedPose = estimation.estimatedPose; - timestamp = estimation.timestampSeconds; - hasNewPose = true; - - } - - public boolean hasNewObservation() { - return hasNewPose; - } - - public void recordVisionObservation() { - if(calculatedPose.toPose2d().getX() < 5 || calculatedPose.toPose2d().getX() > 11) { - RobotState.getInstance() - .recordVisionObservations(calculatedPose.toPose2d(), stdDevs, timestamp); - } - hasNewPose = false; - - log3dPose("Vision/" + name + "/RawPose", calculatedPose); - } - - private Matrix computeStdDevs(double distance) { - double stdDev = Math.max( - VisionConstants.minimumStdDev, - VisionConstants.stdDevEulerMultiplier * Math.exp(distance * VisionConstants.stdDevDistanceMultiplier) - ); - return VecBuilder.fill(stdDev, stdDev, 1000); - } - - /** - * Logs a Pose3d in a format viewable in AdvantageScope - */ - private void log3dPose(String key, Pose3d pose) { - double[] doubles = new double[7]; - doubles[0] = pose.getX(); - doubles[1] = pose.getY(); - doubles[2] = pose.getZ(); - doubles[3] = pose.getRotation().getQuaternion().getW(); - doubles[4] = pose.getRotation().getQuaternion().getX(); - doubles[5] = pose.getRotation().getQuaternion().getY(); - doubles[6] = pose.getRotation().getQuaternion().getZ(); - SmartDashboard.putNumberArray(key, doubles); - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java deleted file mode 100644 index 7653e09..0000000 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; -import frc.robot.Constants.VisionConstants; - -public class Vision extends SubsystemBase { - - private final Camera[] cameras; - - private final Notifier notifier; - - public Vision() { - - cameras = new Camera[] { - new Camera(VisionConstants.cameraNames[0], VisionConstants.vehicleToCameras[0]), - // new Camera(VisionConstants.cameraNames[1], VisionConstants.vehicleToCameras[1]), - new Camera(VisionConstants.cameraNames[2], VisionConstants.vehicleToCameras[2]), - new Camera(VisionConstants.cameraNames[3], VisionConstants.vehicleToCameras[3]) - }; - - notifier = new Notifier(() -> { - for (int i = 0; i < 3; i++) { - cameras[i].periodic(); - } - }); - notifier.startPeriodic(0.02); - - } - - @Override - public void periodic() { - - RobotState.getInstance().getFieldToVehicle(); - - for (int i = 0; i < 3; i++) { - if (cameras[i].hasNewObservation()) { - cameras[i].recordVisionObservation(); - } - } - - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/CustomHolonomicDrive.java b/src/main/java/frc/robot/util/CustomHolonomicDrive.java deleted file mode 100644 index 2177eb2..0000000 --- a/src/main/java/frc/robot/util/CustomHolonomicDrive.java +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.util; - -import java.util.function.Supplier; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; - -/** - * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain - * (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve - * compared to skid-steer style drivetrains because it is possible to individually control forward, - * sideways, and angular velocity. - * - *

The holonomic drive controller takes in one PID controller for each direction, forward and - * sideways, and one profiled PID controller for the angular direction. Because the heading dynamics - * are decoupled from translations, users can specify a custom heading that the drivetrain should - * point toward. This heading reference is profiled for smoothness. - */ -@SuppressWarnings("MemberName") -public class CustomHolonomicDrive { - private Pose2d m_poseError = new Pose2d(); - private Pose2d m_poseTolerance = new Pose2d(.05, .05, new Rotation2d()); - private double kControlFactorY = 0.2; - private double kControlFactorX = 0.2; - // private boolean m_enabled = true; - - private final PIDController m_xyController; - private final PIDController m_zController; - private final SlewRateLimiter m_xLimiter; - private final SlewRateLimiter m_yLimiter; - private final SlewRateLimiter m_zLimiter; - - /** - * Constructs a holonomic drive controller. - * - * @param xyController A PID Controller to respond to error in the field-relative x direction. - * @param zController A PID Controller to respond to error in the field-relative y direction. - */ - @SuppressWarnings("ParameterName") - public CustomHolonomicDrive(PIDController xyController, PIDController zController, SlewRateLimiter xLimiter, - SlewRateLimiter yLimiter, SlewRateLimiter zLimiter) { - m_xyController = xyController; - m_zController = zController; - m_zController.enableContinuousInput(0, 360); - m_xLimiter = xLimiter; - m_yLimiter = yLimiter; - m_zLimiter = zLimiter; - - } - - /** - * Returns true if the pose error is within tolerance of the reference. - * - * @return True if the pose error is within tolerance of the reference. - */ - public boolean atReference() { - final var eTranslate = m_poseError.getTranslation(); - final var tolTranslate = m_poseTolerance.getTranslation(); - return Math.abs(eTranslate.getX()) < tolTranslate.getX() - && Math.abs(eTranslate.getY()) < tolTranslate.getY(); - } - - public double distanceFromXY(Pose2d currentPose, Pose2d poseRef) { - return Math - .sqrt(Math.pow(currentPose.getX() - poseRef.getX(), 2) + Math.pow(currentPose.getY() - poseRef.getY(), 2)); - } - - public Rotation2d distanceFromZ(Pose2d currentPose, Pose2d poseRef) { - return Rotation2d - .fromDegrees(Math.abs(currentPose.getRotation().getDegrees() - poseRef.getRotation().getDegrees())); - } - - public double slewRateX(double xSpeed) { - return m_xLimiter.calculate(xSpeed); - } - - public double slewRateY(double ySpeed) { - return m_yLimiter.calculate(ySpeed); - } - - public double slewRateZ(double zSpeed) { - return m_zLimiter.calculate(zSpeed); - } - - /** - * Sets the pose error which is considered tolerance for use with atReference(). - * - * @param tolerance The pose error which is tolerable. - */ - public void setTolerance(Pose2d tolerance) { - m_poseTolerance = tolerance; - } - - /** - * Returns the next output of the holonomic drive controller. - * - * @param currentPose The current pose. - * @param poseRef The desired pose. - * @param linearVelocityRefMeters The linear velocity reference. - * @param angleRef The angular reference. - * @param angleVelocityRefRadians The angular velocity reference. - * @return The next output of the holonomic drive controller. - */ - @SuppressWarnings("LocalVariableName") - public ChassisSpeeds calculateOnlyY( - Pose2d currentPose, - Pose2d poseRef, - Supplier xSpeed, - Supplier ySpeed, - Supplier angleRef) { - - // Calculate feedforward velocities (field-relative). - // double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos(); - // double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); - // double thetaFF = angleVelocityRefRadians; - m_poseError = poseRef.relativeTo(currentPose); - - // Calculate feedback velocities (based on position error). - double xFeedback = m_xyController.calculate(currentPose.getX(), poseRef.getX()); - double yFeedback = m_xyController.calculate(currentPose.getY(), poseRef.getY()); - double thetaFeedback = m_zController.calculate(currentPose.getRotation().getDegrees(), - poseRef.getRotation().getDegrees()); - - // EricNubControls EricControls = new EricNubControls(); - // double x_speed = EricControls.addDeadzoneScaled(xSpeed.get(), 0.1); - // double y_speed = EricControls.addDeadzoneScaled(ySpeed.get(), 0.1); - - // double x = xFeedback + (kControlFactorX * x_speed); - // double y = yFeedback + (kControlFactorY * y_speed); - double x = xFeedback; - double y = yFeedback; - - double mag = Math.sqrt((x * x) + (y * y)); - - // if you dont want to be at max speed, dont use this function - // double xF = x / mag; - // double yF = y / mag; - - // Calculate feedback velocities (based on angle error). - - // Return next output. - // EricControls.addEricCurve(EricControls.addDeadzoneScaled(angleRef.get(), 0.1)) - return ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed.get() * Constants.DriveConstants.poseMoveTranslationMaxVel / 2, - y * Constants.DriveConstants.poseMoveTranslationMaxVel, - thetaFeedback - * Constants.DriveConstants.poseMoveRotationMaxVel, - currentPose.getRotation()); - } - - @SuppressWarnings("LocalVariableName") - public ChassisSpeeds calculateOnlyX( - Pose2d currentPose, - Pose2d poseRef, - Supplier xSpeed, - Supplier ySpeed, - Supplier angleRef) { - - // Calculate feedforward velocities (field-relative). - // double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos(); - // double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); - // double thetaFF = angleVelocityRefRadians; - m_poseError = poseRef.relativeTo(currentPose); - - // Calculate feedback velocities (based on position error). - double xFeedback = m_xyController.calculate(currentPose.getX(), poseRef.getX()); - double yFeedback = m_xyController.calculate(currentPose.getY(), poseRef.getY()); - double thetaFeedback = m_zController.calculate(currentPose.getRotation().getDegrees(), - poseRef.getRotation().getDegrees()); - - // EricNubControls EricControls = new EricNubControls(); - // double x_speed = EricControls.addDeadzoneScaled(xSpeed.get(), 0.1); - // double y_speed = EricControls.addDeadzoneScaled(ySpeed.get(), 0.1); - - // double x = xFeedback + (kControlFactorX * x_speed); - // double y = yFeedback + (kControlFactorY * y_speed); - double x = xFeedback; - double y = yFeedback; - - double mag = Math.sqrt((x * x) + (y * y)); - - // if you dont want to be at max speed, dont use this function - // double xF = x / mag; - // double yF = y / mag; - - // Calculate feedback velocities (based on angle error). - - // Return next output. - // EricControls.addEricCurve(EricControls.addDeadzoneScaled(angleRef.get(), 0.1)) - return ChassisSpeeds.fromFieldRelativeSpeeds(x * DriveConstants.poseMoveTranslationMaxVel / 2, - ySpeed.get() * DriveConstants.poseMoveTranslationMaxVel, - thetaFeedback - * DriveConstants.poseMoveRotationMaxVel, - currentPose.getRotation()); - } - - @SuppressWarnings("LocalVariableName") - public ChassisSpeeds calculate( - Pose2d currentPose, - Pose2d poseRef) { - - // Calculate feedforward velocities (field-relative). - // double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos(); - // double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); - // double thetaFF = angleVelocityRefRadians; - m_poseError = poseRef.relativeTo(currentPose); - - // Calculate feedback velocities (based on position error). - double xFeedback = m_xyController.calculate(currentPose.getX(), poseRef.getX()); - double yFeedback = m_xyController.calculate(currentPose.getY(), poseRef.getY()); - double thetaFeedback = m_zController.calculate(currentPose.getRotation().getDegrees(), - poseRef.getRotation().getDegrees()); - - // EricNubControls EricControls = new EricNubControls(); - // double x_speed = EricControls.addDeadzoneScaled(xSpeed.get(), 0.1); - // double y_speed = EricControls.addDeadzoneScaled(ySpeed.get(), 0.1); - - // double x = xFeedback + (kControlFactorX * x_speed); - // double y = yFeedback + (kControlFactorY * y_speed); - double x = xFeedback; - double y = yFeedback; - - double mag = Math.sqrt((x * x) + (y * y)); - - // if you dont want to be at max speed, dont use this function - // double xF = x / mag; - // double yF = y / mag; - - // Calculate feedback velocities (based on angle error). - - // Return next output. - // EricControls.addEricCurve(EricControls.addDeadzoneScaled(angleRef.get(), 0.1)) - return ChassisSpeeds.fromFieldRelativeSpeeds(x * DriveConstants.poseMoveTranslationMaxVel, - y * DriveConstants.poseMoveTranslationMaxVel, - thetaFeedback - * DriveConstants.poseMoveRotationMaxVel, - currentPose.getRotation()); - } - - public ChassisSpeeds fullCalulate(Pose2d desPose2d, Pose2d curPose, - ChassisSpeeds curChassisSpeeds) { - // get the error between the current pose and the desired pose - m_poseError = desPose2d.relativeTo(curPose); - // get the error between the current chassis speeds and the desired chassis speeds - ChassisSpeeds m_chassisSpeeds = this.calculate(curPose, desPose2d); - // use slew rate limiter to limit the change in chassis speeds - // m_chassisSpeeds.vxMetersPerSecond = m_xLimiter.calculate(m_chassisSpeeds.vxMetersPerSecond); - // m_chassisSpeeds.vyMetersPerSecond = m_yLimiter.calculate(m_chassisSpeeds.vyMetersPerSecond); - // m_chassisSpeeds.omegaRadiansPerSecond = m_zLimiter.calculate(m_chassisSpeeds.omegaRadiansPerSecond); - - // return the new chassis speeds - return m_chassisSpeeds; - } - - @SuppressWarnings("LocalVariableName") - public ChassisSpeeds calculate( - Pose2d currentPose, - ExtendedPathPoint poseRef, - Supplier xSpeed, - Supplier ySpeed, - Supplier angleRef, Boolean finalPoint) { - - // Calculate feedforward velocities (field-relative). - // double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos(); - // double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin(); - // double thetaFF = angleVelocityRefRadians; - Pose2d pose2dRef = poseRef.getPose2d(); - m_poseError = pose2dRef.relativeTo(currentPose); - - // Calculate feedback velocities (based on position error). - double xFeedback = m_xyController.calculate(currentPose.getX(), pose2dRef.getX()); - double yFeedback = m_xyController.calculate(currentPose.getY(), pose2dRef.getY()); - double thetaFeedback = m_zController.calculate(currentPose.getRotation().getDegrees(), - poseRef.getRotation().getDegrees()); - - // EricNubControls EricControls = new EricNubControls(); - // double x_speed = EricControls.addDeadzoneScaled(xSpeed.get(), 0.1); - // double y_speed = EricControls.addDeadzoneScaled(ySpeed.get(), 0.1); - - // double x = xFeedback + (kControlFactorX * x_speed); - // double y = yFeedback + (kControlFactorY * y_speed); - double x = xFeedback; - double y = yFeedback; - double xF = 0; - double yF = 0; - if (finalPoint) { - xF = x; - yF = y; - } else { - double mag = Math.sqrt((x * x) + (y * y)); - - // if you dont want to be at max speed, dont use this function - xF = x / mag; - yF = y / mag; - } - - // Calculate feedback velocities (based on angle error). - - // Return next output. - // EricControls.addEricCurve(EricControls.addDeadzoneScaled(angleRef.get(), 0.1)) - return ChassisSpeeds.fromFieldRelativeSpeeds(xF * DriveConstants.poseMoveTranslationMaxVel, - yF * DriveConstants.poseMoveTranslationMaxVel, - thetaFeedback - * DriveConstants.poseMoveRotationMaxVel, - currentPose.getRotation()); - } -} diff --git a/src/main/java/frc/robot/util/ExtendedPathPoint.java b/src/main/java/frc/robot/util/ExtendedPathPoint.java deleted file mode 100644 index 20e0866..0000000 --- a/src/main/java/frc/robot/util/ExtendedPathPoint.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.robot.util; - -import com.pathplanner.lib.PathPoint; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.robot.Constants.VisionConstants; - -public class ExtendedPathPoint extends PathPoint { - public Translation2d m_pose; - public Rotation2d m_heading; - public Rotation2d m_holonomicHeading; - public Translation2d m_acceptedPoseError; - public Rotation2d m_acceptedAngularError; - - public ExtendedPathPoint(Translation2d pose, Rotation2d heading, Rotation2d holRotation2d) { - super(pose, heading, heading); - m_pose = pose; - m_heading = heading; - m_holonomicHeading = holRotation2d; - m_acceptedPoseError = new Translation2d(.2, .2); - m_acceptedAngularError = Rotation2d.fromDegrees(3); - } - - public ExtendedPathPoint(Translation2d pose, Rotation2d heading, Rotation2d holRotation2d, - Translation2d acceptedPoseError, - Rotation2d acceptedAngularError) { - super(pose, heading, heading); - m_pose = pose; - m_heading = heading; - m_holonomicHeading = holRotation2d; - m_acceptedPoseError = acceptedPoseError; - m_acceptedAngularError = acceptedAngularError; - } - - public Translation2d getTranslation() { - return m_pose; - } - - public Rotation2d getRotation() { - return m_heading; - } - - public Rotation2d getHolonomicRotation2d() { - return m_holonomicHeading; - } - - public ExtendedPathPoint flipPathPoint() { - // flips current path point about the X axis - // Rotation2d newHeading = Rotation2d.fromDegrees(180).minus(m_heading); - // if (newHeading.getDegrees() < 0) { - // newHeading = newHeading.plus(Rotation2d.fromDegrees(360)); - // } - // Rotation2d newHolonomicHeading = Rotation2d.fromDegrees(180).minus(m_holonomicHeading); - // if (newHolonomicHeading.getDegrees() < 0) { - // newHolonomicHeading = newHolonomicHeading.plus(Rotation2d.fromDegrees(360)); - // } - return new ExtendedPathPoint( - new Translation2d(m_pose.getX(), - VisionConstants.fieldWidth - m_pose.getY()), - m_heading, m_holonomicHeading); - } - - public Pose2d getPose2d() { - return new Pose2d(m_pose, m_holonomicHeading); - } - - public Pose2d getTolerance() { - return new Pose2d(m_acceptedPoseError, m_acceptedAngularError); - } - - public ExtendedPathPoint addTransform(Translation2d transform) { - return new ExtendedPathPoint(m_pose.plus(transform), m_heading, m_holonomicHeading); - } - -} diff --git a/src/main/java/frc/robot/util/PositionHelper.java b/src/main/java/frc/robot/util/PositionHelper.java deleted file mode 100644 index 108b9b6..0000000 --- a/src/main/java/frc/robot/util/PositionHelper.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.util; - -import frc.robot.RobotState; -import frc.robot.Constants.ArmPositions; -import frc.robot.Constants.GamePieceMode; -import frc.robot.Constants.Position; - -public final class PositionHelper { - - public static double[] getDouble(Position position, GamePieceMode mode) { - switch(position) { - case Ground: - return getGround(mode); - case Mid: - return getMid(mode); - case High: - return getHigh(mode); - case Shelf: - return getShelf(mode); - default: - return ArmPositions.stow; - } - } - - private static double[] getGround(GamePieceMode mode) { - if (mode == GamePieceMode.Cube) return ArmPositions.intakeCubeGround; - if (mode == GamePieceMode.ConeDown) { - if (RobotState.getInstance().atBack()) return ArmPositions.intakeConeDownBackGround; - return ArmPositions.intakeConeDownFrontGround; - } - if (mode == GamePieceMode.ConeUp) { - if (RobotState.getInstance().atBack()) return ArmPositions.intakeConeUpBackGround; - return ArmPositions.intakeConeUpFrontGround; - } - return null; - } - - private static double[] getMid(GamePieceMode mode) { - if (mode == GamePieceMode.Cube) return ArmPositions.placeCubeMid; - if (mode == GamePieceMode.ConeDown) return ArmPositions.placeConeDownMid; - if (mode == GamePieceMode.ConeUp) return ArmPositions.placeConeUpMid; - return null; - } - - private static double[] getHigh(GamePieceMode mode) { - if (mode == GamePieceMode.Cube) return ArmPositions.placeCubeHigh; - if (mode == GamePieceMode.ConeDown) return ArmPositions.placeConeDownHigh; - if (mode == GamePieceMode.ConeUp) return ArmPositions.placeConeUpHigh; - return null; - } - - private static double[] getShelf(GamePieceMode mode) { - // if (mode == GamePieceMode.Cube) return ArmPositions.intakeCubeShelf; - // if (mode == GamePieceMode.ConeDown) return ArmPositions.intakeConeDownShelf; - // if (mode == GamePieceMode.ConeUp) return ArmPositions.intakeConeUpShelf; - RobotState.getInstance().setMode(GamePieceMode.ConeUp); - return ArmPositions.intakeConeUpShelf; - // return null; - } -} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..85e0848 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.0.0-beta-5", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.0.0-beta-5" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.0.0-beta-5" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.0.0-beta-5" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.0.0-beta-5", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 8e61586..e1f6e52 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,8 +1,9 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.4", + "version": "2024.0.0-beta-5.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2024", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" ], @@ -11,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.4" + "version": "2024.0.0-beta-5.1" } ], "jniDependencies": [], @@ -19,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.4", + "version": "2024.0.0-beta-5.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -28,7 +29,9 @@ "windowsx86-64", "linuxx86-64", "osxuniversal", - "linuxathena" + "linuxathena", + "linuxarm32", + "linuxarm64" ] } ] diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix6.json similarity index 62% rename from vendordeps/Phoenix.json rename to vendordeps/Phoenix6.json index 55bc099..a6983a5 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix6.json @@ -1,56 +1,32 @@ { - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix (v5)", - "version": "5.30.4", - "frcYear": 2023, - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "24.0.0-beta-3", + "frcYear": 2024, + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", - "javaDependencies": [ + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.30.4" - }, + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], + "javaDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "24.0.0-beta-3" } ], "jniDependencies": [ { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +37,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +50,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +63,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +76,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +89,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +102,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +115,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +128,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +141,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.5", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,10 +156,10 @@ ], "cppDependencies": [ { - "groupId": "com.ctre.phoenix", + "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPI", + "version": "24.0.0-beta-3", + "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -195,39 +171,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.30.4", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,40 +186,10 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenix.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_Phoenix_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "api-cpp-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixSim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix.sim", - "artifactId": "cci-sim", - "version": "5.30.4", - "libName": "CTRE_PhoenixCCISim", + "version": "24.0.0-beta-3", + "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -285,9 +201,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -300,9 +216,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -315,9 +231,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -330,9 +246,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -345,9 +261,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -360,9 +276,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -375,9 +291,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -390,9 +306,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -405,9 +321,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.5", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index cdbbe6e..7eb3755 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,24 +1,25 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2023.1.2", + "version": "2024.0.0", + "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "https://maven.revrobotics.com/" ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2023.json", + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", "javaDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2023.1.2" + "version": "2024.0.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.2", + "version": "2024.0.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2023.1.2", + "version": "2024.0.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -54,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2023.1.2", + "version": "2024.0.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 65dcc03..67bf389 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,35 +3,36 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "frcYear": "2024", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-java", - "version": "wpilib" - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } ], "jniDependencies": [], "cppDependencies": [ - { - "groupId": "edu.wpi.first.wpilibNewCommands", - "artifactId": "wpilibNewCommands-cpp", - "version": "wpilib", - "libName": "wpilibNewCommands", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "linuxarm32", - "linuxarm64", - "windowsx86-64", - "windowsx86", - "linuxx86-64", - "osxuniversal" - ] - } + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxuniversal" + ] + } ] } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json deleted file mode 100644 index dad3105..0000000 --- a/vendordeps/photonlib.json +++ /dev/null @@ -1,41 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2023.4.2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2023.4.2", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2023.4.2" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2023.4.2" - } - ] -} \ No newline at end of file