From 4586608065e3523a9014b6b08dda1e63877e8c02 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo Date: Sat, 21 Mar 2026 13:35:36 -0700 Subject: [PATCH 1/3] Reduced distance to fuel to fix crashing problem reduced the distance between the robot and the fuel by the distance between the center of the robot and the intake --- src/main/java/frc/robot/auto/DriveToClump.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/auto/DriveToClump.java b/src/main/java/frc/robot/auto/DriveToClump.java index 8dddd360..5441ce6a 100644 --- a/src/main/java/frc/robot/auto/DriveToClump.java +++ b/src/main/java/frc/robot/auto/DriveToClump.java @@ -74,7 +74,8 @@ public Command driveToClump() { .MaxLinearAcceleration .in(MetersPerSecondPerSecond); - double distance = error.getNorm(); + double robotRadius = 0.73; //Needs to be tuned + double distance = Math.max(0, error.getNorm() - robotRadius); double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distance)); ChassisSpeeds finalVelocity = From 1610b6f90d1823f65f5cd09765db3dbb5e277df9 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo <238144736+neelabhb-sudo@users.noreply.github.com> Date: Sat, 21 Mar 2026 20:38:01 +0000 Subject: [PATCH 2/3] style: Apply Spotless fixes --- src/main/java/frc/robot/auto/DriveToClump.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/auto/DriveToClump.java b/src/main/java/frc/robot/auto/DriveToClump.java index 5441ce6a..1094c03f 100644 --- a/src/main/java/frc/robot/auto/DriveToClump.java +++ b/src/main/java/frc/robot/auto/DriveToClump.java @@ -74,7 +74,7 @@ public Command driveToClump() { .MaxLinearAcceleration .in(MetersPerSecondPerSecond); - double robotRadius = 0.73; //Needs to be tuned + double robotRadius = 0.73; // Needs to be tuned double distance = Math.max(0, error.getNorm() - robotRadius); double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distance)); From 28d0105142a53130a2a68081d9047b4d27c15902 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo Date: Sat, 28 Mar 2026 11:27:01 -0700 Subject: [PATCH 3/3] Fixed DriveToClump: I basically just calculated a brand new, offset target position 0.73 meters in front of the clump. --- .../java/frc/robot/auto/DriveToClump.java | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/auto/DriveToClump.java b/src/main/java/frc/robot/auto/DriveToClump.java index 1094c03f..d4f30e94 100644 --- a/src/main/java/frc/robot/auto/DriveToClump.java +++ b/src/main/java/frc/robot/auto/DriveToClump.java @@ -56,8 +56,13 @@ public Command driveToClump() { return Commands.none(); } - Translation2d error = - fuelPosition.minus(robot.getSwerveDrive().getPosition2d().getTranslation()); + // Translation2d error = + // fuelPosition.minus(robot.getSwerveDrive().getPosition2d().getTranslation()); + + Translation2d currentPosition = + robot.getSwerveDrive().getPosition2d().getTranslation(); + Translation2d centerError = fuelPosition.minus(currentPosition); + double distanceToCenter = centerError.getNorm(); double maxVelocity = robot @@ -75,20 +80,27 @@ public Command driveToClump() { .in(MetersPerSecondPerSecond); double robotRadius = 0.73; // Needs to be tuned - double distance = Math.max(0, error.getNorm() - robotRadius); - double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distance)); + if (distanceToCenter <= robotRadius) { + return Commands.none(); + } + Translation2d targetPosition = + fuelPosition.minus(centerError.div(distanceToCenter).times(robotRadius)); + Translation2d error = targetPosition.minus(currentPosition); + double distanceToTarget = error.getNorm(); + double finalSpeed = + Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distanceToTarget)); ChassisSpeeds finalVelocity = new ChassisSpeeds( - error.getX() / distance * finalSpeed, - error.getY() / distance * finalSpeed, + error.getX() / distanceToTarget * finalSpeed, + error.getY() / distanceToTarget * finalSpeed, 0); return Commands.either( Commands.deadline( robot .getSwerveDrive() - .driveTo(new Pose2d(fuelPosition, error.getAngle()), finalVelocity), + .driveTo(new Pose2d(targetPosition, error.getAngle()), finalVelocity), robot.getIntakeRollers().intake()), Commands.none(), robot.getIntakeExtension()::isExtended);