diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..fc05ab5 --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1,6 @@ +{ + "download": { + "localDir": "C:\\SysIdLogs", + "serverTeam": "199" + } +} diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1 @@ +{} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..9ecad41 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,39 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the +// README at: https://github.com/devcontainers/templates/tree/main/src/java +{ + "name": "Java", + // Or use a Dockerfile or Docker Compose file. More info: https://containers.dev/guide/dockerfile + "image": "mcr.microsoft.com/devcontainers/java:1-17-bookworm", + + "features": { + "ghcr.io/devcontainers/features/java:1": { + "version": "none", + "installMaven": "false", + "installGradle": "true" + } + }, + + // Use 'forwardPorts' to make a list of ports inside the container available locally. + // "forwardPorts": [], + + // Use 'postCreateCommand' to run commands after the container is created. + // "postCreateCommand": "", + + // Configure tool-specific properties. + "customizations": { + "vscode": { + "extensions": [ + "redhat.java", + "ms-vscode.cpptools", + "vcjava.vscode-java-debug", + "ms-toolsai.jupyter", + "vcjava.vscode-java-dependency" + ] + } + }, + + "postAttachCommand": "./.devcontainer/installWPILibExtension.sh && exit" + + // Uncomment to connect as root instead. More info: https://aka.ms/dev-containers-non-root. + // "remoteUser": "root" +} diff --git a/.devcontainer/installWPILibExtension.sh b/.devcontainer/installWPILibExtension.sh new file mode 100644 index 0000000..f789b8b --- /dev/null +++ b/.devcontainer/installWPILibExtension.sh @@ -0,0 +1,7 @@ +# Modified from ChatGPT suggestions +WPILIB_VSIX_URL=$(curl -s "https://api.github.com/repos/wpilibsuite/vscode-wpilib/releases/latest" | jq -r '.assets[] | select(.name | test(".vsix$")) | .browser_download_url') +INSTALL_LOCATION="/tmp/wpilib-extension/latest.vsix" +echo "$WPILIB_VSIX_URL" +curl --create-dirs -L -o "$INSTALL_LOCATION" "$WPILIB_VSIX_URL" +code --install-extension "$INSTALL_LOCATION" + diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..26bb21b --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,14 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "Center Limelight 4 Piece" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 + } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..96c51f6 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "2025", + "teamNumber": 199 +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 8d59a2e..2482ad8 100644 --- a/build.gradle +++ b/build.gradle @@ -1,8 +1,7 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10" - } java { @@ -21,7 +20,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamNumber() + team = project.frc.getTeamOrDefault(199) debug = project.frc.getDebugOrDefault(false) artifacts { @@ -86,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.deepbluerobotics:lib199:2025-SNAPSHOT" + implementation "com.github.deepbluerobotics:lib199:32da18340af20581b2ef5e17f9d4dfa07c106dbb" } test { diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..c4b7efd --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..b76c19f --- /dev/null +++ b/simgui.json @@ -0,0 +1,31 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/SendableChooser[1]": "String Chooser" + } + }, + "NetworkTables Info": { + "visible": true + } +} +{ + "HALProvider": { + "Other Devices": { + "SPARK Flex [1] RELATIVE ENCODER": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,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,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,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]} \ 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..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,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,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,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,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,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CoralConfigs.path b/src/main/deploy/pathplanner/paths/CoralConfigs.path new file mode 100644 index 0000000..cd412d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralConfigs.path @@ -0,0 +1,135 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.2448049779275094, + "y": 7.447722322258365 + }, + "prevControl": null, + "nextControl": { + "x": 3.244804977927509, + "y": 7.447722322258365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.004174292696725, + "y": 5.244474697559479 + }, + "prevControl": { + "x": 4.7064615000910095, + "y": 5.244474697559479 + }, + "nextControl": { + "x": 5.30188708530244, + "y": 5.244474697559479 + }, + "isLocked": false, + "linkedName": "C11" + }, + { + "anchor": { + "x": 5.289208179311857, + "y": 5.080085654005255 + }, + "prevControl": { + "x": 5.040367278685427, + "y": 5.056039735943611 + }, + "nextControl": { + "x": 5.538049079938287, + "y": 5.1041315720668985 + }, + "isLocked": false, + "linkedName": "C12" + }, + { + "anchor": { + "x": 5.805, + "y": 4.189142416305975 + }, + "prevControl": { + "x": 5.5324143394454595, + "y": 5.159338758385921 + }, + "nextControl": { + "x": 6.07758566055454, + "y": 3.218946074226031 + }, + "isLocked": false, + "linkedName": "C21" + }, + { + "anchor": { + "x": 5.803912909888542, + "y": 3.8582569473798314 + }, + "prevControl": { + "x": 5.667794856321749, + "y": 4.295016773740704 + }, + "nextControl": { + "x": 5.940030963455334, + "y": 3.4214971210189535 + }, + "isLocked": false, + "linkedName": "C22" + }, + { + "anchor": { + "x": 6.364839756618353, + "y": 0.5451487714944108 + }, + "prevControl": { + "x": 6.799189344211289, + "y": 1.457427320518574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -117.26958303209868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartingConfigs.path b/src/main/deploy/pathplanner/paths/StartingConfigs.path new file mode 100644 index 0000000..ce1c8d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StartingConfigs.path @@ -0,0 +1,115 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 8.062264438838344, + "y": 7.644490337970196 + }, + "isLocked": false, + "linkedName": "S1F" + }, + { + "anchor": { + "x": 8.0, + "y": 6.5 + }, + "prevControl": { + "x": 7.971636103762701, + "y": 7.116609069648897 + }, + "nextControl": { + "x": 8.020395624409266, + "y": 6.056614969724504 + }, + "isLocked": false, + "linkedName": "S2F" + }, + { + "anchor": { + "x": 8.0, + "y": 5.5 + }, + "prevControl": { + "x": 7.987043611353682, + "y": 5.749664038245891 + }, + "nextControl": { + "x": 8.012956388646318, + "y": 5.250335961754109 + }, + "isLocked": false, + "linkedName": "S3F" + }, + { + "anchor": { + "x": 8.0, + "y": 4.5 + }, + "prevControl": { + "x": 8.122297824367529, + "y": 4.846496610775691 + }, + "nextControl": { + "x": 7.894024420976902, + "y": 4.199747897017828 + }, + "isLocked": false, + "linkedName": "S4F" + }, + { + "anchor": { + "x": 6.3982675999074194, + "y": 2.8808078241188046 + }, + "prevControl": { + "x": 6.5963632343599095, + "y": 2.728301694370307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..2ca3c4c --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "NEO", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/BuildInfo.java b/src/main/java/org/carlmontrobotics/BuildInfo.java new file mode 100644 index 0000000..2973583 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/BuildInfo.java @@ -0,0 +1,45 @@ +package org.carlmontrobotics; + +import java.util.Properties; +import java.io.File; +import java.io.InputStream; +import java.nio.file.Path; +import java.nio.file.Files; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Filesystem; + +public class BuildInfo implements Sendable { + private Properties props = new Properties(); + + static private BuildInfo instance = null; + + public static BuildInfo getInstance() { + if (instance == null) { + instance = new BuildInfo(); + } + return instance; + } + + private BuildInfo() { + Path path = Path + .of(Filesystem.getDeployDirectory().getAbsolutePath() + File.separator + "BuildInfo.properties"); + try (InputStream is = Files.newInputStream(path)) { + props.load(is); + } catch (Exception ex) { + System.err.println("Error reading build properties from %s".formatted(path)); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + props.stringPropertyNames().forEach(name -> { + var value = props.getProperty(name); + // Workaround bug (https://github.com/lessthanoptimal/gversion-plugin/pull/14) + // where the gversion plugin surrounds values with quotes. + value = value.replaceAll("\"", ""); + builder.publishConstString(name, value); + }); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java new file mode 100644 index 0000000..699f34a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -0,0 +1,119 @@ +package org.carlmontrobotics; + +import java.lang.reflect.Method; +import java.lang.reflect.Modifier; +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public abstract class Config implements Sendable { + public static final Config CONFIG = new Config() { + { + // Override config settings here, like this: + // this.exampleFlagEnabled = true; + + // NOTE: PRs with overrides will NOT be merged because we don't want them + // polluting the master branch. + // Feel free to add them when testing, but remove them before pushing. + } + }; + + // Add additional config settings by declaring a protected field, and... + protected boolean exampleFlagEnabled = false; + protected boolean swimShady = false; + protected boolean setupSysId = true; + protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of + // outtake through SmartDashboard + // Note: disables joystick control of arm and + // outtake command if + // using SmartDashboard + + // ...a public getter starting with "is" for booleans or "get" for other types. + // Do NOT remove this example. It is used by unit tests. + + public boolean isExampleFlagEnabled() { + return exampleFlagEnabled; + } + + public boolean isSwimShady() { + return swimShady; + } + + public boolean isSysIdTesting() { + return setupSysId; + } + + public boolean useSmartDashboardControl() { + return useSmartDashboardControl; + } + + // --- For clarity, place additional config settings ^above^ this line --- + + private static class MethodResult { + String methodName = null; + Object retVal = null; + Object defaultRetVal = null; + + MethodResult(String name, Object retVal, Object defaultRetval) { + this.methodName = name; + this.retVal = retVal; + this.defaultRetVal = defaultRetval; + } + } + + private List getMethodResults() { + var methodResults = new ArrayList(); + var defaultConfig = new Config() { + }; + for (Method m : Config.class.getDeclaredMethods()) { + var name = m.getName(); + if (!Modifier.isPublic(m.getModifiers()) || m.isSynthetic() || m.getParameterCount() != 0 + || !name.matches("^(get|is)[A-Z].*")) { + continue; + } + Object retVal = null; + try { + retVal = m.invoke(this); + } catch (Exception ex) { + retVal = ex; + } + Object defaultRetVal = null; + try { + defaultRetVal = m.invoke(defaultConfig); + } catch (Exception ex) { + defaultRetVal = ex; + } + methodResults.add(new MethodResult(name, retVal, defaultRetVal)); + } + return methodResults; + } + + @Override + public void initSendable(SendableBuilder builder) { + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + builder.publishConstString("%s()".formatted(mr.methodName), + String.format("%s (default is %s)", mr.retVal, mr.defaultRetVal)); + } + }); + } + + @Override + public String toString() { + StringBuilder stringBuilder = new StringBuilder(); + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + stringBuilder.append( + String.format("%s() returns %s (default is %s)", mr.methodName, mr.retVal, mr.defaultRetVal)); + } + }); + if (stringBuilder.isEmpty()) { + stringBuilder.append("Using default config values"); + } else { + stringBuilder.insert(0, "WARNING: USING OVERRIDDEN CONFIG VALUES\n"); + } + return stringBuilder.toString(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..107f9a6 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,15 +1,446 @@ + +// 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 org.carlmontrobotics; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import org.carlmontrobotics.lib199.swerve.SwerveConfig; +import com.pathplanner.lib.path.PathConstraints; + +import static org.carlmontrobotics.Config.CONFIG; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj.CounterBase; +import edu.wpi.first.wpilibj.Encoder; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ public final class Constants { - // public static final class Drivetrain { - // public static final double MAX_SPEED_MPS = 2; - // } - public static final class OI { - public static final class Driver { - public static final int port = 0; - } - public static final class Manipulator { - public static final int port = 1; - } - } + public static final double g = 9.81; // meters per second squared + + public static final class Led { + + } + + public static final class Effectorc { + + } + + public static final class Armc { + + } + + public static final class Drivetrainc { + // #region Subsystem Constants + public static final double wheelBase = 24.75; // CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : + // Units.inchesToMeters(16.75); + public static final double trackWidth = 24.75;// CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : + // Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1e + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150.0 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() + ? new double[] { -90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = { 12, 12, 23, 23 };// sysid for fr that didnt't work{0.099412, 0.13414, + // 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, + // 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = { 0, 0, 0, 0 };// { 0, 0.1, 0, 0 }; + public static final double[] turnkD = { 1, 1.55, 0, 2 };// { 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = { 0.050634, 0.033065, 0.018117, 0.021337 };// sysid for fr that didnt't + // work{0.041796, 0.09111, + // 0.64804, 1.0873}//{ + // 0.13027, 0.17026, 0.2, + // 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = { 2.6153, 2.5924, 2.6495, 2.6705 };// sysid for fr that didnt't + // work{2.6403, 2.6603, 2.6168, + // 2.5002} //{2.6532, 2.7597, + // 2.7445, 2.7698}; + public static final double[] turnkA = { 0.18525, 0.13879, 0.23625, 0.25589 };// sysid for fr that didnt't + // work{0.33266, 0.25535, + // 0.17924, 0.17924} //{ + // 0.17924, 0.17924, 0.17924, + // 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] { 1.75, 1.75, 1.75, 1.75 }; // {2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, 1.915/100}; + public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { false, true, false, true }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + // public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, + // 0.2461 }; + public static final double[] kForwardVolts = { 0, 0, 0, 0 }; // {0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, + // 0.2, 0.2, 0.2 }; + public static final double[] kBackwardVolts = kForwardVolts; + + // kV + // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kForwardVels = { 0, 0, 0, 0 };// {2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 + // };//volts per m/s + public static final double[] kBackwardVels = kForwardVels; + + // kA + // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, + // 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kForwardAccels = { 0, 0, 0, 0 };// {0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, + // 0, 0 };// volts per m/s^2 + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * g * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] { 0.05, 0.0, 0.00 }; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + // public static final Limelight.Transform limelightTransformForPoseEstimation = + // Transform.BOTPOSE_WPIBLUE; + + // #endregion + + // #region Ports + // I think all of these are right + public static final int driveFrontLeftPort = 11; + public static final int driveFrontRightPort = 12; + public static final int driveBackLeftPort = 13; + public static final int driveBackRightPort = 14; + + public static final int turnFrontLeftPort = 1; + public static final int turnFrontRightPort = 2; + public static final int turnBackLeftPort = 3; + public static final int turnBackRightPort = 4; + // to be configured + public static final int canCoderPortFL = 1; // 0 + public static final int canCoderPortFR = 2; + public static final int canCoderPortBL = 3; + public static final int canCoderPortBR = 0; // 1 + + // #endregion + + // #region Command Constants + + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + // baby speed values, i just guessed the percent multiplier. TODO: find actual + // ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; + public static double kAlignMultiplier = 1D / 3D; + public static final double kAlignForward = 0.6; + + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This + // should be very small to avoid actually moving the + // robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion + // #region Subsystem Constants + + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; + public static final double turnIzone = .1; + + public static final double driveIzone = .1; + + public static final class Autoc { + // public static final RobotConfig robotConfig = new RobotConfig( /* + // * put in + // * Constants.Drivetrain.Auto + // */ + // false, // replan at start of path if robot not at start of path? + // false, // replan if total error surpasses total error/spike threshold? + // 1.5, // total error threshold in meters that will cause the path to be + // replanned + // 0.8 // error spike threshold, in meters, that will cause the path to be + // replanned + // ); + public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, + 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the + // angular constraints have no effect. + } + } + + public static final class Limelightc { + + public static final class Apriltag { + + } + } + + public static final double[] kP = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kI = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kD = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + + public static final double[] kS = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kV = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kA = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kG = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + + public static final class OI { + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + + public static final class Driver { + public static final int driverPort = 0; + // public static final int EFFECTOR_TOP_MOTOR_ID = 31; + // public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; + // public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; + // public static final int EFFECTOR_ARM_MOTOR_ID = 34; + // public static final int effectorDistanceSensorID = 5; + /* + * public static final int A = 1; + * public static final int B = 2; + * public static final int X = 3; + * public static final int Y = 4; + */ + // Not neccesary + public static final int leftBumper = 5; + public static final int rightBumper = 6; + + public static final int slowDriveButton = Button.kLeftBumper.value; + public static final int resetFieldOrientationButton = Button.kRightBumper.value; + public static final int toggleFieldOrientedButton = Button.kStart.value; + + public static final int y = Button.kY.value; + public static final int b = Button.kB.value; + public static final int a = Button.kA.value; + public static final int x = Button.kX.value; + } + + public static final class Manipulator { + public static final int manipulatorPort = 2; + // public static final int X = 0; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; + public static final int OuttakeBumper = Button.kRightBumper.value; + public static final int INTAKE_BUMPER = Button.kLeftBumper.value; + } + + public static final double JOY_THRESH = 0.01; + + } + + public static final class Elevatorc { + // ports + public static final int masterPort = 20; + public static final int followerPort = 21; // inverted + //public static final int elevatorTopLimitSwitchPort = 1; + public static final int elevatorBottomLimitSwitchPort = 0; + public static final double GEAR_RATIO = 1.0/20; //TODO: CHANGE TO ACTUAL GEAR RATIO + + // Config + // TODO figure these parts out + public static final double MAX_ACCEL_RAD_P_S = 1; + public static final IdleMode masterIdleMode = IdleMode.kCoast; //TODO: Change back to break + public static final IdleMode followerIdleMode = IdleMode.kCoast; + public static final boolean masterInverted = false; + public static final boolean followerInverted = true; + public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)*1.0/60); + public static final double maxElevatorHeightInches = 52.5; + public static final double minElevatorHeightInches = 0; + + //PID + public static final double kP = 40;//45.476; + public static final double kI = 0; + public static final double kD = 10.609; + //Feedforward + public static final double kS = 0.12666; + public static final double kG = 0.177; + public static final double kV = 8.9921; + public static final double kA = 1.4586; + //Positions + public static final double downPos = 0; + public static final double l1 = 0; + public static final double l2 = 6.5-1.236220; //inches + public static final double l3 = 22.5-1.236220; //inches + public static final double l4 = 52.64-1.236220; + public static final double net = 53.2-1.236220; + public static final double processor = 0; + public static final double bottomAlgaeRemoval = 22.5-1.236220; + public static final double uppperAlgaeRemoval = 38.35-1.236220; + //ScoreENUM + public enum ElevatorPos { + DOWN(downPos), + L1(l1), + L2(l2), + L3(l3), + L4(l4), + NET(net), + PROCESSOR(processor), + BOTTOMALGAE(bottomAlgaeRemoval), + UPPERALGAE(uppperAlgaeRemoval); + + public final double positionInches; + + ElevatorPos(double positionInches) { + this.positionInches = positionInches; + } + + public double getPositioninMeters() { + return Units.degreesToRadians(positionInches); + } + } + + // Tolerance + public static final double elevatorTolerance = 0.4; + + } + + public static final class AlgaeEffectorc { + + // EFFECTOR + + public static final int UPPER_MOTOR_PORT = 32; + public static final int LOWER_MOTOR_PORT =33; + public static final int PINCH_MOTOR_PORT = 31; + public static final int ARM_MOTOR_PORT = 34; + public static final int aChannelEnc = 0; + public static final int bChannelEnc = 1; + + public static final int TOP_ARRAY_ORDER = 0; + public static final int BOTTOM_ARRAY_ORDER = 1; + public static final int PINCHER_ARRAY_ORDER = 2; + public static final int ARM_ARRAY_ORDER = 3; + // the ArrayOrder variables replaced the ones for the kS since they just + // indicate the order and are the same for all PID values + // TODO find these values out vvv + public static double INTAKE_TOP_RPM = 1000; + public static double INTAKE_BOTTOM_RPM = 1000; + public static double INTAKE_PINCHER_RPM = 1000; + + public static double OUTTAKE_TOP_RPM = -2100; + public static double OUTTAKE_BOTTOM_RPM = -2100; + public static double OUTTAKE_PINCHER_RPM = -2100; + + public static double SHOOT_TOP_RPM = -2100;// ask design + public static double SHOOT_BOTTOM_RPM = -2100; + public static double SHOOT_PINCHER_RPM = -2100; + + public static double DEALGAFY_TOP_RPM = 1000; + public static double DEALGAFY_BOTTOM_RPM = 1000; + public static double DEALGAFY_PINCHER_RPM = 1000; + + public static final int TBE_CPR = 8192; // Through-Bore Encoder + public static final double TBE_DPP = 360.0 / TBE_CPR; // Degrees per pulse + public static final boolean invertedTBE = false; // if the encoder needs to read invertedly + public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; + + public static final double ARM_CHAIN_GEARING = 16.0 / 34; + public static final double ARM_GEAR_RATIO = 1.0 / 3; + // TODO figure the zero out once encoder is on + public static final double ARM_TO_ZERO = 0; // Pure vertical down + // TODO ask samo for angle to intake algae from pure vertical down + public static final double ARM_INTAKE_ANGLE = 0; + // TODO Figure these two out if we will be shooting algae + public static final double ARM_RAMP_UP_ANGLE = 0; + public static final double ARM_SHOOT_ANGLE = 0; + // TODO Figure angle for dealgafying + public static final double ARM_DEALGAFYING_ANGLE = 0; + // TODO figure out resting angle of the arm while algae inside + public static final double ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE = 0.0; + // TODO figure out current threshold for pincher wheels + public static final double PINCHER_CURRENT_THRESHOLD = 15.0; + + public static final double UPPER_ANGLE_LIMIT = 0; + public static final double LOWER_ANGLE_LIMIT = -70; + public static final double ROTATION_TO_DEG = 360; + public static final double ARM_DISCONT_DEG = -35; + + public static final double ARM_ERROR_MARGIN = 1; + + } + } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..c672745 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -5,88 +5,319 @@ package org.carlmontrobotics; //199 files -// import org.carlmontrobotics.subsystems.*; -// import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.subsystems.*; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; + +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator; +import org.carlmontrobotics.commands.*; +import static org.carlmontrobotics.Constants.OI.Manipulator.*; + +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; + +import java.util.ArrayList; +import java.util.List; + +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.wpilibj.DriverStation; //controllers import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; - +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands 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.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - +import edu.wpi.first.wpilibj2.command.WaitCommand; //control bindings import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class RobotContainer { + private static boolean babyMode = false; + + // 1. using GenericHID allows us to use different kinds of controllers + // 2. Use absolute paths from constants to reduce confusion + public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); + public final GenericHID manipulatorController = new GenericHID(Manipulator.manipulatorPort); + + //private final Drivetrain drivetrain = new Drivetrain(); + + /* These are assumed to be equal to the AUTO ames in pathplanner */ + /* These must be equal to the pathPlanner path names from the GUI! */ + // Order matters - but the first one is index 1 on the physical selector - index + // 0 is reserved for + // null command. + // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 + // is simplePz + // straight + // private List autoCommands; + // private SendableChooser autoSelector = new SendableChooser(); + + // private boolean hasSetupAutos = false; + // private final String[] autoNames = new String[] {}; + // private final AlgaeEffector algaeEffector = new AlgaeEffector(); + private final Elevator elevator = new Elevator(); + + public RobotContainer() { + { + // Put any configuration overrides to the dashboard and the terminal + // SmartDashboard.putData("CONFIG overrides", Config.CONFIG); + // SmartDashboard.putData(drivetrain); + // System.out.println(Config.CONFIG); + + // SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); + + // SmartDashboard.setDefaultBoolean("babymode", babyMode); + // SmartDashboard.setPersistent("babymode"); + // // safe auto setup... stuff in setupAutos() is not safe to run here - will break + // // robot + // registerAutoCommands(); + // SmartDashboard.putData(autoSelector); + // SmartDashboard.setPersistent("SendableChooser[0]"); + + // autoSelector.addOption("Nothing", 0); + // autoSelector.addOption("Raw Forward", 1); + // autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] + + // int i = 3; + // for (String n : autoNames) { + // autoSelector.addOption(n, i); + // i++; + // } + + // ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); + // autoSelectorTab.add(autoSelector).withSize(2, 1); + } + + setDefaultCommands(); + setBindingsDriver(); + setBindingsManipulator(); + } + + private void setDefaultCommands() { + // drivetrain.setDefaultCommand(new TeleopDrive(drivetrain, + // () -> ProcessedAxisValue(driverController, Axis.kLeftY), + // () -> ProcessedAxisValue(driverController, Axis.kLeftX), + // () -> ProcessedAxisValue(driverController, Axis.kRightX), + // () -> driverController.getRawButton(OI.Driver.slowDriveButton), elevator)); + elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); + } + + private void setBindingsDriver() { + // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new + // RunAlgae(algaeEffector, 1, false)); //wrong + // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new + // RunAlgae(algaeEffector, 2, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new + // RunAlgae(algaeEffector, 3, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new + // RunAlgae(algaeEffector, 0, true)); + new JoystickButton(driverController, 3).whileTrue(elevator.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); //press x to run + new JoystickButton(driverController, 4).whileTrue(elevator.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); //press y to run + new JoystickButton(driverController, 1).whileTrue(elevator.sysIdDynamic(SysIdRoutine.Direction.kForward)); //press a to run + new JoystickButton(driverController, 2).whileTrue(elevator.sysIdDynamic(SysIdRoutine.Direction.kReverse)); //press b to run + + } + + private void setBindingsManipulator() { + // intake + // new JoystickButton(manipulatorController, INTAKE_BUMPER) + // .whileTrue(new SequentialCommandGroup( + // new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), + // new GroundIntakeAlgae(algaeEffector))); + + // outake + + // dealgify + + // new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) + // .whileFalse(new OuttakeAlgae(algaeEffector)); + + // new JoystickButton(manipulatorController, XboxController.Button.kA.value) + // .onTrue(new DealgaficationIntake(algaeEffector)); + + // new JoystickButton(manipulatorController, XboxController.Button.kB.value) + // .onTrue(new ShootAlgae(algaeEffector)); + // new JoystickButton(manipulatorController, XboxController.Button.kY.value) + // .onTrue(new InstantCommand(() -> { + // algaeEffector.stopMotors(); + // })); + } + + /** + * Flips an axis' Y coordinates upside down, but only if the select axis is a + * joystick axis + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double getStickValue(GenericHID hid, Axis axis) { + return hid.getRawAxis(axis.value) + * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); + } + + /** + * Processes an input from the joystick into a value between -1 and 1, + * sinusoidally instead of + * linearly + * + * @param value The value to be processed. + * @return The processed value. + */ + private double inputProcessing(double value) { + double processedInput; + // processedInput = + // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); + processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) + * ((1 - Math.cos(value * Math.PI)) / 2), value); + return processedInput; + } + + /** + * Combines both getStickValue and inputProcessing into a single function for + * processing joystick + * outputs + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double ProcessedAxisValue(GenericHID hid, Axis axis) { + return DeadzonedAxis(inputProcessing(getStickValue(hid, axis))); + } + + /** + * Returns zero if a axis input is inside the deadzone + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double DeadzonedAxis(double axOut) { + return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut; + } + + /** + * Returns a new instance of Trigger based on the given Joystick and Axis + * objects. The Trigger is + * triggered when the absolute value of the stick value on the specified axis + * exceeds a minimum + * threshold value. + * + * @param stick The Joystick object to retrieve stick value from. + * @param axis The Axis object to retrieve value from the Joystick. + * @return A new instance of Trigger based on the given Joystick and Axis + * objects. * @throws + * NullPointerException if either stick or axis is null. + */ + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger(() -> Math + .abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); + } + + private void registerAutoCommands() { + //// AUTO-USABLE COMMANDS + // NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); + } + + private void setupAutos() { + //// CREATING PATHS from files + // if (!hasSetupAutos) { + // autoCommands = new ArrayList();// clear old/nonexistent autos + + // for (int i = 0; i < autoNames.length; i++) { + // String name = autoNames[i]; + + // autoCommands.add(new PathPlannerAuto(name)); + + /* + * // Charles' opinion: we shouldn't have it path find to the starting pose at + * the start of match + * new SequentialCommandGroup( + * AutoBuilder.pathfindToPose( + * PathPlannerAuto.getStaringPoseFromAutoFile(name), + * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0). + * getPreviewStartingHolonomicPose(), + * Autoc.pathConstraints), + * new PathPlannerAuto(name)); + */ + //} + // hasSetupAutos = true; + + // // NOTHING + // autoCommands.add(0, new PrintCommand("Running NULL Auto!")); + // // RAW FORWARD command + // // autoCommands.add(1, new SequentialCommandGroup( + // // new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new + // // WaitCommand(0.5), + // // new LastResortAuto(drivetrain))); + // // dumb PP forward command + // autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); + // } + // // force regeneration each auto call + // autoCommands.set(2, constructPPSimpleAuto());// overwrite this slot each time auto runs + } + + public Command constructPPSimpleAuto() { + /** + * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd + * (m/s): 4.30 + * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max + * AngAccel: 360 + * (guesses!) + */ + // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER + // Pose2d currPos = drivetrain.getPose(); + + // FIXME running red PP file autos seems to break something, so the robot + // drivetrain drives in the wrong direction. + // running blue PP autos is fine though + // Note: alliance detection and path generation work correctly! + // Solution: Redeploy after auto. + // Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) + // ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) + // : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); + + // List bezierPoints = PathPlannerPath.waypointsFromPoses(currPos, endPos); + + // // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, + // // rad/s^2 */ + // PathPlannerPath path = new PathPlannerPath(bezierPoints, + // Autoc.pathConstraints, null, new GoalEndState(0, currPos.getRotation())); + + // path.preventFlipping = false;// don't flip, we do that manually already. + + // return new SequentialCommandGroup( + // new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), // align drivetrain wheels. + // AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); + return new PrintCommand("I HAT EEEYTHI"); + } + + public Command getAutonomousCommand() { + // setupAutos(); + + // Integer autoIndex = autoSelector.getSelected(); - //1. using GenericHID allows us to use different kinds of controllers - //2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); - - public RobotContainer() { - - setDefaultCommands(); - setBindingsDriver(); - setBindingsManipulator(); - } - - private void setDefaultCommands() { - // drivetrain.setDefaultCommand(new TeleopDrive( - // drivetrain, - // () -> ProcessedAxisValue(driverController, Axis.kLeftY)), - // () -> ProcessedAxisValue(driverController, Axis.kLeftX)), - // () -> ProcessedAxisValue(driverController, Axis.kRightX)), - // () -> driverController.getRawButton(OI.Driver.slowDriveButton) - // )); - } - private void setBindingsDriver() {} - private void setBindingsManipulator() {} - - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } - - /** - * Flips an axis' Y coordinates upside down, but only if the select axis is a joystick axis - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double getStickValue(GenericHID hid, Axis axis) { - return hid.getRawAxis(axis.value) * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); - } - /** - * Processes an input from the joystick into a value between -1 and 1, sinusoidally instead of linearly - * - * @param value The value to be processed. - * @return The processed value. - */ - private double inputProcessing(double value) { - double processedInput; - // processedInput = - // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); - processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) * ((1 - Math.cos(value * Math.PI)) / 2), - value); - return processedInput; - } - /** - * Combines both getStickValue and inputProcessing into a single function for processing joystick outputs - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double ProcessedAxisValue(GenericHID hid, Axis axis){ - return inputProcessing(getStickValue(hid, axis)); - } + // if (autoIndex != null && autoIndex != 0) { + // new PrintCommand("Running selected auto: " + autoSelector.toString()); + // return autoCommands.get(autoIndex.intValue()); + // } + return new PrintCommand("No auto :("); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java new file mode 100644 index 0000000..b167cec --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java @@ -0,0 +1,35 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj2.command.Command; + + +public class ArmToPosition extends Command{ + AlgaeEffector algaeArm; + private double goalPosition; //radians - need angle for dealgfication and ground pickup + + + public ArmToPosition(AlgaeEffector algaeArm, double goalPosition){ + addRequirements(this.algaeArm = algaeArm); + this.goalPosition = goalPosition; + + } + + @Override + public void initialize(){ + algaeArm.setArmTarget(goalPosition); //sets target to position in rads + + + } + public void execute(){ + + } + public void end(){ + + } + public boolean isFinished(){ + return algaeArm.armAtGoal(); + + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java b/src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java new file mode 100644 index 0000000..89be36a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java @@ -0,0 +1,47 @@ +package org.carlmontrobotics.commands; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.util.Units; + + +public class DealgaficationIntake extends Command { + AlgaeEffector algae; + private final Timer timer = new Timer(); + public DealgaficationIntake(AlgaeEffector algae) { + addRequirements(this.algae = algae); + } + + @Override + public void initialize() { + //algae.setArmPosition(ARM_DEALGAFYING_ANGLE); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //call command arm toposition + + algae.setTopRPM(DEALGAFY_TOP_RPM); + algae.setBottomRPM(DEALGAFY_BOTTOM_RPM); + algae.setPincherRPM(DEALGAFY_PINCHER_RPM); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + algae.stopMotors(); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.getFPGATimestamp()>5 || algae.isAlgaeIntaked(); + //return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java new file mode 100644 index 0000000..11241b6 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java @@ -0,0 +1,27 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorLPos extends Command { + private Elevator elevator; + private ElevatorPos pos; + public ElevatorLPos(Elevator elevator, ElevatorPos pos) { + this.elevator = elevator; + this.pos = pos; + addRequirements(elevator); + } + @Override + public void initialize() { + + } + @Override + public void execute() { + elevator.setGoal(pos); + } + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java new file mode 100644 index 0000000..81e9678 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java @@ -0,0 +1,59 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class GroundIntakeAlgae extends Command { + private final AlgaeEffector algae; + private final Timer timer = new Timer(); + private boolean done = false; + public GroundIntakeAlgae(AlgaeEffector algae) { + addRequirements(this.algae = algae); + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + //algae.setArmPosition(ARM_INTAKE_ANGLE); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) + + algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); + algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + + /* + * if (algae.isAlgaeIntaked()) { + algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); + done = true; + } + */ + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + algae.stopMotors(); + timer.stop(); + //TODO: Test different times + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return done || timer.getFPGATimestamp()>5; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java new file mode 100644 index 0000000..85a5404 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -0,0 +1,52 @@ +package org.carlmontrobotics.commands; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + + + +public class OuttakeAlgae extends Command { + AlgaeEffector algae; + private final Timer timer = new Timer(); + public OuttakeAlgae(AlgaeEffector algaeEffector) { + addRequirements(this.algae = algaeEffector); + } + + @Override + public void initialize() { + //algae.setArmPosition(ARM_INTAKE_ANGLE); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + algae.setTopRPM(OUTTAKE_TOP_RPM); + algae.setBottomRPM(OUTTAKE_BOTTOM_RPM); + algae.setPincherRPM(OUTTAKE_PINCHER_RPM); + + // if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreestoradian(5)) { + // Algae.setTopRPM(-1 * Constants.AlgaeEffectorc.intakeTopRPM); + // Algae.setBottomRPM(-1 * Constants.AlgaeEffectorc.intakeBottomRPM); + // Algae.setPincherRPM(-1 * Constants.AlgaeEffectorc.intakePincherRPM); + // } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + algae.stopMotors(); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() {//FIXME DO THIS + return timer.getFPGATimestamp()>3; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java new file mode 100644 index 0000000..d1fb25b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java @@ -0,0 +1,51 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj2.command.Command; + +public class RotateToFieldRelativeAngle extends Command { + + public final TeleopDrive teleopDrive; + public final Drivetrain drivetrain; + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public RotateToFieldRelativeAngle(Rotation2d angle, Drivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + rotationPID.setSetpoint(MathUtil.inputModulus(angle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + // SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + //SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + //SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java new file mode 100644 index 0000000..cc988e5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -0,0 +1,48 @@ +package org.carlmontrobotics.commands; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.util.Units; + + +public class ShootAlgae extends Command { + AlgaeEffector algae; + private final Timer timer = new Timer(); + public ShootAlgae(AlgaeEffector algae) { + addRequirements(this.algae = algae); + } + + @Override + public void initialize() { + //algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + timer.reset(); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + + algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + algae.setPincherRPM(Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + algae.stopMotors(); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.getFPGATimestamp() > 3; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java new file mode 100644 index 0000000..74877d0 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -0,0 +1,175 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE; +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Elevatorc; +import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.Robot; +import org.carlmontrobotics.subsystems.Drivetrain; +import static org.carlmontrobotics.RobotContainer.*; +import org.carlmontrobotics.subsystems.Elevator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class TeleopDrive extends Command { + + private static double robotPeriod = Robot.kDefaultPeriod; + private final Drivetrain drivetrain; + private DoubleSupplier fwd; + private DoubleSupplier str; + private DoubleSupplier rcw; + private BooleanSupplier slow; + private double currentForwardVel = 0; + private double currentStrafeVel = 0; + private double prevTimestamp; + private boolean babyMode; + Elevator elevator; + AlgaeEffector arm; + private double height; + /** + * Creates a new TeleopDrive. + */ + + + public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow, Elevator elevator) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + this.elevator = elevator; + this.height = elevator.getCurrentHeight(); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); + // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); + // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); + // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); + prevTimestamp = Timer.getFPGATimestamp(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentTime = Timer.getFPGATimestamp(); + robotPeriod = currentTime - prevTimestamp; + double[] speeds = getRequestedSpeeds(); + // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); + prevTimestamp = currentTime; + + babyMode = SmartDashboard.getBoolean("babymode", false); + + // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); + // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); + // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); + // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); + + // SmartDashboard.putNumber("fwd", speeds[0]); + // SmartDashboard.putNumber("strafe", speeds[1]); + // SmartDashboard.putNumber("turn", speeds[2]); + drivetrain.drive(speeds[0], speeds[1], speeds[2]); + } + + public double[] getRequestedSpeeds() { + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, + double forward = fwd.getAsDouble(); + double strafe = str.getAsDouble(); + double rotateClockwise = rcw.getAsDouble(); + // SmartDashboard.putNumber("fwdIN", forward); + // SmartDashboard.putNumber("strafeIN", strafe); + // SmartDashboard.putNumber("turnIN", rotateClockwise); + if (Math.abs(forward) <= Constants.OI.JOY_THRESH) + forward = 0.0; + else + forward *= maxForward; + if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) + strafe = 0.0; + else + strafe *= maxStrafe; + if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) + rotateClockwise = 0.0; + else + rotateClockwise *= maxRCW; + + double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; + double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; + if(babyMode == true){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } + + if( height >= Elevatorc.l1 && arm.getArmPos() >= ARM_INTAKE_ANGLE) + { + rotationMultiplier = 0; + strafe = 0; + } + + + + + // double driveMultiplier = kNormalDriveSpeed; + // double rotationMultiplier = kNormalDriveRotation; + + forward *= driveMultiplier; + strafe *= driveMultiplier; + rotateClockwise *= rotationMultiplier; + + // Limit acceleration of the robot + double accelerationX = (forward - currentForwardVel) / robotPeriod; + double accelerationY = (strafe - currentStrafeVel) / robotPeriod; + double translationalAcceleration = Math.hypot(accelerationX, accelerationY); + // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); + if (translationalAcceleration > autoMaxAccelMps2 && false) { + Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); + currentForwardVel += limitedVelocityVector.getX(); + currentStrafeVel += limitedVelocityVector.getY(); + } else { + currentForwardVel = forward; + currentStrafeVel = strafe; + } + // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); + + // ATM, there is no rotational acceleration limit + // currentForwardVel = forward; + // currentStrafeVel = strafe; + // If the above math works, no velocity should be greater than the max velocity, + // so we don't need to limit it. + + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; + } + + public boolean hasDriverInput() { + return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH; + } + + // 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; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java new file mode 100644 index 0000000..2adf52f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java @@ -0,0 +1,64 @@ +package org.carlmontrobotics.commands; + +import java.util.function.DoubleSupplier; + +import static org.carlmontrobotics.Constants.Elevatorc.*; +import org.carlmontrobotics.subsystems.Elevator; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class TeleopElevator extends Command { + private Elevator elevator; + private DoubleSupplier joystickSupplier; + private TrapezoidProfile.State goalState; + private double lastTime; + public TeleopElevator(Elevator elevator, DoubleSupplier joystickSupplier) { + this.joystickSupplier = joystickSupplier; + addRequirements(this.elevator = elevator); + } + @Override + public void initialize() { + goalState = new TrapezoidProfile.State(elevator.getCurrentHeight(), elevator.getEleVel()); + lastTime = Timer.getFPGATimestamp(); + } + public double getReqSpeeds() { + return MAX_ACCEL_RAD_P_S*joystickSupplier.getAsDouble(); //(MAX_ACCEL in radians/s^2 times joystick) + } + @Override + public void execute() { + double speeds = getReqSpeeds(); + //SmartDashboard.putNumber("speeds", speeds); + + if (speeds == 0) {// if no input, don't set any goals. + lastTime = Timer.getFPGATimestamp();// update deltaT even when not running + return; + } + + double currTime = Timer.getFPGATimestamp(); + double deltaT = currTime - lastTime;// only move by a tick of distance at once + lastTime = currTime; + + double goalEleRad = goalState.position + speeds * deltaT;// speed*time = dist + + goalEleRad = MathUtil.clamp(goalEleRad, minElevatorHeightInches, maxElevatorHeightInches); + // goalArmRad = MathUtil.clamp(goalArmRad, + // armSubsystem.getArmPos() + Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S, + // armSubsystem.getArmPos() - Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S); + + goalState.position = goalEleRad; + goalState.velocity = 0; + // don't put in constants bc it's always zero + elevator.setGoal(goalState.position); + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java new file mode 100644 index 0000000..d5bb99d --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -0,0 +1,240 @@ +package org.carlmontrobotics.subsystems; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.commands.DealgaficationIntake; +import org.carlmontrobotics.commands.GroundIntakeAlgae; +import org.carlmontrobotics.commands.OuttakeAlgae; +import org.carlmontrobotics.commands.ShootAlgae; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.math.MathUtil; +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 static org.carlmontrobotics.Constants.AlgaeEffectorc.*; +import static org.carlmontrobotics.Constants.*; + +public class AlgaeEffector extends SubsystemBase { + + // motors + private final SparkFlex topMotor = new SparkFlex(UPPER_MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex bottomMotor = new SparkFlex(LOWER_MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex pincherMotor = new SparkFlex(PINCH_MOTOR_PORT, MotorType.kBrushless); + private final SparkMax armMotor = new SparkMax(ARM_MOTOR_PORT, MotorType.kBrushless); + + private SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); + private SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + private SparkFlexConfig topMotorConfig = new SparkFlexConfig(); + private SparkMaxConfig armMotorConfig = new SparkMaxConfig(); + + private final RelativeEncoder topEncoder = topMotor.getEncoder(); + private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); + private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); + private final RelativeEncoder armEncoder = armMotor.getEncoder(); + private final RelativeEncoder armAbsoluteEncoder = armMotor.getAlternateEncoder(); + + private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); + + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[TOP_ARRAY_ORDER], + kV[TOP_ARRAY_ORDER], kA[TOP_ARRAY_ORDER]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[BOTTOM_ARRAY_ORDER], + kV[BOTTOM_ARRAY_ORDER], kA[BOTTOM_ARRAY_ORDER]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[PINCHER_ARRAY_ORDER], + kV[PINCHER_ARRAY_ORDER], kA[PINCHER_ARRAY_ORDER]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(kS[ARM_ARRAY_ORDER], + kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); + // feedforward for arm was added + + // Arm Trapezoid Profile + private TrapezoidProfile armTrapProfile; + private TrapezoidProfile.State armGoalState = new TrapezoidProfile.State(0, 0); // position,velocity (0,0) + + private static double kDt; + private TrapezoidProfile.State setPoint; + + private double armFeedVolts; + private double kG; + // private final ArmFeedforward armFeed = new + // ArmFeedforward(kS[ARM_ARRAY_ORDER], kG[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], + // kA[ARM_ARRAY_ORDER]); + + // -------------------------------------------------------------------------------------------- + public AlgaeEffector() { + configureMotors(); + kDt = 0.02; + setPoint = getArmState(); + + } + // ---------------------------------------------------------------------------------------- + + private void configureMotors() { + topMotorConfig.closedLoop.pid( + Constants.kP[TOP_ARRAY_ORDER], + Constants.kI[TOP_ARRAY_ORDER], + Constants.kD[TOP_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + bottomMotorConfig.closedLoop.pid( + Constants.kP[BOTTOM_ARRAY_ORDER], + Constants.kI[BOTTOM_ARRAY_ORDER], + Constants.kD[BOTTOM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + pincherMotorConfig.closedLoop.pid( + Constants.kP[PINCHER_ARRAY_ORDER], + Constants.kI[PINCHER_ARRAY_ORDER], + Constants.kD[PINCHER_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + armMotorConfig.closedLoop.pid( + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + armMotorConfig.idleMode(IdleMode.kBrake); + armMotorConfig.closedLoop.pid( + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + armMotorConfig.encoder.positionConversionFactor(ROTATION_TO_DEG); + } + + public void setTopRPM(double toprpm) { + pidControllerTop.setReference(toprpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + topFeedforward.calculate(toprpm); + } + + public void setBottomRPM(double bottomrpm) { + pidControllerBottom.setReference(bottomrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + bottomFeedforward.calculate(bottomrpm); + } + + public void setPincherRPM(double pincherrpm) { + pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(pincherrpm); + } + + // arm methods + // drives arm from set point to goal position + public void setArmPosition() { + + setPoint = getArmState(); + armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); + + armFeedVolts = armFeedforward.calculate(armGoalState.position, armGoalState.velocity); + if ((getArmPos() < LOWER_ANGLE_LIMIT) + || (getArmPos() > UPPER_ANGLE_LIMIT)) { + armFeedVolts = armFeedforward.calculate(getArmPos(), 0); + + } + + pidControllerArm.setReference(armGoalState.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, + armFeedVolts); + // ((setPoint.position),ControlType.kPosition,armFeedVolts); + } + + // use trapezoid + public void setArmTarget(double targetPos) { + armGoalState.position = getArmClappedGoal(targetPos); + armGoalState.velocity = 0; + } + + // returns the arm position and velocity based on encoder and position + public TrapezoidProfile.State getArmState() { + TrapezoidProfile.State armState = new TrapezoidProfile.State(getArmPos(), getArmVel()); + return armState; + } + // overload + public boolean armAtGoal() { + return Math.abs(getArmPos() - armGoalState.position) <= ARM_ERROR_MARGIN; + } + + public double getArmClappedGoal(double goalAngle) { + return MathUtil.clamp( + MathUtil.inputModulus(goalAngle, ARM_DISCONT_DEG, + ARM_DISCONT_DEG + 360), + LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT); + } + + public double getArmPos() { + // figures out the position of the arm in degrees based off pure vertical down + // TODO update the arm to get in degrees after someone will figure out what the + // .getPosition gets for the TBE + return armAbsoluteEncoder.getPosition() * ARM_CHAIN_GEARING - ARM_TO_ZERO; + } + + public double getArmVel() { + return armAbsoluteEncoder.getVelocity(); + } + + public void stopArm() { + armMotor.set(0); + + } + + // public double getArmVel() { + // return armAbsoluteEncoder.getVelocity(); + // } + + public void runRPM() { + // TODO: Change RPM according to design + setTopRPM(1000); + setBottomRPM(2100); + setPincherRPM(2100); + } + + public void stopMotors() { + setTopRPM(0); + setBottomRPM(0); + setPincherRPM(0); + } + + public boolean checkIfAtTopRPM(double rpm) { + return topEncoder.getVelocity() == rpm; + } + + public boolean checkIfAtBottomRPM(double rpm) { + return bottomEncoder.getVelocity() == rpm;// give like a 5% error or somethin + } + + public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpeed) { + topMotor.set(topSpeed); + bottomMotor.set(bottomSpeed); + pincherMotor.set(pincherSpeed); + } + + public boolean isAlgaeIntaked() { + return pincherMotor.getOutputCurrent() > PINCHER_CURRENT_THRESHOLD; + } + + @Override + public void periodic() { + setArmPosition(); + SmartDashboard.putNumber("Arm Angle", getArmPos()); + SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); + SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); + SmartDashboard.putNumber("Arm Velocity", getArmVel()); + SmartDashboard.putData("Dealgafication", new DealgaficationIntake(this));// need params for these + SmartDashboard.putData("Intake Algae", new GroundIntakeAlgae(this)); + SmartDashboard.putData("Outtake Algae", new OuttakeAlgae(this)); + SmartDashboard.putData("Shoot Algae", new ShootAlgae(this)); + + } + +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..f6c83ba --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,1151 @@ +//Resolve 71 errors starting from Holonomics + +package org.carlmontrobotics.subsystems; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.Arrays; +import java.util.Map; +import java.util.function.Supplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Drivetrainc; +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Robot; +// import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; +import org.carlmontrobotics.commands.TeleopDrive; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.MotorErrors; +import org.carlmontrobotics.lib199.SensorFactory; +import org.carlmontrobotics.lib199.swerve.SwerveModule; +import static org.carlmontrobotics.Config.CONFIG; + +import com.ctre.phoenix6.hardware.CANcoder; +//import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; + +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +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.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +// import edu.wpi.first.units.Angle; +// import edu.wpi.first.units.Distance; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +// import edu.wpi.first.units.Velocity; +// import edu.wpi.first.units.Voltage; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Meter; +import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; +// Make sure this code is extraneous +// import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +public class Drivetrain extends SubsystemBase { + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); + // ^used by PathPlanner for chaining paths + + private SwerveDriveKinematics kinematics = null; + // private SwerveDriveOdometry odometry = null; + private SwerveDrivePoseEstimator poseEstimator = null; + + private SwerveModule modules[]; + private boolean fieldOriented = true; + private double fieldOffset = 0; + // FIXME not for permanent use!! + private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; + private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; + public final float initPitch; + public final float initRoll; + + // debug purposes + private SwerveModule moduleFL; + private SwerveModule moduleFR; + private SwerveModule moduleBL; + private SwerveModule moduleBR; + + private final Field2d field = new Field2d(); + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + + private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + double kP = 0; + double kI = 0; + double kD = 0; + public Drivetrain() { + SmartDashboard.putNumber("Goal Velocity", 0); + SmartDashboard.putNumber("kP", 0); + SmartDashboard.putNumber("kI", 0); + SmartDashboard.putNumber("kD", 0); + + // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); + // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); + // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", + // lastSetTheta); + + // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); + // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); + SmartDashboard.putNumber("GoalPos", 0); + // Calibrate Gyro + { + + double initTimestamp = Timer.getFPGATimestamp(); + double currentTimestamp = initTimestamp; + while (gyro.isCalibrating() && currentTimestamp - initTimestamp < 10) { + currentTimestamp = Timer.getFPGATimestamp(); + try { + Thread.sleep(1000);// 1 second + } catch (InterruptedException e) { + e.printStackTrace(); + break; + } + System.out.println("Calibrating the gyro..."); + } + gyro.reset(); + // this.resetFieldOrientation(); + System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); + System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + } + + // Setup Kinematics + { + // Define the corners of the robot relative to the center of the robot using + // Translation2d objects. + // Positive x-values represent moving toward the front of the robot whereas + // positive y-values represent moving toward the left of the robot. + Translation2d locationFL = new Translation2d(wheelBase / 2, trackWidth / 2); + Translation2d locationFR = new Translation2d(wheelBase / 2, -trackWidth / 2); + Translation2d locationBL = new Translation2d(-wheelBase / 2, trackWidth / 2); + Translation2d locationBR = new Translation2d(-wheelBase / 2, -trackWidth / 2); + + kinematics = new SwerveDriveKinematics(locationFL, locationFR, locationBL, locationBR); + } + + // Initialize modules + { + // initPitch = 0; + // initRoll = 0; + Supplier pitchSupplier = () -> 0F; + Supplier rollSupplier = () -> 0F; + initPitch = gyro.getPitch(); + initRoll = gyro.getRoll(); + // Supplier pitchSupplier = () -> gyro.getPitch(); + // Supplier rollSupplier = () -> gyro.getRoll(); + + + moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, + driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), + turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), + turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, + driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), + turnMotors[1] = new SparkMax(turnFrontRightPort, MotorType.kBrushless), + turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, + driveMotors[2] = new SparkMax(driveBackLeftPort, MotorType.kBrushless), + turnMotors[2] = new SparkMax(turnBackLeftPort, MotorType.kBrushless), + turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, + driveMotors[3] = new SparkMax(driveBackRightPort, MotorType.kBrushless), + turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), + turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + turnPidControllers[0] = turnMotors[0].getClosedLoopController(); + turnPidControllers[1] = turnMotors[1].getClosedLoopController(); + turnPidControllers[2] = turnMotors[2].getClosedLoopController(); + turnPidControllers[3] = turnMotors[3].getClosedLoopController(); + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + SmartDashboard.putData("Module FL",moduleFL); + SmartDashboard.putData("Module FR",moduleFR); + SmartDashboard.putData("Module BL",moduleBL); + SmartDashboard.putData("Module BR",moduleBR); + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : driveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : turnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + for (CANcoder coder : turnEncoders) { + coder.getAbsolutePosition().setUpdateFrequency(500); + coder.getPosition().setUpdateFrequency(500); + coder.getVelocity().setUpdateFrequency(500); + + } + + SmartDashboard.putData("Field", field); + + // for(SparkMax driveMotor : driveMotors) + // driveMotor.setSmartCurrentLimit(80); + + // Must call this method for SysId to run + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + } + + // odometry = new SwerveDriveOdometry(kinematics, + // Rotation2d.fromDegrees(getHeading()), getModulePositions(), + // new Pose2d()); + + poseEstimator = new SwerveDrivePoseEstimator( + getKinematics(), + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d()); + + // Setup autopath builder + //configurePPLAutoBuilder(); + // SmartDashboard.putNumber("chassis speeds x", 0); + // SmartDashboard.putNumber("chassis speeds y", 0); + + // SmartDashboard.putNumber("chassis speeds theta", 0); + // SmartDashboard.putData(this); + + } + + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + @Override + public void periodic() { + // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + // double goal = SmartDashboard.getNumber("GoalPos", 0); + // PIDController pid = new PIDController(kP, kI, kD); + // kP = SmartDashboard.getNumber("kP", 0); + // kI = SmartDashboard.getNumber("kI", 0); + // kD = SmartDashboard.getNumber("kD", 0); + //pid.setIZone(20); + //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); + // SparkMaxConfig config = new SparkMaxConfig(); + + //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); + // System.out.println(kP); + // config.closedLoop.pid(kP + // ,kI,kD); + // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); + // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + // //moduleFL.move(0.0000001, 180); + //moduleFL.move(0.01, 180); + // moduleFR.move(0.000000001, 0); + // moduleBR.move(0.0000001, 0); + // moduleFL.move(0.000001, 0); + // moduleBL.move(0.000001, 0); + // turnPidControllers[0].setReference(goal + + // , ControlType.kPosition, ClosedLoopSlot.kSlot0); + + + // 167 -> -200 + // 138 -> 360 + // for (CANcoder coder : turnEncoders) { + // SignalLogger.writeDouble("Regular position " + coder.toString(), + // coder.getPosition().getValue()); + // SignalLogger.writeDouble("Velocity " + coder.toString(), + // coder.getVelocity().getValue()); + // SignalLogger.writeDouble("Absolute position " + coder.toString(), + // coder.getAbsolutePosition().getValue()); + // } + // lobotomized to prevent ucontrollabe swerve behavior + // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); + // moduleFL.periodic(); + // moduleFR.periodic(); + // moduleBL.periodic(); + // moduleBR.periodic(); + // double goal = SmartDashboard.getNumber("bigoal", 0); + for (SwerveModule module : modules) { + //module.turnPeriodic(); + // module.turnPeriodic(); + module.periodic(); + // module.move(0, goal); + } + + // field.setRobotPose(odometry.getPoseMeters()); + + field.setRobotPose(poseEstimator.getEstimatedPosition()); + + // odometry.update(gyro.getRotation2d(), getModulePositions()); + + poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); + + // updateMT2PoseEstimator(); + + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + + // setPose(new Pose2d(getPose().getTranslation().getX(), + // getPose().getTranslation().getY(), + // Rotation2d.fromDegrees(getHeading()))); + + // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); + // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); + // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); + // SmartDashboard.putNumber("Robot Heading", getHeading()); + // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); + // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); + // SmartDashboard.putBoolean("Field Oriented", fieldOriented); + // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); + // SmartDashboard.putNumber("Compass Offset", compassOffset); + // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); + SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); + SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); + SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); + SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + for (SwerveModule module : modules) + SendableRegistry.addChild(this, module); + + builder.addBooleanProperty("Magnetic Field Disturbance", + gyro::isMagneticDisturbance, null); + builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, + fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), + null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), + null); + builder.addDoubleProperty("Pose Estimator Theta", + () -> + getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); + builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); + builder.addDoubleProperty("Pitch", gyro::getPitch, null); + builder.addDoubleProperty("Roll", gyro::getRoll, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> + this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + + } + + // #region Drive Methods + + /** + * Drives the robot using the given x, y, and rotation speed + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive + */ + public void drive(double forward, double strafe, double rotation) { + drive(getSwerveStates(forward, strafe, rotation)); + } + + public void drive(SwerveModuleState[] moduleStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxSpeed); + for (int i = 0; i < 4; i++) { + // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); + // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); + } + } + +// public void configurePPLAutoBuilder() { +// /** +// * PATHPLANNER SETTINGS +// * Robot Width (m): .91 +// * Robot Length(m): .94 +// * Max Module Spd (m/s): 4.30 +// * Default Constraints +// * Max Vel: 1.54, Max Accel: 6.86 +// * Max Angvel: 360, Max AngAccel: 180 (guesses!) +// */ +// AutoBuilder.configure( +// this::getPose, // :D +// this::setPose, // :D +// this::getSpeeds, // :D +// (ChassisSpeeds cs) -> { +// //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; +// // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); + +// drive(kinematics.toSwerveModuleStates(cs)); +// }, // :D +// new PPHolonomicDriveController( +// new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals +// new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals +// Robot.kDefaultPeriod//robot period +// ), +// Autoc.robotConfig, +// () -> { +// // Boolean supplier that controls when the path will be mirrored for the red alliance +// // This will flip the path being followed to the red side of the field. +// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE +// var alliance = DriverStation.getAlliance(); +// if (alliance.isPresent()) +// return alliance.get() == DriverStation.Alliance.Red; +// //else: +// return false; +// }, // :D +// this // :D +// ); + + + /* AutoBuilder.configureHolonomic( + () -> getPose().plus(new Transform2d(autoGyroOffset.getTranslation(),autoGyroOffset.getRotation())),//position supplier + (Pose2d pose) -> { autoGyroOffset=pose.times(-1); }, //position reset (by subtracting current pos) + this::getSpeeds, //chassisSpeed supplier + (ChassisSpeeds cs) -> drive( + cs.vxMetersPerSecond, + -cs.vyMetersPerSecond, + //flipped because drive assumes up is negative, but PPlanner assumes up is positive + cs.omegaRadiansPerSecond + ), + new HolonomicPathFollowerConfig( + new PIDConstants(drivekP[0], drivekI[0], drivekD[0], driveIzone), //translation (drive) pid vals + new PIDConstants(turnkP_avg, 0., 0., turnIzone), //rotation pid vals + maxSpeed, + swerveRadius, + Autoc.replanningConfig, + Robot.robot.getPeriod()//robot period + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + return alliance.get() == DriverStation.Alliance.Red; + //else: + return false; + }, + this + ); + */ + +// } + + public void autoCancelDtCommand() { + if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; + + // Use hasDriverInput to get around acceleration limiting on slowdown + if (((TeleopDrive) getDefaultCommand()).hasDriverInput()) { + Command currentDtCommand = getCurrentCommand(); + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof RotateToFieldRelativeAngle) + && currentDtCommand != null) { + currentDtCommand.cancel(); + } + } + } + + public void stop() { + for (SwerveModule module : modules) + module.move(0, 0); + } + + public boolean isStopped() { + return Math.abs(getSpeeds().vxMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().vyMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().omegaRadiansPerSecond) < 0.1; + } + + /** + * Constructs and returns a ChassisSpeeds objects using forward, strafe, and + * rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A ChassisSpeeds object. + */ + private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rotation) { + ChassisSpeeds speeds; + if (fieldOriented) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, rotation, + Rotation2d.fromDegrees(getHeading())); + } else { + speeds = new ChassisSpeeds(forward, strafe, rotation); + } + return speeds; + } + + /** + * Constructs and returns four SwerveModuleState objects, one for each side, + * using forward, strafe, and rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A SwerveModuleState array, one for each side of the drivetrain (FL, + * FR, etc.). + */ + private SwerveModuleState[] getSwerveStates(double forward, double strafe, double rotation) { + return kinematics.toSwerveModuleStates(getChassisSpeeds(forward, -strafe, rotation)); + } + + // #endregion + + // #region Getters and Setters + + // returns a value from -180 to 180 + public double getHeading() { + double x = gyro.getAngle(); + if (fieldOriented) + x -= fieldOffset; + return Math.IEEEremainder(x * (isGyroReversed ? -1.0 : 1.0), 360); + } + + public double getHeadingDeg() { + return getHeading();//...wait. + } + + public SwerveModulePosition[] getModulePositions() { + return Arrays.stream(modules).map(SwerveModule::getCurrentPosition).toArray(SwerveModulePosition[]::new); + } + + public Pose2d getPose() { + // return odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); + } + + private Rotation2d simGyroOffset = new Rotation2d(); + public void setPose(Pose2d initialPose) { + Rotation2d gyroRotation = gyro.getRotation2d(); + // odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + + poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); + // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future + // We need the offset so that we can compensate for it during simulationPeriodic(). + simGyroOffset = initialPose.getRotation().minus(gyroRotation); + //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); + } + + // Resets the gyro, so that the direction the robotic currently faces is + // considered "forward" + public void resetHeading() { + gyro.reset(); + } + + public double getPitch() { + return gyro.getPitch(); + } + + public double getRoll() { + return gyro.getRoll(); + } + + public boolean getFieldOriented() { + return fieldOriented; + } + + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + } + + public void resetFieldOrientation() { + fieldOffset = gyro.getAngle(); + } + + public void resetPoseEstimator() { + // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + + poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + gyro.reset(); + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(modules).map(SwerveModule::getCurrentState) + .toArray(SwerveModuleState[]::new)); + } + + public void toggleMode() { + for (SwerveModule module : modules) + module.toggleMode(); + } + + public void brake() { + for (SwerveModule module : modules) + module.brake(); + } + + public void coast() { + for (SwerveModule module : modules) + module.coast(); + } + + public double[][] getPIDConstants() { + return new double[][] { + xPIDController, + yPIDController, + thetaPIDController + }; + } + + // #region SysId Code + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; + + // Mutable holder for unit-safe linear distance values, persisted to avoid + // reallocation. + private final MutDistance[] m_distance = new MutDistance[4]; + // Mutable holder for unit-safe linear velocity values, persisted to avoid + // reallocation. + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; + // edu.wpi.first.math.util.Units.Rotations beans; + private final MutAngle[] m_revs = new MutAngle[4]; + private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; + + private enum SysIdTest { + FRONT_DRIVE, + BACK_DRIVE, + ALL_DRIVE, + // FLBR_TURN, + // FRBL_TURN, + // ALL_TURN + FL_ROT, + FR_ROT, + BL_ROT, + BR_ROT + } + + private SendableChooser sysIdChooser = new SendableChooser<>(); + + // ROUTINES FOR SYSID + // private SysIdRoutine.Config defaultSysIdConfig = new + // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(5)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), + Volts.of(2.891), Seconds.of(10)); + + // DRIVE + private void motorLogShort_drive(SysIdRoutineLog log, int id) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + + // Create a new SysId routine for characterizing the drive. + private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + // Tell SysId how to give the driving voltage to the motors. + (Voltage volts) -> { + driveMotors[0].setVoltage(volts.in(Volts)); + driveMotors[1].setVoltage(volts.in(Volts)); + modules[2].coast(); + modules[3].coast(); + }, + log -> {// FRONT + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + }, + this)); + + private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + modules[0].coast(); + modules[1].coast(); + modules[2].brake(); + modules[3].brake(); + driveMotors[2].setVoltage(volts.in(Volts)); + driveMotors[3].setVoltage(volts.in(Volts)); + }, + log -> {// BACK + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + for (SparkMax dm : driveMotors) { + dm.setVoltage(volts.in(Volts)); + } + }, + log -> { + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine sysidroutshort_turn(int id, String logname) { + return new SysIdRoutine( + defaultSysIdConfig, + // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(3)), + new SysIdRoutine.Mechanism( + (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + log -> log.motor(logname + "_turn") + .voltage(m_appliedVoltage[id + 4].mut_replace( + // ^because drivemotors take up the first 4 slots of the unit holders + turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) + .angularPosition( + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) + .angularVelocity(m_revs_vel[id].mut_replace( + turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), + this)); + } + + // as always, fl/fr/bl/br + private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { + sysidroutshort_turn(0, "fl"), // woaw, readable code??? + sysidroutshort_turn(1, "fr"), + sysidroutshort_turn(2, "bl"), + sysidroutshort_turn(3, "br") + }; + + private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); + + // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, + // int height){ + // sysIdTab.add(name, dir).withSize(width, height); + // } + void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); + } + + void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); + } + + private void sysIdSetup() { + // SysId Setup + { + Supplier stopNwait = () -> new SequentialCommandGroup( + new InstantCommand(this::stop), new WaitCommand(2)); + + /* + * Alex's old sysId tests + * sysIdTab.add("All sysid tests", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()) + * )); + */ + + sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); + sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + + sysIdTab + .add(sysIdChooser) + .withSize(2, 1); + + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); + sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); + sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); + // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); + // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); + // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); + sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); + sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); + sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); + sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); + + + sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? + .withSize(2, 1); + //sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + //sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + + sysIdTab.add("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add(this); + + for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors + m_appliedVoltage[i] = Volt.mutable(0); + } + for (int i = 0; i < 4; i++) { + m_distance[i] = Meter.mutable(0); + m_velocity[i] = MetersPerSecond.mutable(0); + + m_revs[i] = Rotation.mutable(0); + m_revs_vel[i] = RotationsPerSecond.mutable(0); + } + + // SmartDashboard.putNumber("Desired Angle", 0); + + // SmartDashboard.putNumber("kS", 0); + } + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + private SysIdTest selector() { + //SysIdTest test = sysIdChooser.getSelected(); + SysIdTest test = SysIdTest.BACK_DRIVE; + System.out.println("Test Selected: " + test); + return test; + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only quasistatic forward") + : new PrintCommand("Running front only quasistatic backward"), + frontOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only quasistatic forward") + : new PrintCommand("Running back only quasistatic backward"), + backOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all drive quasistatic forward") + : new PrintCommand("Running all drive quasistatic backward"), + allWheelsDriveRoutine.quasistatic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate quasistatic forward") + : new PrintCommand("Running FL rotate quasistatic backward"), + rotateRoutine[0].quasistatic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate quasistatic forward") + : new PrintCommand("Running FR rotate quasistatic backward"), + rotateRoutine[1].quasistatic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate quasistatic forward") + : new PrintCommand("Running BL rotate quasistatic backward"), + rotateRoutine[2].quasistatic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate quasistatic forward") + : new PrintCommand("Running BR rotate quasistatic backward"), + rotateRoutine[3].quasistatic(direction))) + + // //TURN + // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fL-bR turn quasistatic forward") : + // new PrintCommand("Running fL-bR turn quasistatic backward"), + // flbrTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fR-bL turn quasistatic forward") : + // new PrintCommand("Running fR-bL turn quasistatic backward"), + // frblTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running all turn quasistatic forward") : + // new PrintCommand("Running all turn quasistatic backward"), + // allWheelsTurn.quasistatic(direction) + // )) + ), + this::selector); + } + + // public Command sysIdDynamic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyDrive.dynamic(direction); + // case 1: + // return backOnlyDrive.dynamic(direction); + // case 2: + // return allWheelsDrive.dynamic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + private Command allTheSYSID(SysIdRoutine.Direction direction) { + return new SequentialCommandGroup( + frontOnlyDriveRoutine.dynamic(direction), + backOnlyDriveRoutine.dynamic(direction), + allWheelsDriveRoutine.dynamic(direction), + rotateRoutine[0].dynamic(direction), + rotateRoutine[1].dynamic(direction), + rotateRoutine[2].dynamic(direction), + rotateRoutine[3].dynamic(direction), + + frontOnlyDriveRoutine.quasistatic(direction), + backOnlyDriveRoutine.quasistatic(direction), + allWheelsDriveRoutine.quasistatic(direction), + rotateRoutine[0].quasistatic(direction), + rotateRoutine[1].quasistatic(direction), + rotateRoutine[2].quasistatic(direction), + rotateRoutine[3].quasistatic(direction)); + } + + public Command allTheSYSID() { + return new SequentialCommandGroup( + allTheSYSID(SysIdRoutine.Direction.kForward), + allTheSYSID(SysIdRoutine.Direction.kReverse)); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only dynamic forward") + : new PrintCommand("Running front only dynamic backward"), + frontOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only dynamic forward") + : new PrintCommand("Running back only dynamic backward"), + backOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all wheels dynamic forward") + : new PrintCommand("Running all wheels dynamic backward"), + allWheelsDriveRoutine.dynamic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate dynamic forward") + : new PrintCommand("Running FL rotate dynamic backward"), + rotateRoutine[0].dynamic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate dynamic forward") + : new PrintCommand("Running FR rotate dynamic backward"), + rotateRoutine[1].dynamic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate dynamic forward") + : new PrintCommand("Running BL rotate dynamic backward"), + rotateRoutine[2].dynamic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate dynamic forward") + : new PrintCommand("Running BR rotate dynamic backward"), + rotateRoutine[3].dynamic(direction)))), + this::selector); + } + + public void keepRotateMotorsAtDegrees(int angle) { + for (SwerveModule module : modules) { + module.turnPeriodic(); + module.move(0.0000000000001, angle); + } + } + + public double getGyroRate() { + return gyro.getRate(); + } + // #endregion +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java new file mode 100644 index 0000000..2b779c6 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -0,0 +1,359 @@ +// 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 org.carlmontrobotics.subsystems; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.math.util.Units; + +import java.lang.reflect.Method; +import java.util.Map; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; + +import static org.carlmontrobotics.Config.CONFIG; +import static org.carlmontrobotics.Constants.Drivetrainc.velocityTolerance; +import static org.carlmontrobotics.Constants.Elevatorc.*; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.pathplanner.lib.path.GoalEndState; +import com.playingwithfusion.CANVenom.BrakeCoastMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.KalmanFilterLatencyCompensator; +import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; + +public class Elevator extends SubsystemBase { + /** Creates a new Elevator. */ + //Master + private SparkMax masterMotor; + private SparkMaxConfig masterConfig = new SparkMaxConfig(); + private RelativeEncoder masterEncoder; + //Follower + private SparkMax followerMotor; + private SparkMaxConfig followerConfig = new SparkMaxConfig(); + private RelativeEncoder followerEncoder; + // Limit Switches + // private DigitalInput topLimitSwitch; no upper limit switch + private DigitalInput bottomLimitSwitch; + private double maxVelocityMetersPerSecond = 5; + //Vars + private double heightGoal; + private int elevatorState; + //PID + private PIDController pidElevatorController; + private ElevatorFeedforward feedforwardElevatorController; + // private Timer timer; + private Timer encoderTimer; + private final SysIdRoutine sysIdRoutine; + private double lastMeasuredTime; + private double currTime; + private double lastElevPos; + private double lastElevVel; + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0);//AH: its a holder, not a number. + //Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0);//AH: 2 for 2 elevator motors + //Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);//AH: ITS A HOLDER :o + private double goalHeight; + //MetersPerSecond.mutable(0); + + //AH: need a config to run a test + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(5)//AH: maximum sysID test time + ); + + private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Elevator SysID"); + + private void sysIdSetup() { + sysIdTab.add("Quasistatic backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add("Quasistatic forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dynamic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dynamic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + } + + public Elevator() { + + encoderTimer = new Timer(); + SmartDashboard.putNumber("Goal", goalHeight); + //motors + // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); + masterMotor = MotorControllerFactory.createSparkMax(masterPort, MotorConfig.NEO); + masterEncoder = masterMotor.getEncoder(); + + // followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); + followerMotor = MotorControllerFactory.createSparkMax(followerPort, MotorConfig.NEO); + followerEncoder = followerMotor.getEncoder(); + + configureMotors(); + //Calibration + // topLimitSwitch = new DigitalInput(elevatorTopLimitSwitchPort); + bottomLimitSwitch = new DigitalInput(elevatorBottomLimitSwitchPort); + // timer = new Timer(); + // timer.start(); + + + //PID + pidElevatorController = new PIDController(kP, kI, kD); + //FeedForward + feedforwardElevatorController = new ElevatorFeedforward(kS, kG, kV, kA); + + + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig,//AH: use custom voltage config + new SysIdRoutine.Mechanism( + voltage -> { + masterMotor.setVoltage(voltage); + }, + log -> { + log.motor("Elevator-Mastr")//AH: you have 2 motors, must log both + .voltage( + m_appliedVoltage + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE + .linearPosition(m_distance.mut_replace(masterEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity.mut_replace(masterEncoder.getVelocity(), MetersPerSecond));//AH: use metric units always + }, + this) + ); + + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + + } + + private void configureMotors () { + //Master Config + masterConfig + .inverted(masterInverted) + .idleMode(masterIdleMode); + masterConfig.encoder + .positionConversionFactor(masterPositionConversionFactor) + .velocityConversionFactor(masterVelocityConversionFactor); + masterConfig.closedLoop + .pid(kP, kI, kD) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + masterMotor.configure(masterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + //I don't know if this is needed. Response: Not rly. Only the follow. + //Follower Config + followerConfig.apply(masterConfig); + followerConfig.follow(masterPort, followerInverted); + followerMotor.configure(followerConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public void setGoal(double goal) { + heightGoal = goal; + } + + public void setGoal(ElevatorPos goal) { + heightGoal = goal.getPositioninMeters(); + } + + public double getGoal() { + return heightGoal; + } + + public double getCurrentHeight() { + return masterEncoder.getPosition();//conversion done in config/constants + } + + // public boolean elevatorAtMax() { + // return !topLimitSwitch.get(); + // } + + public boolean elevatorAtMin() { + return !bottomLimitSwitch.get();//limit switches are opposite + } + + // public void updateEncoders() {//what the fuck?? + // // if (elevatorAtMax()) { + // // masterEncoder.setPosition(maxElevatorHeightInches); + // // timer.reset(); + // // timer.start(); + // // } + // if (elevatorAtMin()) { + // masterEncoder.setPosition(minElevatorHeightInches); + // timer.reset(); + // timer.start(); + // } + // } + + public void goToGoal() { + System.out.println("GOing to GOAL"); + System.out.println(heightGoal); + double vel = pidElevatorController.calculate(masterEncoder.getPosition(), heightGoal); + double feed = feedforwardElevatorController.calculate(0); + masterMotor.setVoltage(vel + feed); + + } + + public void setSpeed(double speed){ + masterMotor.set(speed); + } + + public void stopElevator(){ + masterMotor.set(0); + } + + // public double getPos() { + // return masterEncoder.getPosition(); + // } + + public boolean atGoalHeight() { + // if (heightGoal == maxElevatorHeightInches) { + // return elevatorAtMax(); + // } + if (heightGoal == minElevatorHeightInches) { + return elevatorAtMin(); + } + + else { + return (Math.abs(getCurrentHeight() - heightGoal) <= elevatorTolerance); + } + + } + //private boolean isEncoderDisconnected() { + // double currentElevPos = getPos(); + // double currentRelativeElevVel = masterEncoder.getVelocity(); + + // if ((currentRelativeElevVel != 0)) { + // lastMeasuredTime = currTime; + // lastElevPos = currentElevPos; + // lastElevVel = currentRelativeElevVel; + // return false; + // } else { + // if(lastMeasuredTime > currTime+1 && lastElevPos == currentElevPos) { + // return true; + // } + // } + // // currTime - lastMeasuredTime < + // // DISCONNECTED_ENCODER_TIMEOUT_SEC; + + + // } + +//safetyMethod is used to check during sysid if the elevator height and voltage are at the safe threshold + public boolean isUNSAFE(){ + if (1.33>= masterEncoder.getPosition() + || maxVelocityMetersPerSecond <= masterEncoder.getVelocity() + || 0 <=masterEncoder.getPosition()){ + return false; + } + return true; + } + + public boolean isSAFE() { + return !isUNSAFE(); + } + + public boolean isBruh() { + if(getCurrentHeight()>1.33 || getCurrentHeight() < 0) { + return false; + + } + return true; + } + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + // BooleanSupplier bruh = Elevator::safetyMethod(); + return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isBruh()); + //use onlyWhile to decorate the command and therefore add safety limits (for height and voltage) + //TO-DO: fix safety method (add velocity) and also other bugs + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isBruh()); + } + public double getEleVel() { + return masterEncoder.getVelocity(); + } + + + @Override + public void periodic() { + + // if (elevatorAtMax()){ + // SmartDashboard.putString("ElevatorState", "🔴STOP🔴"); + // } + // //masterMotor.set(0); + // goalHeight = SmartDashboard.getNumber("Goal", 0); + // System.out.println(goalHeight); + setGoal(.75); + SmartDashboard.putBoolean("SAFE?", isBruh()); + + if (elevatorAtMin()) { + SmartDashboard.putString("ElevatorState", "🟢GO🟢"); + } + else { + SmartDashboard.putString("ElevatorState", "🟡AT MIN🟡"); + }//add one for max height + //add one for if unsafe + SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); + // SmartDashboard.putNumber("Since Calibrated", timer.get()); + // updateEncoders(); + goToGoal(); + + + if(isBruh() && masterMotor.getBusVoltage() > 0) { + //masterMotor.set(0); + System.err.println("Bad Bad nightmare bad. Elevator unsafe"); + //hey tell them they're unsafe and a bad happened + } + // if (isEncoderDisconnected()) { + // masterMotor.set(0); + + // } + } +}