From a8a597985f6a25a95fccf09759ae3bbd255925b9 Mon Sep 17 00:00:00 2001 From: Team 2648 Date: Wed, 16 Apr 2025 17:46:16 -0400 Subject: [PATCH] good 2 piece --- .../pathplanner/paths/30 Right to HP.path | 2 +- .../pathplanner/paths/330 Right to HP.path | 2 +- .../deploy/pathplanner/paths/L Backup.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 9 ++++++-- .../java/frc/robot/subsystems/Drivetrain.java | 22 +++++++++---------- 5 files changed, 21 insertions(+), 16 deletions(-) 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 5915979..60710c8 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "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 b62b8cb..549fc93 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -46,7 +46,7 @@ ], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 944db10..2c7a178 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -34,7 +34,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxAcceleration": 2.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f291ef..71cac96 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -360,8 +360,13 @@ public class RobotContainer { NamedCommands.registerCommand( "Processor Position", moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) - .alongWith(manipulator.runManipulator(() -> 0.85, false)) - .until(() -> driver.a().getAsBoolean()) + .raceWith(manipulator.runManipulator(() -> 0.85, false)) + ); + + NamedCommands.registerCommand( + "Pickup Algae L2", + moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) + .raceWith(manipulator.runManipulator(() -> 0.85, false)) ); } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index cc4695c..6b6ff78 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -111,8 +111,8 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() }, new Pose2d(), - VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(0.5)), - VecBuilder.fill(1, 1, Units.degreesToRadians(4000)) + VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)), + VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); @@ -177,17 +177,17 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -209,13 +209,13 @@ public class Drivetrain extends SubsystemBase { if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360))); }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); } if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){