From 826a87371cf190bb101b6455e6bb177329a9bf48 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 24 Feb 2026 22:55:28 -0700 Subject: [PATCH] auto align wackiness --- .../java/frc4388/robot/RobotContainer.java | 8 +- .../robot/commands/alignment/AutoAlign.java | 82 +++++-------------- .../frc4388/robot/constants/Constants.java | 4 +- 3 files changed, 24 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9aa6d00..7c2b0cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,9 +11,7 @@ import java.io.File; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.trajectory.PathPlannerTrajectory; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; @@ -29,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.commands.alignment.AutoAlign; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; @@ -41,7 +38,6 @@ import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; -import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -124,8 +120,8 @@ public class RobotContainer { ); private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> - new AutoAlign(m_robotSwerveDrive, m_vision, true)), + // TEST NEW AUTO ALIGN + //new AutoAlign(m_robotSwerveDrive, m_vision), new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter), new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter), new WaitCommand(3), diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 9ed2859..39164ab 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -14,101 +14,59 @@ import frc4388.utility.structs.Gains; public class AutoAlign extends Command { - private PID xPID = new PID(AutoConstants.XY_GAINS, 0); - private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; SwerveDrive swerveDrive; Vision vision; - boolean waitVelocityZero; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; - this.waitVelocityZero = waitVelocityZero && false; addRequirements(swerveDrive); } - public static double tagRelativeXError = -1; - private static void setTagRelativeXError(double val){ - tagRelativeXError = val; - } - @Override public void initialize() { - xPID.initialize(); - yPID.initialize(); + rotPID.initialize(); this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); } - double xerr; - double yerr; + double roterr; - double xoutput; - double youtput; double rotoutput; @Override public void execute() { - xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis); - yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis); - - roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees())); - if(roterr > 180){ - roterr -= 360; - }else if(roterr < -180){ - roterr += 360; - } - + Pose2d robotPose = vision.getPose2d(); + if (robotPose == null) return; + + double dx = targetpos.getX() - robotPose.getX(); + double dy = targetpos.getY() - robotPose.getY(); + + Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx)); + + roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); + if (roterr > 180) roterr -= 360; + if (roterr < -180) roterr += 360; - SmartDashboard.putNumber("PID X Error", xerr); - SmartDashboard.putNumber("PID Y Error", yerr); SmartDashboard.putNumber("PID Rot Error", roterr); - xoutput = xPID.update(xerr); - youtput = yPID.update(yerr); - - xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; - - - - Translation2d leftStick = new Translation2d( - Math.max(Math.min(-youtput, 1), -1), - Math.max(Math.min(-xoutput, 1), -1) - ); - - - - setTagRelativeXError( - new Translation2d(xerr, yerr).getAngle() - .rotateBy(targetpos.getRotation()) - .getCos()); - - swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation()); - } + swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); + } @Override public final boolean isFinished() { - boolean finished = ( - (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && - (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) && - (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD) - ); - - if(finished) + boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; + if (finished) { swerveDrive.softStop(); - + } return finished; } - - - private class PID { protected Gains gains; private double output = 0; diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 20e0a13..96e667e 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -47,7 +47,7 @@ public final class Constants { public static final class AutoConstants { // public static final Gains XY_GAINS = new Gains(5,0.6,0.0); - public static final Gains XY_GAINS = new Gains(8,0,0.0); + public static final Gains ROT_GAINS = new Gains(8,0,0.0); // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP); // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI); // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD); @@ -61,7 +61,7 @@ public final class Constants { public static final Trim Y_OFFSET_TRIM = new Trim("Y Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0); public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 5; // Degrees + public static final double ROT_TOLERANCE = 10; // Degrees public static final double MIN_XY_PID_OUTPUT = 0.0; public static final double MIN_ROT_PID_OUTPUT = 0.0;