diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left Event.auto b/src/main/deploy/pathplanner/autos/2.5 Coral Left.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Two Coral Left Event.auto rename to src/main/deploy/pathplanner/autos/2.5 Coral Left.auto diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left.auto b/src/main/deploy/pathplanner/autos/3 Coral Left.auto similarity index 76% rename from src/main/deploy/pathplanner/autos/Two Coral Left.auto rename to src/main/deploy/pathplanner/autos/3 Coral Left.auto index 34a2c99..7e98e95 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Left.auto @@ -10,18 +10,6 @@ "pathName": "Start to 30 Right" } }, - { - "type": "named", - "data": { - "name": "Lift L4" - } - }, - { - "type": "path", - "data": { - "pathName": "J Approach" - } - }, { "type": "named", "data": { @@ -62,19 +50,38 @@ { "type": "named", "data": { - "name": "Lift L4" + "name": "Shoot Coral L4" } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "K Approach" + "commands": [ + { + "type": "path", + "data": { + "pathName": "L Backup" + } + }, + { + "type": "named", + "data": { + "name": "HP Pickup" + } + } + ] } }, { "type": "named", "data": { - "name": "Shoot Coral L4" + "name": "Collect Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "HP to K" } } ] 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 0e4ad3e..a44ed39 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": 3.5, - "maxAcceleration": 1.75, + "maxVelocity": 5.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -120.06858282186238 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file 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 7b9ae9a..b78098f 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": 3.5, - "maxAcceleration": 1.75, + "maxVelocity": 5.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, @@ -62,5 +62,5 @@ "velocity": 0, "rotation": -59.99999999999999 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path new file mode 100644 index 0000000..5fdf785 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HP to K.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.1987704918032787, + "y": 7.189754098360656 + }, + "prevControl": null, + "nextControl": { + "x": 1.7142418032786886, + "y": 6.638319672131147 + }, + "isLocked": false, + "linkedName": "HP Left Position" + }, + { + "anchor": { + "x": 3.9679303278688525, + "y": 5.211782786885246 + }, + "prevControl": { + "x": 3.2246926229508195, + "y": 6.230737704918033 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Lift L4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 1.75, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.49491285058726 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -53.97262661489646 + }, + "useDefaultConstraints": true +} \ 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 index 616e685..64b1ce9 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 1.75, + "maxVelocity": 5.0, + "maxAcceleration": 2.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/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a35956f..8508d22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -238,10 +238,10 @@ public class RobotContainer { //new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); 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).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.1))); + NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); + NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01))); NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) - .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1))); + .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.01))); NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)); } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 41880bd..49184d4 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -12,12 +12,12 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 5.5; - public static final double kMaxAccelerationMetersPerSecondSquared = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 4; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 3; - public static final double kPYController = 3; + public static final double kPXController = 3.5; + public static final double kPYController = 5; public static final double kPThetaController = 5.5; // Constraint for the motion profiled robot angle controller diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 00a5a82..9fe92ec 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -174,7 +174,7 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){