diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto b/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto index 59ccb7c..cef636d 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto @@ -44,19 +44,7 @@ { "type": "path", "data": { - "pathName": "HP to 330 Left" - } - }, - { - "type": "named", - "data": { - "name": "Lift L4" - } - }, - { - "type": "path", - "data": { - "pathName": "K Approach" + "pathName": "HP to 330 Right" } }, { @@ -64,6 +52,12 @@ "data": { "name": "Shoot Coral L4" } + }, + { + "type": "path", + "data": { + "pathName": "L Backup" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left.auto b/src/main/deploy/pathplanner/autos/Two Coral Left.auto index 2acbf2e..34a2c99 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral Left.auto @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "HP to 330 Left" + "pathName": "HP to 330 Right" } }, { diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index 9484f75..0e4ad3e 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/330 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path index e85f75a..7b9ae9a 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/E to HP.path b/src/main/deploy/pathplanner/paths/E to HP.path index 82d05e7..e856c8b 100644 --- a/src/main/deploy/pathplanner/paths/E to HP.path +++ b/src/main/deploy/pathplanner/paths/E to HP.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/H Backup.path b/src/main/deploy/pathplanner/paths/H Backup.path index 8d9d773..71b3072 100644 --- a/src/main/deploy/pathplanner/paths/H Backup.path +++ b/src/main/deploy/pathplanner/paths/H Backup.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Left.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path similarity index 72% rename from src/main/deploy/pathplanner/paths/HP to 330 Left.path rename to src/main/deploy/pathplanner/paths/HP to 330 Right.path index 5c3fe60..ce5ff97 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Left.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -17,24 +17,36 @@ { "anchor": { "x": 3.6202868852459016, - "y": 5.859118852459017 + "y": 5.031967213114754 }, "prevControl": { "x": 3.304747501684258, - "y": 6.385017825061756 + "y": 5.557866185717494 }, "nextControl": null, "isLocked": false, - "linkedName": "Before K" + "linkedName": "L" } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Lift L4" + } + } + } + ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/HP to D.path b/src/main/deploy/pathplanner/paths/HP to D.path index 9618c7f..7d26406 100644 --- a/src/main/deploy/pathplanner/paths/HP to D.path +++ b/src/main/deploy/pathplanner/paths/HP to D.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/J Approach.path b/src/main/deploy/pathplanner/paths/J Approach.path index 7d7d20f..ec084ae 100644 --- a/src/main/deploy/pathplanner/paths/J Approach.path +++ b/src/main/deploy/pathplanner/paths/J Approach.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/J Backup.path b/src/main/deploy/pathplanner/paths/J Backup.path index 3f52a71..60a71be 100644 --- a/src/main/deploy/pathplanner/paths/J Backup.path +++ b/src/main/deploy/pathplanner/paths/J Backup.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/K Approach.path b/src/main/deploy/pathplanner/paths/K Approach.path index 8417384..eb8c3e5 100644 --- a/src/main/deploy/pathplanner/paths/K Approach.path +++ b/src/main/deploy/pathplanner/paths/K Approach.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6202868852459016, - "y": 5.859118852459017 + "x": 3.35655737704918, + "y": 5.78719262295082 }, "prevControl": null, "nextControl": { - "x": 3.6663590190101236, - "y": 5.6134008057257505 + "x": 3.402629510813402, + "y": 5.541474576217554 }, "isLocked": false, "linkedName": "Before K" }, { "anchor": { - "x": 3.7290765411991873, - "y": 5.091647695859483 + "x": 3.6322745901639335, + "y": 5.019979508196721 }, "prevControl": { - "x": 3.693113426445089, - "y": 5.343389499138171 + "x": 3.596311475409835, + "y": 5.271721311475409 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 1.0, + "maxAcceleration": 0.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -59.69923999693802 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path new file mode 100644 index 0000000..11ce175 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6202868852459016, + "y": 5.031967213114754 + }, + "prevControl": null, + "nextControl": { + "x": 3.4877871502451065, + "y": 5.243966789116026 + }, + "isLocked": false, + "linkedName": "L" + }, + { + "anchor": { + "x": 3.35655737704918, + "y": 5.511475409836065 + }, + "prevControl": { + "x": 3.5603483606557376, + "y": 5.235758196721312 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 1.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -63.43494882292201 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.69923999693802 + }, + "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 69ceee6..efaa1b2 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -44,8 +44,8 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Right Start to E.path b/src/main/deploy/pathplanner/paths/Right Start to E.path index 8ffb89f..40befdf 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to E.path +++ b/src/main/deploy/pathplanner/paths/Right Start to E.path @@ -52,8 +52,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Start to 30 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path index fb0742f..ba0cede 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -45,8 +45,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Start to H.path b/src/main/deploy/pathplanner/paths/Start to H.path index b26d283..516ea05 100644 --- a/src/main/deploy/pathplanner/paths/Start to H.path +++ b/src/main/deploy/pathplanner/paths/Start to H.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/fein.path b/src/main/deploy/pathplanner/paths/fein.path index 90954a0..edd51cc 100644 --- a/src/main/deploy/pathplanner/paths/fein.path +++ b/src/main/deploy/pathplanner/paths/fein.path @@ -49,8 +49,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, + "maxVelocity": 3.5, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 0ad5cef..05d1cf0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 2.0, - "defaultMaxAccel": 1.0, + "defaultMaxVel": 3.5, + "defaultMaxAccel": 1.75, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 400.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bf89433..a35956f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -239,7 +239,7 @@ public class RobotContainer { NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); - NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).until(() -> manipulator.getCoralBeamBreak() == false).andThen(manipulator.runManipulator(() -> 0, false)).withTimeout(0.1)); + NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.1))); NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1))); NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index d2b1fc8..00a5a82 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -178,7 +178,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -196,7 +196,7 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.15, 0.15, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){