diff --git a/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto index 848e046..ddb4f8a 100644 --- a/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto +++ b/src/main/deploy/pathplanner/autos/Robot Reveal Night.auto @@ -10,6 +10,12 @@ "name": "Robot Rev Up" } }, + { + "type": "named", + "data": { + "name": "Intake Retracted" + } + }, { "type": "path", "data": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bda7d7f..56a442f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -101,10 +101,10 @@ public class RobotContainer { private Command LidarIntake = new SequentialCommandGroup( // 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 InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), - new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(m_lidar.getClosestBall(), new Rotation2d(0)), true) + // RobotIntakeDown, + // new WaitCommand(1), + // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())), + new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true) // new RunCommand( // () -> m_robotSwerveDrive.driveWithInput( @@ -123,14 +123,18 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.setShooterReady(), m_robotShooter) ); + private Command IntakeRetracted = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake) + ); + private Command RobotShoot = new SequentialCommandGroup( // TEST NEW AUTO ALIGN //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), - new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake), new WaitCommand(2), + IntakeRetracted, + new WaitCommand(3), new InstantCommand(()->m_robotShooter.setShooterNOTShoot(), m_robotShooter), new InstantCommand(() -> m_robotShooter.setShooterNotReady(), m_robotShooter) ); @@ -155,6 +159,7 @@ public class RobotContainer { }, true); NamedCommands.registerCommand("Robot Rev Up", RobotReadyToShoot); + NamedCommands.registerCommand("Intake Retracted", IntakeRetracted); NamedCommands.registerCommand("Robot Shoot", RobotShoot); NamedCommands.registerCommand("Lidar Intake", LidarIntake); NamedCommands.registerCommand("Robot Intake Down", RobotIntakeDown); diff --git a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java index 82121d7..770280d 100644 --- a/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/alignment/AutoAlign.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.FieldPositions; @@ -14,15 +15,19 @@ import frc4388.utility.structs.Gains; public class AutoAlign extends Command { private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); - private Pose2d targetpos; + private Lidar lidar; private boolean isLidar; + + private Pose2d targetpos; + private double targetRotation; + SwerveDrive swerveDrive; Vision vision; - public AutoAlign(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos, boolean isLidar) { + public AutoAlign(SwerveDrive swerveDrive, Vision vision, Lidar lidar, boolean isLidar) { this.swerveDrive = swerveDrive; this.vision = vision; - this.targetpos = targetpos; + this.lidar = lidar; this.isLidar = isLidar; addRequirements(swerveDrive); } @@ -30,6 +35,7 @@ public class AutoAlign extends Command { @Override public void initialize() { rotPID.initialize(); + this.targetRotation = swerveDrive.getPose2d().getRotation().getDegrees(); //this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0)); } @@ -40,36 +46,43 @@ public class AutoAlign extends Command { @Override public void execute() { - Rotation2d desiredHeading; - Pose2d robotPose = vision.getPose2d(); - if (robotPose == null) return; + double desiredHeading; + // Pose2d robotPose = vision.getPose2d(); + targetpos = new Pose2d(lidar.getClosestBall(), new Rotation2d(0)); + // if (robotPose == null) return; + if (targetpos == null) return; + if (targetpos.getTranslation() == null) return; - double dx = targetpos.getX() - robotPose.getX(); - double dy = targetpos.getY() - robotPose.getY(); + + double dx = targetpos.getX();// - robotPose.getX(); + double dy = targetpos.getY();// - robotPose.getY(); if (!isLidar){ - desiredHeading = new Rotation2d(Math.atan2(dy, dx)+ Math.PI); + desiredHeading = (Math.atan2(dy, dx)+ Math.PI) * (180. / Math.PI) + 180; }else{ - desiredHeading = new Rotation2d(Math.atan2(dx, dy)); + desiredHeading = (Math.atan2(dx, dy)) * (180. / Math.PI);// + 180; } - roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); - if (roterr > 180) roterr -= 360; - if (roterr < -180) roterr += 360; + targetRotation = swerveDrive.getPose2d().getRotation().getDegrees() - desiredHeading; - SmartDashboard.putNumber("PID Rot Error", roterr); - System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); - swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading); + // roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees(); + // if (roterr > 180) roterr -= 360; + // if (roterr < -180) roterr += 360; + + SmartDashboard.putNumber("Target Rotation!", targetRotation); + // System.out.println("Target: " + targetpos + "Heading: " + desiredHeading + "Error: " + roterr); + swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(targetRotation)); } @Override public final boolean isFinished() { - boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; - if (finished) { - swerveDrive.softStop(); - } - return finished; + // boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE; + // if (finished) { + // swerveDrive.softStop(); + // } + // return finished; + return false; } private class PID { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 77101ba..4298429 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 = 91; - public static final String GIT_SHA = "b3b32b00fed827f8efe77d0f6710d36222a209ac"; - public static final String GIT_DATE = "2026-02-25 17:17:52 MST"; + public static final int GIT_REVISION = 96; + public static final String GIT_SHA = "3745cc2b1869e5850c93507277539c7cfed606c1"; + public static final String GIT_DATE = "2026-02-25 18:59:11 MST"; public static final String GIT_BRANCH = "AutoTesting"; - public static final String BUILD_DATE = "2026-02-25 17:44:49 MST"; - public static final long BUILD_UNIX_TIME = 1772066689466L; + public static final String BUILD_DATE = "2026-02-25 20:19:51 MST"; + public static final long BUILD_UNIX_TIME = 1772075991136L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index b46648a..5a9e1c2 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -45,13 +45,13 @@ public class IntakeConstants { public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(0.1) + .withKP(0.03) .withKI(0.0) .withKD(0.0); - public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.05); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.03); public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0);