diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 48e6982..e0f4b06 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -103,16 +103,19 @@ public class RobotContainer { // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball RobotIntakeDown, new WaitCommand(1), - new RunCommand( - () -> m_robotSwerveDrive.driveWithInput( - m_lidar.getClosestBall(), - new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), - false - ), - m_robotSwerveDrive - ) - .withTimeout(5.0) - .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) + new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true) + + // new RunCommand( + // () -> m_robotSwerveDrive.driveWithInput( + // m_lidar.getClosestBall(), + // new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0), + // false + // ), + // m_robotSwerveDrive + // ) + // .withTimeout(5.0) + // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)) ); private Command RobotReadyToShoot = new SequentialCommandGroup( @@ -122,7 +125,7 @@ public class RobotContainer { private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN - new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0))), + new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)), false), 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 d7c0687..79334f6 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -15,14 +15,15 @@ public class AutoAlign extends Command { private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); private Pose2d targetpos; - + private boolean isLidar; SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) { this.swerveDrive = swerveDrive; this.vision = vision; this.targetpos = targetpos; + this.isLidar = isLidar; addRequirements(swerveDrive); } @@ -39,8 +40,6 @@ public class AutoAlign extends Command { @Override public void execute() { - - Pose2d robotPose = vision.getPose2d(); if (robotPose == null) return; @@ -54,9 +53,9 @@ public class AutoAlign extends Command { if (roterr < -180) roterr += 360; SmartDashboard.putNumber("PID Rot Error", roterr); - + System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); - } + } @Override public final boolean isFinished() { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 3b38cbf..77101ba 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 87; - public static final String GIT_SHA = "34a7ed050d2967603ebf12cd0bad0b7264aeae01"; - public static final String GIT_DATE = "2026-02-24 17:49:26 MST"; + public static final int GIT_REVISION = 91; + public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac"; + public static final String GIT_DATE = "2026-02-25 17:17:52 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-24 18:48:38 MST"; - public static final long BUILD_UNIX_TIME = 1771984118729L; + public static final String BUILD_DATE = "2026-02-25 17:44:49 MST"; + public static final long BUILD_UNIX_TIME = 1772066689466L; public static final int DIRTY = 1; private BuildConstants(){}