From e04bc20802de08282e8cf5edca9a3fe4045fb6db Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Fri, 17 Apr 2026 16:02:54 -0400 Subject: [PATCH] Comp Commit --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 9 ++-- src/main/java/frc/robot/Utils.java | 6 +++ .../commands/AutoTurretShootCommand.java | 3 ++ .../frc/robot/subsystems/DriveSubsystem.java | 54 ++++++++++++++----- .../frc/robot/subsystems/TurretSubsystem.java | 7 +-- 6 files changed, 61 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index da05f41..7ef4d67 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -151,7 +151,7 @@ public final class Constants { public static final double DEFLECTOR_STORED_ANGLE = 0; public static final double DEFLECTOR_SET_ANGLE1 = 100; public static final double DEFLECTOR_SET_ANGLE2 = 200; - public static final double DEFLECTOR_SET_ANGLE3 = 300; // Return to 300 once hood is fixed + public static final double DEFLECTOR_SET_ANGLE3 = 300; public static final double DEFLECTOR_SERVO_RATIO = 1.0; } @@ -175,7 +175,7 @@ public final class Constants { // Left/Right are from driver's perspective. Coordinates assume origin at blue side of field. // Rotation are zero because they don't matter. public static Pose3d BLUE_LEFT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); - public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 6.35, 0, new Rotation3d()); + public static Pose3d BLUE_RIGHT_ALLIANCE_ZONE_PASS_TARGET_POSE = new Pose3d(1.99, 1.72, 0, new Rotation3d()); public static Pose3d BLUE_LEFT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 6.35, 0, new Rotation3d()); public static Pose3d BLUE_RIGHT_NEUTRAL_ZONE_PASS_TARGET_POSE = new Pose3d(8.27, 1.72, 0, new Rotation3d()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd578a7..e4d1052 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoTurretCommand; import frc.robot.commands.AutoTurretShootCommand; import frc.robot.commands.Reverse; +import frc.robot.commands.SetDeflectorCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -72,10 +73,10 @@ public class RobotContainer { * */ private void configureBindings() { - /*m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); - m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); - m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); - m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2));*/ + // m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); + // m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); + // m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); + // m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); //m_driverController.back().whileTrue(new AutoShootCommandChassisTurretPathPlanner(driveSubsystem, hopperSubsystem, turretSubsystem)); m_driverController.leftBumper().onTrue(Commands.runOnce(() -> turretSubsystem.homeTurret(), turretSubsystem)); diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index 06a1a91..0bde9b0 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/src/main/java/frc/robot/Utils.java @@ -4,6 +4,9 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Constants; import frc.robot.Constants.ShootingRegion; import frc.robot.Constants.ShootingRegionDimensions; + +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.units.measure.Angle; @@ -91,6 +94,9 @@ public class Utils { region = Constants.ShootingRegion.RED_RIGHT_OPPONENT_ALLIANCE_ZONE; } } + + Logger.recordOutput("Utils-ShootingRegion", region); + return region; } diff --git a/src/main/java/frc/robot/commands/AutoTurretShootCommand.java b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java index 7681348..33e9a0b 100644 --- a/src/main/java/frc/robot/commands/AutoTurretShootCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java @@ -42,6 +42,7 @@ public class AutoTurretShootCommand extends Command{ hopperSubsystem.spindexerOn(); CommandScheduler.getInstance().schedule(waitCommand); alliance = DriverStation.getAlliance().get(); + Logger.recordOutput("AutoTurretShoot-State", "SHOOTING"); } @Override @@ -73,6 +74,8 @@ public class AutoTurretShootCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + Logger.recordOutput("AutoTurretShoot-State", "NOT SHOOTING"); + DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index c2cdf9d..f18fd65 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -1,8 +1,9 @@ package frc.robot.subsystems; +import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat; + import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; -import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveBuilder; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveDriveSystem; import org.fairportrobotics.frc.robolib.drivesystems.swerve.SwerveModule; @@ -10,8 +11,6 @@ import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -23,12 +22,12 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; - import frc.robot.Constants; public class DriveSubsystem extends TestableSubsystem { @@ -137,7 +136,7 @@ public class DriveSubsystem extends TestableSubsystem { .build(); // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 999999)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.1)); this.setDefaultCommand(Commands.run(new Runnable() { @@ -199,16 +198,45 @@ public class DriveSubsystem extends TestableSubsystem { Logger.recordOutput("RobotPose", driveSystem.getRobotPose3d()); - LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); - if (frontCameraPose.tagCount >= 1) { - driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); - Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); + LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.FRONT_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.SetRobotOrientation(Constants.CameraConstanst.BACK_CAMERA_NAME, driveSystem.getRobotPose2d().getRotation().getDegrees(), 0, 0, 0, 0, 0); + + // MegaTag 2 impl + if(RobotState.isDisabled()){ + LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 1); + LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 1); + }else{ + LimelightHelpers.SetIMUMode(Constants.CameraConstanst.FRONT_CAMERA_NAME, 4); + LimelightHelpers.SetIMUMode(Constants.CameraConstanst.BACK_CAMERA_NAME, 4); } - LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); - if (backCameraPose.tagCount >= 1) { - driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); - Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); + + LimelightHelpers.PoseEstimate frontTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.FRONT_CAMERA_NAME); + LimelightHelpers.PoseEstimate backTag = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.CameraConstanst.BACK_CAMERA_NAME); + + if(frontTag.tagCount >= 2){ + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontTag.pose), frontTag.timestampSeconds); + Logger.recordOutput("FrontPose-Estimate", frontTag.pose); + Logger.recordOutput("FrontPose-IsMegaTag2", frontTag.isMegaTag2); } + + if(backTag.tagCount >= 2){ + driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backTag.pose), backTag.timestampSeconds); + Logger.recordOutput("BackPose-Estimate", backTag.pose); + Logger.recordOutput("BackPose-IsMegaTag2", frontTag.isMegaTag2); + } + + + // MegaTag 1 Impl + // LimelightHelpers.PoseEstimate frontCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.FRONT_CAMERA_NAME); + // if (frontCameraPose.tagCount >= 2) { + // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(frontCameraPose.pose), frontCameraPose.timestampSeconds); + // Logger.recordOutput("FrontPoseEstimate", frontCameraPose.pose); + // } + // LimelightHelpers.PoseEstimate backCameraPose = LimelightHelpers.getBotPoseEstimate_wpiBlue(Constants.CameraConstanst.BACK_CAMERA_NAME); + // if (backCameraPose.tagCount >= 2) { + // driveSystem.getPoseEstimator().addVisionMeasurement(new Pose3d(backCameraPose.pose), backCameraPose.timestampSeconds); + // Logger.recordOutput("BackPoseEstimate", backCameraPose.pose); + // } } @Override diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index f45b61e..997a07a 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -115,7 +115,7 @@ public class TurretSubsystem extends TestableSubsystem { SparkMaxConfig config = new SparkMaxConfig().apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); config.voltageCompensation(10); - config.closedLoop.p(0.0002).i(0.000001).d(0.0005); // I = 0.0000001 + config.closedLoop.p(0.0001).i(0.000004).d(0.0010); // I = 0.0000001 config.closedLoop.feedForward.kS(0.2).kV(0.000).kA(0.000); // config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); @@ -243,6 +243,7 @@ public class TurretSubsystem extends TestableSubsystem { Logger.recordOutput("TurretSubsystem-TurretOffset", turretOffsetInRotations); Logger.recordOutput("TurretSubsystem-TurretForwardLimitHit", turretMotorForwardLimit.refresh().getValue()); Logger.recordOutput("TurretSubsystem-TurretReverseLimitHit", turretMotorReverseLimit.refresh().getValue()); + Logger.recordOutput("TurretSubsystem-TurretPositionError", turretPositionError.refresh().getValueAsDouble()); } } @@ -261,7 +262,7 @@ public class TurretSubsystem extends TestableSubsystem { Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5)); -Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations()); + Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations()); setTurretRobotRelative(((turretAngle.getRotations()))); } @@ -303,7 +304,7 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation } public boolean isTurretAtTarget(){ - return turretPositionError.getValueAsDouble() <= 0.5; + return turretPositionError.refresh().getValueAsDouble() <= 0.1; } private double getRequestedPosTurretRelative() {