From 27c527e8b55154969b0fa1269f16c82ba915d685 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 22:22:33 -0400 Subject: [PATCH 1/3] init commit --- .../alignment/RoughAlignClosestBranch.java | 44 ++++++++++++++++ .../robot/constants/AlignmentPosition.java | 52 ++++++++++++++----- .../frc/robot/constants/GameConstants.java | 2 +- 3 files changed, 84 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java diff --git a/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java new file mode 100644 index 00000000..0374a597 --- /dev/null +++ b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java @@ -0,0 +1,44 @@ +package frc.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.AlignmentPosition; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.PathPlannerUtils; +import frc.robot.utils.logging.commands.LoggableCommand; +import org.littletonrobotics.junction.Logger; + +public class RoughAlignClosestBranch extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final Timer timer = new Timer(); + private Pose2d position; + + public RoughAlignClosestBranch(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + timer.restart(); + try { + AlignmentPosition alignmentTarget = + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + Logger.recordOutput("TargetReefRoughAlignment", alignmentTarget); + Pose2d position = alignmentTarget.getPathPlannerPosition(); + Logger.recordOutput("TargetReefRoughPose", position); + PathPlannerUtils.pathToPose(position, 0).schedule(); + } catch (Exception e) { + DriverStation.reportError(e.getMessage(), true); + } + } + + @Override + public boolean isFinished() { + return drivetrain.getPose().getTranslation().getDistance(position.getTranslation()) + < Constants.AUTO_ALIGN_THRESHOLD + || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); + } +} diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index f02f8165..66b88b21 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -12,41 +12,48 @@ public enum AlignmentPosition { // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets - A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(2.8076000000000, 4.0614305050000, Rotation2d.fromDegrees(0))), - B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(2.8076000000000, 3.7328134950000, Rotation2d.fromRadians(0))), - C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.6176981000607, 2.5872384265798, Rotation2d.fromDegrees(60))), - D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.9022887788364, 2.4229299215798, Rotation2d.fromDegrees(60))), - E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.2994349400607, 2.5517079215798 , Rotation2d.fromDegrees(120))), - F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.5840256188364, 2.7160164265798, Rotation2d.fromDegrees(120))), - G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(6.1710736800000, 3.9903694950000, Rotation2d.fromDegrees(180))), - H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(6.1710736800000, 4.3189865050000, Rotation2d.fromDegrees(180))), - I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.3609755799393, 5.4645615734202, Rotation2d.fromDegrees(240))), - J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.0763849011636, 5.6288700784202, Rotation2d.fromDegrees(240))), - K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX), + K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.6792387399393, 5.5000920784202, Rotation2d.fromDegrees(300))), - L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX); + L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.3946480611636, 5.3357835734202, Rotation2d.fromDegrees(300))); // spotless:on private final Pose2d bluePosition; private final Pose2d redPosition; private final Apriltag blueTag; private final Apriltag redTag; + private final Pose2d bluePathPlannerPosition; + private final Pose2d redPathPlannerPosition; - AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) { + AlignmentPosition( + Pose2d position, Apriltag blueTag, Apriltag redTag, Pose2d pathPlannerPosition) { this.bluePosition = position; + this.bluePathPlannerPosition = pathPlannerPosition; Rotation2d redRotation = Rotation2d.fromRadians(Math.PI + position.getRotation().getRadians()); double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX(); double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY(); + double redPathPlannerX = FieldLocation.LENGTH_OF_FIELD - pathPlannerPosition.getX(); + double redPathPlannerY = FieldLocation.HEIGHT_OF_FIELD - pathPlannerPosition.getY(); + this.redPathPlannerPosition = new Pose2d(redPathPlannerX, redPathPlannerY, redRotation); this.redPosition = new Pose2d(redX, redY, redRotation); this.blueTag = blueTag; this.redTag = redTag; @@ -60,6 +67,14 @@ public Pose2d getRedPosition() { return redPosition; } + public Pose2d getBluePathPlannerPosition() { + return bluePathPlannerPosition; + } + + public Pose2d getRedPathPlannerPosition() { + return redPathPlannerPosition; + } + public Pose2d getPosition() { Optional allianceColor = Robot.getAllianceColor(); return allianceColor @@ -69,6 +84,17 @@ public Pose2d getPosition() { .orElse(new Pose2d()); } + public Pose2d getPathPlannerPosition() { + Optional allianceColor = Robot.getAllianceColor(); + return allianceColor + .map( + alliance -> + alliance == DriverStation.Alliance.Blue + ? getBluePathPlannerPosition() + : getRedPathPlannerPosition()) + .orElse(new Pose2d()); + } + public static AlignmentPosition getClosest(Translation2d currentPosition) { int closestIndex = 0; double closestDistance = diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d430ec25..6241f097 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -90,7 +90,7 @@ public class GameConstants { // Treshholds public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; - public static final double AUTO_ALIGN_THRESHOLD = 2.3; // degrees //TODO: change later + public static final double AUTO_ALIGN_THRESHOLD = 0.2; // degrees //TODO: change later public static final double ELEVATOR_ENCODER_THRESHHOLD = 1; // TODO: Change Later public static final int HIHI_EXTENDER_TICK_LIMIT = 10; From d28cde18d7e29281268b9f46b94d76213d9c8cde Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 22:22:58 -0400 Subject: [PATCH 2/3] small stuff --- src/main/java/frc/robot/commands/SuperAutoScore.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/commands/SuperAutoScore.java b/src/main/java/frc/robot/commands/SuperAutoScore.java index 251068c9..39cc606a 100644 --- a/src/main/java/frc/robot/commands/SuperAutoScore.java +++ b/src/main/java/frc/robot/commands/SuperAutoScore.java @@ -1,6 +1,7 @@ package frc.robot.commands; import frc.robot.commands.alignment.AlignClosestBranch; +import frc.robot.commands.alignment.RoughAlignClosestBranch; import frc.robot.commands.coral.ShootCoral; import frc.robot.commands.elevator.ElevatorToStoredPosition; import frc.robot.constants.Constants; @@ -20,6 +21,7 @@ public SuperAutoScore( Vision vision) { super( new SetSuperAutoScorePosition(drivetrain, elevator, vision), + new RoughAlignClosestBranch(drivetrain), new LoggableParallelCommandGroup( new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator)), new LoggableWaitCommand(0.2), From 6e29633b8147df131f9e3f82c877ec53c42c72c6 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Wed, 26 Mar 2025 23:07:51 -0400 Subject: [PATCH 3/3] small changes --- src/main/java/frc/robot/RobotContainer.java | 3 ++- .../frc/robot/commands/SuperAutoScore.java | 7 ++--- .../alignment/RoughAlignClosestBranch.java | 26 +++++++++---------- .../robot/constants/AlignmentPosition.java | 24 ++++++++--------- .../frc/robot/constants/GameConstants.java | 2 +- .../robot/utils/logging/PathPlannerUtils.java | 6 ++--- 6 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c64a5aae..1a74b3df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -474,7 +474,8 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - + SmartDashboard.putData( + "Align", new SuperAutoScore(drivetrain, elevatorSubsystem, coralSubsystem, vision)); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/SuperAutoScore.java b/src/main/java/frc/robot/commands/SuperAutoScore.java index 39cc606a..8c5dd633 100644 --- a/src/main/java/frc/robot/commands/SuperAutoScore.java +++ b/src/main/java/frc/robot/commands/SuperAutoScore.java @@ -21,9 +21,10 @@ public SuperAutoScore( Vision vision) { super( new SetSuperAutoScorePosition(drivetrain, elevator, vision), - new RoughAlignClosestBranch(drivetrain), - new LoggableParallelCommandGroup( - new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator)), + new LoggableSequentialCommandGroup( + new RoughAlignClosestBranch(drivetrain), + new LoggableParallelCommandGroup( + new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator))), new LoggableWaitCommand(0.2), new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); } diff --git a/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java index 0374a597..c8db6063 100644 --- a/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java +++ b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java @@ -1,7 +1,6 @@ package frc.robot.commands.alignment; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.AlignmentPosition; import frc.robot.constants.Constants; @@ -23,22 +22,21 @@ public RoughAlignClosestBranch(SwerveDrivetrain drivetrain) { @Override public void initialize() { timer.restart(); - try { - AlignmentPosition alignmentTarget = - AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); - Logger.recordOutput("TargetReefRoughAlignment", alignmentTarget); - Pose2d position = alignmentTarget.getPathPlannerPosition(); - Logger.recordOutput("TargetReefRoughPose", position); - PathPlannerUtils.pathToPose(position, 0).schedule(); - } catch (Exception e) { - DriverStation.reportError(e.getMessage(), true); - } + AlignmentPosition alignmentTarget = + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + Logger.recordOutput("TargetReefRoughAlignment", alignmentTarget); + Pose2d position = alignmentTarget.getPathPlannerPosition(); + Logger.recordOutput("TargetReefRoughPose", position); + PathPlannerUtils.pathToPose(position, 0).schedule(); } @Override public boolean isFinished() { - return drivetrain.getPose().getTranslation().getDistance(position.getTranslation()) - < Constants.AUTO_ALIGN_THRESHOLD - || timer.hasElapsed(Constants.AUTO_ALIGN_TIMEOUT); + if (drivetrain.getPose().getTranslation().getDistance(position.getTranslation()) + < Constants.ALIGNMENT_DISTANCE_THRESHOLD) { + return true; + } else { + return false; + } } } diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index 66b88b21..2de7e33e 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -12,29 +12,29 @@ public enum AlignmentPosition { // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets - A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(2.8076000000000, 4.0614305050000, Rotation2d.fromDegrees(0))), + A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 4.0614305050000, Rotation2d.fromDegrees(0))), - B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(2.8076000000000, 3.7328134950000, Rotation2d.fromRadians(0))), + B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 3.7328134950000, Rotation2d.fromRadians(0))), - C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.6176981000607, 2.5872384265798, Rotation2d.fromDegrees(60))), + C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.7176981000607, 2.7604435073367, Rotation2d.fromDegrees(60))), - D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.9022887788364, 2.4229299215798, Rotation2d.fromDegrees(60))), + D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(4.0022887788364, 2.5961350023367, Rotation2d.fromDegrees(60))), - E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.2994349400607, 2.5517079215798 , Rotation2d.fromDegrees(120))), + E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.1994349400607, 2.7249130023367 , Rotation2d.fromDegrees(120))), - F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.5840256188364, 2.7160164265798, Rotation2d.fromDegrees(120))), + F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.4840256188364, 2.8892215073367, Rotation2d.fromDegrees(120))), - G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(6.1710736800000, 3.9903694950000, Rotation2d.fromDegrees(180))), + G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 3.9903694950000, Rotation2d.fromDegrees(180))), - H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(6.1710736800000, 4.3189865050000, Rotation2d.fromDegrees(180))), + H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 4.3189865050000, Rotation2d.fromDegrees(180))), - I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.3609755799393, 5.4645615734202, Rotation2d.fromDegrees(240))), + I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.2609755799393, 5.2913564926633, Rotation2d.fromDegrees(240))), - J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.0763849011636, 5.6288700784202, Rotation2d.fromDegrees(240))), + J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(4.9763849011636, 5.4556649976633, Rotation2d.fromDegrees(240))), - K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.6792387399393, 5.5000920784202, Rotation2d.fromDegrees(300))), + K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.7792387399393, 5.3268869976633, Rotation2d.fromDegrees(300))), - L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.3946480611636, 5.3357835734202, Rotation2d.fromDegrees(300))); + L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.4946480611636, 5.1625784926633, Rotation2d.fromDegrees(300))); // spotless:on private final Pose2d bluePosition; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 6241f097..abefd6c5 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -90,7 +90,7 @@ public class GameConstants { // Treshholds public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; - public static final double AUTO_ALIGN_THRESHOLD = 0.2; // degrees //TODO: change later + public static final double AUTO_ALIGN_THRESHOLD = 0.05; // TODO: change later public static final double ELEVATOR_ENCODER_THRESHHOLD = 1; // TODO: Change Later public static final int HIHI_EXTENDER_TICK_LIMIT = 10; diff --git a/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java b/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java index e291c0d3..4c010845 100644 --- a/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java @@ -15,9 +15,9 @@ public class PathPlannerUtils { private static final PathConstraints defualtPathConstraints = new PathConstraints( Constants.MAX_VELOCITY, - Constants.MAX_VELOCITY, - Math.toRadians(1000), - Math.toRadians(1000)); + Constants.MAX_PATHPLANNER_ACCEL, + Constants.MAX_ANGULAR_SPEED, + Math.toRadians(Constants.MAX_PATHPLANNER_ANGULAR_ACCEL)); public static PathPlannerPath createManualPath( Pose2d startPose, Pose2d targetPos, double endVelocity) {