diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index a37efb8..b7c3421 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,12 +6,22 @@ "GKC", "GKC Amp", "GKC Source", - "lame", + "Madtown", + "Middle Race", + "Middle Split 3", + "Middle Split 2", + "Source 5-4", + "Source_5-4", "amp_side", + "lame", "source_side", "temp" ], - "autoFolders": [], + "autoFolders": [ + "IRI", + "Madtown", + "Source 5-4" + ], "defaultMaxVel": 4.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/.vscode/settings.json b/.vscode/settings.json index 536d975..201d831 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -62,6 +62,7 @@ "Xstance" ], "wpilib.autoStartRioLog": false, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "java.debug.settings.onBuildFailureProceed": true } \ No newline at end of file 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..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "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 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..44e6330 --- /dev/null +++ b/simgui.json @@ -0,0 +1,65 @@ +{ + "HALProvider": { + "RoboRIO": { + "window": { + "visible": true + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/Pose": "Field2d", + "/Shuffleboard/COMMANDS/Brake Swerve": "Command", + "/Shuffleboard/COMMANDS/Coast Swerve": "Command", + "/Shuffleboard/COMMANDS/Force Zero All": "Command", + "/Shuffleboard/COMMANDS/Lob Note": "Command", + "/Shuffleboard/COMMANDS/NewShootAMpAuto": "Command", + "/Shuffleboard/COMMANDS/SetPoseRedSubStart": "Command", + "/Shuffleboard/COMMANDS/ShootAmp": "Command", + "/Shuffleboard/COMMANDS/Stop Logger": "Command", + "/Shuffleboard/COMMANDS/Unjam Shooter": "Command", + "/Shuffleboard/ELEVATOR/Brake": "Command", + "/Shuffleboard/ELEVATOR/Coast": "Command", + "/Shuffleboard/ELEVATOR/GoTo DesiredLen": "Command", + "/Shuffleboard/ELEVATOR/ZERO SUBSYSTEM": "Command", + "/Shuffleboard/INTAKE/Start": "Command", + "/Shuffleboard/INTAKE/Stop": "Command", + "/Shuffleboard/MATCH/Auto": "String Chooser", + "/Shuffleboard/MATCH/Full confidence": "Command", + "/Shuffleboard/PHOTON/Collect Note Auto": "Command", + "/Shuffleboard/PHOTON/Drive To Note": "Command", + "/Shuffleboard/PHOTON/Drive To Note Auto": "Command", + "/Shuffleboard/SHOOTER/Reverse Feeder": "Command", + "/Shuffleboard/SHOOTER/Spit Note": "Command", + "/Shuffleboard/SHOOTER/Stop Feed": "Command", + "/Shuffleboard/SHOOTER/Stop Shoot": "Command", + "/Shuffleboard/WRIST/GoTo Desired DEG": "Command", + "/Shuffleboard/WRIST/Re-Home": "Command", + "/Shuffleboard/WRIST/Set Brake": "Command", + "/Shuffleboard/WRIST/Set Coast": "Command", + "/SmartDashboard/Module 0": "Mechanism2d", + "/SmartDashboard/Module 1": "Mechanism2d", + "/SmartDashboard/Module 2": "Mechanism2d", + "/SmartDashboard/Module 3": "Mechanism2d" + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "Server": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "visible": true + } +} diff --git a/src/main/deploy/pathplanner/autos/Madtown Initial.auto b/src/main/deploy/pathplanner/autos/Madtown Initial.auto new file mode 100644 index 0000000..12fe0c1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Madtown Initial.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4, + "y": 3.3 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Madtown A" + } + } + ] + } + }, + "folder": "Madtown", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto b/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto new file mode 100644 index 0000000..1753161 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle Race Cleanup.auto @@ -0,0 +1,99 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3731168748868172, + "y": 5.547507658600387 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootSubwooferFirstHalf" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DefaultWrist" + } + }, + { + "type": "named", + "data": { + "name": "DefaultShooter" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle Race 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Race 2 Success" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "DisableWristLockDown" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Race 3" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "DisableWristLockDown" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": "IRI", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle Split 2.auto b/src/main/deploy/pathplanner/autos/Middle Split 2.auto new file mode 100644 index 0000000..2fc5f2c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle Split 2.auto @@ -0,0 +1,105 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.52 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle Split 2 A" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 2 B" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 2 C" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 2 D" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 2 E" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "DefaultWrist" + } + }, + { + "type": "named", + "data": { + "name": "DefaultShooter" + } + } + ] + } + } + ] + } + }, + "folder": "IRI", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Middle Split 3.auto b/src/main/deploy/pathplanner/autos/Middle Split 3.auto new file mode 100644 index 0000000..7bf6285 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Middle Split 3.auto @@ -0,0 +1,99 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.37, + "y": 5.52 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Middle Split 3 A" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 3 B" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 3 C" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 3 D" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Middle Split 3 E" + } + }, + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "DefaultWrist" + } + }, + { + "type": "named", + "data": { + "name": "DefaultShooter" + } + } + ] + } + } + ] + } + }, + "folder": "IRI", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 5N4N.auto b/src/main/deploy/pathplanner/autos/Source 5-4 5N4N.auto new file mode 100644 index 0000000..6970744 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 5N4N.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 C" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 5N4Y.auto b/src/main/deploy/pathplanner/autos/Source 5-4 5N4Y.auto new file mode 100644 index 0000000..27c8531 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 5N4Y.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 C" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 5Y4N.auto b/src/main/deploy/pathplanner/autos/Source 5-4 5Y4N.auto new file mode 100644 index 0000000..86af7b1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 5Y4N.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 D" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 AB.auto b/src/main/deploy/pathplanner/autos/Source 5-4 AB.auto new file mode 100644 index 0000000..214861f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 AB.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.39, + "y": 1.57 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ResetPoopMonitor" + } + }, + { + "type": "path", + "data": { + "pathName": "Source 5-4 AB" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 B 5Y4Y.auto b/src/main/deploy/pathplanner/autos/Source 5-4 B 5Y4Y.auto new file mode 100644 index 0000000..fe47744 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 B 5Y4Y.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 C" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Source 5-4 E" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 Initial.auto b/src/main/deploy/pathplanner/autos/Source 5-4 Initial.auto new file mode 100644 index 0000000..214861f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 Initial.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.39, + "y": 1.57 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ResetPoopMonitor" + } + }, + { + "type": "path", + "data": { + "pathName": "Source 5-4 AB" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 Preload Deadline.auto b/src/main/deploy/pathplanner/autos/Source 5-4 Preload Deadline.auto new file mode 100644 index 0000000..b874f0c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 Preload Deadline.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 F" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 Preload.auto b/src/main/deploy/pathplanner/autos/Source 5-4 Preload.auto new file mode 100644 index 0000000..93aeebf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 Preload.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 F" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source 5-4 Shoot Pose.auto b/src/main/deploy/pathplanner/autos/Source 5-4 Shoot Pose.auto new file mode 100644 index 0000000..3d33d8c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source 5-4 Shoot Pose.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 4.0, + "y": 3.0 + }, + "rotation": -30.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source 5-4 E" + } + } + ] + } + }, + "folder": "Source 5-4", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Source Race 5-4.auto b/src/main/deploy/pathplanner/autos/Source Race 5-4.auto new file mode 100644 index 0000000..d63a251 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Source Race 5-4.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.39, + "y": 1.57 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Source_5-4_a" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Source_5-4_b" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + }, + { + "type": "path", + "data": { + "pathName": "Source_5-4_c" + } + }, + { + "type": "named", + "data": { + "name": "AutoCollectNote" + } + }, + { + "type": "named", + "data": { + "name": "PathFindToSourceShot" + } + }, + { + "type": "named", + "data": { + "name": "AutoAimAndShoot" + } + } + ] + } + }, + "folder": "IRI", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index c431341..33ad786 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,1633 +1 @@ -{ - "field_size": { - "x": 16.54, - "y": 8.21 - }, - "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, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - 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, - true, - true - ], - [ - true, - true, - 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, - false, - false, - false, - true, - true, - true, - 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, - true, - true, - true, - true, - true, - 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, - true, - true - ], - [ - true, - true, - 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, - true, - true, - true, - true, - true, - 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, - true, - true, - true, - true, - true, - 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, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - 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, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - 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, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - 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, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - 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 +{"field_size":{"x":16.54,"y":8.21},"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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true],[true,true,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,false,false,false,true,true,true,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,true,true,true,true,true,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,true,true],[true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,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,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,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,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,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/Madtown A.path b/src/main/deploy/pathplanner/paths/Madtown A.path new file mode 100644 index 0000000..d969996 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Madtown A.path @@ -0,0 +1,335 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3966528828907423, + "y": 3.3 + }, + "prevControl": null, + "nextControl": { + "x": 2.3966528828907414, + "y": 3.3 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6869626339311825, + "y": 3.2903536828120585 + }, + "prevControl": { + "x": 2.6869626339311887, + "y": 3.2903536828120585 + }, + "nextControl": { + "x": 4.543967076658733, + "y": 3.2903536828120585 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.764753644797117, + "y": 4.04 + }, + "prevControl": { + "x": 4.956238267788856, + "y": 3.7822759207030145 + }, + "nextControl": { + "x": 6.573269021805378, + "y": 4.2977240792969855 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 3.95 + }, + "prevControl": { + "x": 8.1, + "y": 4.325000000000001 + }, + "nextControl": { + "x": 8.1, + "y": 3.575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3, + "y": 3.1 + }, + "prevControl": { + "x": 7.3, + "y": 3.6 + }, + "nextControl": { + "x": 7.3, + "y": 2.2325143713187727 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.3, + "y": 1.2 + }, + "prevControl": { + "x": 7.3, + "y": 1.6681604100559801 + }, + "nextControl": { + "x": 7.3, + "y": 0.44999999999999996 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7, + "y": 3.1 + }, + "prevControl": { + "x": 3.0257402108653095, + "y": 2.361505763897888 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0, + "rotateFast": true + }, + { + "waypointRelativePos": 4, + "rotationDegrees": -22.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 12.825184337094994, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 25.255901555655427, + "rotateFast": false + }, + { + "waypointRelativePos": 5, + "rotationDegrees": -22.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow Poop", + "minWaypointRelativePos": 2.8, + "maxWaypointRelativePos": 3.4499999999999997, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + }, + { + "name": "Conditional Collect", + "minWaypointRelativePos": 3.8500000000000005, + "maxWaypointRelativePos": 5.1499999999999995, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Disable LL", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DisableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "Enable LL", + "waypointRelativePos": 2.4000000000000004, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "Poop Prep", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 1.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + } + ] + } + } + }, + { + "name": "Poop Stop", + "waypointRelativePos": 3.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStop" + } + } + ] + } + } + }, + { + "name": "Start Poop Monitor", + "waypointRelativePos": 2.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StartPoopMonitor" + } + } + ] + } + } + }, + { + "name": "Stop Poop Monitor", + "waypointRelativePos": 3.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopPoopMonitor" + } + } + ] + } + } + }, + { + "name": "Start Watching For Note", + "waypointRelativePos": 3.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableWatchForNote" + } + } + ] + } + } + }, + { + "name": "Stop Watching For Note", + "waypointRelativePos": 5.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "DisableWatchForNote" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 36.38469900375056, + "rotateFast": false + }, + "reversed": false, + "folder": "Madtown", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Madtown B.path b/src/main/deploy/pathplanner/paths/Madtown B.path new file mode 100644 index 0000000..1932d7a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Madtown B.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.985724085049807, + "y": 2.551416667551395 + }, + "prevControl": null, + "nextControl": { + "x": 6.75699601861632, + "y": 1.691052013024826 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7, + "y": 3.1 + }, + "prevControl": { + "x": 4.213030214988503, + "y": 1.6904610688211372 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Madtown B End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Madtown", + "previewStartingState": { + "rotation": -22.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Madtown C.path b/src/main/deploy/pathplanner/paths/Madtown C.path new file mode 100644 index 0000000..7e57e22 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Madtown C.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.981061263963517, + "y": 0.8990330014867446 + }, + "prevControl": null, + "nextControl": { + "x": 6.532172524529916, + "y": 1.287261569140526 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7, + "y": 3.1 + }, + "prevControl": { + "x": 4.263944461846189, + "y": 2.206212216360257 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Madtown B End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Madtown", + "previewStartingState": { + "rotation": -22.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Madtown Scratch Pad.path b/src/main/deploy/pathplanner/paths/Madtown Scratch Pad.path new file mode 100644 index 0000000..eff8fd9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Madtown Scratch Pad.path @@ -0,0 +1,90 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.35, + "y": 5.15 + }, + "prevControl": null, + "nextControl": { + "x": 4.380906600715505, + "y": 6.041604495249359 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 12.27186588750642, + "y": 5.0 + }, + "prevControl": { + "x": 11.27186588750642, + "y": 5.0 + }, + "nextControl": { + "x": 13.27186588750642, + "y": 5.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7, + "y": 3.15 + }, + "prevControl": { + "x": 8.056183643882349, + "y": 4.317235889459942 + }, + "nextControl": { + "x": -0.6561836438823487, + "y": 1.9827641105400575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 12.998530989914899, + "y": 3.15 + }, + "prevControl": { + "x": 6.266774150223737, + "y": 2.483710236392898 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 2, + "rotationDegrees": -30.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -150.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Madtown", + "previewStartingState": { + "rotation": -3.3428019514140104, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Race 1.path b/src/main/deploy/pathplanner/paths/Middle Race 1.path new file mode 100644 index 0000000..5ee36a5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Race 1.path @@ -0,0 +1,202 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3731168748868172, + "y": 5.547507658600387 + }, + "prevControl": null, + "nextControl": { + "x": 2.123116874886817, + "y": 5.547507658600387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.547507658600387 + }, + "prevControl": { + "x": 2.15, + "y": 5.547507658600387 + }, + "nextControl": { + "x": 4.150000000000004, + "y": 5.547507658600387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.25, + "y": 4.4 + }, + "prevControl": { + "x": 4.311387895511962, + "y": 4.744974371959044 + }, + "nextControl": { + "x": 6.188612104488037, + "y": 4.055025628040957 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.15, + "y": 4.25 + }, + "prevControl": { + "x": 8.15, + "y": 4.0 + }, + "nextControl": { + "x": 8.15, + "y": 4.5 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.5, + "y": 4.25 + }, + "prevControl": { + "x": 6.465925826289069, + "y": 3.991180954897479 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle_race_1_end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "First Shoot Zone", + "minWaypointRelativePos": 0.95, + "maxWaypointRelativePos": 1.15, + "constraints": { + "maxVelocity": 1.125, + "maxAcceleration": 1.125, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "First Intake", + "waypointRelativePos": 0.55, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + }, + { + "name": "First Shot", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + } + }, + { + "name": "Stow Everything", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferSecondHalf" + } + } + ] + } + } + }, + { + "name": "Second Intake", + "waypointRelativePos": 2.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.5, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Race", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Race 2 Fail.path b/src/main/deploy/pathplanner/paths/Middle Race 2 Fail.path new file mode 100644 index 0000000..ecfdec7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Race 2 Fail.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.5, + "y": 4.25 + }, + "prevControl": null, + "nextControl": { + "x": 4.534074173710932, + "y": 4.508819045102521 + }, + "isLocked": false, + "linkedName": "middle_race_1_end" + }, + { + "anchor": { + "x": 1.7067267121368734, + "y": 4.6 + }, + "prevControl": { + "x": 1.61397633018067, + "y": 6.794007558609735 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle_race_2_end" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -25.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Race", + "previewStartingState": { + "rotation": 0, + "velocity": 2.5 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path b/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path new file mode 100644 index 0000000..ac1cd9b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Race 2 Success.path @@ -0,0 +1,121 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.5, + "y": 4.25 + }, + "prevControl": null, + "nextControl": { + "x": 4.6120192584713875, + "y": 4.728852252843395 + }, + "isLocked": false, + "linkedName": "middle_race_1_end" + }, + { + "anchor": { + "x": 2.894494070455155, + "y": 5.545689939904186 + }, + "prevControl": { + "x": 3.8341330839924415, + "y": 5.357907257770535 + }, + "nextControl": { + "x": 2.159036728303594, + "y": 5.692667822170061 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7067267121368734, + "y": 4.6 + }, + "prevControl": { + "x": 1.232086993593837, + "y": 5.347007440759673 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "middle_race_2_end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Shoot Zone", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 1.1, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Shoot Middle Note", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + } + }, + { + "name": "Lock Down Wrist", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableWristLockDown" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -23.799169144518192, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Race", + "previewStartingState": { + "rotation": 0, + "velocity": 2.5 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Race 3.path b/src/main/deploy/pathplanner/paths/Middle Race 3.path new file mode 100644 index 0000000..093aa87 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Race 3.path @@ -0,0 +1,121 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6, + "y": 4.25 + }, + "prevControl": null, + "nextControl": { + "x": 2.1698176727367158, + "y": 4.864364033216744 + }, + "isLocked": false, + "linkedName": "middle_race_3_start" + }, + { + "anchor": { + "x": 2.6, + "y": 5.5 + }, + "prevControl": { + "x": 2.6, + "y": 4.750000000000001 + }, + "nextControl": { + "x": 2.6, + "y": 5.95 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6822325119621304, + "y": 6.4 + }, + "prevControl": { + "x": 0.9602585505465432, + "y": 5.708079774079494 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Shoot Zone", + "minWaypointRelativePos": 0.85, + "maxWaypointRelativePos": 1.15, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Shoot Note", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + } + }, + { + "name": "Lock Down Wrist", + "waypointRelativePos": 1.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableWristLockDown" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 25.848690122206257, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Race", + "previewStartingState": { + "rotation": -25.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 A.path b/src/main/deploy/pathplanner/paths/Middle Split 2 A.path new file mode 100644 index 0000000..18c26f1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 A.path @@ -0,0 +1,301 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.367536726441503, + "y": 5.52 + }, + "prevControl": null, + "nextControl": { + "x": 2.046376909981702, + "y": 6.03154276967924 + }, + "isLocked": false, + "linkedName": "Middle Split A Start" + }, + { + "anchor": { + "x": 2.9, + "y": 6.25 + }, + "prevControl": { + "x": 2.05, + "y": 6.25 + }, + "nextControl": { + "x": 3.9, + "y": 6.25 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.836521079212586, + "y": 6.25 + }, + "prevControl": { + "x": 4.868260539606293, + "y": 6.340625 + }, + "nextControl": { + "x": 6.804781618818879, + "y": 6.159375 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.023042158425174, + "y": 5.6 + }, + "prevControl": { + "x": 8.023042158425174, + "y": 5.85 + }, + "nextControl": { + "x": 8.023042158425174, + "y": 5.35 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6, + "y": 6.298228527210456 + }, + "prevControl": { + "x": 6.432506242723144, + "y": 6.298228527210456 + }, + "nextControl": { + "x": 4.8500000000000005, + "y": 6.298228527210456 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.85, + "y": 5.52 + }, + "prevControl": { + "x": 3.85, + "y": 6.02 + }, + "nextControl": { + "x": 3.85, + "y": 4.77 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.859987605018093, + "y": 4.85 + }, + "prevControl": { + "x": 3.359987605018093, + "y": 4.85 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "Middle Split 2 A End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 5.0, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.0, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow Shoot", + "minWaypointRelativePos": 4.85, + "maxWaypointRelativePos": 5.25, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + }, + { + "name": "Slow Poop", + "minWaypointRelativePos": 2.8, + "maxWaypointRelativePos": 3.45, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.05, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 1.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + } + ] + } + } + }, + { + "name": "Poop Stop", + "waypointRelativePos": 1.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStop" + } + } + ] + } + } + }, + { + "name": "Collect", + "waypointRelativePos": 2.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + }, + { + "name": "Shoot", + "waypointRelativePos": 4.949999999999999, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + } + }, + { + "name": "Shoot Prep", + "waypointRelativePos": 3.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootPrep" + } + } + ] + } + } + }, + { + "name": "Enable LL Update", + "waypointRelativePos": 1.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 0.5177878023859686, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 2", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 B.path b/src/main/deploy/pathplanner/paths/Middle Split 2 B.path new file mode 100644 index 0000000..6510363 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 B.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.859987605018093, + "y": 4.85 + }, + "prevControl": null, + "nextControl": { + "x": 1.8685168815150186, + "y": 4.85 + }, + "isLocked": true, + "linkedName": "Middle Split 2 A End" + }, + { + "anchor": { + "x": 2.574434465922116, + "y": 4.251375729406378 + }, + "prevControl": { + "x": 1.6834279417337483, + "y": 4.705366229145925 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "Middle Split 2 B End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -27.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 2", + "previewStartingState": { + "rotation": 0, + "velocity": 2.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 C.path b/src/main/deploy/pathplanner/paths/Middle Split 2 C.path new file mode 100644 index 0000000..67a75ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 C.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.574434465922116, + "y": 4.251375729406378 + }, + "prevControl": null, + "nextControl": { + "x": 2.1000819228478216, + "y": 4.940645401857148 + }, + "isLocked": false, + "linkedName": "Middle Split 2 B End" + }, + { + "anchor": { + "x": 2.574434465922116, + "y": 5.45 + }, + "prevControl": { + "x": 1.574434465922116, + "y": 5.45 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "Middle Split 2 C End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 2", + "previewStartingState": { + "rotation": -27.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 D.path b/src/main/deploy/pathplanner/paths/Middle Split 2 D.path new file mode 100644 index 0000000..b5741a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 D.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.574434465922116, + "y": 5.45 + }, + "prevControl": null, + "nextControl": { + "x": 2.1702827461401535, + "y": 5.744383062347823 + }, + "isLocked": false, + "linkedName": "Middle Split 2 C End" + }, + { + "anchor": { + "x": 2.7, + "y": 6.1 + }, + "prevControl": { + "x": 1.2384449028221471, + "y": 5.762573418484202 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split 2 D End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 13.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 2", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 E.path b/src/main/deploy/pathplanner/paths/Middle Split 2 E.path new file mode 100644 index 0000000..5b4cb6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 E.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.7, + "y": 6.1 + }, + "prevControl": null, + "nextControl": { + "x": 1.9500000000000002, + "y": 6.1 + }, + "isLocked": false, + "linkedName": "Middle Split 2 D End" + }, + { + "anchor": { + "x": 2.583178836076225, + "y": 6.859106235587274 + }, + "prevControl": { + "x": 1.9272356673353055, + "y": 6.4954600347517335 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 29.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 2", + "previewStartingState": { + "rotation": 14.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 3 A.path b/src/main/deploy/pathplanner/paths/Middle Split 3 A.path new file mode 100644 index 0000000..c4eb735 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 3 A.path @@ -0,0 +1,311 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.367536726441503, + "y": 5.52 + }, + "prevControl": null, + "nextControl": { + "x": 2.100299850460456, + "y": 5.083390425116072 + }, + "isLocked": true, + "linkedName": "Middle Split A Start" + }, + { + "anchor": { + "x": 2.66, + "y": 4.85724057271631 + }, + "prevControl": { + "x": 2.16, + "y": 4.85724057271631 + }, + "nextControl": { + "x": 3.16, + "y": 4.85724057271631 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9, + "y": 4.86 + }, + "prevControl": { + "x": 3.4, + "y": 4.86 + }, + "nextControl": { + "x": 4.4, + "y": 4.86 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.7571737944515915, + "y": 4.45 + }, + "prevControl": { + "x": 5.124327238109168, + "y": 4.559951014772466 + }, + "nextControl": { + "x": 6.369006258443314, + "y": 4.343699982069227 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.2, + "y": 4.0 + }, + "prevControl": { + "x": 8.2, + "y": 4.25 + }, + "nextControl": { + "x": 8.2, + "y": 3.75 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.7571737944515915, + "y": 4.106057724789198 + }, + "prevControl": { + "x": 6.631745669908794, + "y": 3.672870388193883 + }, + "nextControl": { + "x": 5.14847332367427, + "y": 4.407555341074306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.05, + "y": 5.52 + }, + "prevControl": { + "x": 4.05, + "y": 4.808411409097608 + }, + "nextControl": { + "x": 4.05, + "y": 6.290909696720522 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.859987605018093, + "y": 6.291168699962351 + }, + "prevControl": { + "x": 3.4508240398929786, + "y": 6.28282113081219 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split A End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -16.466591995883878, + "rotateFast": false + }, + { + "waypointRelativePos": 6, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 7, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Shoot Zone", + "minWaypointRelativePos": 5.800000000000001, + "maxWaypointRelativePos": 6.050000000000001, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 1.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + } + ] + } + } + }, + { + "name": "Poop Stop", + "waypointRelativePos": 2.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStop" + } + } + ] + } + } + }, + { + "name": "Collect Note", + "waypointRelativePos": 3.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + }, + { + "name": "Shoot Note", + "waypointRelativePos": 5.8500000000000005, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "InstantShoot" + } + } + ] + } + } + }, + { + "name": "Shoot Prep", + "waypointRelativePos": 4.949999999999999, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootPrep" + } + } + ] + } + } + }, + { + "name": "Enable LL Update", + "waypointRelativePos": 1.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 3", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 3 B.path b/src/main/deploy/pathplanner/paths/Middle Split 3 B.path new file mode 100644 index 0000000..ebf6f60 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 3 B.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.859987605018093, + "y": 6.291168699962351 + }, + "prevControl": null, + "nextControl": { + "x": 2.128617400010086, + "y": 6.233018579164377 + }, + "isLocked": false, + "linkedName": "Middle Split A End" + }, + { + "anchor": { + "x": 2.5629101850610545, + "y": 6.878943773229128 + }, + "prevControl": { + "x": 1.6598529318842452, + "y": 6.434959828250936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split B End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 25.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 3", + "previewStartingState": { + "rotation": 0, + "velocity": 2.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 3 C.path b/src/main/deploy/pathplanner/paths/Middle Split 3 C.path new file mode 100644 index 0000000..4ad0eed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 3 C.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5629101850610545, + "y": 6.878943773229128 + }, + "prevControl": null, + "nextControl": { + "x": 1.720261297630179, + "y": 6.1718774025739345 + }, + "isLocked": false, + "linkedName": "Middle Split B End" + }, + { + "anchor": { + "x": 2.5629101850610545, + "y": 5.53 + }, + "prevControl": { + "x": 1.5629101850610545, + "y": 5.53 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split C End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 3", + "previewStartingState": { + "rotation": 24.0, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 3 D.path b/src/main/deploy/pathplanner/paths/Middle Split 3 D.path new file mode 100644 index 0000000..60415c5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 3 D.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5629101850610545, + "y": 5.53 + }, + "prevControl": null, + "nextControl": { + "x": 1.8203562578629509, + "y": 5.4245786302371615 + }, + "isLocked": false, + "linkedName": "Middle Split C End" + }, + { + "anchor": { + "x": 2.9, + "y": 4.89 + }, + "prevControl": { + "x": 2.0546563889369676, + "y": 4.9788491937775055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split D End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -10.492914756798106, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 3", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Middle Split 3 E.path b/src/main/deploy/pathplanner/paths/Middle Split 3 E.path new file mode 100644 index 0000000..919c40b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Middle Split 3 E.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 4.89 + }, + "prevControl": null, + "nextControl": { + "x": 1.8561421464990995, + "y": 4.741802110630542 + }, + "isLocked": false, + "linkedName": "Middle Split D End" + }, + { + "anchor": { + "x": 2.549151306467538, + "y": 4.2289072542397355 + }, + "prevControl": { + "x": 1.6114035133442615, + "y": 4.576224222573308 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Middle Split E End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Collect Note", + "waypointRelativePos": 0.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -22.932413047699225, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Split 3", + "previewStartingState": { + "rotation": -5.550000000000011, + "velocity": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 0000000..5412d5a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.673845694891705, + "y": 5.958427021164473 + }, + "prevControl": null, + "nextControl": { + "x": 4.692897161747657, + "y": 5.958427021164473 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.45, + "y": 7.32 + }, + "prevControl": { + "x": 2.8165187458805407, + "y": 7.694528454562855 + }, + "nextControl": { + "x": 0.0834812541194605, + "y": 6.945471545437146 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": -0.08645042244799081, + "y": 5.554039120604119 + }, + "prevControl": { + "x": -0.8990166017995915, + "y": 6.194779517405559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -173.63304008230688, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 37a6339..0438299 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6302980759535015, - "y": 4.259813557857729 + "x": 6.7134375418924455, + "y": 1.5558094641941607 }, "prevControl": null, "nextControl": { - "x": 3.6302980759535015, - "y": 4.259813557857729 + "x": 7.713437541892443, + "y": 1.5558094641941607 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 15.198630069279815, + "y": 5.5 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 14.198630069279815, + "y": 5.5 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, "folder": "temp", "previewStartingState": { - "rotation": -24.096301564327376, + "rotation": 31.20987486596953, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 A.path b/src/main/deploy/pathplanner/paths/Source 5-4 A.path new file mode 100644 index 0000000..c98ecd7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 A.path @@ -0,0 +1,132 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3899999999999997, + "y": 1.57 + }, + "isLocked": false, + "linkedName": "source_race_start" + }, + { + "anchor": { + "x": 4.0, + "y": 1.57 + }, + "prevControl": { + "x": 2.75, + "y": 1.57 + }, + "nextControl": { + "x": 5.25, + "y": 1.57 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.0, + "y": 0.7792380150020553 + }, + "prevControl": { + "x": 7.0, + "y": 0.7792380150020553 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source 5-4 A End" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + }, + { + "type": "named", + "data": { + "name": "DisableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 0.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + }, + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "StartPoopMonitor", + "waypointRelativePos": 1.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StartPoopMonitor" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.8, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 AB.path b/src/main/deploy/pathplanner/paths/Source 5-4 AB.path new file mode 100644 index 0000000..2e43663 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 AB.path @@ -0,0 +1,211 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3899999999999997, + "y": 1.57 + }, + "isLocked": false, + "linkedName": "source_race_start" + }, + { + "anchor": { + "x": 4.0, + "y": 1.57 + }, + "prevControl": { + "x": 2.75, + "y": 1.57 + }, + "nextControl": { + "x": 5.25, + "y": 1.57 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.05, + "y": 0.8 + }, + "prevControl": { + "x": 8.042929903394992, + "y": 0.3000499887648808 + }, + "nextControl": { + "x": 8.058188401784435, + "y": 1.3790290844434518 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.1, + "y": 2.4324831175046824 + }, + "prevControl": { + "x": 6.6, + "y": 2.4324831175046824 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Source 5-4 B End" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Poop Zone", + "minWaypointRelativePos": 1.7999999999999998, + "maxWaypointRelativePos": 2.15, + "constraints": { + "maxVelocity": 3.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + }, + { + "type": "named", + "data": { + "name": "DisableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 0.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + }, + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "StartPoopMonitor", + "waypointRelativePos": 1.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StartPoopMonitor" + } + } + ] + } + } + }, + { + "name": "Poop Stop", + "waypointRelativePos": 2.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStop" + } + } + ] + } + } + }, + { + "name": "Collect Note", + "waypointRelativePos": 2.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + }, + { + "name": "Stop Poop Monitor", + "waypointRelativePos": 2.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopPoopMonitor" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 B.path b/src/main/deploy/pathplanner/paths/Source 5-4 B.path new file mode 100644 index 0000000..f2732ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 B.path @@ -0,0 +1,210 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3899999999999997, + "y": 1.57 + }, + "isLocked": false, + "linkedName": "source_race_start" + }, + { + "anchor": { + "x": 4.0, + "y": 1.57 + }, + "prevControl": { + "x": 2.75, + "y": 1.57 + }, + "nextControl": { + "x": 5.25, + "y": 1.57 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.05, + "y": 0.9 + }, + "prevControl": { + "x": 8.042929903394992, + "y": 0.4000499887648808 + }, + "nextControl": { + "x": 8.058188401784435, + "y": 1.479029084443452 + }, + "isLocked": false, + "linkedName": null + "linkedName": "Source 5-4 B End" + } + ], + "rotationTargets": [], + + + + + + + + + + + + "constraintZones": [ + { + "name": "Poop Zone", + "minWaypointRelativePos": 1.7999999999999998, + "maxWaypointRelativePos": 2.15, + "constraints": { + "maxVelocity": 3.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + }, + { + "type": "named", + "data": { + "name": "DisableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "Poop Start", + "waypointRelativePos": 0.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + }, + { + "type": "named", + "data": { + "name": "EnableLLPoseUpdate" + } + } + ] + } + } + }, + { + "name": "StartPoopMonitor", + "waypointRelativePos": 1.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StartPoopMonitor" + } + } + ] + } + } + }, + { + "name": "Poop Stop", + "waypointRelativePos": 2.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStop" + } + } + ] + } + } + }, + { + "name": "Collect Note", + "waypointRelativePos": 2.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + }, + { + "name": "Stop Poop Monitor", + "waypointRelativePos": 2.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopPoopMonitor" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 C.path b/src/main/deploy/pathplanner/paths/Source 5-4 C.path new file mode 100644 index 0000000..33da35c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 C.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.1, + "y": 2.4324831175046824 + }, + "prevControl": null, + "nextControl": { + "x": 7.115299256931702, + "y": 2.258229145955275 + }, + "isLocked": false, + "linkedName": "Source 5-4 B End" + }, + { + "anchor": { + "x": 5.6, + "y": 1.8 + }, + "prevControl": { + "x": 6.35, + "y": 1.8 + }, + "nextControl": { + "x": 4.85, + "y": 1.8 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.819152044288993, + "y": 2.426423563648954 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "SourceShootPose" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Shoot Prep", + "waypointRelativePos": 0.15, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootPrep" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 D.path b/src/main/deploy/pathplanner/paths/Source 5-4 D.path new file mode 100644 index 0000000..ea5a57c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 D.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.1, + "y": 2.4324831175046824 + }, + "prevControl": null, + "nextControl": { + "x": 7.649448998994113, + "y": 2.9626616811888984 + }, + "isLocked": false, + "linkedName": "Source 5-4 B End" + }, + { + "anchor": { + "x": 6.506672272169776, + "y": 2.6 + }, + "prevControl": { + "x": 6.506672272169776, + "y": 3.1 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.0, + "rotation": -90.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 E.path b/src/main/deploy/pathplanner/paths/Source 5-4 E.path new file mode 100644 index 0000000..9006c30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 E.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 4.819152044288991, + "y": 2.426423563648954 + }, + "isLocked": false, + "linkedName": "SourceShootPose" + }, + { + "anchor": { + "x": 4.900290324658922, + "y": 2.0774124530184195 + }, + "prevControl": { + "x": 4.4907143025144265, + "y": 2.3642006711939425 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.5, + "rotation": -35.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 F.path b/src/main/deploy/pathplanner/paths/Source 5-4 F.path new file mode 100644 index 0000000..f550dde --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 F.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.6547349626516095, + "y": 2.2992726415386784 + }, + "isLocked": false, + "linkedName": "SourceShootPose" + }, + { + "anchor": { + "x": 3.4, + "y": 1.57 + }, + "prevControl": { + "x": 1.4001925927813272, + "y": 1.5422451079387747 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 2.25, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 5-4 Starting Poses.path b/src/main/deploy/pathplanner/paths/Source 5-4 Starting Poses.path new file mode 100644 index 0000000..21799b6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source 5-4 Starting Poses.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3899999999999997, + "y": 1.57 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.17104681599446, + "y": 1.57 + }, + "prevControl": { + "x": 14.17104681599446, + "y": 1.57 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source 5-4", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_a.path b/src/main/deploy/pathplanner/paths/Source_5-4_a.path new file mode 100644 index 0000000..1a81fee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_a.path @@ -0,0 +1,185 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.39, + "y": 1.57 + }, + "prevControl": null, + "nextControl": { + "x": 2.3559258262890683, + "y": 1.3111809548974793 + }, + "isLocked": false, + "linkedName": "source_race_start" + }, + { + "anchor": { + "x": 7.9, + "y": 0.75 + }, + "prevControl": { + "x": 6.9, + "y": 0.75 + }, + "nextControl": { + "x": 8.9, + "y": 0.75 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.294906624680149, + "y": 1.75 + }, + "prevControl": { + "x": 8.294906624680149, + "y": 1.0696015291969474 + }, + "nextControl": { + "x": 8.294906624680149, + "y": 3.25 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.337328917134686, + "y": 1.8947383787418037 + }, + "prevControl": { + "x": 5.997467039336302, + "y": 1.6544677517560307 + }, + "nextControl": { + "x": 4.6325594515452515, + "y": 2.1512534862360564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.506672272169776, + "y": 2.770348970313103 + }, + "prevControl": { + "x": 7.3258243164587675, + "y": 2.1967725339620565 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "source_5-4_a_end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -35.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 90.0, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": -21.497331108184007, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow Poop 5", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.65, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + } + } + ], + "eventMarkers": [ + { + "name": "Poop Prep", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopPrep" + } + } + ] + } + } + }, + { + "name": "Poop Preload", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "PoopStart" + } + } + ] + } + } + }, + { + "name": "Intake 4", + "waypointRelativePos": 1.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeNoteAuto" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.626989153286004, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_b.path b/src/main/deploy/pathplanner/paths/Source_5-4_b.path new file mode 100644 index 0000000..320b431 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_b.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.506672272169776, + "y": 2.770348970313103 + }, + "prevControl": null, + "nextControl": { + "x": 7.268424917383689, + "y": 2.049506130334599 + }, + "isLocked": false, + "linkedName": "source_5-4_a_end" + }, + { + "anchor": { + "x": 5.1371265490650195, + "y": 1.8184062559531378 + }, + "prevControl": { + "x": 4.294084321457229, + "y": 2.3051369130007173 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -30.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source_5-4_c.path b/src/main/deploy/pathplanner/paths/Source_5-4_c.path new file mode 100644 index 0000000..888469c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Source_5-4_c.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.506672272169776, + "y": 2.770348970313103 + }, + "prevControl": null, + "nextControl": { + "x": 5.144247571083395, + "y": 2.9024912738772244 + }, + "isLocked": false, + "linkedName": "source_5-4_a_end" + }, + { + "anchor": { + "x": 2.346802158592743, + "y": 1.8885030190488628 + }, + "prevControl": { + "x": 1.3997681661993824, + "y": 2.414172605471947 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 360.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": -28.58825946325851, + "rotateFast": false + }, + "reversed": false, + "folder": "Source_5-4", + "previewStartingState": { + "rotation": -35.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5ec05b7..8a9941c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Candles.CandleSide; public class Robot extends TimedRobot { private Command autoCommand; @@ -20,24 +21,34 @@ public class Robot extends TimedRobot { @Override public void robotInit() { robotContainer = new RobotContainer(); - robotContainer.CANDLES.setAnimationBoth( - new LarsonAnimation(255, 255, 0, 0, 1.0, 8, BounceMode.Front, 1)); + robotContainer.CANDLES.setAnimation( + CandleSide.BOTH, new LarsonAnimation(255, 255, 0, 0, 1.0, 8, BounceMode.Front, 1)); PathfindingCommand.warmupCommand().schedule(); } @Override public void robotPeriodic() { - robotContainer.updatePoseVision(); + RobotContainer.DRIVETRAIN.updateOdometry(); + // robotContainer.updatePoseVision(); + RobotContainer.DRIVETRAIN.megatag2Update(); CommandScheduler.getInstance().run(); + // System.out.println("LobRpm: " + RobotContainer.get().lobRPM.getDouble(100.0)); + // System.out.println("Distance to Lob: " + + // Util.getDistanceToAllianceLob(RobotContainer.get().getPose())); } @Override public void disabledInit() { - robotContainer.CANDLES.setAnimationBoth(new SingleFadeAnimation(255, 0, 0, 0, 0.7, 8)); + robotContainer.CANDLES.setAnimation( + CandleSide.BOTH, new SingleFadeAnimation(255, 0, 0, 0, 0.7, 8)); + robotContainer.DRIVETRAIN.setShouldMegatag2Update(false); } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + // System.out.println(Util.getRotationToAllianceSpeaker(new Pose2d(new Translation2d(7.21, + // 2.24), new Rotation2d(90.0)))); + } @Override public void disabledExit() {} @@ -45,8 +56,8 @@ public void disabledExit() {} @Override public void autonomousInit() { // m_robotContainer.CANDLES.setAnimationBoth(new SingleFadeAnimation(255, 255, 255, 0, 1.5, 8)); - robotContainer.CANDLES.setAnimationRight( - new LarsonAnimation(255, 255, 255, 0, 0.1, 8, BounceMode.Front, 1)); + robotContainer.CANDLES.setAnimation( + CandleSide.RIGHT, new LarsonAnimation(255, 255, 255, 0, 0.1, 8, BounceMode.Front, 1)); autoCommand = robotContainer.getAutonomousCommand(); if (autoCommand != null) { autoCommand.schedule(); @@ -66,6 +77,7 @@ public void teleopInit() { if (autoCommand != null) { autoCommand.cancel(); } + robotContainer.DRIVETRAIN.setShouldMegatag2Update(true); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 19e4529..391c71f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,9 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -29,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.control.AimLockWrist; @@ -45,13 +48,19 @@ import frc.robot.commands.control.StopAll; import frc.robot.commands.control.amp.FireRevAmp; import frc.robot.commands.control.amp.PrepRevAmp; +import frc.robot.commands.control.auto.AutoAimAndShoot; import frc.robot.commands.control.auto.AutoAimLockWrist; +import frc.robot.commands.control.auto.AutoCollectNote; import frc.robot.commands.control.auto.AutoIdleShooter; +import frc.robot.commands.control.auto.AutoState; import frc.robot.commands.control.auto.InstantShoot; +import frc.robot.commands.control.auto.Madtown; +import frc.robot.commands.control.auto.Source_5_4; import frc.robot.commands.control.note.IntakeNoteSequence; import frc.robot.commands.control.note.IntakeNoteSequenceAuto; import frc.robot.commands.control.note.LobNote; import frc.robot.commands.control.note.PoopNote; +import frc.robot.commands.control.note.PoopTest; import frc.robot.commands.control.note.ShootNote; import frc.robot.commands.control.note.ShootNoteAimbotFixed; import frc.robot.commands.control.note.ShootNoteSequence; @@ -59,10 +68,9 @@ import frc.robot.commands.movement.CollectNoteAuto; import frc.robot.commands.movement.DriveToNote; import frc.robot.commands.movement.DriveToNoteAuto; -import frc.robot.commands.movement.PointAtAprilTag; +import frc.robot.commands.movement.PointAtSpeaker; import frc.robot.commands.movement.SwerveCommand; import frc.robot.commands.qol.AsyncRumble; -import frc.robot.commands.qol.DefaultCANdle; import frc.robot.constants.Constants; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.AmpSensors; @@ -70,30 +78,34 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.PoopMonitor; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Vision; import frc.robot.subsystems.Wrist; -import frc.robot.util.Limelight; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; public class RobotContainer { private final SendableChooser AUTO_CHOOSER = new SendableChooser<>(); + public GenericEntry lobRPM = Shuffleboard.getTab("MAIN").add("LobRPM", 3000.0).getEntry(); public final ShuffleboardTab COMMANDS_TAB = Shuffleboard.getTab("COMMANDS"); public final ShuffleboardTab MATCH_TAB = Shuffleboard.getTab("MATCH"); public final ShuffleboardTab PHOTON_TAB = Shuffleboard.getTab("PHOTON"); public final ShuffleboardTab SHOOTER_TAB = Shuffleboard.getTab("SHOOTER"); public final ShuffleboardTab PROTO_TAB = Shuffleboard.getTab("PROTO"); - public final Vision INTAKE_PHOTON = new Vision("Arducam_OV9782_USB_Camera", 0.651830, 60); - public final CommandSwerveDrivetrain DRIVETRAIN = DriveConstants.DriveTrain; // My drivetrain + public static final Vision INTAKE_PHOTON = new Vision("Arducam_OV9782_USB_Camera", 0.651830, 60); + public static final CommandSwerveDrivetrain DRIVETRAIN = + DriveConstants.DriveTrain; // My drivetrain public final Candles CANDLES = new Candles(Constants.LEFT_CANDLE, Constants.RIGHT_CANDLE); public final Intake INTAKE = new Intake(Constants.INTAKE_TOP_ID, Constants.INTAKE_BOTTOM_ID); - public final Shooter SHOOTER = + public static final Shooter SHOOTER = new Shooter( Constants.SHOOTER_LEADER_ID, Constants.SHOOTER_FOLLOWER_ID, Constants.SHOOTER_FEEDER_ID); - public final Wrist WRIST = new Wrist(Constants.WRIST_ID); + public static final Wrist WRIST = new Wrist(Constants.WRIST_ID); public final Elevator ELEVATOR = new Elevator(Constants.ELEVATOR_LEADER_ID, Constants.ELEVATOR_FOLLOWER_ID); public final AmpSensors AMP_SENSORS = new AmpSensors(); + public static final PoopMonitor POOP_MONITOR = new PoopMonitor(); private final CommandXboxController DRIVER_CONTROLLER = new CommandXboxController(0); private final CommandXboxController CO_DRIVER_CONTROLLER = new CommandXboxController(1); private final SlewRateLimiter TranslationXSlewRate = @@ -102,10 +114,11 @@ public class RobotContainer { new SlewRateLimiter(Constants.translationSlewRate); // private final SlewRateLimiter RotationalSlewRate = // new SlewRateLimiter(Constants.rotationSlewRate); - public static boolean isForwardAmpPrimed = false; - public static boolean isReverseAmpPrimed = false; + public static boolean isAmpPrepped = false; + public static boolean isAmpShot = false; public static boolean isClimbPrimed = false; public static boolean aimAtTargetAuto = false; + public static boolean teleopShouldPointToNote = false; private double MaxSpeed = DriveConstants.kSpeedAt12VoltsMps; private double MaxAngularRate = 1.5 * Math.PI; private boolean VisionUpdate = false; @@ -120,6 +133,27 @@ public class RobotContainer { private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private final Telemetry logger = new Telemetry(MaxSpeed); private static RobotContainer instance; + private static AutoState autoState = AutoState.DEFAULT; + + public static AutoState getAutoState() { + return autoState; + } + + public static void setAutoState(final AutoState newState) { + autoState = newState; + } + + public static void enableTeleopPointToNote() { + teleopShouldPointToNote = true; + } + + public static void disableTeleopPointToNote() { + teleopShouldPointToNote = false; + } + + public static boolean shouldTelopPointToNote() { + return teleopShouldPointToNote; + } public RobotContainer() { instance = this; @@ -131,33 +165,64 @@ public RobotContainer() { configureDriverController(); configureCoDriverController(); configureDefaultCommands(); + // COMMANDS_TAB.add("PoopTest", new PoopTest(SHOOTER, INTAKE, WRIST)); } private void configureDriverController() { DRIVER_CONTROLLER.b().onTrue(new ShootSubwooferFlat(ELEVATOR, WRIST, SHOOTER)); + DRIVER_CONTROLLER.a().onTrue(new LobNote(SHOOTER, WRIST, ELEVATOR, lobRPM)); + + // DRIVER_CONTROLLER + // .y() + // .onTrue( + // new ConditionalCommand( + // new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) + // .andThen(() -> isReverseAmpPrimed = false), + // new PrepRevAmp(ELEVATOR, WRIST) + // .andThen(new WaitCommand(0.8)) + // .andThen(new FireRevAmp(SHOOTER)) + // .andThen(new WaitCommand(0.1)) + // .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) + // .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)), + // () -> isReverseAmpPrimed)); DRIVER_CONTROLLER .y() .onTrue( new ConditionalCommand( - new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) - .andThen(() -> isReverseAmpPrimed = false), + // Compare prepped to not prepped (If prepped, try shoot. Else, prep) + new ConditionalCommand( + // Compare shot to not shot (If shot, go home. If not, try shot) + new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE) + .andThen( + () -> { + isAmpPrepped = false; + isAmpShot = false; + }), + new ConditionalCommand( + // Compare working shot (If working, shoot. Else rumble.) + new FireRevAmp(SHOOTER) + .andThen(new WaitCommand(0.1)) + .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) + .andThen(new InstantCommand(() -> isAmpShot = true)), + new AsyncRumble( + DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 400L), + () -> AMP_SENSORS.getBothSensors()), + () -> isAmpShot), new PrepRevAmp(ELEVATOR, WRIST) - .andThen(new WaitCommand(0.8)) - .andThen(new FireRevAmp(SHOOTER)) - .andThen(new WaitCommand(0.1)) - .andThen(new InstantCommand(() -> ELEVATOR.setLengthInches(4.2))) - .andThen(new InstantCommand(() -> isReverseAmpPrimed = true)), - () -> isReverseAmpPrimed)); - // DRIVER_CONTROLLER - // .back() - // .onTrue( - // DRIVETRAIN.runOnce( - // () -> { - // DRIVETRAIN.seedFieldRelative(); - // DRIVETRAIN.getPigeon2().reset(); - // })); - DRIVER_CONTROLLER.back().onTrue(new InstantCommand(() -> updatePoseVision(0.01, false))); + .andThen(new InstantCommand(() -> isAmpPrepped = true)), + () -> isAmpPrepped)); + DRIVER_CONTROLLER + .back() + .onTrue( + DRIVETRAIN + .runOnce( + () -> { + DRIVETRAIN.seedFieldRelative(); + DRIVETRAIN.getPigeon2().reset(); + }) + .andThen(new InstantCommand(() -> updatePoseVision(0.01, false)))); + // DRIVER_CONTROLLER.back().onTrue(new InstantCommand(() -> updatePoseVision(0.01, false))); DRIVER_CONTROLLER .start() .onTrue( @@ -169,22 +234,48 @@ private void configureDriverController() { .onTrue( new IntakeNoteSequence(SHOOTER, INTAKE, WRIST, ELEVATOR) .andThen( - new AsyncRumble( - DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 700L))); + new AsyncRumble(DRIVER_CONTROLLER.getHID(), RumbleType.kBothRumble, 1.0, 700L)) + // No instantcommand wrapper? + ) + .whileTrue(new InstantCommand(() -> enableTeleopPointToNote())) + .onFalse(new InstantCommand(() -> disableTeleopPointToNote())); DRIVER_CONTROLLER - .rightTrigger() + .rightTrigger(0.2) .whileTrue( - new PointAtAprilTag( + new PointAtSpeaker( DRIVETRAIN, () -> -TranslationXSlewRate.calculate(DRIVER_CONTROLLER.getLeftY()), () -> -TranslationYSlewRate.calculate(DRIVER_CONTROLLER.getLeftX()), - () -> DRIVER_CONTROLLER.getRightX())); - + () -> DRIVER_CONTROLLER.getRightX(), + () -> false, + () -> false)); DRIVER_CONTROLLER .rightBumper() .onTrue(new ShootNote(SHOOTER, ELEVATOR, Constants.Shooter.SHOOTER_RPM)); - DRIVER_CONTROLLER.leftTrigger().onTrue(new LobNote(SHOOTER, WRIST, ELEVATOR)); + // DRIVER_CONTROLLER.leftTrigger(0.1).onTrue(new LobNote(SHOOTER, WRIST, ELEVATOR)); + + DRIVER_CONTROLLER + .leftTrigger(0.2) + .whileTrue( + new ParallelDeadlineGroup( + new SequentialCommandGroup( + new WaitUntilCommand( + () -> + DRIVER_CONTROLLER.getHID().getLeftTriggerAxis() > 0.90 + && Util.isPointedAtLob(DRIVETRAIN)), + new LobNote(SHOOTER, WRIST, ELEVATOR, lobRPM)), + new PointAtSpeaker( + DRIVETRAIN, + () -> -TranslationXSlewRate.calculate(DRIVER_CONTROLLER.getLeftY()), + () -> -TranslationYSlewRate.calculate(DRIVER_CONTROLLER.getLeftX()), + () -> DRIVER_CONTROLLER.getRightX(), + () -> true, + () -> false + + // Other logic for aiming etc + ))); + // WIP // DRIVER_CONTROLLER // .leftTrigger() @@ -218,9 +309,10 @@ private void configureDriverController() { } private void configureCoDriverController() { - CO_DRIVER_CONTROLLER - .leftBumper() - .onTrue(Util.pathfindToPose(Util.findNearestPoseToTrapClimbs(getPose()))); + + // CO_DRIVER_CONTROLLER + // .leftBumper() + // .onTrue(Util.pathfindToPose(Util.findNearestPoseToTrapClimbs(getPose()))); CO_DRIVER_CONTROLLER.start().onTrue(new StopAll(WRIST, SHOOTER, INTAKE, ELEVATOR)); CO_DRIVER_CONTROLLER.rightBumper().onTrue(new PoopNote(SHOOTER, 2500)); @@ -268,10 +360,23 @@ private void configureCoDriverController() { new ParallelDeadlineGroup( new IntakeNoteSequence(SHOOTER, INTAKE, WRIST, ELEVATOR), new DriveToNoteAuto(DRIVETRAIN, INTAKE_PHOTON, SHOOTER, INTAKE, WRIST, ELEVATOR))); + + CO_DRIVER_CONTROLLER + .leftStick() + .onTrue( + new InstantCommand( + () -> { + if (CommandSwerveDrivetrain.getAlliance() == Alliance.Blue) { + DRIVETRAIN.seedFieldRelative( + new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0))); + } else { + DRIVETRAIN.seedFieldRelative( + new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180))); + } + })); } private void configureDrivetrain() { - DRIVETRAIN.setDefaultCommand( // Drivetrain will execute this command periodically new SwerveCommand( DRIVETRAIN, @@ -380,6 +485,7 @@ public void configureShuffleboard() { }) .ignoringDisable(true)); COMMANDS_TAB.add("ShootAmp", new ShootAmp(SHOOTER, ELEVATOR, WRIST)); + COMMANDS_TAB.add("PoopTest", new PoopTest(SHOOTER, INTAKE, WRIST)); COMMANDS_TAB.add( "Brake Swerve", new InstantCommand( @@ -390,7 +496,7 @@ public void configureShuffleboard() { } }) .ignoringDisable(true)); - COMMANDS_TAB.add("Lob Note", new LobNote(SHOOTER, WRIST, ELEVATOR)); + // COMMANDS_TAB.add("Lob Note", new LobNote(SHOOTER, WRIST, ELEVATOR)); // COMMANDS_TAB.add("Fast Point", new FastPoint(DRIVETRAIN, SPEAKER_LIMELIGHT)); COMMANDS_TAB.add("NewShootAMpAuto", new NewShootAmpAuto(SHOOTER, ELEVATOR, WRIST)); COMMANDS_TAB.add( @@ -401,6 +507,20 @@ public void configureShuffleboard() { WRIST.setZero(); }) .ignoringDisable(true)); + COMMANDS_TAB.add( + "SetPoseRedSubStart", + new InstantCommand( + () -> + DRIVETRAIN.seedFieldRelative( + new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180)))) + .ignoringDisable(true)); + COMMANDS_TAB.add( + "SetPoseBlueSubStart", + new InstantCommand( + () -> + DRIVETRAIN.seedFieldRelative( + new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0)))) + .ignoringDisable(true)); AUTO_CHOOSER.addOption( "SYSID-QUAS-F", DRIVETRAIN.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); @@ -438,27 +558,45 @@ public void configureShuffleboard() { // addAuto("driven_source_score"); // addAuto("heart_source_shoot"); // addAuto("heart_source_og"); - addAuto("Taxi-Amp"); - addAuto("Taxi-Source"); + // addAuto("Taxi-Amp"); + // addAuto("Taxi-Source"); // addAuto("temp_center"); // addAuto("GKC-SOURCE-A"); // addAuto("GKC-SOURCE-B"); // addAuto("GKC-AMP-A"); // addAuto("GKC-AMP-B"); // addAuto("GKC-Source-J"); - addAuto("GKC Source 5-4-3"); - addAuto("GKC Source 4-3-2"); + // addAuto("GKC Source 5-4-3"); + // addAuto("GKC Source 4-3-2"); // addAuto("GKC-Amp-J"); - addAuto("GKC-Amp-2-1Blue"); - addAuto("GKC-Amp-2-1Red"); - addAuto("GKC-Amp-1-2Blue"); - addAuto("GKC-Amp-1-2Red"); - addAuto("AGKC-Amp-1-2Red"); - addAuto("GKC-Amp-Skip-1-2"); - addAuto("lame"); + // addAuto("GKC-Amp-2-1Blue"); + // addAuto("GKC-Amp-2-1Red"); + // addAuto("GKC-Amp-1-2Blue"); + // addAuto("GKC-Amp-1-2Red"); + // addAuto("AGKC-Amp-1-2Red"); + // addAuto("GKC-Amp-Skip-1-2"); + addAuto("Middle Race Cleanup"); + addAuto("Middle Split 2"); + addAuto("Middle Split 3"); + // addAuto("Source Race 5-4"); + // addAuto("lame"); + // AUTO_CHOOSER.addOption( + // "Middle Race Cleanup", + // new MiddleRaceCleanup(DRIVETRAIN, INTAKE, ELEVATOR, WRIST, SHOOTER, INTAKE_PHOTON)); // addAuto("GKC-Amp-J2"); - AUTO_CHOOSER.addOption("Do Nothing", new InstantCommand()); + // AUTO_CHOOSER.addOption("Do Nothing", new InstantCommand()); + AUTO_CHOOSER.addOption("Source 5-4", new Source_5_4()); + AUTO_CHOOSER.addOption("Madtown", new Madtown()); MATCH_TAB.add("Auto", AUTO_CHOOSER); + MATCH_TAB.add( + "Full confidence", + new InstantCommand( + () -> + DRIVETRAIN.addVisionMeasurement( + LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo").pose, + LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo") + .timestampSeconds, + VecBuilder.fill(0.1, 0.1, 0.1)))); // MATCH_TAB.addBoolean("Vision Updated", () -> VisionUpdate); // MATCH_TAB.addBoolean("is Alliance red", () -> CommandSwerveDrivetrain.getAlliance() == @@ -468,7 +606,8 @@ public void configureShuffleboard() { public void configureDefaultCommands() { WRIST.setDefaultCommand(new AimLockWrist(WRIST, SHOOTER, ELEVATOR)); SHOOTER.setDefaultCommand(new IdleShooter(SHOOTER)); - CANDLES.setDefaultCommand(new DefaultCANdle(CANDLES, SHOOTER)); + // FIXME default candle execute loop overruns + // CANDLES.setDefaultCommand(new DefaultCANdle(CANDLES, SHOOTER)); } public void configureNamedCommands() { @@ -524,25 +663,22 @@ public void configureNamedCommands() { "PoopPrep", new InstantCommand( () -> { - SHOOTER.setRPMShootNoSpin(750); + setAutoState(AutoState.POOP_PREP); INTAKE.setVolts(-8.0); - WRIST.setDegrees(15); - }, - SHOOTER, - WRIST, - INTAKE)); + // WRIST.setDegrees(15); + })); NamedCommands.registerCommand( "PoopPrep2500", new InstantCommand( () -> { SHOOTER.setRPMShootNoSpin(4000); - WRIST.setDegrees(25); + // WRIST.setDegrees(25); }, SHOOTER, WRIST, INTAKE)); NamedCommands.registerCommand( - "PoopStart", new InstantCommand(() -> SHOOTER.setFeederVoltage(8.0))); + "PoopStart", new InstantCommand(() -> RobotContainer.setAutoState(AutoState.POOPING))); NamedCommands.registerCommand( "StartIntakeAndShoot", new InstantCommand( @@ -562,12 +698,9 @@ public void configureNamedCommands() { "PoopStop", new InstantCommand( () -> { - SHOOTER.stopShooter(); - SHOOTER.stopFeeder(); + setAutoState(AutoState.DEFAULT); INTAKE.stopCollecting(); - }, - SHOOTER, - INTAKE)); + })); NamedCommands.registerCommand("ShootSubwoofer", new ShootSubwoofer(ELEVATOR, WRIST, SHOOTER)); NamedCommands.registerCommand("ShootAmp", new ShootAmp(SHOOTER, ELEVATOR, WRIST)); NamedCommands.registerCommand("GoHome", new GoHome(ELEVATOR, WRIST, SHOOTER, INTAKE)); @@ -581,11 +714,17 @@ public void configureNamedCommands() { "OverrideRotationSpeakerEnable", new InstantCommand(() -> aimAtTargetAuto = true)); NamedCommands.registerCommand( "OverrideRotationSpeakerDisable", new InstantCommand(() -> aimAtTargetAuto = false)); - NamedCommands.registerCommand("DefaultWrist", new AutoAimLockWrist(WRIST)); - NamedCommands.registerCommand("DefaultShooter", new AutoIdleShooter(SHOOTER)); + NamedCommands.registerCommand( + "DefaultWrist", new AutoAimLockWrist(WRIST, () -> getAutoState())); + NamedCommands.registerCommand( + "DefaultShooter", new AutoIdleShooter(SHOOTER, () -> getAutoState())); NamedCommands.registerCommand("InstantShoot", new InstantShoot(SHOOTER)); NamedCommands.registerCommand( - "IntakeNoteAuto", new IntakeNoteSequenceAuto(SHOOTER, INTAKE, ELEVATOR)); + "ShootPrep", new InstantCommand(() -> setAutoState(AutoState.SHOOT_PREP))); + NamedCommands.registerCommand( + "IntakeNoteAuto", + new IntakeNoteSequenceAuto( + SHOOTER, INTAKE, ELEVATOR)); // .finallyDo(() -> setAutoState(AutoState.SHOOT_PREP))); NamedCommands.registerCommand( "TempLog", new InstantCommand( @@ -594,8 +733,28 @@ public void configureNamedCommands() { "InstantShoot: distance: " + Util.getDistanceToSpeaker() + ", angle: " - + Util.getInterpolatedWristAngle(), + + Util.getInterpolatedWristAngleSpeaker(), false))); + NamedCommands.registerCommand("AutoCollectNote", new AutoCollectNote(3.0)); // 2.5 worked + NamedCommands.registerCommand("AutoAimAndShoot", new AutoAimAndShoot(DRIVETRAIN, SHOOTER)); + NamedCommands.registerCommand( + "EnableWristLockDown", new InstantCommand(() -> WRIST.enableWristLockdown())); + NamedCommands.registerCommand( + "DisableWristLockDown", new InstantCommand(() -> WRIST.disableWristLockdown())); + NamedCommands.registerCommand("PathFindToSourceShot", Util.PathFindToAutoSourceShot()); + NamedCommands.registerCommand( + "ShootSubwooferFirstHalf", new ShootSubwooferFirstHalf(ELEVATOR, WRIST, SHOOTER)); + NamedCommands.registerCommand("StartPoopMonitor", POOP_MONITOR.StartPoopMonitorCommand()); + NamedCommands.registerCommand("StopPoopMonitor", POOP_MONITOR.StopPoopMonitorCommand()); + NamedCommands.registerCommand("ResetPoopMonitor", POOP_MONITOR.ResetPoopMonitorCommand()); + NamedCommands.registerCommand( + "EnableLLPoseUpdate", new InstantCommand(() -> DRIVETRAIN.setShouldMegatag2Update(true))); + NamedCommands.registerCommand( + "DisableLLPoseUpdate", new InstantCommand(() -> DRIVETRAIN.setShouldMegatag2Update(false))); + NamedCommands.registerCommand( + "EnableWatchForNote", new InstantCommand(() -> Madtown.SHOULD_WATCH_FOR_NOTE = true)); + NamedCommands.registerCommand( + "DisableWatchForNote", new InstantCommand(() -> Madtown.SHOULD_WATCH_FOR_NOTE = false)); } public void addAuto(String autoName) { @@ -625,7 +784,7 @@ public Pose2d getPose() { return DRIVETRAIN.getPose(); } - public boolean shouldRejectLL3G(final Limelight.PoseEstimate botPose) { + public boolean shouldRejectLL3G(final LimelightHelpers.PoseEstimate botPose) { if (botPose.tagCount < 2) { return true; } @@ -662,11 +821,12 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { boolean isDisabled = DriverStation.isDisabled(); boolean hasUpdatedPose = false; - Limelight.PoseEstimate bestPose = null; + LimelightHelpers.PoseEstimate bestPose = null; double bestConfidence = 0.0; for (String limelight3G : Constants.Vision.LL3GS) { - Limelight.PoseEstimate botPoseLL3G = Limelight.getBotPoseEstimate_wpiBlue(limelight3G); + LimelightHelpers.PoseEstimate botPoseLL3G = + LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight3G); if (!shouldRejectLL3G(botPoseLL3G)) { double currentConfidence = calculateConfidence(botPoseLL3G); if (currentConfidence > bestConfidence) { @@ -686,7 +846,8 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { if (canTrustLL3) { if (!hasUpdatedPose) { for (String limelight3 : Constants.Vision.LL3S) { - Limelight.PoseEstimate botPoseLL3 = Limelight.getBotPoseEstimate_wpiBlue(limelight3); + LimelightHelpers.PoseEstimate botPoseLL3 = + LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight3); if (!shouldRejectLL3G(botPoseLL3)) { hasUpdatedPose = true; updatePose(botPoseLL3, isDisabled, rotationConfidence); @@ -704,7 +865,7 @@ public void updatePoseVision(double rotationConfidence, boolean canTrustLL3) { } public void updatePose( - Limelight.PoseEstimate botPose, boolean isDisabled, double rotationConfidence) { + LimelightHelpers.PoseEstimate botPose, boolean isDisabled, double rotationConfidence) { double currentConfidence = calculateConfidence(botPose); if (DriverStation.isAutonomous()) { currentConfidence = currentConfidence * 6; @@ -802,7 +963,7 @@ public void updatePose( // // } // } - private double calculateConfidence(Limelight.PoseEstimate botPose) { + private double calculateConfidence(LimelightHelpers.PoseEstimate botPose) { // final double botPoseToPoseDistance = // botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation()); final double speakerReference = Util.speakerTagCount(botPose.rawFiducials); diff --git a/src/main/java/frc/robot/commands/conditional/MonitorBoolean.java b/src/main/java/frc/robot/commands/conditional/MonitorBoolean.java new file mode 100644 index 0000000..f3538a2 --- /dev/null +++ b/src/main/java/frc/robot/commands/conditional/MonitorBoolean.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.conditional; + +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.BooleanSupplier; + +public class MonitorBoolean extends Command { + private BooleanSupplier monitorBoolean; + private boolean latchSuccess; + private Runnable onSuccess; + private Runnable onFail; + + /** Creates a new MonitorLineBreak. */ + public MonitorBoolean(BooleanSupplier monitor, Runnable runOnSuccess, Runnable runOnFail) { + this.monitorBoolean = monitor; + this.onSuccess = runOnSuccess; + this.onFail = runOnFail; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (monitorBoolean.getAsBoolean()) latchSuccess = true; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (latchSuccess) { + onSuccess.run(); + return; + } else { + onFail.run(); + return; + } + // if (interrupted) { + // onFail.run(); + // } else { + // onSuccess.run(); + // } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return latchSuccess; + } +} diff --git a/src/main/java/frc/robot/commands/control/AimLockWrist.java b/src/main/java/frc/robot/commands/control/AimLockWrist.java index cd0e852..a83bb4a 100644 --- a/src/main/java/frc/robot/commands/control/AimLockWrist.java +++ b/src/main/java/frc/robot/commands/control/AimLockWrist.java @@ -33,13 +33,16 @@ public void execute() { if (RobotContainer.isClimbPrimed) { return; } - if (RobotContainer.isForwardAmpPrimed || RobotContainer.isReverseAmpPrimed) { + if (RobotContainer.isAmpPrepped) { return; } // if (Util.canSeeTarget(speakerLimelight)) { if (shooter.isCenterBroken()) { - double degrees = Util.getInterpolatedWristAngle(); - wrist.setDegrees(degrees); + if (!Util.isValidShot()) { + wrist.setDegrees(35.0); + } else { + wrist.setDegrees(Util.getInterpolatedWristAngleSpeaker()); + } // } else { // wrist.goHome(); diff --git a/src/main/java/frc/robot/commands/control/AimLockWristAuto.java b/src/main/java/frc/robot/commands/control/AimLockWristAuto.java index fc2ad91..7ccda04 100644 --- a/src/main/java/frc/robot/commands/control/AimLockWristAuto.java +++ b/src/main/java/frc/robot/commands/control/AimLockWristAuto.java @@ -4,8 +4,8 @@ package frc.robot.commands.control; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; import frc.robot.subsystems.Wrist; import frc.robot.util.Util; @@ -26,9 +26,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if (wrist.shouldLockDownWrist()) { + wrist.setDegrees(Constants.Wrist.INITIAL_ANGLE_DEGREES + 5.0); + return; + } if (Util.isValidShot()) { - double degrees = Util.getInterpolatedWristAngle(); - DriverStation.reportWarning("TRYING DEGREES " + degrees, false); + double degrees = Util.getInterpolatedWristAngleSpeaker(); + // DriverStation.reportWarning("TRYING DEGREES " + degrees, false); wrist.setDegrees(degrees); } } diff --git a/src/main/java/frc/robot/commands/control/Climb.java b/src/main/java/frc/robot/commands/control/Climb.java index d7350b9..e169c2d 100644 --- a/src/main/java/frc/robot/commands/control/Climb.java +++ b/src/main/java/frc/robot/commands/control/Climb.java @@ -26,10 +26,10 @@ public Climb(Elevator Elevator, Wrist Wrist, Shooter Shooter) { new InstantCommand( () -> Elevator.setLengthInchesSlot1(Constants.Climb.CLIMB_PULLDOWN_HEIGHT)), new WaitUntilCommand(Elevator::isAtSetpoint), - new WaitCommand(0.9), + new WaitCommand(0.6), // 0.9 new InstantCommand(() -> Elevator.setLengthInches(Constants.Climb.CLIMB_LEVEL_HEIGHT)), new WaitUntilCommand(Elevator::isAtSetpoint), - new WaitCommand(2.0), + new WaitCommand(0.6), // 2.0 new InstantCommand(Wrist::goHome, Wrist), new InstantCommand(Wrist::stop, Wrist), new InstantCommand(() -> Wrist.setDegrees(Constants.Wrist.INITIAL_ANGLE_DEGREES), Wrist), diff --git a/src/main/java/frc/robot/commands/control/GoHome.java b/src/main/java/frc/robot/commands/control/GoHome.java index 49a5188..d856bb3 100644 --- a/src/main/java/frc/robot/commands/control/GoHome.java +++ b/src/main/java/frc/robot/commands/control/GoHome.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.RobotContainer; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -23,6 +24,8 @@ public GoHome(Elevator elevator, Wrist wrist, Shooter shooter, Intake intake) { new InstantCommand(wrist::goHome), new InstantCommand(shooter::stopShooter), new InstantCommand(shooter::stopFeeder), - new InstantCommand(intake::stopCollecting)); + new InstantCommand(intake::stopCollecting), + new InstantCommand(() -> RobotContainer.isAmpPrepped = false), + new InstantCommand(() -> RobotContainer.isAmpShot = false)); } } diff --git a/src/main/java/frc/robot/commands/control/IdleShooter.java b/src/main/java/frc/robot/commands/control/IdleShooter.java index 210c5f0..1120353 100644 --- a/src/main/java/frc/robot/commands/control/IdleShooter.java +++ b/src/main/java/frc/robot/commands/control/IdleShooter.java @@ -39,19 +39,24 @@ public void execute() { shooter.stopShooter(); return; } - if (RobotContainer.isForwardAmpPrimed) { + if (RobotContainer.isAmpPrepped) { shooter.setRPMShoot(Constants.Shooter.SHOOTER_AMP_RPM); return; } - if (shooter.isCenterBroken()) { - if (validShotDebouncer.calculate(Util.isValidShot())) { - shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_RPM_CLOSE); + if (Util.isLobShot()) { + shooter.setRPMShoot( + Util.getShooterSpeedFromDistanceForLob( + Util.getDistanceToAllianceLob(RobotContainer.get().getPose()))); + return; + } + if (shooter.isCenterBroken() || shooter.isRearBroken()) { + shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_SHOOTING_RPM); + } else { + if (validShotDebouncer.calculate(Util.isValidShotAmpInclusive())) { + shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_SHOOTING_RPM); } else { - shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_RPM); + shooter.stopShooter(); } - - } else { - shooter.stopShooter(); } } diff --git a/src/main/java/frc/robot/commands/control/JiggleTrap.java b/src/main/java/frc/robot/commands/control/JiggleTrap.java new file mode 100644 index 0000000..a53ab24 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/JiggleTrap.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.subsystems.Wrist; +import frc.robot.util.Util; + +public class JiggleTrap extends Command { + private boolean doTheJiggle; + private final Wrist WRIST; + + /** Creates a new JiggleTrap. */ + public JiggleTrap(Wrist wrist) { + this.WRIST = wrist; + addRequirements(wrist); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + doTheJiggle = + Util.isWithinTolerance(WRIST.getDegrees(), Constants.Trap.TRAP_WRIST_DEGREES, 3.0); + if (doTheJiggle) { + WRIST.setDegreesSlot1(Constants.Trap.TRAP_WRIST_DEGREES - 7.0); + } else { + WRIST.setDegreesSlot1(Constants.Trap.TRAP_WRIST_DEGREES); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/control/ShootSubwoofer.java b/src/main/java/frc/robot/commands/control/ShootSubwoofer.java index 2adacb1..95eb873 100644 --- a/src/main/java/frc/robot/commands/control/ShootSubwoofer.java +++ b/src/main/java/frc/robot/commands/control/ShootSubwoofer.java @@ -24,7 +24,7 @@ public ShootSubwoofer(final Elevator elevator, final Wrist wrist, final Shooter new InstantCommand( () -> { elevator.setLengthInches(6.5); - shooter.setRPMShoot(2750); + shooter.setRPMShoot(2250); }, elevator, shooter), diff --git a/src/main/java/frc/robot/commands/control/ShootSubwooferFirstHalf.java b/src/main/java/frc/robot/commands/control/ShootSubwooferFirstHalf.java index 51b79f5..d3594ed 100644 --- a/src/main/java/frc/robot/commands/control/ShootSubwooferFirstHalf.java +++ b/src/main/java/frc/robot/commands/control/ShootSubwooferFirstHalf.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Shooter; @@ -24,15 +23,14 @@ public ShootSubwooferFirstHalf( addCommands( new InstantCommand( () -> { + wrist.setDegrees(52); elevator.setLengthInches(6.5); - shooter.setRPMShoot(2750); + shooter.setRPMShoot(2000); // 2750 }, elevator, shooter), - new WaitUntilCommand(elevator::isAtSetpoint), - new InstantCommand(() -> wrist.setDegrees(52)), - new WaitCommand(0.44), - new WaitUntilCommand(wrist::isAtSetpoint), + new WaitUntilCommand( + () -> elevator.isAtSetpoint() && wrist.isAtSetpoint() && shooter.isShooterAtSetpoint()), new InstantCommand(() -> shooter.setFeederVoltage(7.5), shooter), new WaitUntilCommand(() -> !shooter.isCenterBroken()) diff --git a/src/main/java/frc/robot/commands/control/ShootTall.java b/src/main/java/frc/robot/commands/control/ShootTall.java index c21d671..9dd1af7 100644 --- a/src/main/java/frc/robot/commands/control/ShootTall.java +++ b/src/main/java/frc/robot/commands/control/ShootTall.java @@ -25,7 +25,7 @@ public ShootTall(final Elevator elevator, final Wrist wrist, final Shooter shoot new InstantCommand( () -> { elevator.setLengthInches(28); - wrist.setDegrees(23); + wrist.setDegrees(21); shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); }, elevator, diff --git a/src/main/java/frc/robot/commands/control/ShootTrap.java b/src/main/java/frc/robot/commands/control/ShootTrap.java index a1bc56b..8e5ca29 100644 --- a/src/main/java/frc/robot/commands/control/ShootTrap.java +++ b/src/main/java/frc/robot/commands/control/ShootTrap.java @@ -59,7 +59,9 @@ public ShootTrap(final Elevator elevator, final Wrist wrist, final Shooter shoot new WaitUntilCommand( () -> lineBreakDebouncer.calculate(!shooter.isCenterBroken())), // probably debounce this - new InstantCommand(shooter::stopFeeder, shooter)); + // new InstantCommand(shooter::stopFeeder, shooter), + new WaitCommand(0.7), + new JiggleTrap(wrist)); // new WaitUntilCommand(() -> lineBreakDebouncer.calculate(shooter.isCenterBroken())), // new InstantCommand(() -> wrist.setDegrees(35.0), shooter), // new WaitCommand(0.4), diff --git a/src/main/java/frc/robot/commands/control/amp/PrepRevAmp.java b/src/main/java/frc/robot/commands/control/amp/PrepRevAmp.java index 0c19ab6..88b361d 100644 --- a/src/main/java/frc/robot/commands/control/amp/PrepRevAmp.java +++ b/src/main/java/frc/robot/commands/control/amp/PrepRevAmp.java @@ -22,7 +22,7 @@ public PrepRevAmp(Elevator elevator, Wrist wrist) { addCommands( new InstantCommand( () -> { - elevator.setLengthInches(Constants.REV_ELEVATOR_AMP_HEIGHT); + elevator.setLengthInches(Constants.REV_ELEVATOR_AMP_HEIGHT + 2.0); wrist.setDegrees(Constants.REV_WRIST_AMP_DEGREES); }, elevator, diff --git a/src/main/java/frc/robot/commands/control/auto/AutoAimAndShoot.java b/src/main/java/frc/robot/commands/control/auto/AutoAimAndShoot.java new file mode 100644 index 0000000..bdfb661 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/AutoAimAndShoot.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.commands.movement.PointAtSpeaker; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Shooter; +import frc.robot.util.Util; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoAimAndShoot extends SequentialCommandGroup { + /** Creates a new AutoAimAndShoot. */ + public AutoAimAndShoot(final CommandSwerveDrivetrain drivetrain, final Shooter shooter) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new ParallelDeadlineGroup( + new WaitUntilCommand( + () -> { + final boolean isPointed = Util.isPointedAtSpeaker(drivetrain); + final boolean isRPM = shooter.isShooterAtSetpoint(); + System.out.println("angle: " + isPointed + ", speed: " + isRPM); + return isPointed; + }) + .withTimeout(0.5), + new PointAtSpeaker(drivetrain, () -> 0.0, () -> 0.0, () -> 0.0)), + new InstantShoot(shooter)); + } +} diff --git a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java index 9bb3626..9ad8a38 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java @@ -5,40 +5,57 @@ package frc.robot.commands.control.auto; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.subsystems.Wrist; import frc.robot.util.Util; +import java.util.function.Supplier; public class AutoAimLockWrist extends Command { private final Wrist wrist; + private final Supplier autoStateSupplier; /** Creates a new AimLockWrist. */ public AutoAimLockWrist(Wrist wrist) { - this.wrist = wrist; + this(wrist, () -> null); + } + + public AutoAimLockWrist(final Wrist wrist, final Supplier autoStateSupplier) { addRequirements(wrist); - // Use addRequirements() here to declare subsystem dependencies. + this.wrist = wrist; + this.autoStateSupplier = autoStateSupplier; } // Called when the command is initially scheduled. @Override - public void initialize() { - DriverStation.reportWarning("DefaultWrist: initialize", false); - } + public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // double degrees = Util.getInterpolatedWristAngle(); - // // TODO find actual values, prevent wrist collision when the elevator is all the way down. - // DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false); - wrist.setDegrees(MathUtil.clamp(Util.getInterpolatedWristAngle(), 10.0, 35.0)); + switch (autoStateSupplier.get()) { + case COLLECTING: + double poseX = RobotContainer.DRIVETRAIN.getPose().getX(); + if (poseX < 6.0 || poseX > 16.56 - 6.0) { + wrist.setDegrees(MathUtil.clamp(Util.getInterpolatedWristAngleSpeaker(), 10.0, 35.0)); + } else { + wrist.setDegrees(26); + } + + break; + case POOPING: + wrist.setDegrees(22); + break; + case SHOOT_PREP: + case SHOOTING: + case DEFAULT: + default: + wrist.setDegrees(MathUtil.clamp(Util.getInterpolatedWristAngleSpeaker(), 10.0, 35.0)); + } } @Override - public void end(boolean interrupted) { - DriverStation.reportWarning("DefaultWrist: end, " + interrupted, false); - } + public void end(boolean interrupted) {} @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java b/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java new file mode 100644 index 0000000..7dc8228 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/AutoCollectNote.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import frc.robot.RobotContainer; +import frc.robot.commands.control.note.IntakeNoteSequenceAuto; +import frc.robot.commands.movement.DriveToNote2; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoCollectNote extends ParallelDeadlineGroup { + /** Creates a new AutoCollectNote. */ + public AutoCollectNote(final double initialVelocity) { + // Add the deadline command in the super() call. Add other commands using + // addCommands(). + super( + new IntakeNoteSequenceAuto( + RobotContainer.SHOOTER, RobotContainer.get().INTAKE, RobotContainer.get().ELEVATOR)); + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand(() -> RobotContainer.WRIST.enableWristLockdown()) + .andThen( + new DriveToNote2( + RobotContainer.DRIVETRAIN, RobotContainer.INTAKE_PHOTON, initialVelocity)) + .finallyDo(() -> RobotContainer.WRIST.disableWristLockdown())); + // addCommands(new DriveToNote2(drivetrain, vision, initialVelocity)); + } +} diff --git a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java index fd13758..0f7cb9d 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java @@ -5,24 +5,31 @@ package frc.robot.commands.control.auto; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.Shooter; +import java.util.function.Supplier; public class AutoIdleShooter extends Command { private static final double VALID_SHOT_DEBOUNCE_TIME = 0.2; + private static final double POOP_RPM = 750; /** Creates a new IdleShooter. */ private final Shooter shooter; + private final Supplier autoStateSupplier; + // private final Debouncer validShotDebouncer; public AutoIdleShooter(Shooter shooter) { + this(shooter, () -> null); + } + + public AutoIdleShooter(final Shooter shooter, final Supplier autoStateSupplier) { addRequirements(shooter); this.shooter = shooter; - // this.validShotDebouncer = new Debouncer(VALID_SHOT_DEBOUNCE_TIME, DebounceType.kFalling); - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(shooter); + this.autoStateSupplier = autoStateSupplier; } // Called when the command is initially scheduled. @@ -39,10 +46,54 @@ public void execute() { // } // if (shooter.isCenterBroken() && // validShotDebouncer.calculate(Util.isValidShot(SPEAKER_LIMELIGHT))) { - shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); - // } else { - // shooter.stopShooter(); - // } + final AutoState state = autoStateSupplier.get(); + switch (state) { + case COLLECTING: + if (shooter.isCenterBroken()) { + shooter.stopFeeder(); + } else { + shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + } + final double poseX = RobotContainer.DRIVETRAIN.getPose().getX(); + if (poseX < 6.0 || poseX > 16.56 - 6.0) { + shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); + } else { + shooter.setRPMShootNoSpin(POOP_RPM); + } + break; + case POOP_PREP: + shooter.stopFeeder(); + shooter.setRPMShootNoSpin(POOP_RPM); + break; + case POOPING: + shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + shooter.setRPMShootNoSpin(POOP_RPM); + break; + case SHOOT_PREP: + if (shooter.isRearBroken() && !shooter.isCenterBroken()) { + shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + } else { + shooter.stopFeeder(); + } + shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); + break; + case SHOOTING: + shooter.setFeederVoltage(Constants.Shooter.FEEDER_AUTO_VOLTS + 2.0); + shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); + break; + default: + shooter.setRPMShoot(Constants.Shooter.SHOOTER_RPM); + if (shooter.isCenterBroken()) { + shooter.stopFeeder(); + if (state != AutoState.SHOOTING && state != AutoState.SHOOT_PREP) { + RobotContainer.setAutoState(AutoState.SHOOT_PREP); + } + } else if (shooter.isRearBroken() && !shooter.isCenterBroken()) { + shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + } else { + shooter.stopFeeder(); + } + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/control/auto/AutoState.java b/src/main/java/frc/robot/commands/control/auto/AutoState.java new file mode 100644 index 0000000..93bcd66 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/AutoState.java @@ -0,0 +1,10 @@ +package frc.robot.commands.control.auto; + +public enum AutoState { + COLLECTING, + DEFAULT, + POOP_PREP, + POOPING, + SHOOT_PREP, + SHOOTING +} diff --git a/src/main/java/frc/robot/commands/control/auto/InstantShoot.java b/src/main/java/frc/robot/commands/control/auto/InstantShoot.java index 65a0a40..b087681 100644 --- a/src/main/java/frc/robot/commands/control/auto/InstantShoot.java +++ b/src/main/java/frc/robot/commands/control/auto/InstantShoot.java @@ -7,7 +7,7 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; +import frc.robot.RobotContainer; import frc.robot.subsystems.Shooter; public class InstantShoot extends Command { @@ -27,18 +27,20 @@ public InstantShoot(final Shooter shooter) { @Override public void initialize() { m_shotDebouncer = new Debouncer(SHOT_DEBOUNCE_TIME, DebounceType.kFalling); + RobotContainer.setAutoState(AutoState.SHOOTING); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.setFeederVoltage(Constants.Shooter.FEEDER_AUTO_VOLTS + 2.0); + // m_shooter.setFeederVoltage(Constants.Shooter.FEEDER_AUTO_VOLTS + 2.0); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_shooter.stopFeeder(); + // m_shooter.stopFeeder(); + RobotContainer.setAutoState(AutoState.COLLECTING); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/control/auto/Madtown.java b/src/main/java/frc/robot/commands/control/auto/Madtown.java new file mode 100644 index 0000000..f391cca --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/Madtown.java @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +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.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.RobotContainer; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.util.Util; +import java.util.function.BooleanSupplier; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Madtown extends ParallelCommandGroup { + + public static boolean SHOULD_WATCH_FOR_NOTE = false; + private static boolean INITIAL_WAS_INTERRUPTED = false; + + /** Creates a new Madtown. */ + public Madtown() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + BooleanSupplier isBlue = () -> CommandSwerveDrivetrain.getAlliance() == Alliance.Blue; + addCommands( + new SequentialCommandGroup( + new ParallelRaceGroup( + AutoBuilder.buildAuto("Madtown Initial"), + new WaitUntilCommand( + () -> { + final boolean shouldInterrupt = + SHOULD_WATCH_FOR_NOTE + && RobotContainer.INTAKE_PHOTON.hasTargets() + && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 20.0; + INITIAL_WAS_INTERRUPTED = shouldInterrupt; + return shouldInterrupt; + })), + new ConditionalCommand( + new AutoCollectNote( + 2.75) // TODO if this fails near note 4, then try for note 5, probably won't + // have time in 15 seconds + .withTimeout(1.0) + .andThen(Util.PathFindToAutoSourceCloseShot()) + .andThen( + new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER)), + new InstantCommand(), + () -> INITIAL_WAS_INTERRUPTED), + new RotateUntilNote(isBlue), + new AutoCollectNote(2.5), + Util.PathFindToAutoMadtownShot(), + new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER), + // new InstantShoot(RobotContainer.SHOOTER), + new RotateUntilNote(isBlue), + new AutoCollectNote(2.75), + Util.PathFindToAutoMadtownShot(), + new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER) + // new InstantShoot(RobotContainer.SHOOTER) + ), + new AutoAimLockWrist(RobotContainer.WRIST, RobotContainer::getAutoState), + new AutoIdleShooter(RobotContainer.SHOOTER, RobotContainer::getAutoState)); + } +} diff --git a/src/main/java/frc/robot/commands/control/auto/MiddleRaceCleanup.java b/src/main/java/frc/robot/commands/control/auto/MiddleRaceCleanup.java new file mode 100644 index 0000000..746e4ce --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/MiddleRaceCleanup.java @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +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.SequentialCommandGroup; +import frc.robot.commands.control.ShootSubwooferFirstHalf; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Vision; +import frc.robot.subsystems.Wrist; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class MiddleRaceCleanup extends SequentialCommandGroup { + /** Creates a new MiddleRaceCleanup. */ + public MiddleRaceCleanup( + final CommandSwerveDrivetrain drivetrain, + final Intake intake, + final Elevator elevator, + final Wrist wrist, + final Shooter shooter, + final Vision vision) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + // Temporarily hard code flipping the path. + final PathPlannerPath step1 = PathPlannerPath.fromPathFile("Middle Race 1").flipPath(); + final PathPlannerPath step2Success = + PathPlannerPath.fromPathFile("Middle Race 2 Success").flipPath(); + final PathPlannerPath step2Fail = PathPlannerPath.fromPathFile("Middle Race 2 Fail").flipPath(); + final PathPlannerPath step3 = PathPlannerPath.fromPathFile("Middle Race 3").flipPath(); + addCommands( + new ShootSubwooferFirstHalf(elevator, wrist, shooter), + new ParallelCommandGroup( + new SequentialCommandGroup( + new InstantCommand( + () -> { + drivetrain.seedFieldRelative(step1.getPreviewStartingHolonomicPose()); + }), + AutoBuilder.followPath(step1), + new ConditionalCommand( + AutoBuilder.followPath(step2Success), + AutoBuilder.followPath(step2Fail), + () -> shooter.isRearBroken() || shooter.isCenterBroken()), + new AutoCollectNote(step2Success.getGoalEndState().getVelocity()), + new AutoAimAndShoot(drivetrain, shooter), + AutoBuilder.followPath(step3), + new AutoCollectNote(step3.getGoalEndState().getVelocity()), + new AutoAimAndShoot(drivetrain, shooter)), + new AutoAimLockWrist(wrist), + new AutoIdleShooter(shooter))); + } +} diff --git a/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java b/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java new file mode 100644 index 0000000..7f1f256 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import java.util.function.BooleanSupplier; + +public class RotateUntilNote extends Command { + + private static final SwerveRequest.ApplyChassisSpeeds swerveRequest = + new SwerveRequest.ApplyChassisSpeeds(); + + private double shouldTurnClockwise; + private BooleanSupplier isClockwise; + + /** Creates a new RotateInPlace. */ + public RotateUntilNote(final BooleanSupplier isClockwise) { + this.isClockwise = isClockwise; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.DRIVETRAIN); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + shouldTurnClockwise = isClockwise.getAsBoolean() ? -1.0 : 1.0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + final double velocityScale = RobotContainer.INTAKE_PHOTON.hasTargets() ? 0.5 : 1.5; + RobotContainer.DRIVETRAIN.setControl( + swerveRequest.withSpeeds( + new ChassisSpeeds(0.0, 0.0, (Math.PI * velocityScale) * shouldTurnClockwise))); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.DRIVETRAIN.setControl(swerveRequest.withSpeeds(new ChassisSpeeds())); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return RobotContainer.INTAKE_PHOTON.hasTargets() + && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 7.5; + } +} diff --git a/src/main/java/frc/robot/commands/control/auto/Source_5_4.java b/src/main/java/frc/robot/commands/control/auto/Source_5_4.java new file mode 100644 index 0000000..fa617b6 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/auto/Source_5_4.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.RobotContainer; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class Source_5_4 extends ParallelCommandGroup { + + private static final Pose2d BLUE_STARTING_POSE = + PathPlannerAuto.getStaringPoseFromAutoFile("Source 5-4 Initial"); + private static final Pose2d RED_STARTING_POSE = + new Pose2d( + 16.56 - BLUE_STARTING_POSE.getX(), + BLUE_STARTING_POSE.getY(), + BLUE_STARTING_POSE.getRotation().plus(Rotation2d.fromDegrees(180.0))); + + /** Creates a new Source_5_4. */ + public Source_5_4() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new SequentialCommandGroup( + new InstantCommand( + () -> { + RobotContainer.DRIVETRAIN.seedFieldRelative( + RobotContainer.DRIVETRAIN.getAlliance().equals(Alliance.Red) + ? RED_STARTING_POSE + : BLUE_STARTING_POSE); + RobotContainer.setAutoState(AutoState.DEFAULT); + }), + AutoBuilder.buildAuto("Source 5-4 AB"), // Source 5-4 Initial + new ConditionalCommand( + AutoBuilder.buildAuto("Source 5-4 B 5Y4Y"), + new ConditionalCommand( + AutoBuilder.buildAuto("Source 5-4 5Y4N"), + new ConditionalCommand( + AutoBuilder.buildAuto("Source 5-4 5N4Y"), + AutoBuilder.buildAuto("Source 5-4 5N4N"), + () -> + !RobotContainer.POOP_MONITOR.hasPooped() + && RobotContainer.SHOOTER.hasNote()), + () -> + RobotContainer.POOP_MONITOR.hasPooped() + && !RobotContainer.SHOOTER.hasNote()), + () -> RobotContainer.POOP_MONITOR.hasPooped() && RobotContainer.SHOOTER.hasNote()), + new ParallelDeadlineGroup( + new WaitUntilCommand(() -> RobotContainer.SHOOTER.hasNote()), + AutoBuilder.buildAuto("Source 5-4 Preload"))), + // new IntakeNoteSequenceAuto( + // RobotContainer.SHOOTER, + // RobotContainer.get().INTAKE, + // RobotContainer.get().ELEVATOR))), + // .andThen(Util.PathFindToAutoSourceShot()) + // .andThen(new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER))), + new AutoAimLockWrist(RobotContainer.WRIST, RobotContainer::getAutoState), + new AutoIdleShooter(RobotContainer.SHOOTER, RobotContainer::getAutoState)); + } +} diff --git a/src/main/java/frc/robot/commands/control/note/IntakeNoteSequence.java b/src/main/java/frc/robot/commands/control/note/IntakeNoteSequence.java index be4e816..c3f02e3 100644 --- a/src/main/java/frc/robot/commands/control/note/IntakeNoteSequence.java +++ b/src/main/java/frc/robot/commands/control/note/IntakeNoteSequence.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; @@ -28,9 +29,10 @@ public IntakeNoteSequence(Shooter shooter, Intake intake, Wrist wrist, Elevator new InstantCommand( () -> { shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + // TODO PUT THIS SOMEWHERE ELSE + shooter.setRPMShoot(Constants.Shooter.SHOOTER_IDLE_RPM_CLOSE); intake.setRPM(Constants.INTAKE_RPM); - wrist.setDegrees(21); // testing - + wrist.setDegrees(12); // testing elevator.goHome(); }, shooter, @@ -38,6 +40,7 @@ public IntakeNoteSequence(Shooter shooter, Intake intake, Wrist wrist, Elevator wrist), new WaitCommand(0.1), new WaitUntilCommand(shooter::isRearBroken), + new InstantCommand(() -> RobotContainer.disableTeleopPointToNote()), // new InstantCommand(intake::stopTop, intake), new WaitUntilCommand(() -> shooter.isCenterBroken()), new InstantCommand( diff --git a/src/main/java/frc/robot/commands/control/note/IntakeNoteSequenceAuto.java b/src/main/java/frc/robot/commands/control/note/IntakeNoteSequenceAuto.java index 1305094..86c96c0 100644 --- a/src/main/java/frc/robot/commands/control/note/IntakeNoteSequenceAuto.java +++ b/src/main/java/frc/robot/commands/control/note/IntakeNoteSequenceAuto.java @@ -6,8 +6,9 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.RobotContainer; +import frc.robot.commands.control.auto.AutoState; import frc.robot.constants.Constants; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Intake; @@ -25,39 +26,19 @@ public IntakeNoteSequenceAuto(Shooter shooter, Intake intake, Elevator elevator) addCommands( new InstantCommand( () -> { - shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + RobotContainer.setAutoState(AutoState.COLLECTING); intake.setRPM(Constants.INTAKE_RPM); elevator.goHome(); }, intake), - new WaitCommand(0.1), - new WaitUntilCommand(shooter::isRearBroken), - // new InstantCommand(intake::stopTop, intake), - new WaitUntilCommand(() -> shooter.isCenterBroken()), - new InstantCommand( - () -> { - shooter.stopFeeder(); - intake.stopCollecting(); - }, - intake)); - } - - public IntakeNoteSequenceAuto(Shooter shooter, Intake intake, Elevator elevator, boolean val) { - addCommands( - new InstantCommand( - () -> { - shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS_AGRESSIVE); - intake.setRPM(Constants.INTAKE_RPM); - elevator.goHome(); - }, - intake), - new WaitCommand(0.1), - new WaitUntilCommand(shooter::isRearBroken), + // new WaitCommand(0.1), should be able to be removed + // new WaitUntilCommand(shooter::isRearBroken), // new InstantCommand(intake::stopTop, intake), new WaitUntilCommand(() -> shooter.isCenterBroken()), new InstantCommand( () -> { shooter.stopFeeder(); + RobotContainer.setAutoState(AutoState.DEFAULT); intake.stopCollecting(); }, intake)); diff --git a/src/main/java/frc/robot/commands/control/note/LobNote.java b/src/main/java/frc/robot/commands/control/note/LobNote.java index 4d6b6b5..9900929 100644 --- a/src/main/java/frc/robot/commands/control/note/LobNote.java +++ b/src/main/java/frc/robot/commands/control/note/LobNote.java @@ -6,20 +6,22 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Wrist; +import frc.robot.util.Util; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class LobNote extends SequentialCommandGroup { - private final Debouncer lineBreakDebouncer; + private Debouncer lineBreakDebouncer; private final double DEBOUNCE_TIME = 0.08; @@ -27,23 +29,24 @@ public class LobNote extends SequentialCommandGroup { // addCommands(new FooCommand(), new BarCommand()); /** Creates a new LobNote. */ - public LobNote(Shooter SHOOTER, Wrist WRIST, Elevator ELEVATOR) { - - lineBreakDebouncer = new Debouncer(DEBOUNCE_TIME, DebounceType.kFalling); + public LobNote(Shooter SHOOTER, Wrist WRIST, Elevator ELEVATOR, GenericEntry LOB_RPM) { addRequirements(SHOOTER, WRIST, ELEVATOR); addCommands( new InstantCommand( () -> { - WRIST.setDegrees(33); - SHOOTER.setRPMShoot(Constants.Shooter.SHOOTER_LOB_RPM); + lineBreakDebouncer = new Debouncer(DEBOUNCE_TIME, DebounceType.kFalling); + double dist = Util.getDistanceToAllianceLob(RobotContainer.get().getPose()); + WRIST.setDegrees(35.0); + if (dist > 5.5) { + SHOOTER.setRPMShoot(Util.getShooterSpeedFromDistanceForLob(dist)); + } else { + SHOOTER.setRPMShoot(1800.0); + } ELEVATOR.setLengthInches(0); }, SHOOTER, WRIST), - new WaitCommand(0.1), - new WaitUntilCommand( - () -> WRIST.isAtSetpoint() && SHOOTER.isShooterAtSetpoint() && ELEVATOR.isAtSetpoint()), - new WaitCommand(0.4), + new WaitUntilCommand(() -> WRIST.isAtSetpoint()), new InstantCommand( () -> SHOOTER.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS_AGRESSIVE), SHOOTER), @@ -55,7 +58,6 @@ public LobNote(Shooter SHOOTER, Wrist WRIST, Elevator ELEVATOR) { SHOOTER.stopShooter(); ELEVATOR.goHome(); }, - SHOOTER), - new WaitCommand(0.1)); + SHOOTER)); } } diff --git a/src/main/java/frc/robot/commands/control/note/PoopTest.java b/src/main/java/frc/robot/commands/control/note/PoopTest.java new file mode 100644 index 0000000..7df0b07 --- /dev/null +++ b/src/main/java/frc/robot/commands/control/note/PoopTest.java @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.control.note; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.constants.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Wrist; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class PoopTest extends SequentialCommandGroup { + /** Creates a new PoopTest. */ + public PoopTest(Shooter shooter, Intake intake, Wrist wrist) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + double POOP_RPM = 700; // 550s (CHANGE IN AUTOIDLESHOOTERTOO!!) + addRequirements(shooter, intake, wrist); + addCommands( + new InstantCommand( + () -> { + intake.setVolts(-8.0); + shooter.stopFeeder(); + shooter.setRPMShootNoSpin(POOP_RPM); + }), + new WaitCommand(1), + new InstantCommand( + () -> { + shooter.setFeederVoltage(Constants.Shooter.FEEDER_FEEDFWD_VOLTS); + shooter.setRPMShootNoSpin(POOP_RPM); + wrist.setDegrees(22); + })); + } +} diff --git a/src/main/java/frc/robot/commands/control/note/ShootNoteSequence.java b/src/main/java/frc/robot/commands/control/note/ShootNoteSequence.java index ba489b9..72ece4b 100644 --- a/src/main/java/frc/robot/commands/control/note/ShootNoteSequence.java +++ b/src/main/java/frc/robot/commands/control/note/ShootNoteSequence.java @@ -6,9 +6,8 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.*; -import frc.robot.commands.movement.PointAtAprilTag; +import frc.robot.commands.movement.PointAtSpeaker; import frc.robot.constants.Constants; import frc.robot.subsystems.*; import frc.robot.util.Util; @@ -59,12 +58,13 @@ public ShootNoteSequence( addCommands( new ParallelCommandGroup( // new FastPoint(drivetrain, photonVision), - new PointAtAprilTag(drivetrain, () -> 0.0, () -> 0.0, () -> 0.0), + new PointAtSpeaker(drivetrain, () -> 0.0, () -> 0.0, () -> 0.0), new InstantCommand( () -> { - DriverStation.reportWarning( - "Wrist Angle Attempted" + Util.getInterpolatedWristAngle(), false); - wrist.setDegrees(Util.getInterpolatedWristAngle()); + // DriverStation.reportWarning( + // "Wrist Angle Attempted" + Util.getInterpolatedWristAngleSpeaker(), + // false); + wrist.setDegrees(Util.getInterpolatedWristAngleSpeaker()); shooter.setRPMShoot(shootRPM); }, shooter, diff --git a/src/main/java/frc/robot/commands/movement/CollectNoteAuto.java b/src/main/java/frc/robot/commands/movement/CollectNoteAuto.java index 734b051..a97f696 100644 --- a/src/main/java/frc/robot/commands/movement/CollectNoteAuto.java +++ b/src/main/java/frc/robot/commands/movement/CollectNoteAuto.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.control.note.IntakeNoteSequence; import frc.robot.subsystems.*; @@ -71,7 +70,7 @@ public CollectNoteAuto( // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("DriveToNote - init"); + // System.out.println("DriveToNote - init"); // Apply the output to the swerve this.initialPose = drivetrain.getPose(); rotationController.reset(); @@ -90,8 +89,8 @@ public void initialize() { @Override public void execute() { if (!canSeePieceDebouncer.calculate(photonVision.hasTargets())) { - DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); - System.out.println("DriveToNote Can't see gamePice"); + // DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); + // System.out.println("DriveToNote Can't see gamePice"); drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); return; } @@ -129,12 +128,12 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DriverStation.reportWarning("DriveToNote Command Finished", false); - System.out.println("DriveToNote Command Finished"); + // DriverStation.reportWarning("DriveToNote Command Finished", false); + // System.out.println("DriveToNote Command Finished"); drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); if (isDistanceTraveledTooFar()) { - DriverStation.reportWarning("DriveToNote Drove Too Far", false); - System.out.println("DriveToNote Drove Too Far"); + // DriverStation.reportWarning("DriveToNote Drove Too Far", false); + // System.out.println("DriveToNote Drove Too Far"); } // Backups such that the command drives too far the motors will still stop diff --git a/src/main/java/frc/robot/commands/movement/DriveToNote.java b/src/main/java/frc/robot/commands/movement/DriveToNote.java index 2b30aea..1171103 100644 --- a/src/main/java/frc/robot/commands/movement/DriveToNote.java +++ b/src/main/java/frc/robot/commands/movement/DriveToNote.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Vision; @@ -55,7 +54,7 @@ public DriveToNote( // Called when the command is initially scheduled. @Override public void initialize() { - System.out.println("DriveToNote - init"); + // System.out.println("DriveToNote - init"); // Apply the output to the swerve this.initialPose = drivetrain.getPose(); rotationController.reset(); @@ -67,8 +66,8 @@ public void initialize() { @Override public void execute() { if (!canSeePieceDebouncer.calculate(photonVision.hasTargets())) { - DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); - System.out.println("DriveToNote Can't see gamePice"); + // DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); + // System.out.println("DriveToNote Can't see gamePice"); drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); return; } @@ -92,12 +91,12 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DriverStation.reportWarning("DriveToNote Command Finished", false); - System.out.println("DriveToNote Command Finished"); + // DriverStation.reportWarning("DriveToNote Command Finished", false); + // System.out.println("DriveToNote Command Finished"); drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); if (isDistanceTraveledTooFar()) { - DriverStation.reportWarning("DriveToNote Drove Too Far", false); - System.out.println("DriveToNote Drove Too Far"); + // DriverStation.reportWarning("DriveToNote Drove Too Far", false); + // System.out.println("DriveToNote Drove Too Far"); } } diff --git a/src/main/java/frc/robot/commands/movement/DriveToNote2.java b/src/main/java/frc/robot/commands/movement/DriveToNote2.java new file mode 100644 index 0000000..c5c727a --- /dev/null +++ b/src/main/java/frc/robot/commands/movement/DriveToNote2.java @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.movement; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc.robot.RobotContainer; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Vision; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveToNote2 extends PIDCommand { + + private static Debouncer createNoteDebouncer() { + return new Debouncer(0.4, DebounceType.kFalling); // 0.6 + } + + private static final SwerveRequest.ApplyChassisSpeeds swerveRequest = + new SwerveRequest.ApplyChassisSpeeds(); + private static Debouncer canSeeNoteDebouncer = createNoteDebouncer(); + + /** Creates a new DriveToNote2. */ + public DriveToNote2( + final CommandSwerveDrivetrain drivetrain, final Vision vision, final double initialVelocity) { + // FIXME probably mixing degrees and radians + super( + // The controller that the command will use + new PIDController(0.1, 0.0, 0.0), + // This should return the measurement + () -> vision.getYawVal(), + // This should return the setpoint (can also be a constant) + () -> 0.0, + // This uses the output + output -> { + if (RobotContainer.SHOOTER.isRearBroken()) { + drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds())); + return; + } + drivetrain.setControl( + swerveRequest.withSpeeds( + new ChassisSpeeds( + canSeeNoteDebouncer.calculate(vision.hasTargets()) ? initialVelocity : 0.0, + 0, + output * 0.75))); + }); + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. + addRequirements(drivetrain); + getController().enableContinuousInput(-180, 180); + getController().setTolerance(0.25, 0.1); + } + + @Override + public void initialize() { + canSeeNoteDebouncer = createNoteDebouncer(); + canSeeNoteDebouncer.calculate(true); + super.initialize(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; // Assume parallel deadline command stops this command. + } +} diff --git a/src/main/java/frc/robot/commands/movement/DriveToNoteAuto.java b/src/main/java/frc/robot/commands/movement/DriveToNoteAuto.java index bfa78f2..de7e88d 100644 --- a/src/main/java/frc/robot/commands/movement/DriveToNoteAuto.java +++ b/src/main/java/frc/robot/commands/movement/DriveToNoteAuto.java @@ -11,7 +11,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.commands.control.note.IntakeNoteSequence; @@ -74,7 +73,7 @@ public DriveToNoteAuto( // Called when the command is initially scheduled. @Override public void initialize() { - DriverStation.reportWarning("DriveToNote - init", false); + // DriverStation.reportWarning("DriveToNote - init", false); // Apply the output to the swerve this.initialPose = drivetrain.getPose(); @@ -88,7 +87,7 @@ public void initialize() { @Override public void execute() { if (!canSeePieceDebouncer.calculate(photonVision.hasTargets())) { - DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); + // DriverStation.reportWarning("DriveToPiece Can't see gamePicee", false); // System.out.println("DriveToNote Can't see gamePice"); drivetrain.setControl( swerveRequest.withSpeeds(new ChassisSpeeds(previousForwardBackwardSpeed, 0, 0))); @@ -118,7 +117,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DriverStation.reportWarning("DriveToNote Command Finished", false); + // DriverStation.reportWarning("DriveToNote Command Finished", false); drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); new InstantCommand( () -> { @@ -128,9 +127,9 @@ public void end(boolean interrupted) { }, shooter, intake); - if (isDistanceTraveledTooFar()) { - DriverStation.reportWarning("DriveToNote Drove Too Far", false); - } + // if (isDistanceTraveledTooFar()) { + // DriverStation.reportWarning("DriveToNote Drove Too Far", false); + // } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/movement/FastPoint.java b/src/main/java/frc/robot/commands/movement/FastPoint.java index 77dd6ba..7fd199d 100644 --- a/src/main/java/frc/robot/commands/movement/FastPoint.java +++ b/src/main/java/frc/robot/commands/movement/FastPoint.java @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.Limelight; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; public class FastPoint extends Command { @@ -39,7 +39,7 @@ public FastPoint(CommandSwerveDrivetrain drivetrain, String limelightname) { @Override public void initialize() { - initialYaw = Limelight.getTX(limelightname); // TX or TY? + initialYaw = LimelightHelpers.getTX(limelightname); // TX or TY? initalRotation = drivetrain.getPose().getRotation().getDegrees(); thetaController.setSetpoint(initalRotation - initialYaw); @@ -59,14 +59,14 @@ public void execute() { .withVelocityY(0.0) // Drive left with negative X (left) .withRotationalRate( rotationalVelocity); // Drive counterclockwise with negative X (left) - System.out.println(rotationalVelocity); + // System.out.println(rotationalVelocity); drivetrain.setControl(driveRequest); } @Override public boolean isFinished() { // return false; - return Util.isWithinTolerance(Limelight.getTX(limelightname), 0.0, 0.02); // TX or TY? + return Util.isWithinTolerance(LimelightHelpers.getTX(limelightname), 0.0, 0.02); // TX or TY? } @Override diff --git a/src/main/java/frc/robot/commands/movement/PointAtAprilTag.java b/src/main/java/frc/robot/commands/movement/PointAtSpeaker.java similarity index 67% rename from src/main/java/frc/robot/commands/movement/PointAtAprilTag.java rename to src/main/java/frc/robot/commands/movement/PointAtSpeaker.java index f9485f6..0634504 100644 --- a/src/main/java/frc/robot/commands/movement/PointAtAprilTag.java +++ b/src/main/java/frc/robot/commands/movement/PointAtSpeaker.java @@ -9,50 +9,59 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.util.Util; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -public class PointAtAprilTag extends Command { +public class PointAtSpeaker extends Command { private final CommandSwerveDrivetrain drivetrain; private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(Constants.MaxSpeed * 0.1) - .withRotationalDeadband(Constants.MaxAngularRate * 0.1) // Add a 10% deadband + .withRotationalDeadband(Constants.MaxAngularRate * 0.05) // Add a 5% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric private DoubleSupplier velocityXSupplier = () -> 0.0; // getAllianceLob private DoubleSupplier velocityYSupplier = () -> 0.0; private DoubleSupplier rotationSupplier = () -> 0.0; private double desiredRotation; private final PIDController THETA_CONTROLLER; - private boolean shouldLob = false; + private BooleanSupplier shouldLob = () -> false; + private BooleanSupplier shouldStop = () -> false; double rotationRate = 0; - public PointAtAprilTag( + public PointAtSpeaker( CommandSwerveDrivetrain drivetrain, DoubleSupplier velocityXSupplier, DoubleSupplier velocityYSupplier, DoubleSupplier rotationSupplier) { - this(drivetrain, velocityXSupplier, velocityYSupplier, rotationSupplier, false); + this( + drivetrain, + velocityXSupplier, + velocityYSupplier, + rotationSupplier, + () -> false, + () -> false); } - public PointAtAprilTag( + public PointAtSpeaker( CommandSwerveDrivetrain drivetrain, DoubleSupplier velocityXSupplier, DoubleSupplier velocityYSupplier, DoubleSupplier rotationSupplier, - boolean shouldLob) { + BooleanSupplier shouldLob, + BooleanSupplier shouldStop) { this.drivetrain = drivetrain; this.velocityXSupplier = velocityXSupplier; this.velocityYSupplier = velocityYSupplier; this.rotationSupplier = rotationSupplier; this.shouldLob = shouldLob; - THETA_CONTROLLER = new PIDController(0.15, 0.0, 0.0); // (0.183, 0.1, 0.0013) - THETA_CONTROLLER.enableContinuousInput(-180, 180); + this.shouldStop = shouldStop; + THETA_CONTROLLER = new PIDController(0.13, 0.01, 0.0); // (0.183, 0.1, 0.0013) + THETA_CONTROLLER.enableContinuousInput(-180.0, 180.0); THETA_CONTROLLER.setTolerance(0.01, 0.01); addRequirements(drivetrain); } @@ -66,28 +75,20 @@ public double getRotationRate() { @Override public void execute() { - Pose2d current = drivetrain.getPose(); + // Pose2d current = drivetrain.getPose(); + Pose2d pose = drivetrain.getPose(); desiredRotation = - shouldLob - ? current.getRotation().minus(Util.getRotationToAllianceLob(current)).getDegrees() - : current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees(); - DriverStation.reportWarning(desiredRotation + " degrees", false); - - // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new - // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new - // Rotation2d(desiredRotation))); - double rotationRate = 0.0; - if (Util.alliance == DriverStation.Alliance.Blue) { - System.out.println("+90"); - rotationRate = - THETA_CONTROLLER.calculate( - drivetrain.getPose().getRotation().getDegrees() + 90, desiredRotation); - } else { - System.out.println("-90"); - rotationRate = - THETA_CONTROLLER.calculate( - drivetrain.getPose().getRotation().getDegrees() + 90, desiredRotation); - } + shouldLob.getAsBoolean() + ? Util.getRotationToAllianceLob(pose).getDegrees() + : Util.getRotationToAllianceSpeaker(pose).getDegrees(); + double rotationRate = + THETA_CONTROLLER.calculate(pose.getRotation().getDegrees(), desiredRotation); + // System.out.println( + // "Current Angle: " + // + drivetrain.getPose().getRotation().getDegrees() + // + " | Desired Angle: " + // + desiredRotation); + // System.out.println("Rotation Rate: " + rotationRate); drivetrain.setControl( drive .withVelocityX( @@ -103,7 +104,7 @@ public void execute() { @Override public boolean isFinished() { - return false; + return shouldStop.getAsBoolean(); } @Override diff --git a/src/main/java/frc/robot/commands/movement/SquareUpToAprilTag.java b/src/main/java/frc/robot/commands/movement/SquareUpToAprilTag.java deleted file mode 100644 index 3133d71..0000000 --- a/src/main/java/frc/robot/commands/movement/SquareUpToAprilTag.java +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.movement; - -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Vision; -import frc.robot.util.Util; - -public class SquareUpToAprilTag extends Command { - private final String LOG_PREFIX = "[EXECUTE] "; - - private final PIDController LATERAL_CONTROLLER = new PIDController(0.1, 0.001, 0.01); - private final PIDController ROTATIONAL_CONTROLLER = new PIDController(0.15, 0, 0.01); - private final PIDController DISTANCE_CONTROLLER = new PIDController(0.60, 1, 0.1); - - private final LinearFilter LATERAL_FILTER = LinearFilter.movingAverage(15); - private final LinearFilter DISTANCE_FILTER = LinearFilter.movingAverage(8); - private final LinearFilter SKEW_FILTER = LinearFilter.movingAverage(8); - - private final double ACCEPTABLE_SKEW_ERROR = 0.0; // Degrees within acceptance - private final double ACCEPTABLE_DISTANCE_ERROR = 0.05; // metersP within acceptance - private final Vision photonVision; - private final CommandSwerveDrivetrain drivetrain; - private double ACCEPTABLE_DISTANCE = 3; // 1.3 - private double distanceError; - private double xOffset; - private double skew; - private double distanceToTarget; - private SwerveRequest.ApplyChassisSpeeds swerveRequest = new SwerveRequest.ApplyChassisSpeeds(); - private int noVisibleTargetLoops = 0; - private double targetHeight = 1.45; // 1.23 Meters - - public SquareUpToAprilTag( - CommandSwerveDrivetrain drivetrain, - Vision photonVision, - double targetHeight, - double acceptableDistance) { - this.drivetrain = drivetrain; - this.photonVision = photonVision; - this.targetHeight = targetHeight; - this.ACCEPTABLE_DISTANCE = acceptableDistance; - } - - @Override - public void initialize() { - System.out.println("Squaring up to AprilTag..."); - xOffset = photonVision.getYawVal(); - skew = photonVision.getSkewVal(); - distanceToTarget = - Vision.calculateDistanceToTarget( - photonVision.getPitchVal(), - photonVision.getCameraHeight(), - targetHeight, - photonVision.getCameraDegrees()); - } - - @Override - public void execute() { - System.out.println(LOG_PREFIX + "In execute()"); - if (!photonVision.hasTargets()) { - System.out.println(LOG_PREFIX + "No visible target."); - noVisibleTargetLoops++; - drivetrain.setControl(swerveRequest); - - } else { - noVisibleTargetLoops = 0; - xOffset = photonVision.getYawVal(); - skew = photonVision.getSkewVal(); - distanceToTarget = - Vision.calculateDistanceToTarget( - photonVision.getPitchVal(), - photonVision.getCameraHeight(), - targetHeight, - photonVision.getCameraDegrees()); - - if (skew > 70) { - skew = skew - 90; - } - - skew = SKEW_FILTER.calculate(skew); - - System.out.println(LOG_PREFIX + "Skew: " + skew); - - double forwardBackwardSpeed = 0; - double rotationRate = 0; - double lateralSpeed = 0; - - distanceError = distanceToTarget - ACCEPTABLE_DISTANCE; - forwardBackwardSpeed = DISTANCE_CONTROLLER.calculate(distanceError); - - rotationRate = ROTATIONAL_CONTROLLER.calculate(-xOffset); - lateralSpeed = LATERAL_CONTROLLER.calculate(skew); - - swerveRequest = - swerveRequest.withSpeeds( - new ChassisSpeeds( - -DISTANCE_FILTER.calculate(forwardBackwardSpeed), - LATERAL_FILTER.calculate(lateralSpeed), - -rotationRate)); - - // Apply the request to the drivetrain - drivetrain.setControl(swerveRequest); - } - } - - @Override - public boolean isFinished() { - return (Util.isWithinTolerance(distanceToTarget, ACCEPTABLE_DISTANCE, ACCEPTABLE_DISTANCE_ERROR) - && Math.abs(skew) <= ACCEPTABLE_SKEW_ERROR) - || (noVisibleTargetLoops >= 10); - } - - @Override - public void end(boolean interrupted) { - System.out.println(LOG_PREFIX + "Reached end()"); - drivetrain.setControl(swerveRequest.withSpeeds(new ChassisSpeeds(0, 0, 0))); - - if (interrupted) {} - } -} diff --git a/src/main/java/frc/robot/commands/movement/SwerveCommand.java b/src/main/java/frc/robot/commands/movement/SwerveCommand.java index 38043f5..0b3c0db 100644 --- a/src/main/java/frc/robot/commands/movement/SwerveCommand.java +++ b/src/main/java/frc/robot/commands/movement/SwerveCommand.java @@ -2,8 +2,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.util.Util; @@ -50,8 +50,9 @@ public SwerveCommand( this.ROTATION_SUPPLIER = rotationSupplier; addRequirements(drivetrain); - THETA_CONTROLLER = new PIDController(0.11, 0.0, 0.0); + THETA_CONTROLLER = new PIDController(0.2, 0.0, 0.0); THETA_CONTROLLER.enableContinuousInput(-180, 180); + THETA_CONTROLLER.setTolerance(0.25, 0.1); } @Override @@ -71,7 +72,7 @@ public void execute() { if (isCardinalLocking && !Util.isWithinTolerance(ROTATION_SUPPLIER.getAsDouble(), 0.0, 0.25)) { isCardinalLocking = false; setPoint = 0.0; - DriverStation.reportWarning("Stop using DPad.", false); + // DriverStation.reportWarning("Stop using DPad.", false); } else if (POV_DEGREE.getAsInt() >= 0) { isCardinalLocking = true; switch (POV_DEGREE.getAsInt()) { @@ -92,31 +93,40 @@ public void execute() { } } if (isCardinalLocking) { - THETA_CONTROLLER.setSetpoint(setPoint); rotationPercentage = - THETA_CONTROLLER.calculate( - DRIVETRAIN.getPose().getRotation().getDegrees(), THETA_CONTROLLER.getSetpoint()); + THETA_CONTROLLER.calculate(DRIVETRAIN.getPose().getRotation().getDegrees(), setPoint); + } + + if (RobotContainer.shouldTelopPointToNote() + && RobotContainer.INTAKE_PHOTON.hasTargets() + && !RobotContainer.get().SHOOTER.isCenterBroken()) { + double heading = DRIVETRAIN.getPose().getRotation().getDegrees(); + double visionAdjust = RobotContainer.INTAKE_PHOTON.getYawVal(); + rotationPercentage = THETA_CONTROLLER.calculate(heading, heading - visionAdjust); } // WIP - trying to auto point to amp feed and then speaker depending on pose by default - // if (!isCardinalLocking && Util.isWithinTolerance(ROTATION_SUPPLIER.getAsDouble(), 0.0, 0.25) - // && RobotContainer.get().SHOOTER.isCenterBroken()) { - // - // THETA_CONTROLLER.setSetpoint(Util.getRotationToAllianceSpeaker(DRIVETRAIN.getPose()).getDegrees()); - // System.out.println(Util.getRotationToAllianceSpeaker(DRIVETRAIN.getPose()).getDegrees()); - // rotationPercentage = - // THETA_CONTROLLER.calculate( - // DRIVETRAIN.getPose().getRotation().getDegrees(), THETA_CONTROLLER.getSetpoint()); + // if (!isCardinalLocking + // && Util.isWithinTolerance(ROTATION_SUPPLIER.getAsDouble(), 0.0, 0.25) + // && RobotContainer.get().SHOOTER.isCenterBroken() + // ) { + // final Pose2d currentPose = DRIVETRAIN.getPose(); + // final Rotation2d targetRotation = Math.abs(Util.getDistanceToSpeaker()) > 6.0 ? + // Util.getRotationToAllianceLob(currentPose) : Util.getRotationToAllianceSpeaker(currentPose); + // // rotationPercentage = + // // THETA_CONTROLLER.calculate( + // // currentPose.getRotation().getDegrees(), + // Util.getRotationToAllianceSpeaker(currentPose).getDegrees()); + // System.out.println("currentRotation" + currentPose.getRotation().getDegrees() + ", + // desiredRotation: " + targetRotation.getDegrees()); // } - double rotationalVelocity = rotationPercentage; - DRIVETRAIN.setControl( DRIVE_REQUEST .withVelocityX(xPercentage) // Drive forward with // negative Y (forward) .withVelocityY(yPercentage) // Drive left with negative X (left) - .withRotationalRate(rotationalVelocity)); + .withRotationalRate(rotationPercentage)); } @Override diff --git a/src/main/java/frc/robot/commands/qol/AsyncCANdle.java b/src/main/java/frc/robot/commands/qol/AsyncCANdle.java index af41e40..9d9a0fd 100644 --- a/src/main/java/frc/robot/commands/qol/AsyncCANdle.java +++ b/src/main/java/frc/robot/commands/qol/AsyncCANdle.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Candles; +import frc.robot.subsystems.Candles.CandleSide; public class AsyncCANdle extends Command { private final Candles candles; @@ -33,7 +34,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - candles.setColorLeft(color.red, color.green, color.blue); + candles.setColor(CandleSide.LEFT, color); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/qol/DefaultCANdle.java b/src/main/java/frc/robot/commands/qol/DefaultCANdle.java index 0be94bb..3c6194e 100644 --- a/src/main/java/frc/robot/commands/qol/DefaultCANdle.java +++ b/src/main/java/frc/robot/commands/qol/DefaultCANdle.java @@ -4,15 +4,22 @@ package frc.robot.commands.qol; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Candles; import frc.robot.subsystems.Shooter; import frc.robot.util.Util; public class DefaultCANdle extends Command { + + private static final Color8Bit AQUA = new Color8Bit(Color.kAqua); + private static final Color8Bit BROWN = new Color8Bit(Color.kBrown); + private static final Color8Bit GREEN = new Color8Bit(Color.kGreen); + private static final Color8Bit RED = new Color8Bit(Color.kRed); + private final Shooter SHOOTER; private final Candles CANDLES; - private final double BLINK_CONSTANT = 0.002; // in seconds? /** Creates a new DefaultCANdle. */ public DefaultCANdle(Candles CANDLES, Shooter SHOOTER) { @@ -28,29 +35,21 @@ public void initialize() {} @Override public void execute() { if (!SHOOTER.isRearBroken()) { - CANDLES.setColorLeftRed(); + CANDLES.setColor(Candles.CandleSide.LEFT, RED); return; } if (!SHOOTER.isCenterBroken()) { - CANDLES.setColorLeftBrown(); - + CANDLES.setColor(Candles.CandleSide.LEFT, BROWN); return; } - CANDLES.setColorLeft(0, 128, 128); + CANDLES.setColor(Candles.CandleSide.LEFT, AQUA); if (!Util.isValidShot()) { - CANDLES.setColorRightRed(); - return; + CANDLES.setColor(Candles.CandleSide.RIGHT, RED); + } else { + CANDLES.setColor(Candles.CandleSide.RIGHT, GREEN); } - - // if (Util.isWithinTolerance(Limelight.getTY(SPEAKER_LIMELIGHT), 0.0, 1) - // && ((Timer.getFPGATimestamp()) / BLINK_CONSTANT) == 0) { - // CANDLES.setColorRightOff(); - // return; - // } - - CANDLES.setColorRightGreen(); } @Override diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 49bb787..1802c3d 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -63,15 +63,14 @@ public static class Trap { public static final double TRAP_ELEVATOR_HEIGHT_MIDWAY = 24.0; public static final double TRAP_WRIST_DEGREES = 119.0; public static final double TRAP_WRIST_DEGREES_MIDWAY = 85.0; - public static final double TRAP_RPM_SPEED = - 375; // suggesting but not doing 350 at GKC // 425, 475 worked but touched the top + public static final double TRAP_RPM_SPEED = 440; // 225 } public static class Wrist { public static final int PROXIMITY_SENSOR_LEFT_ID = 9; public static final int PROXIMITY_SENSOR_RIGHT_ID = 8; // TODO: Update left and right - public static final double WRIST_KP = 200.0; + public static final double WRIST_KP = 250.0; // 200 public static final double WRIST_KI = 0.0; public static final double WRIST_KD = 0.01; public static final double WRIST_KV = 0.03; @@ -99,7 +98,7 @@ public static class Wrist { } public static class Shooter { - public static final double FEEDER_FEEDFWD_VOLTS = 4; // 6 // 4 + public static final double FEEDER_FEEDFWD_VOLTS = 4.25; // 4 public static final double FEEDER_FEEDFWD_VOLTS_AGRESSIVE = 6; // 6 // 4 public static final double FEEDER_SHOOT_VOLTS = 8; // 4 public static final double FEEDER_RETRACT_VOLTS = -2; @@ -108,6 +107,7 @@ public static class Shooter { public static final double SHOOTER_RPM = 4500; public static final double SHOOTER_RPM_CLOSERANGE = 3500; // NEEDS to be smaller public static final double SHOOTER_LOB_RPM = 3000; // NEEDS to be smaller + public static final double SHOOTER_IDLE_SHOOTING_RPM = 3000; // NEEDS to be smaller public static final double SHOOTER_IDLE_RPM = 2500; // 2500 public static final double SHOOTER_IDLE_RPM_CLOSE = 4200; // 2500 public static final double SHOOTER_IDLE_CLOSERANGE_RPM = 2500; // NEEDS to be smaller @@ -166,17 +166,10 @@ public static class Climber { // InterpolatingDouble> // TODO Update Limelight Constants with new position // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR = // (Meters, Wrist Degrees) // new InterpolatingTreeMap<>(); - - public static final InterpolatingTreeMap< - InterpolatingDouble, - InterpolatingDouble> // TODO Update Limelight Constants with new position - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE = // (Meters, Wrist Degrees) - new InterpolatingTreeMap<>(); - public static final InterpolatingTreeMap< InterpolatingDouble, InterpolatingDouble> // TODO Update Limelight Constants with new position - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE = // (Meters, Wrist Degrees) + DISTANCE_TO_LOB_RPM = // (Meters, Wrist Degrees) new InterpolatingTreeMap<>(); public static final InterpolatingTreeMap< InterpolatingDouble, @@ -185,98 +178,6 @@ public static class Climber { new InterpolatingTreeMap<>(); static { - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-8.10), new InterpolatingDouble(2.292)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-9.19), new InterpolatingDouble(2.395)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-10.15), new InterpolatingDouble(2.496)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-11.75), new InterpolatingDouble(2.702)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-12.63), new InterpolatingDouble(2.785)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-13.40), new InterpolatingDouble(2.877)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-13.93), new InterpolatingDouble(2.946)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-14.45), new InterpolatingDouble(3.019)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-14.91), new InterpolatingDouble(3.091)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-15.36), new InterpolatingDouble(3.171)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-15.97), new InterpolatingDouble(3.267)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-16.70), new InterpolatingDouble(3.397)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-17.15), new InterpolatingDouble(3.491)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-17.70), new InterpolatingDouble(3.599)); - - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-17.94), new InterpolatingDouble(3.671)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-18.42), new InterpolatingDouble(3.761)); - - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-18.84), new InterpolatingDouble(3.883)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-19.18), new InterpolatingDouble(3.975)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-19.45), new InterpolatingDouble(4.053)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-19.70), new InterpolatingDouble(4.114)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-20.01), new InterpolatingDouble(4.181)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-20.29), new InterpolatingDouble(4.256)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_BLUESIDE.put( - new InterpolatingDouble(-20.60), new InterpolatingDouble(4.434)); - - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-7.56), new InterpolatingDouble(2.267)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-10.06), new InterpolatingDouble(2.525)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-11.15), new InterpolatingDouble(2.627)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-12.67), new InterpolatingDouble(2.817)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-13.98), new InterpolatingDouble(2.992)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-14.93), new InterpolatingDouble(3.127)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-15.61), new InterpolatingDouble(3.231)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-16.19), new InterpolatingDouble(3.332)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-16.75), new InterpolatingDouble(3.426)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-17.05), new InterpolatingDouble(3.485)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-17.73), new InterpolatingDouble(3.622)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-17.95), new InterpolatingDouble(3.668)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-18.40), new InterpolatingDouble(3.758)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-18.87), new InterpolatingDouble(3.867)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-19.15), new InterpolatingDouble(3.933)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-19.59), new InterpolatingDouble(4.04)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-19.89), new InterpolatingDouble(4.124)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-20.30), new InterpolatingDouble(4.194)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-20.46), new InterpolatingDouble(4.251)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-20.68), new InterpolatingDouble(4.318)); - PITCH_TO_DISTANCE_RELATIVE_SPEAKER_REDSIDE.put( - new InterpolatingDouble(-20.97), new InterpolatingDouble(4.415)); - DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put( new InterpolatingDouble(2.334), new InterpolatingDouble(35.50)); DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put( @@ -316,20 +217,13 @@ public static class Climber { new InterpolatingDouble(5.89), new InterpolatingDouble(21.47)); DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.put( new InterpolatingDouble(6.15), new InterpolatingDouble(21.45)); - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put(new InterpolatingDouble(0.9), new - - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-8.3), new InterpolatingDouble(33.0)); // 10 ft away - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-9.7), new InterpolatingDouble(32.0)); // 10 ft away - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-10.5), new InterpolatingDouble(31.0)); // 10 ft away - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-11.2), new InterpolatingDouble(30.5)); // 11 ft away - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-12.5), new InterpolatingDouble(29.0)); // 12 ft away - // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( - // new InterpolatingDouble(-14.3), new InterpolatingDouble(28.5)); // + + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(10.28), new InterpolatingDouble(3000.0 * 1.15)); + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(9.5), new InterpolatingDouble(2750.0 * 1.15)); + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(8.6), new InterpolatingDouble(2700.0 * 1.15)); + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(8.3), new InterpolatingDouble(2620.0 * 1.15)); + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(8.0), new InterpolatingDouble(2580.0 * 1.15)); + DISTANCE_TO_LOB_RPM.put(new InterpolatingDouble(7.88), new InterpolatingDouble(2440.0 * 1.15)); } public static class Vision { diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java index 832c739..cc56116 100644 --- a/src/main/java/frc/robot/constants/DriveConstants.java +++ b/src/main/java/frc/robot/constants/DriveConstants.java @@ -1,6 +1,8 @@ package frc.robot.constants; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.ClosedLoopOutputType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -72,8 +74,17 @@ public class DriveConstants { private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); + private static final CurrentLimitsConfigs driveCurrentLimits = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(110) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(70) + .withSupplyCurrentLimitEnable(true) + .withSupplyTimeThreshold(0.5); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() + .withDriveMotorInitialConfigs( + new TalonFXConfiguration().withCurrentLimits(driveCurrentLimits)) .withDriveMotorGearRatio(kDriveGearRatio) .withSteerMotorGearRatio(kSteerGearRatio) .withWheelRadius(kWheelRadiusInches) diff --git a/src/main/java/frc/robot/subsystems/AmpSensors.java b/src/main/java/frc/robot/subsystems/AmpSensors.java index 8643a75..30c71bb 100644 --- a/src/main/java/frc/robot/subsystems/AmpSensors.java +++ b/src/main/java/frc/robot/subsystems/AmpSensors.java @@ -18,15 +18,21 @@ public class AmpSensors extends SubsystemBase { public AmpSensors() { m_sensor_left = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_LEFT_ID); m_sensor_right = new DigitalInput(Constants.Wrist.PROXIMITY_SENSOR_RIGHT_ID); - Shuffleboard.getTab("MAIN").addBoolean("Amp Sensor", () -> getBothSensors()); + if (Constants.shouldShuffleboard) { + Shuffleboard.getTab("MAIN").addBoolean("Amp Left", () -> getSensorLeft()); + Shuffleboard.getTab("MAIN").addBoolean("Amp Rights", () -> getSensorRight()); + Shuffleboard.getTab("MAIN").addBoolean("Amp Sensor", () -> getBothSensors()); + } } public boolean getSensorLeft() { - return m_sensor_left.get(); + // 6.2 inches from wall + return !m_sensor_left.get(); } public boolean getSensorRight() { - return m_sensor_right.get(); + // 6.2 inches from wall + return !m_sensor_right.get(); } public boolean getBothSensors() { diff --git a/src/main/java/frc/robot/subsystems/Candles.java b/src/main/java/frc/robot/subsystems/Candles.java index 548da8d..0042c85 100644 --- a/src/main/java/frc/robot/subsystems/Candles.java +++ b/src/main/java/frc/robot/subsystems/Candles.java @@ -6,52 +6,53 @@ import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; +import edu.wpi.first.wpilibj.util.Color8Bit; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Candles extends SubsystemBase { - private final CANdle LEFT_CANDLE; - private final CANdle RIGHT_CANDLE; - - /** Creates a new Candles. */ - public Candles(int LEFT_CANDLE_ID, int RIGHT_CANDLE_ID) { - LEFT_CANDLE = new CANdle(LEFT_CANDLE_ID, "canfd"); - RIGHT_CANDLE = new CANdle(RIGHT_CANDLE_ID, "canfd"); - } - - public void setAnimationBoth(Animation animation) { - LEFT_CANDLE.clearAnimation(0); - RIGHT_CANDLE.clearAnimation(0); - LEFT_CANDLE.animate(animation); - RIGHT_CANDLE.animate(animation); - } - public void setAnimationRight(Animation animation) { - RIGHT_CANDLE.animate(animation); - } + public enum CandleSide { + BOTH(0), + LEFT(1), + RIGHT(2); - public void setAnimationLeft(Animation animation) { - LEFT_CANDLE.animate(animation); - } + private int candle; - public void setColorBoth(int r, int g, int b) { - LEFT_CANDLE.setLEDs(r, g, b); - RIGHT_CANDLE.setLEDs(r, g, b); - } + CandleSide(int i) { + this.candle = i; + } - public void setColorBothRed() { - setColorBoth(255, 0, 0); + public int side() { + return candle; + } } - public void setColorBothGreen() { - setColorBoth(0, 255, 0); - } + private final CANdle LEFT_CANDLE; + private final CANdle RIGHT_CANDLE; - public void setColorBothBlue() { - setColorBoth(255, 0, 255); + /** Creates a new Candles. */ + public Candles(int LEFT_CANDLE_ID, int RIGHT_CANDLE_ID) { + LEFT_CANDLE = new CANdle(LEFT_CANDLE_ID, "canfd"); + RIGHT_CANDLE = new CANdle(RIGHT_CANDLE_ID, "canfd"); } - public void setColorBothBrown() { - setColorBoth(128, 64, 0); + public void setAnimation(CandleSide side, Animation animation) { + switch (side) { + case LEFT: + LEFT_CANDLE.clearAnimation(0); + LEFT_CANDLE.animate(animation); + break; + case RIGHT: + RIGHT_CANDLE.clearAnimation(0); + RIGHT_CANDLE.animate(animation); + break; + case BOTH: + LEFT_CANDLE.clearAnimation(0); + RIGHT_CANDLE.clearAnimation(0); + LEFT_CANDLE.animate(animation); + RIGHT_CANDLE.animate(animation); + break; + } } public void stop() { @@ -61,52 +62,19 @@ public void stop() { RIGHT_CANDLE.clearAnimation(1); } - public void setColorLeft(int r, int g, int b) { - LEFT_CANDLE.setLEDs(r, g, b); - } - - public void setColorLeftRed() { - setColorLeft(255, 0, 0); - } - - public void setColorLeftGreen() { - setColorLeft(0, 255, 0); - } - - public void setColorLeftBlue() { - setColorLeft(255, 0, 255); - } - - public void setColorLeftBrown() { - setColorLeft(128, 64, 0); - } - - public void setColorLeftOff() { - setColorLeft(0, 0, 0); - } - - public void setColorRight(int r, int g, int b) { - RIGHT_CANDLE.setLEDs(r, g, b); - } - - public void setColorRightRed() { - setColorRight(255, 0, 0); - } - - public void setColorRightGreen() { - setColorRight(0, 255, 0); - } - - public void setColorRightBlue() { - setColorRight(255, 0, 255); - } - - public void setColorRightBrown() { - setColorRight(128, 64, 0); - } - - public void setColorRightOff() { - setColorRight(0, 0, 0); + public void setColor(CandleSide side, Color8Bit color) { + switch (side) { + case LEFT: + LEFT_CANDLE.setLEDs(color.red, color.green, color.blue); + break; + case RIGHT: + RIGHT_CANDLE.setLEDs(color.red, color.green, color.blue); + break; + case BOTH: + LEFT_CANDLE.setLEDs(color.red, color.green, color.blue); + RIGHT_CANDLE.setLEDs(color.red, color.green, color.blue); + break; + } } public void off() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 4a40885..333cca8 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -14,9 +14,13 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -25,7 +29,9 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.RobotContainer; +import frc.robot.constants.Constants; import frc.robot.constants.DriveConstants; +import frc.robot.util.LimelightHelpers; import frc.robot.util.Util; import java.util.Optional; import java.util.function.Supplier; @@ -58,6 +64,8 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private final SwerveRequest.SysIdSwerveSteerGains SteerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private boolean shouldMt2Update = true; + /* Use one of these sysidroutines for your particular test */ private SysIdRoutine SysIdRoutineTranslation = new SysIdRoutine( @@ -130,11 +138,11 @@ private void configurePathPlanner() { this.setControl( AutoRequest.withSpeeds(speeds)), // Consumer of ChassisSpeeds to drive the robot new HolonomicPathFollowerConfig( - new PIDConstants(12.5, 0, 0), - new PIDConstants(11.0, 0, 0), + new PIDConstants(13.5, 0, 0), + new PIDConstants(11.3, 0, 0), DriveConstants.kSpeedAt12VoltsMps, driveBaseRadius, - new ReplanningConfig()), + new ReplanningConfig(true, true)), () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance @@ -149,7 +157,7 @@ public Optional getRotationTargetOverride() { // Some condition that should decide if we want to override rotation if (RobotContainer.aimAtTargetAuto) { Pose2d pose = getPose(); - double currentDegrees = pose.getRotation().getDegrees() + 90.0; + double currentDegrees = pose.getRotation().getDegrees(); double desiredRotation = currentDegrees - Util.getRotationToAllianceSpeaker(pose).getDegrees(); @@ -213,6 +221,105 @@ public static Alliance getAlliance() { return alliance; } + /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used + below are robot specific, and should be tuned. */ + private final SwerveDrivePoseEstimator m_poseEstimator = + new SwerveDrivePoseEstimator( + m_kinematics, + getPigeon2().getRotation2d(), + m_modulePositions, + new Pose2d(), + VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), + VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); + + /** Updates the field relative position of the robot. */ + public void updateOdometry() { + m_poseEstimator.update(m_pigeon2.getRotation2d(), m_modulePositions); + + boolean useMegaTag2 = true; // set to false to use MegaTag1 + boolean doRejectUpdate = false; + if (useMegaTag2 == false) { + LimelightHelpers.PoseEstimate mt1 = + LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-leftlo"); + + if (mt1.tagCount == 1 && mt1.rawFiducials.length == 1) { + if (mt1.rawFiducials[0].ambiguity > .7) { + doRejectUpdate = true; + } + if (mt1.rawFiducials[0].distToCamera > 3) { + doRejectUpdate = true; + } + } + if (mt1.tagCount == 0) { + doRejectUpdate = true; + } + + if (!doRejectUpdate) { + m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.5, .5, 9999999)); + m_poseEstimator.addVisionMeasurement(mt1.pose, mt1.timestampSeconds); + } + } else if (useMegaTag2 == true) { + LimelightHelpers.SetRobotOrientation( + "limelight-leftlo", this.getPose().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight-leftlo"); + if (Math.abs( + Units.radiansToDegrees(this.getCurrentRobotChassisSpeeds().omegaRadiansPerSecond)) + > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision + // updates + { + doRejectUpdate = true; + } + if (mt2.tagCount == 0) { + doRejectUpdate = true; + } + if (!doRejectUpdate) { + m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.7, .7, 9999999)); + m_poseEstimator.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); + } + } + } + + public boolean shouldMegatag2Update() { + return shouldMt2Update; + } + + public void setShouldMegatag2Update(final boolean shouldUpdate) { + shouldMt2Update = shouldUpdate; + } + + public void megatag2Update() { + final double yaw = getPose().getRotation().getDegrees(); + for (String limelightName : Constants.Vision.LL3GS) { + LimelightHelpers.SetRobotOrientation(limelightName, yaw, 0, 0, 0, 0, 0); + } + if (!shouldMegatag2Update()) { + return; + } + final ChassisSpeeds chassisSpeeds = getCurrentRobotChassisSpeeds(); + if (Math.abs(getPigeon2().getRate()) > 540 + || (Math.abs(chassisSpeeds.vxMetersPerSecond) > 2.0 + || Math.abs(chassisSpeeds.vyMetersPerSecond) > 2.0)) { + return; + } + for (String limelightName : Constants.Vision.LL3GS) { + LimelightHelpers.PoseEstimate mt2 = + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + if (mt2.tagCount == 0) { + break; + } + addVisionMeasurement( + mt2.pose, + mt2.timestampSeconds, + VecBuilder.fill( + Math.pow(0.8, mt2.tagCount) * (mt2.avgTagDist / 2.0), + Math.pow(0.8, mt2.tagCount) * (mt2.avgTagDist / 2.0), + 9999999)); + } + } + + // FIXME make it make sens + @Override public void periodic() { /* Periodically try to apply the operator perspective */ @@ -233,4 +340,8 @@ public void periodic() { }); } } + + public Translation2d[] getModuleLocations() { + return m_moduleLocations; + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index bfa08c6..abe4781 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -92,7 +92,7 @@ public void setLengthInchesSlot1(double LENGTH) { public void incrementElevator(double IncrementAmount) { IncrementValue = IncrementValue + IncrementAmount; - System.out.println("Elevator Increment Value now: " + IncrementValue); + // System.out.println("Elevator Increment Value now: " + IncrementValue); } public double getIncrementValue() { @@ -153,10 +153,13 @@ public void setupShuffleboard() { ELEVATOR_TAB.add("ZERO SUBSYSTEM", new ZeroElevator(this)); ELEVATOR_TAB.add( "GoTo DesiredLen", new InstantCommand(() -> setLengthInches(length.getDouble(0)))); - ELEVATOR_TAB.addDouble("ActualLen In.", this::getLengthInches); - ELEVATOR_TAB.add("Coast", new InstantCommand(this::coastElevator).ignoringDisable(true)); - ELEVATOR_TAB.add("Brake", new InstantCommand(this::brakeElevator).ignoringDisable(true)); - ELEVATOR_TAB.addDouble("ERROR", () -> ELEVATOR_LEADER.getClosedLoopError().getValueAsDouble()); + if (Constants.shouldShuffleboard) { + ELEVATOR_TAB.addDouble("ActualLen In.", this::getLengthInches); + ELEVATOR_TAB.add("Coast", new InstantCommand(this::coastElevator).ignoringDisable(true)); + ELEVATOR_TAB.add("Brake", new InstantCommand(this::brakeElevator).ignoringDisable(true)); + ELEVATOR_TAB.addDouble( + "ERROR", () -> ELEVATOR_LEADER.getClosedLoopError().getValueAsDouble()); + } // ELEVATOR_TAB.addnumber("Elevator voltage", ); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 3f0fbdd..c0de0b9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; public class Intake extends SubsystemBase { private final TalonFX INTAKE_TOP; @@ -88,10 +89,12 @@ public double getRPMBottom() { } public void setupShuffleboard() { - INTAKE_TAB.addDouble("Current Top", this::getAmps); - INTAKE_TAB.addDouble("RL-RPM Bot", this::getRPMBottom); - GenericEntry customRPMIn = INTAKE_TAB.add("Desired RPM", 900).getEntry(); - INTAKE_TAB.add("Start", new InstantCommand(() -> setRPM(customRPMIn.getDouble(900)))); - INTAKE_TAB.add("Stop", new InstantCommand(this::stopCollecting)); + if (Constants.shouldShuffleboard) { + INTAKE_TAB.addDouble("Current Top", this::getAmps); + INTAKE_TAB.addDouble("RL-RPM Bot", this::getRPMBottom); + GenericEntry customRPMIn = INTAKE_TAB.add("Desired RPM", 900).getEntry(); + INTAKE_TAB.add("Start", new InstantCommand(() -> setRPM(customRPMIn.getDouble(900)))); + INTAKE_TAB.add("Stop", new InstantCommand(this::stopCollecting)); + } } } diff --git a/src/main/java/frc/robot/subsystems/PoopMonitor.java b/src/main/java/frc/robot/subsystems/PoopMonitor.java new file mode 100644 index 0000000..8706499 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PoopMonitor.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; + +public class PoopMonitor extends SubsystemBase { + + private boolean hasPooped = false; + private boolean shouldMonitor = false; + + public boolean hasPooped() { + return hasPooped; + } + + public void resetPooped() { + hasPooped = false; + } + + public void startMonitor() { + shouldMonitor = true; + } + + public void stopMonitor() { + shouldMonitor = false; + } + + public Command StartPoopMonitorCommand() { + return new InstantCommand(() -> startMonitor()); + } + + public Command StopPoopMonitorCommand() { + return new InstantCommand(() -> stopMonitor()); + } + + public Command ResetPoopMonitorCommand() { + return new InstantCommand(() -> resetPooped()); + } + + /** Creates a new PoopMonitor. */ + public PoopMonitor() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + if (shouldMonitor && !hasPooped && RobotContainer.SHOOTER.hasNote()) { + hasPooped = true; + // DriverStation.reportWarning("Pooped a note!", false); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5807fa8..8707ba5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -122,6 +122,10 @@ public boolean isRearBroken() { return SHOOTER_FOLLOWER.getReverseLimit().asSupplier().get().value == 0; } + public boolean hasNote() { + return isCenterBroken() || isRearBroken(); + } + public void setFeederVoltage(double voltage) { FEEDER.setVoltage(voltage); // FEEDER_TEMP.setVoltage(voltage); @@ -164,10 +168,12 @@ public boolean isShooterAtSetpoint() { } public void setupShuffleboard() { - SHOOTER_TAB.addDouble("Lead RPM", this::getRPMLeader); - SHOOTER_TAB.addDouble("ERROR", this::getError); - SHOOTER_TAB.addBoolean("Center Beam Break", this::isCenterBroken); - SHOOTER_TAB.addBoolean("Rear Beam Break", this::isRearBroken); + if (Constants.shouldShuffleboard) { + SHOOTER_TAB.addDouble("Lead RPM", this::getRPMLeader); + SHOOTER_TAB.addDouble("ERROR", this::getError); + SHOOTER_TAB.addBoolean("Center Beam Break", this::isCenterBroken); + SHOOTER_TAB.addBoolean("Rear Beam Break", this::isRearBroken); + } SHOOTER_TAB.add("Stop Shoot", new InstantCommand(this::stopShooter)); SHOOTER_TAB.add("Stop Feed", new InstantCommand(this::stopFeeder)); SHOOTER_TAB.add( diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 1fe0d0e..2d6ea5b 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -50,24 +50,26 @@ public Vision( this.CAMERA_PITCH_RADIANS = Units.degreesToRadians(cameraAngleDegrees); this.validFiducials = validIDs; tab = Shuffleboard.getTab("CAM_" + photonCameraName); - tab.addDouble("pitch", this::getPitchVal); - tab.addDouble("yaw", this::getYawVal); - tab.addDouble( - "distance to speaker", - () -> - PhotonUtils.calculateDistanceToTargetMeters( - this.getCameraHeight(), - Constants.SPEAKER_APRILTAG_HEIGHT, - Units.degreesToRadians(this.getCameraDegrees()), - 0.0)); - tab.addDouble( - "Our distance to speaker", - () -> - calculateDistanceToTarget( - this.getPitchVal(), - this.getCameraHeight(), - Constants.SPEAKER_APRILTAG_HEIGHT, - this.getCameraDegrees())); + if (Constants.shouldShuffleboard) { + tab.addDouble("pitch", this::getPitchVal); + tab.addDouble("yaw", this::getYawVal); + tab.addDouble( + "distance to speaker", + () -> + PhotonUtils.calculateDistanceToTargetMeters( + this.getCameraHeight(), + Constants.SPEAKER_APRILTAG_HEIGHT, + Units.degreesToRadians(this.getCameraDegrees()), + 0.0)); + tab.addDouble( + "Our distance to speaker", + () -> + calculateDistanceToTarget( + this.getPitchVal(), + this.getCameraHeight(), + Constants.SPEAKER_APRILTAG_HEIGHT, + this.getCameraDegrees())); + } // this.camera.setPipelineIndex(Constants.Tape01); } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java index 5f8a0ca..76f341a 100644 --- a/src/main/java/frc/robot/subsystems/Wrist.java +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -16,6 +16,8 @@ public class Wrist extends SubsystemBase { private final TalonFX WRIST_MOTOR; private final ShuffleboardTab WRIST_TAB = Shuffleboard.getTab("WRIST"); + private boolean shouldLockWristDown = false; + // Constructor public Wrist(final int wristMotorID) { WRIST_MOTOR = new TalonFX(wristMotorID, "rio"); @@ -41,6 +43,7 @@ public Wrist(final int wristMotorID) { WRIST_MOTOR.getConfigurator().apply(WRIST_CONFIG); setZero(); WRIST_MOTOR.setNeutralMode(NeutralModeValue.Brake); + disableWristLockdown(); } public void goHome() { @@ -120,14 +123,28 @@ public void setZero() { WRIST_MOTOR.setPosition(Constants.Wrist.INITIAL_ANGLE_DEGREES / 360.0); } + public void enableWristLockdown() { + shouldLockWristDown = true; + } + + public void disableWristLockdown() { + shouldLockWristDown = false; + } + + public boolean shouldLockDownWrist() { + return shouldLockWristDown; + } + public void setupShuffleboard() { WRIST_TAB.add("Set Coast", new InstantCommand(this::setCoast, this).ignoringDisable(true)); WRIST_TAB.add("Set Brake", new InstantCommand(this::setBrake, this).ignoringDisable(true)); WRIST_TAB.add("Re-Home", new ZeroWrist(this)); - WRIST_TAB.addDouble("Degrees", this::getDegrees); - WRIST_TAB.addDouble("Error", this::getError); - GenericEntry entry2 = WRIST_TAB.add("Desired DEG", 26.23).getEntry(); - WRIST_TAB.add( - "GoTo Desired DEG", new InstantCommand(() -> setDegrees(entry2.get().getDouble()))); + if (Constants.shouldShuffleboard) { + WRIST_TAB.addDouble("Degrees", this::getDegrees); + WRIST_TAB.addDouble("Error", this::getError); + GenericEntry entry2 = WRIST_TAB.add("Desired DEG", 26.23).getEntry(); + WRIST_TAB.add( + "GoTo Desired DEG", new InstantCommand(() -> setDegrees(entry2.get().getDouble()))); + } } } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/LimelightHelpers.java similarity index 93% rename from src/main/java/frc/robot/util/Limelight.java rename to src/main/java/frc/robot/util/LimelightHelpers.java index b7e880e..c0ab3dd 100644 --- a/src/main/java/frc/robot/util/Limelight.java +++ b/src/main/java/frc/robot/util/LimelightHelpers.java @@ -1,4 +1,4 @@ -// LimelightHelpers v1.4.0 (March 21, 2024) +// LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot.util; @@ -24,7 +24,7 @@ import java.net.URL; import java.util.concurrent.CompletableFuture; -public class Limelight { +public class LimelightHelpers { public static class LimelightTarget_Retro { @@ -472,7 +472,7 @@ private static double extractBotPoseEntry(double[] inData, int position) { } private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = Limelight.getLimelightNTTableEntry(limelightName, entryName); + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); var poseArray = poseEntry.getDoubleArray(new double[0]); var pose = toPose2D(poseArray); double latency = extractBotPoseEntry(poseArray, 6); @@ -752,6 +752,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the BLUE alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -775,6 +786,17 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -853,6 +875,33 @@ public static void setCropWindow( setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace( String limelightName, double forward, @@ -918,7 +967,7 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) public static LimelightResults getLatestResults(String limelightName) { long start = System.nanoTime(); - Limelight.LimelightResults results = new Limelight.LimelightResults(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); if (mapper == null) { mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java index 6221c93..a438c04 100644 --- a/src/main/java/frc/robot/util/Util.java +++ b/src/main/java/frc/robot/util/Util.java @@ -14,33 +14,36 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.RobotContainer; +import frc.robot.commands.control.auto.AutoState; import frc.robot.constants.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.util.Limelight.RawFiducial; +import frc.robot.util.LimelightHelpers.RawFiducial; import java.util.List; public class Util { public static final AprilTagFieldLayout field = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - public static Pose3d TAG_4_POSE; // RED ALLIANCE SPEAKER CENTER - public static Pose3d TAG_7_POSE; // BLUE ALLIANCE SPEAKER CENTER - public static Pose2d TAG_11_POSE_2D; // LEFT SIDE RED ALLIANCE TRAP - public static Pose2d TAG_12_POSE_2D; // RIGHT SIDE RED ALLIANCE TRAP - public static Pose2d TAG_13_POSE_2D; // FAR SIDE RED ALLIANCE TRAP - public static Pose2d TAG_14_POSE_2D; // FAR SIDE BLUE ALLIANCE TRAP - public static Pose2d TAG_15_POSE_2D; // LEFT SIDE BLUE ALLIANCE TRAP - public static Pose2d TAG_16_POSE_2D; // RIGHT SIDE BLUE ALLIANCE TRAP + public static Pose2d TAG_4_POSE; // RED ALLIANCE SPEAKER CENTER + public static Pose2d TAG_7_POSE; // BLUE ALLIANCE SPEAKER CENTER + public static Pose3d TAG_5_POSE; // RED AMP + public static Pose3d TAG_6_POSE; // BLUE AMP + public static Pose2d MANUAL_RED_AMP; // RED AMP + public static Pose2d MANUAL_BLUE_AMP; // BLUE AMP + public static Pose2d MANUAL_RED_LOB; // RED AMP + public static Pose2d MANUAL_BLUE_LOB; // BLUE AMP public static Pose2d TAG_LOB_BLUE; public static Pose2d TAG_LOB_RED; public static List TRAP_TAGS; + public static List AMP_TAGS; public static DriverStation.Alliance alliance; public static final double DEADBAND = 0.05; @@ -50,101 +53,123 @@ public Util() { .ifPresent( pose -> { Util.TAG_4_POSE = - pose; // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); + pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); }); field .getTagPose(7) .ifPresent( pose -> { Util.TAG_7_POSE = - pose; // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(12) - .ifPresent( - pose -> { - Util.TAG_12_POSE_2D = - pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(11) - .ifPresent( - pose -> { - Util.TAG_11_POSE_2D = - pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(13) - .ifPresent( - pose -> { - Util.TAG_13_POSE_2D = - pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(14) - .ifPresent( - pose -> { - Util.TAG_14_POSE_2D = pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); }); - field - .getTagPose(15) - .ifPresent( - pose -> { - Util.TAG_15_POSE_2D = - pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(16) - .ifPresent( - pose -> { - Util.TAG_16_POSE_2D = - pose.toPose2d(); // .transformBy(new Transform3d(0, 0.565, 0, new Rotation3d())); - }); - field - .getTagPose(5) - .ifPresent( - pose -> { - Util.TAG_LOB_RED = - pose.toPose2d().transformBy(new Transform2d(0.0, -1.0, new Rotation2d(0.0))); - ; - }); - field - .getTagPose(6) - .ifPresent( - pose -> { - Util.TAG_LOB_BLUE = - pose.toPose2d().transformBy(new Transform2d(0.0, -1.0, new Rotation2d())); - }); - TRAP_TAGS = - List.of( - TAG_11_POSE_2D, - TAG_12_POSE_2D, - TAG_13_POSE_2D, - TAG_14_POSE_2D, - TAG_15_POSE_2D, - TAG_16_POSE_2D); + MANUAL_RED_AMP = new Pose2d(14.7, 7.42, new Rotation2d(Math.toRadians(90.0))); + MANUAL_BLUE_AMP = new Pose2d(1.84, 7.42, new Rotation2d(Math.toRadians(90.0))); + MANUAL_RED_LOB = new Pose2d(14.7, 5.95, new Rotation2d(Math.toRadians(90.0))); + MANUAL_BLUE_LOB = new Pose2d(1.84, 5.95, new Rotation2d(Math.toRadians(90.0))); + TRAP_TAGS = List.of(); DriverStation.getAlliance().ifPresent(ouralliance -> alliance = ouralliance); } - public static Pose3d getAllianceSpeakerCenter() { + public static Pose2d getAllianceSpeaker() { // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE; return CommandSwerveDrivetrain.getAlliance() == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE; } + public static Pose2d getAllianceAmp() { + // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE; + return CommandSwerveDrivetrain.getAlliance() == DriverStation.Alliance.Blue + ? MANUAL_BLUE_AMP + : MANUAL_RED_AMP; + } + public static Pose2d getAllianceLob() { // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE; return CommandSwerveDrivetrain.getAlliance() == DriverStation.Alliance.Blue - ? TAG_LOB_BLUE - : TAG_LOB_RED; + ? MANUAL_BLUE_LOB + : MANUAL_RED_LOB; } public static Rotation2d getRotationToAllianceLob(Pose2d opose) { - // return alliance == DriverStation.Alliance.Blue ? TAG_7_POSE : TAG_4_POSE; - Transform2d pose = Util.getAllianceLob().minus(opose); - return new Rotation2d(Math.atan2(pose.getX(), pose.getY())); + Transform2d delta = + new Transform2d( + Util.getAllianceLob().getTranslation().minus(opose.getTranslation()), new Rotation2d()); + return new Rotation2d(Math.atan2(delta.getY(), delta.getX())) + .plus(Rotation2d.fromDegrees(180.0)); + } + + public static double getShooterSpeedFromDistanceForLob(double distance) { + return Constants.DISTANCE_TO_LOB_RPM.getInterpolated(new InterpolatingDouble(distance)).value; + } + + public static double getDistanceToAllianceLob(Pose2d opose) { + return RobotContainer.get() + .getPose() + .getTranslation() + .getDistance(getAllianceLob().getTranslation()); + } + + // private static final Pose2d SOURCE_SHOOT_POSE = + // PathPlannerAuto.getStaringPoseFromAutoFile("Source 5-4 Shoot Pose"); + // public static final Pose2d BLUE_AUTO_SOURCE_SHOOTING_POSE = SOURCE_SHOOT_POSE; + // // new Pose2d(SOURCE_SHOOT_POSE.getX(), SOURCE_SHOOT_POSE.getY(), + // Rotation2d.fromDegrees(-30.0)); + // public static final Pose2d RED_AUTO_SOURCE_SHOOTING_POSE = + // new Pose2d(16.56 - BLUE_AUTO_SOURCE_SHOOTING_POSE.getX(), + // BLUE_AUTO_SOURCE_SHOOTING_POSE.getY(), + // BLUE_AUTO_SOURCE_SHOOTING_POSE.getRotation().plus(Rotation2d.fromDegrees(180.0))); + // // new Pose2d(13.04, 3.22, Rotation2d.fromDegrees(-150)); + + public static final Pose2d BLUE_AUTO_SOURCE_CLOSE_SHOT = + new Pose2d(3.7, 3.15, Rotation2d.fromDegrees(-30)); + public static final Pose2d RED_AUTO_SOURCE_CLOSE_SHOT = + new Pose2d( + 16.56 - BLUE_AUTO_SOURCE_CLOSE_SHOT.getX(), + BLUE_AUTO_SOURCE_CLOSE_SHOT.getY(), + Rotation2d.fromDegrees(-150)); + + public static Command PathFindToAutoSourceCloseShot() { + return new InstantCommand(() -> RobotContainer.setAutoState(AutoState.SHOOT_PREP)) + .andThen( + new ConditionalCommand( + Util.pathfindToPose(BLUE_AUTO_SOURCE_CLOSE_SHOT), + Util.pathfindToPose(RED_AUTO_SOURCE_CLOSE_SHOT), + () -> CommandSwerveDrivetrain.getAlliance().equals(Alliance.Blue))); + } + + public static final Pose2d BLUE_AUTO_SOURCE_SHOOTING_POSE = + new Pose2d(4, 3, Rotation2d.fromDegrees(-30.0)); + public static final Pose2d RED_AUTO_SOURCE_SHOOTING_POSE = + new Pose2d( + 16.56 - BLUE_AUTO_SOURCE_SHOOTING_POSE.getX(), + BLUE_AUTO_SOURCE_SHOOTING_POSE.getY(), + Rotation2d.fromDegrees(-144.5)); + + public static Command PathFindToAutoSourceShot() { + return new InstantCommand(() -> RobotContainer.setAutoState(AutoState.SHOOT_PREP)) + .andThen( + new ConditionalCommand( + Util.pathfindToPose(BLUE_AUTO_SOURCE_SHOOTING_POSE), + Util.pathfindToPose(RED_AUTO_SOURCE_SHOOTING_POSE), + () -> CommandSwerveDrivetrain.getAlliance().equals(Alliance.Blue))); + } + + public static final Pose2d BLUE_AUTO_MADTOWN_SHOOTING_POSE = + new Pose2d(4.35, 5.15, Rotation2d.fromDegrees(-88)); + public static final Pose2d RED_AUTO_MADTOWN_SHOOTING_POSE = + new Pose2d( + 16.56 - BLUE_AUTO_MADTOWN_SHOOTING_POSE.getX(), + BLUE_AUTO_MADTOWN_SHOOTING_POSE.getY(), + Rotation2d.fromDegrees(-178.0)); + + public static Command PathFindToAutoMadtownShot() { + return new InstantCommand(() -> RobotContainer.setAutoState(AutoState.SHOOT_PREP)) + .andThen( + new ConditionalCommand( + Util.pathfindToPose(BLUE_AUTO_MADTOWN_SHOOTING_POSE), + Util.pathfindToPose(RED_AUTO_MADTOWN_SHOOTING_POSE), + () -> CommandSwerveDrivetrain.getAlliance().equals(Alliance.Blue))); } public static boolean isWithinTolerance( @@ -153,69 +178,68 @@ public static boolean isWithinTolerance( } public static boolean canSeeTarget(String limelight) { - return Limelight.getBotPoseEstimate_wpiBlue(limelight).tagCount > 0; + return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight).tagCount > 0; } public static void setupUtil() {} - public static double getDistance(String limelight) { - var result = Limelight.getBotPoseEstimate_wpiBlue(limelight); - if (result.tagCount > 0) { - return new Pose3d(result.pose) - .getTranslation() - .getDistance( - getAllianceSpeakerCenter() - .transformBy( - new Transform3d( - new Translation3d(0, 0, -getAllianceSpeakerCenter().getZ()), - new Rotation3d())) - .getTranslation()); - } - return 0.0; - } - // TODO flex on alliance tag pose public static double getDistanceToSpeaker() { - return new Pose3d(RobotContainer.get().getPose()) + return RobotContainer.get() + .getPose() .getTranslation() - .getDistance( - getAllianceSpeakerCenter() - .transformBy( - new Transform3d( - new Translation3d(0, 0, -getAllianceSpeakerCenter().getZ()), - new Rotation3d())) - .getTranslation()); + .getDistance(getAllianceSpeaker().getTranslation()); } - public static Rotation2d getRotationToAllianceSpeaker(Pose2d opose) { - // return - // opose.getTranslation().minus(Util.getAllianceSpeakerCenter().getTranslation().toTranslation2d()).getAngle(); - Transform2d pose = Util.getAllianceSpeakerCenter().toPose2d().minus(opose); - return new Rotation2d(Math.atan2(pose.getX(), pose.getY())); + public static double getDistanceToAmp() { + return RobotContainer.get() + .getPose() + .getTranslation() + .getDistance(getAllianceAmp().getTranslation()); } - public static double getInterpolatedWristAngle(String limelight) { - return Constants.DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.getInterpolated( - new InterpolatingDouble(Util.getDistance(limelight))) - .value; + public static double getDistanceToLob() { + return RobotContainer.get() + .getPose() + .getTranslation() + .getDistance(getAllianceLob().getTranslation()); + } + + // System.out.println("SPEAKER: " + Util.getAllianceSpeaker()); + // System.out.println("SUBTRACT: " + delta); + // System.out.println(delta.getAngle()); + + // TODO rotate to bias angle depending on robot pose for increased accuracy + // This should be equivalent to aiming a bit in front of the speaker + public static Rotation2d getRotationToAllianceSpeaker(Pose2d opose) { + // Pose2d newpose = new Pose2d(opose.getTranslation(), new Rotation2d(90.0)); + Translation2d speakerTranslation = Util.getAllianceSpeaker().getTranslation(); + Translation2d robotTranslation = opose.getTranslation(); + Transform2d delta = + new Transform2d(speakerTranslation.minus(robotTranslation), new Rotation2d()); + return new Rotation2d(Math.atan2(delta.getY(), delta.getX())) + .plus(Rotation2d.fromDegrees(180.0)); } - public static double getInterpolatedWristAngle() { + public static double getInterpolatedWristAngleSpeaker() { return Constants.DISTANCE_TO_WRISTANGLE_RELATIVE_SPEAKER.getInterpolated( new InterpolatingDouble(Util.getDistanceToSpeaker())) .value; } - public static boolean isValidShot(String limelight) { - double dist = Util.getDistance(limelight); - if (dist > 2.0 && dist < 8.3) { - return true; - } else return false; + public static boolean isValidShot() { + double dist = Util.getDistanceToSpeaker(); + return (dist > 2.25 && dist < 5.25); } - public static boolean isValidShot() { + public static boolean isValidShotAmpInclusive() { double dist = Util.getDistanceToSpeaker(); - return dist > 2.25 && dist < 5.25; + double ampDist = Util.getDistanceToAmp(); + return (dist > 2.25 && dist < 5.25) || ampDist < 4.00; + } + + public static boolean isLobShot() { + return Util.getDistanceToLob() > 5.5; } public static double squareValue(double value) { @@ -256,4 +280,20 @@ public static double speakerTagCount(final RawFiducial[] fiducials) { } return tagCount; } + + public static boolean isPointedAtSpeaker(CommandSwerveDrivetrain drivetrain) { + Pose2d current = drivetrain.getPose(); + return Util.isWithinTolerance( + current.getRotation().getDegrees(), + Util.getRotationToAllianceSpeaker(current).getDegrees(), + 1.5); + } + + public static boolean isPointedAtLob(CommandSwerveDrivetrain drivetrain) { + Pose2d current = drivetrain.getPose(); + return Util.isWithinTolerance( + current.getRotation().getDegrees(), + Util.getRotationToAllianceLob(current).getDegrees(), + 3.0); + } } diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d172..0322385 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,