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..b5ad6c3 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,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 +86,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:2025-SNAPSHOT" } 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..fdc8b05 --- /dev/null +++ b/simgui.json @@ -0,0 +1,21 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK Flex [1] RELATIVE ENCODER": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/Shuffleboard/Auto Chooser Tab/SendableChooser[0]": "String Chooser", + "/SmartDashboard/SendableChooser[0]": "String Chooser" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto new file mode 100644 index 0000000..be9f8c0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L1 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto new file mode 100644 index 0000000..e1b55e9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L2 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto new file mode 100644 index 0000000..f03d42f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L3 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto new file mode 100644 index 0000000..0a0293c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center 1 Piece L4 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Piece(2)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Forward.auto b/src/main/deploy/pathplanner/autos/Center Forward.auto new file mode 100644 index 0000000..d1af341 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto new file mode 100644 index 0000000..3905c9d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L1 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto new file mode 100644 index 0000000..85c5c4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L2 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto new file mode 100644 index 0000000..9532d4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L3 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto new file mode 100644 index 0000000..adf5d22 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Piece L4 Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(Go-Back)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto new file mode 100644 index 0000000..613872e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L1 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto new file mode 100644 index 0000000..23c27dc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L2 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto new file mode 100644 index 0000000..9ef50b0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L3 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto new file mode 100644 index 0000000..6ab6103 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Piece L4 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto new file mode 100644 index 0000000..5212094 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L1 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto new file mode 100644 index 0000000..6553275 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L2 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto new file mode 100644 index 0000000..f0f718d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L3 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto new file mode 100644 index 0000000..df9d5a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 3 Piece L4 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto new file mode 100644 index 0000000..543cf51 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L1 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto new file mode 100644 index 0000000..c32a19a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L2 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto new file mode 100644 index 0000000..dc5f19b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L3 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto new file mode 100644 index 0000000..8febe09 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 4 Piece L4 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Piece(3)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Forward.auto b/src/main/deploy/pathplanner/autos/Left Forward.auto new file mode 100644 index 0000000..3bb1da7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto new file mode 100644 index 0000000..2fa2b02 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L1 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto new file mode 100644 index 0000000..969c8a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L2 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto new file mode 100644 index 0000000..240d3ce --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L3 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto new file mode 100644 index 0000000..20da153 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Piece L4 Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "1 piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto new file mode 100644 index 0000000..08f211a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L1 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto new file mode 100644 index 0000000..7c510ca --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L2 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto new file mode 100644 index 0000000..c46a95b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L3 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto new file mode 100644 index 0000000..687fff2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Piece L4 Auto.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "2 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto new file mode 100644 index 0000000..083e375 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L1 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto new file mode 100644 index 0000000..36b2444 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L2 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto new file mode 100644 index 0000000..b44c443 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L3 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto new file mode 100644 index 0000000..39e2e31 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 3 Piece L4 Auto.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "3 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto new file mode 100644 index 0000000..54857e1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L1 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L1", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto new file mode 100644 index 0000000..7e970dc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L2 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L2", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto new file mode 100644 index 0000000..0c16c73 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L3 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L3", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto b/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto new file mode 100644 index 0000000..d003402 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 4 Piece L4 Auto.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Piece(1)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(2)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(3)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(4)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(5)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(6)" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Piece(7)" + } + } + ] + } + }, + "resetOdom": true, + "folder": "4 Piece L4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Forward.auto b/src/main/deploy/pathplanner/autos/Right Forward.auto new file mode 100644 index 0000000..d8e00b3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Forward.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Forward", + "choreoAuto": false +} \ 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/Center Forward.path b/src/main/deploy/pathplanner/paths/Center Forward.path new file mode 100644 index 0000000..bbd1b4f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.06637323943662, + "y": 3.741549295774647 + }, + "prevControl": null, + "nextControl": { + "x": 7.641197183098591, + "y": 3.755721830985915 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.790845070422535, + "y": 3.741549295774647 + }, + "prevControl": { + "x": 7.258538732394367, + "y": 3.7273767605633794 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Piece(1).path b/src/main/deploy/pathplanner/paths/Center Piece(1).path new file mode 100644 index 0000000..0af9886 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.00968309859155, + "y": 4.039172535211267 + }, + "prevControl": null, + "nextControl": { + "x": 7.759683101292217, + "y": 4.039209282099408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.770422535211268, + "y": 4.039172535211267 + }, + "prevControl": { + "x": 6.195598591549295, + "y": 4.010827464788732 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Center Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Piece(2).path b/src/main/deploy/pathplanner/paths/Center Piece(2).path new file mode 100644 index 0000000..e3b5bd0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.7987676056338024, + "y": 4.039172535211267 + }, + "prevControl": null, + "nextControl": { + "x": 6.464876760563381, + "y": 4.053345070422535 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.918397887323944, + "y": 4.039172535211267 + }, + "prevControl": { + "x": 6.450704225352113, + "y": 4.053345070422535 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -38.99099404250553 + }, + "reversed": false, + "folder": "Center Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": 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..5a181b7 --- /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": "Daniel Stuff", + "idealStartingState": { + "velocity": 0, + "rotation": -117.26958303209868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Forward.path b/src/main/deploy/pathplanner/paths/Left Forward.path new file mode 100644 index 0000000..78eed49 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.080545774647888, + "y": 7.0012323943661965 + }, + "prevControl": null, + "nextControl": { + "x": 7.414436619718311, + "y": 6.972887323943662 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 7.0012323943661965 + }, + "prevControl": { + "x": 5.75625, + "y": 7.0012323943661965 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(1).path b/src/main/deploy/pathplanner/paths/Left Piece(1).path new file mode 100644 index 0000000..998ba82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.023855633802818, + "y": 7.114612676056338 + }, + "prevControl": null, + "nextControl": { + "x": 7.47112676056338, + "y": 6.661091549295775 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.175176056338028, + "y": 5.14463028169014 + }, + "prevControl": { + "x": 5.727904929577464, + "y": 5.5131161971830975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -117.97947438848014 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(2).path b/src/main/deploy/pathplanner/paths/Left Piece(2).path new file mode 100644 index 0000000..dbde03d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.175176056338028, + "y": 5.14463028169014 + }, + "prevControl": null, + "nextControl": { + "x": 3.871302816901408, + "y": 5.754049295774647 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3344190140845058, + "y": 7.142957746478873 + }, + "prevControl": { + "x": 2.312323943661971, + "y": 6.6469190140845065 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 126.2538377374448 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -118.81079374297319 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(3).path b/src/main/deploy/pathplanner/paths/Left Piece(3).path new file mode 100644 index 0000000..e9ccd66 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(3).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3344190140845058, + "y": 7.142957746478873 + }, + "prevControl": null, + "nextControl": { + "x": 1.7737676056331966, + "y": 6.746126760569904 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8146126760563375, + "y": 5.158802816901408 + }, + "prevControl": { + "x": 3.290228873239436, + "y": 5.654841549295774 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.05460409907721 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 126.46923439005198 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(4).path b/src/main/deploy/pathplanner/paths/Left Piece(4).path new file mode 100644 index 0000000..a9f555d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(4).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8287852112676046, + "y": 5.14463028169014 + }, + "prevControl": null, + "nextControl": { + "x": 4.268133802816295, + "y": 4.747799295781171 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2352112676056324, + "y": 7.057922535211268 + }, + "prevControl": { + "x": 2.0288732394366185, + "y": 6.505193661971831 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 123.6900675259798 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 126.46923439005198 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path b/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path new file mode 100644 index 0000000..949ce82 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Piece(Go-Back).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.146830985915492, + "y": 5.187147887323943 + }, + "prevControl": null, + "nextControl": { + "x": 5.416109154929577, + "y": 5.456426056338028 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.145158450704225, + "y": 7.256338028169014 + }, + "prevControl": { + "x": 6.861707746478873, + "y": 6.972887323943662 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Left Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -119.74488129694231 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Forward.path b/src/main/deploy/pathplanner/paths/Right Forward.path new file mode 100644 index 0000000..a32fad7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.06637323943662, + "y": 0.7653169014084494 + }, + "prevControl": null, + "nextControl": { + "x": 7.570334507042254, + "y": 0.7511443661971826 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.81294014084507, + "y": 0.7653169014084494 + }, + "prevControl": { + "x": 6.450704225352113, + "y": 0.7369718309859138 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Forward", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(1).path b/src/main/deploy/pathplanner/paths/Right Piece(1).path new file mode 100644 index 0000000..dc3d7e9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(1).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.038028169014085, + "y": 1.062940140845069 + }, + "prevControl": null, + "nextControl": { + "x": 7.386091549295775, + "y": 1.3747359154929564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 2.9053697183098577 + }, + "prevControl": { + "x": 5.841285211267606, + "y": 2.4518485915492945 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -58.240519915187086 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(2).path b/src/main/deploy/pathplanner/paths/Right Piece(2).path new file mode 100644 index 0000000..86fad16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(2).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.118485915492958, + "y": 2.8486795774647873 + }, + "prevControl": null, + "nextControl": { + "x": 4.6649647887323935, + "y": 2.5652288732394357 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2068661971830972, + "y": 0.9495598591549276 + }, + "prevControl": { + "x": 1.9013204225352105, + "y": 1.3322183098591531 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -126.46923439005185 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -64.88516511385541 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(3).path b/src/main/deploy/pathplanner/paths/Right Piece(3).path new file mode 100644 index 0000000..6c4db13 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(3).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.2068661971830972, + "y": 0.9495598591549276 + }, + "prevControl": null, + "nextControl": { + "x": 1.7879401408450692, + "y": 1.2188380281690128 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.189348591549296, + "y": 2.8345070422535197 + }, + "prevControl": { + "x": 5.4201178223185265, + "y": 2.930660888407366 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -61.476881393687925 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(4).path b/src/main/deploy/pathplanner/paths/Right Piece(4).path new file mode 100644 index 0000000..9dd8363 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(4).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.1610035211267595, + "y": 2.8486795774647873 + }, + "prevControl": null, + "nextControl": { + "x": 4.126408450704225, + "y": 2.3101232394366185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2352112676056324, + "y": 0.992077464788731 + }, + "prevControl": { + "x": 1.4352112676056326, + "y": 1.1420774647887308 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -123.02386755579657 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -56.309932474020215 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(5).path b/src/main/deploy/pathplanner/paths/Right Piece(5).path new file mode 100644 index 0000000..9a57cfc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(5).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.0934859154929564, + "y": 1.0771126760563368 + }, + "prevControl": null, + "nextControl": { + "x": 3.276056338028168, + "y": 2.5085387323943653 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7720950704225347, + "y": 2.891197183098591 + }, + "prevControl": { + "x": 3.554703599534407, + "y": 2.7490248488316853 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -129.5596679689945 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(6).path b/src/main/deploy/pathplanner/paths/Right Piece(6).path new file mode 100644 index 0000000..f1553bf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(6).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8146126760563375, + "y": 2.9337147887323933 + }, + "prevControl": null, + "nextControl": { + "x": 2.90757042253521, + "y": 2.125880281690139 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2068661971830972, + "y": 1.0345950704225333 + }, + "prevControl": { + "x": 1.4477992957746464, + "y": 1.261355633802815 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -126.86989764584399 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Piece(7).path b/src/main/deploy/pathplanner/paths/Right Piece(7).path new file mode 100644 index 0000000..7db80b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Piece(7).path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3344190140845058, + "y": 0.9070422535211253 + }, + "prevControl": null, + "nextControl": { + "x": 2.7091549295774637, + "y": 1.8424295774647876 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8571302816901403, + "y": 2.8345070422535197 + }, + "prevControl": { + "x": 2.978433098591548, + "y": 2.125880281690139 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -126.86989764584399 + }, + "reversed": false, + "folder": "Right Paths", + "idealStartingState": { + "velocity": 0, + "rotation": -125.31121343963326 + }, + "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..eee380c --- /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": "Daniel Stuff", + "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..1d3b2c9 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,57 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "Center Paths", + "Daniel Stuff", + "Forward", + "Left Paths", + "Other Stuff", + "Right Paths" + ], + "autoFolders": [ + "1 Piece L1", + "1 piece L2", + "1 piece L3", + "1 piece L4", + "2 Piece L1", + "2 Piece L2", + "2 Piece L3", + "2 Piece L4", + "3 Piece L1", + "3 Piece L2", + "3 Piece L3", + "3 Piece L4", + "4 Piece L1", + "4 Piece L2", + "4 Piece L3", + "4 Piece L4", + "Forward" + ], + "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/Config.java b/src/main/java/org/carlmontrobotics/Config.java new file mode 100644 index 0000000..9a68135 --- /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 = false; + 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..02b5b57 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,15 +1,310 @@ package org.carlmontrobotics; +import org.carlmontrobotics.lib199.swerve.SwerveConfig; + +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.AngularMomentumUnit; +import edu.wpi.first.units.MassUnit; +import edu.wpi.first.units.MomentOfInertiaUnit; +import edu.wpi.first.units.measure.Mass; +import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; + +import static org.carlmontrobotics.Config.CONFIG; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; // } + + + + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + + public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final class OI { + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final double JOY_THRESH = 0.01; public static final class Driver { - public static final int port = 0; + public static final int driverPort = 0; + public static final int effectorMotorID = 4; + 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 class Manipulator { - public static final int port = 1; + 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 IntakeBumper = Button.kLeftBumper.value; } } + public static final class AlgaeEffectorc { + public static final int upperMotorID = 17; + public static final int lowerMotorID = 24; + public static final int pinchMotorID = 56; + public static final int TopkS = 1; + public static final int BottomkS = 1; + public static final int PincherkS = 1; + + public static double intakeTopRPM = 1000; + public static double intakeBottomRPM = 1000; + public static double intakePincherRPM = 1000; + + public static double outtakeTopRPM = 2100; + public static double outtakeBottomRPM = 2100; + public static double outtakePincherRPM = 2100; + + } + + 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; */ //coefficient of friction. less means less max acceleration. + public static final double ROBOTMASS_KG = 135;//max is 135kg + //moment of inertia, kg/mm + //calculated by integral of mass * radius^2 for every point of the robot + //easy way? just do total mass * radius^2 + public static final double MOI = ROBOTMASS_KG * swerveRadius*swerveRadius; + + 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 * 9.81; // 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 * 9.81 * 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( + // Mass mass, kg + ROBOTMASS_KG, + // double Moment Oof Inertia, kg/mm + MOI,//==1 + // ModuleConfig moduleConfig, + new ModuleConfig( + // double wheelRadiusMeters, + swerveRadius, + // double maxDriveVelocityMPS, + autoMaxSpeedMps, + // double wheelCOF, + mu, + // DCMotor driveMotor, + DCMotor.getNEO(4),//FIXME is it 1 for 1 each or 4 for 1 robot + // double driveGearing, + driveGearing, + // double driveCurrentLimit, + autoMaxVolt, + // int numMotors + 4 + ), + // Translation2d... moduleOffsets + new Translation2d(wheelBase / 2, trackWidth / 2), + new Translation2d(wheelBase / 2, -trackWidth / 2), + new Translation2d(-wheelBase / 2, trackWidth / 2), + new Translation2d(-wheelBase / 2, -trackWidth / 2) + ); + // public static final ReplanningConfig repConfig = new ReplanningConfig( /* + // * 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 CoralEffectorc { + + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static double intakeRPM = -1000; + public static double outtakeRPM = 2100; + //TODO: Change after testing + public static final int DETECT_DISTANCE_INCHES = 3; + public static final int effectorMotorID = 0; + public static final int effectorDistanceSensorID = 0; + } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..3ddb80f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -7,20 +7,46 @@ //199 files // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI; +import static org.carlmontrobotics.Constants.OI.*; +import java.util.ArrayList; +import java.util.List; + +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator.*; +import org.carlmontrobotics.Constants.OI.Manipulator; +import org.carlmontrobotics.subsystems.Drivetrain; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; //controllers import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PS5Controller.Button; 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.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; //control bindings +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -29,31 +55,129 @@ public class RobotContainer { //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 final GenericHID driverController = new GenericHID(OI.Driver.driverPort); + public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); + private final Drivetrain drivetrain = new Drivetrain(); - public RobotContainer() { + private final SendableChooser autoChooser; + /*private final String[] autoNames = new String[] { + "Left 1 Piece L1 Auto", "Center 1 Piece L1 Auto", "Right 1 Piece L1 Auto", + "Left 1 Piece L2 Auto", "Center 1 Piece L2 Auto", "Right 1 Piece L2 Auto", + "Left 1 Piece L3 Auto", "Center 1 Piece L3 Auto", "Right 1 Piece L3 Auto", + "Left 1 Piece L4 Auto", "Center 1 Piece L4 Auto", "Right 1 Piece L4 Auto", + + "Left 2 Piece L1 Auto", "Right 2 Piece L1 Auto", + "Left 2 Piece L2 Auto", "Right 2 Piece L2 Auto", + "Left 2 Piece L3 Auto", "Right 2 Piece L3 Auto", + "Left 2 Piece L4 Auto", "Right 2 Piece L4 Auto", + + "Left 3 Piece L1 Auto", "Right 3 Piece L1 Auto", + "Left 3 Piece L2 Auto", "Right 3 Piece L2 Auto", + "Left 3 Piece L3 Auto", "Right 3 Piece L3 Auto", + "Left 3 Piece L4 Auto", "Right 3 Piece L4 Auto", - setDefaultCommands(); + "Left 4 Piece L1 Auto", "Right 4 Piece L1 Auto", + "Left 4 Piece L2 Auto", "Right 4 Piece L2 Auto", + "Left 4 Piece L3 Auto", "Right 4 Piece L3 Auto", + "Left 4 Piece L4 Auto", "Right 4 Piece L4 Auto", + + "Left Forward", "Center Forward", "Right Forward", + + + };*/ + + + + + public RobotContainer() { 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) - // )); + RegisterAutoCommands(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } + + + private void setDefaultCommands() {} private void setBindingsDriver() {} + private void setBindingsManipulator() {} - public Command getAutonomousCommand() { + private void RegisterAutoCommands(){ + //I made some of the constants up, so change once merged + //AlgaeEffector + // NamedCommands.registerCommand("GroundIntakeAlgae", new GroundIntakeAlgae(algaeEffector)); + // NamedCommands.registerCommand("DealgaficationIntake", new DealgaficationIntake(algaeEffector)); + // NamedCommands.registerCommand("ShootAlgae", new ShootAlgae(algaeEffector)); + //CoralEffector + // NamedCommands.registerCommand("CoralIntake", new CoralIntake(coralEffector)); + // NamedCommands.registerCommand("CoralOutake", new CoralOutake(coralEffector)); + // //AlgaeArm + // NamedCommands.registerCommand("ArmToDeAlgafy", new ArmToPosition(AlgaeEffector, Armc.DeAlgafy_Angle)); + // NamedCommands.registerCommand("ArmToIntake", new ArmToPosition(AlgaeEffector, Armc.Intake_Angle)); + // NamedCommands.registerCommand("ArmToShoot", new ArmToPosition(AlgaeEffector, Armc.Shoot_Angle)); + // //Elevator + // NamedCommands.registerCommand("ElevatorIntake", new ElevatorLPos(Elevator, Elevatorc.IntakePos)); + // NamedCommands.registerCommand("ElevatorL1", new ElevatorLPos(Elevator, Elevatorc.L1Pos)); + // NamedCommands.registerCommand("ElevatorL2", new ElevatorLPos(Elevator, Elevatorc.L2Pos)); + // NamedCommands.registerCommand("ElevatorL3", new ElevatorLPos(Elevator, Elevatorc.L3Pos)); + // NamedCommands.registerCommand("ElevatorL4", new ElevatorLPos(Elevator, Elevatorc.L4Pos)); + + // //Limelight + // NamedCommands.registerCommand("AlignToCoralStation", new AlignToCoralStation(Limelight, drivetrain)); + // NamedCommands.registerCommand("AlignToReef", new AlignToReef(Limelight, drivetrain)); + // NamedCommands.registerCommand("MoveToLeftBranch", new MoveToLeftBranch(Limelight, LimelightHelpers, drivetrain)); + // NamedCommands.registerCommand("MoveToRightBranch", new MoveToRightBranch(Limelight, LimelightHelpers, drivetrain)); + + //Sequential and Parralel Commands + /*NamedCommands.registerCommand("L2", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL2(Elevator, Elevatorc.L2Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("L3", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL3(Elevator, Elevatorc.L3Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("L4", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToReef(drivetrain, limelight), + new ElevatorL3(Elevator, Elevatorc.L4Pos), + new ArmToPos(arm, Armc.DeAlgafy_Angle)), + new DealgaficationIntake(algaeEffector), + new CoralOutake(coralEffector), + new ElevatorIntake(Elevator, Elevatorc.IntakePos)))));*/ + + /*NamedCommands.registerCommand("IntakeCoral", + new SequentialCommandGroup( + new ParallelCommandGroup( + new AlignToCoralStation(drivetrain, limelight), + new IntakeCoral(coralEffector))));*/ + + } + + /*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 @@ -89,4 +213,9 @@ private double inputProcessing(double value) { private double ProcessedAxisValue(GenericHID hid, Axis axis){ return inputProcessing(getStickValue(hid, axis)); } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } } + diff --git a/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java b/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java new file mode 100644 index 0000000..8ca8d26 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/LastResortAuto.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class LastResortAuto extends Command { + private final Timer timer = new Timer(); + /** Creates a new LastResortAuto. */ + private final Drivetrain drivetrain; + private boolean prev; + + int MAX_SECONDS_DRIVE = 4; + + public LastResortAuto(Drivetrain drivetrain) { + addRequirements(this.drivetrain = drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.reset(); + timer.start(); + prev = drivetrain.getFieldOriented(); + drivetrain.setFieldOriented(false); + drivetrain.drive(1, 0, 0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drivetrain.setFieldOriented(prev); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return timer.hasElapsed(4); + } +} \ 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/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java new file mode 100644 index 0000000..1689d27 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -0,0 +1,152 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Robot; +import org.carlmontrobotics.subsystems.Drivetrain; +import static org.carlmontrobotics.RobotContainer.*; + +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; + /** + * Creates a new TeleopDrive. + */ + public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + } + + // 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; + } + // 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/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..865cf7b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,1139 @@ +//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 javax.swing.text.html.CSS; + +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() { + //AutoBuilder(); + 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(); + AutoBuilder(); + System.out.println("BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB"); + + // 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() { + // AutoBuilder(); + + // 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()); + } + } + +///--------------------------------------------------------- + +//TODO: AUTOBUILDER + public void AutoBuilder() { + System.out.println("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"); + // All other subsystem initialization + // ... + + // Load the RobotConfig from the GUI settings. You should probably + // store this in your Constants file + RobotConfig config; + // try{ + // config = RobotConfig.fromGUISettings(); + // } catch (Exception e) { + // config = Constants. + // // Handle exception as needed + // e.printStackTrace(); + // } + config = Constants.Drivetrainc.Autoc.robotConfig; + + // Configure AutoBuilder last + + AutoBuilder.configure( + //Supplier poseSupplier, + this::getPose, // Robot pose supplier + //Consumer resetPose, + this::setPose, // Method to reset odometry (will be called if your auto has a starting pose) + //Supplier robotRelativeSpeedsSupplier, + this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + //BiConsumer output, + (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + //PathFollowingController controller, + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants FIXME do these need to be accurate? + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + //RobotConfig robotConfig, + config, // The robot configuration + //BooleanSupplier shouldFlipPath, + () -> { + // 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;//stolen from official pplib + } + return false; + }, + //Subsystem... driveRequirements + this // Reference to this subsystem to set requirements + ); + } + +//---------------------------------------------------------- + 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 +} \ No newline at end of file