From a06b90ca0119d182cdb143f3910240fadb17839b Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Mon, 31 Mar 2025 17:57:30 -0600 Subject: [PATCH] end of denver --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 122 +++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 3 +- .../java/frc4388/robot/subsystems/Vision.java | 172 ++++++++---------- 4 files changed, 191 insertions(+), 110 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 56d161a..7f0cbfa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -373,8 +373,8 @@ public final class Constants { // The standard deviations of our vision estimated poses, which affect correction rate // X, Y, Theta // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 - public static final Matrix kSingleTagStdDevs = VecBuilder.fill(3, 3, 4); - public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } public static final class FieldConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 11135de..37a6cbb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -130,6 +130,7 @@ public class RobotContainer { private Command waitFeedStation = new waitSupplier(m_robotElevator::readyToMove); // private Command waitDebuger = new waitSupplier(m_driverXbox::getYButtonPressed); + // private Command waitDebugerManual = new waitSupplier(m_driverXbox::getYButtonPressed); private Command waitDebuger = new waitSupplier(() -> true); // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); @@ -138,7 +139,7 @@ public class RobotContainer { // new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, // true, false); // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); - private Command AprilLidarAlignL4Right = new SequentialCommandGroup( + private Command AprilLidarAlignL4RightFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new ConditionalCommand(Commands.none(), new SequentialCommandGroup( @@ -175,11 +176,57 @@ public class RobotContainer { // new ConditionalCommand( // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), - // () -> m_robotElevator.hasCoral()), + // () -> m_robotElevator.hasCoral()) new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); + private Command AprilLidarAlignL4RightSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.RIGHT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true), + waitDebuger.asProxy(), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + // () -> m_robotElevator.hasCoral()) + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + + private Command WannaSeeMeDunk = new SequentialCommandGroup( + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand( + m_robotSwerveDrive, + new Translation2d(0,1), + new Translation2d(), + AutoConstants.L4_DRIVE_TIME, + true + ), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + new InstantCommand(m_robotSwerveDrive::startSlowPeriod, m_robotSwerveDrive) + ); + /* private Command AprilLidarAlignL4Right = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), @@ -253,7 +300,7 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); */ - private Command AprilLidarAlignL4Left = new SequentialCommandGroup( + private Command AprilLidarAlignL4LeftFullAuto = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( @@ -290,8 +337,8 @@ public class RobotContainer { new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), - // new ConditionalCommand( - // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // // new ConditionalCommand( + // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), // () -> m_robotElevator.hasCoral()), @@ -300,6 +347,51 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); + private Command AprilLidarAlignL4LeftSemiAuto = new SequentialCommandGroup( + new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + + // new IfCommand(() -> m_robotElevator.isL4Primed(), new SequentialCommandGroup( + + new ConditionalCommand(Commands.none(), new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_PREP, Side.LEFT, true) + ), () -> m_robotElevator.isL4Primed()), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true), + waitDebuger.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // waitDebuger.asProxy(), + // new ParallelRaceGroup( + // new WaitCommand(1), + // new MoveUntilSuply( + // m_robotSwerveDrive, + // new Translation2d(0,-0.5), + // new Translation2d(), m_robotElevator::getEndeffectorLimit, true) + // ), + // new InstantCommand(m_robotSwerveDrive::softStop, m_robotSwerveDrive), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + + // new waitEndefectorRefrence(m_robotElevator), + + + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.L4_DRIVE_TIME, true), + + // // // new ConditionalCommand( + // // // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator), + // // () -> m_robotElevator.hasCoral()), + + new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + ); + private Command AprilLidarAlignL3Left = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), @@ -312,7 +404,7 @@ public class RobotContainer { new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.LEFT, true), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true), waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), @@ -504,8 +596,8 @@ public class RobotContainer { ), //new InstantCommand(() -> Constants.AutoConstants.Y_OFFSET_TRIM.set(0)), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive))); - NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4Left); - NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4Right); + NamedCommands.registerCommand("place-coral-left-l4", AprilLidarAlignL4LeftFullAuto); + NamedCommands.registerCommand("place-coral-right-l4", AprilLidarAlignL4RightFullAuto); NamedCommands.registerCommand("place-coral-left-l3", AprilLidarAlignL3Left); NamedCommands.registerCommand("place-coral-right-l3", AprilLidarAlignL3Right); NamedCommands.registerCommand("place-coral-left-l2", AprilLidarAlignL2Left); @@ -641,14 +733,14 @@ public class RobotContainer { 1, Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) ), - getDeadbandedDriverController().getRight(), 0.30 + getDeadbandedDriverController().getRight(), 0.15 ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); - // // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); + .onTrue(WannaSeeMeDunk.asProxy()); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy()); @@ -667,10 +759,10 @@ public class RobotContainer { // Button box new JoystickButton(getButtonBox(), ButtonBox.Five) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.One) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new JoystickButton(getButtonBox(), ButtonBox.Six) .onTrue(AprilLidarAlignL3Left); @@ -729,10 +821,10 @@ public class RobotContainer { // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Left); + .onTrue(AprilLidarAlignL4LeftSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(AprilLidarAlignL4Right); + .onTrue(AprilLidarAlignL4RightSemiAuto); new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode) .onTrue(AprilLidarAlignL3Left); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0fb38ef..c1f9335 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -351,7 +351,8 @@ public class SwerveDrive extends Subsystem { rotTarget += deltaAngle; } - swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); + vision.addVisionMeasurement(swerveDriveTrain); + // swerveDriveTrain.addVisionMeasurement(vision.lastVisionPose, time); //back to the ~~future~~ *past* } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index c662a40..4f34193 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import java.time.Instant; +import java.util.ArrayList; import java.util.List; import java.util.Optional; @@ -49,6 +50,7 @@ public class Vision extends Subsystem { private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; + private List poses = new ArrayList<>(); private boolean isTagDetected = false; private boolean isTagProcessed = false; @@ -136,7 +138,8 @@ public class Vision extends Subsystem { // double Yaw = 0; double latency = 0; - Pose2d pose = null; + // Pose2d pose = null; + poses.clear(); for(int i = 0; i < cameras.length; i++){ PhotonCamera camera = cameras[i]; @@ -163,11 +166,13 @@ public class Vision extends Subsystem { // If the tag was failed to be processed if(estimatedRobotPose.isEmpty()) continue; + + poses.add(estimatedRobotPose.get()); - if(pose == null) - pose = estimatedRobotPose.get().estimatedPose.toPose2d(); - else - pose = pose.interpolate(pose, 0.5); + // if(pose == null) + // pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + // else + // pose = pose.interpolate(pose, 0.5); // X += pose.getX(); // Y += pose.getY(); @@ -181,43 +186,20 @@ public class Vision extends Subsystem { } - lastLatency = latency / cams; + // lastLatency = latency / cams; - if(isTagProcessed){ - // Instant now = Instant.now(); - - // double curAngle = (Yaw/cams); - - // Pose2d e = new Pose2d(); + // if(isTagProcessed){ + // lastVisionPose = pose; + // // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); + // // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); + // // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); + // // SmartDashboard.putNumber("Rotations", rotations); - - // // double curAngle = + (((Math.random() * 2) - 1 + 360) % 360) - 180; // Generate loopover noise - - - // if(lastVisionTime != null && Math.abs(now.getEpochSecond() - lastVisionTime.getEpochSecond()) <= 1){ - // double diff = curAngle - lastVisionPose.getRotation().getDegrees() + rotations*360; - - // if(diff > 180){ - // rotations -= 1; - // }else if(diff < -180){ - // rotations += 1; - // } - // } - - - - lastVisionPose = pose; - // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); - // lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); - - // SmartDashboard.putNumber("curAngle", pose.getRotation().getRotations()); - // SmartDashboard.putNumber("Rotations", rotations); - - lastVisionTime = now; - } + // lastVisionTime = now; + // } } @@ -247,66 +229,66 @@ public class Vision extends Subsystem { return visionEst; // Will be empty visionEst = estimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets(), estimator); + // updateEstimationStdDevs(visionEst, change.getTargets(), estimator); return visionEst; } - /** - * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard - * deviations based on number of tags, estimation strategy, and distance from the tags. - * - * @param estimatedPose The estimated pose to guess standard deviations for. - * @param targets All targets in this camera frame - */ - private void updateEstimationStdDevs( - Optional estimatedPose, - List targets, - PhotonPoseEstimator estimator) { - if (estimatedPose.isEmpty()) { - // No pose input. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; + // /** + // * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + // * deviations based on number of tags, estimation strategy, and distance from the tags. + // * + // * @param estimatedPose The estimated pose to guess standard deviations for. + // * @param targets All targets in this camera frame + // */ + // private void updateEstimationStdDevs( + // Optional estimatedPose, + // List targets, + // PhotonPoseEstimator estimator) { + // if (estimatedPose.isEmpty()) { + // // No pose input. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // Pose present. Start running Heuristic - var estStdDevs = VisionConstants.kSingleTagStdDevs; - int numTags = 0; - double avgDist = 0; + // } else { + // // Pose present. Start running Heuristic + // var estStdDevs = VisionConstants.kSingleTagStdDevs; + // int numTags = 0; + // double avgDist = 0; - // Precalculation - see how many tags we found, and calculate an average-distance metric - for (var tgt : targets) { - var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; + // // Precalculation - see how many tags we found, and calculate an average-distance metric + // for (var tgt : targets) { + // var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); + // if (tagPose.isEmpty()) continue; - double distance = tagPose - .get() - .toPose2d() - .getTranslation() - .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + // double distance = tagPose + // .get() + // .toPose2d() + // .getTranslation() + // .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); - if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { - numTags++; - avgDist += distance; - } - } + // if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) { + // numTags++; + // avgDist += distance; + // } + // } - if (numTags == 0) { - // No tags visible. Default to single-tag std devs - curStdDevs = VisionConstants.kSingleTagStdDevs; - } else { - // One or more tags visible, run the full heuristic. - avgDist /= numTags; - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - curStdDevs = estStdDevs; - } - } - } + // if (numTags == 0) { + // // No tags visible. Default to single-tag std devs + // curStdDevs = VisionConstants.kSingleTagStdDevs; + // } else { + // // One or more tags visible, run the full heuristic. + // avgDist /= numTags; + // // Decrease std devs if multiple targets are visible + // if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // // Increase std devs based on (average) distance + // if (numTags == 1 && avgDist > 4) + // estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + // else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + // curStdDevs = estStdDevs; + // } + // } + // } /** * Returns the latest standard deviations of the estimated pose from {@link @@ -331,11 +313,13 @@ public class Vision extends Subsystem { // } public Pose2d getPose2d() { - if(isTagDetected && isTagProcessed) - // return lastVisionPose; - return lastPhysOdomPose; - else + if(lastPhysOdomPose != null) return lastPhysOdomPose; + return new Pose2d(); + // if(isTagDetected && isTagProcessed) + // // return lastVisionPose; + // else + // return lastPhysOdomPose; } public static double getTime() { @@ -347,7 +331,11 @@ public class Vision extends Subsystem { } - + public void addVisionMeasurement( SwerveDrivetrain drivetrain){ + for(EstimatedRobotPose pose : poses){ + drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds)); + } + }