diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24c219af..7410abe5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -221,7 +221,6 @@ public RobotContainer() { if(lightsExist) {lightsInst();} if(indexerExists) {indexInit();} if(intakeExists && shooterExists && indexerExists && angleShooterExists) {indexCommandInst();} - Limelight.useDetectorLimelight(useDetectorLimelight); configureDriveTrain(); configureBindings(); autoInit(); @@ -273,7 +272,7 @@ private void climberInst() { ClimberDownSup)); } private void indexInit() { - indexer = new IndexerSubsystem(intakeExists ? intake::isNoteSeen : () -> false); + indexer = new IndexerSubsystem(intakeExists ? RobotState.getInstance()::isNoteSeen : () -> false); } private void indexCommandInst() { defaulNoteHandlingCommand = new IndexCommand(indexer, ShootIfReadySup, AimWhileMovingSup, shooter, intake, driveTrain, angleShooterSubsystem, HumanPlaySup, StageAngleSup, SubwooferAngleSup, GroundIntakeSup, FarStageAngleSup, OperatorRevToZero, intakeReverse, OverStagePassSup, OppositeStageShotSup); @@ -341,13 +340,13 @@ public boolean runsWhenDisabled() { if(Preferences.getBoolean("Detector Limelight", false)) { Command AutoGroundIntake = new SequentialCommandGroup( new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem), - new InstantCommand(()->intake.setNoteHeld(true)) + new InstantCommand(()->RobotState.getInstance().IsNoteHeld = true) ); autoPickup = new ParallelRaceGroup( new SequentialCommandGroup( new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(30), angleShooterSubsystem), new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem), - new InstantCommand(()->intake.setNoteHeld(true))), + new InstantCommand(()->RobotState.getInstance().setNoteHeld(true))), new SequentialCommandGroup( new CollectNote(driveTrain, limelight), new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain), @@ -358,7 +357,7 @@ public boolean runsWhenDisabled() { podiumAutoPickup = new ParallelRaceGroup( new SequentialCommandGroup( new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem), - new InstantCommand(()->intake.setNoteHeld(true))), + new InstantCommand(()->RobotState.getInstance().setNoteHeld(true))), new SequentialCommandGroup( new PodiumCollectNote(driveTrain, limelight), new DriveTimeCommand(-1, 0, 0, 0.5, driveTrain), @@ -387,7 +386,7 @@ public boolean runsWhenDisabled() { new Trigger(GroundIntakeSup).whileTrue(new GroundIntake(intake, indexer, driveTrain, angleShooterSubsystem)); } if(intakeExists&&indexerExists) { - new Trigger(intake::isNoteSeen).and(()->!intake.isNoteHeld()).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5)); + new Trigger(()->RobotState.getInstance().isNoteSeen()).and(()->!RobotState.getInstance().IsNoteHeld).and(()->DriverStation.isTeleop()).and(()->!AimWhileMovingSup.getAsBoolean()).onTrue(new IndexerNoteAlign(indexer, intake).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).withTimeout(5)); } SmartDashboard.putData("move 1 meter", new MoveMeters(driveTrain, 1, 0.2, 0.2, 0.2)); @@ -579,7 +578,7 @@ public void execute() { if(sideWheelsExists){ NamedCommands.registerCommand("intakeSideWheels", new InstantCommand(()-> intake.intakeSideWheels(1))); } - NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!intake.isNoteSeen())); + NamedCommands.registerCommand("note isn't held", new WaitUntil(()->!RobotState.getInstance().isNoteSeen())); } if(indexerExists&&shooterExists) { NamedCommands.registerCommand("initialShot", new InitialShot(shooter, indexer, 0.9, 0.1, angleShooterSubsystem)); @@ -624,7 +623,7 @@ public void ampShotInit() { new WaitUntil(()->(shooter.validShot() && driveTrain.getChassisSpeeds().vxMetersPerSecond == 0)), new InstantCommand(()->indexer.magicRPS(indexerAmpSpeed), indexer),//45 worked but a bit too high new WaitCommand(0.5), - new InstantCommand(()->intake.setNoteHeld(false)) + new InstantCommand(()->RobotState.getInstance().setNoteHeld(false)) ); SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); /** @@ -646,7 +645,7 @@ public void ampShotInit() { new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), new WaitCommand(0.5), - new InstantCommand(()->intake.setNoteHeld(false)) + new InstantCommand(()->RobotState.getInstance().setNoteHeld(false)) ); new Trigger(AmpAngleSup).whileTrue(orbitAmpShot); SmartDashboard.putData("amp shot", scoreAmp); @@ -688,6 +687,13 @@ public void robotPeriodic() { currentAlliance = DriverStation.getAlliance().get(); SmartDashboard.putBoolean("RobotPeriodicRan", true); SmartDashboard.putString("AlliancePeriodic", currentAlliance == null? "null" : currentAlliance == Alliance.Red? "Red": "Blue" ); + + if(Preferences.getBoolean("Use Limelight", false)) { + limelight.updateLoggingWithPoses(); + } + + SmartDashboard.putBoolean("is note in", RobotState.getInstance().isNoteSeen()); + SmartDashboard.putBoolean("is note held", RobotState.getInstance().IsNoteHeld); // logPower(); } public void disabledPeriodic() { diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 72896d9e..3afbcbcb 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -34,7 +34,7 @@ public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer, Drivetrain // Called when the command is initially scheduled. @Override public void initialize() { - if (intake.isNoteSeen()){ + if (RobotState.getInstance().isNoteSeen()){ RobotState.getInstance().lightsReset = true; } angleShooter.setDesiredShooterAngle(30); @@ -75,6 +75,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return intake.isNoteSeen(); + return RobotState.getInstance().isNoteSeen(); } } diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index a4d6cc4c..7506e414 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -116,12 +116,12 @@ public void execute() { } else { auto = false; } - if (!intake.isNoteSeen()) { + if (!RobotState.getInstance().isNoteSeen()) { // intake.intakeYes(IntakeConstants.INTAKE_SPEED); // only code that runs the intake if(runsEmpty<21) {runsEmpty++;} if(runsEmpty>=20) { idleReving = false; - intake.setNoteHeld(false); + RobotState.getInstance().IsNoteHeld = false; if(humanPlayerSupplier.getAsBoolean()) { m_Indexer.set(IndexerConstants.HUMAN_PLAYER_INDEXER_SPEED); shooter.setTargetVelocity(HUMAN_PLAYER_RPS, HUMAN_PLAYER_RPS, 40, 40); @@ -168,8 +168,8 @@ public void execute() { } if (indexer) { m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_POWER); - if(!intake.isNoteSeen()) { - intake.setNoteHeld(false); + if(!RobotState.getInstance().isNoteSeen()) { + RobotState.getInstance().IsNoteHeld = false; } } else { m_Indexer.off(); diff --git a/src/main/java/frc/robot/commands/IndexerNoteAlign.java b/src/main/java/frc/robot/commands/IndexerNoteAlign.java index d0d08ce4..f4813f10 100644 --- a/src/main/java/frc/robot/commands/IndexerNoteAlign.java +++ b/src/main/java/frc/robot/commands/IndexerNoteAlign.java @@ -11,6 +11,7 @@ import frc.robot.settings.Constants.IndexerConstants; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RobotState; import frc.robot.subsystems.ShooterSubsystem; // NOTE: Consider using this command inline, rather than writing a subclass. For more @@ -35,9 +36,9 @@ public IndexerNoteAlign(IndexerSubsystem indexer, IntakeSubsystem intake) { new WaitCommand(()->0.6), new InstantCommand(()->indexer.magicRPS(10), indexer), new WaitCommand(()->0.3), - new WaitUntil(()->intake.isNoteSeen()), + new WaitUntil(()->RobotState.getInstance().isNoteSeen()), new WaitCommand(()->0.1), - new InstantCommand(()->intake.setNoteHeld(true)) + new InstantCommand(()->RobotState.getInstance().IsNoteHeld = true) ); } } diff --git a/src/main/java/frc/robot/commands/ManualShoot.java b/src/main/java/frc/robot/commands/ManualShoot.java index fc083a8d..a1471e64 100644 --- a/src/main/java/frc/robot/commands/ManualShoot.java +++ b/src/main/java/frc/robot/commands/ManualShoot.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; +import frc.robot.subsystems.RobotState; import frc.robot.settings.Constants.IndexerConstants; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -45,7 +46,7 @@ public void execute() { @Override public void end(boolean interrupted) { indexer.off(); - intake.setNoteHeld(false); + RobotState.getInstance().IsNoteHeld = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/NamedCommands/AutoGroundIntake.java b/src/main/java/frc/robot/commands/NamedCommands/AutoGroundIntake.java index e1440885..cad69789 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/AutoGroundIntake.java +++ b/src/main/java/frc/robot/commands/NamedCommands/AutoGroundIntake.java @@ -14,6 +14,7 @@ import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RobotState; public class AutoGroundIntake extends Command { /** Creates a new ConditionalIndexer. */ IndexerSubsystem indexer; @@ -55,12 +56,12 @@ public void execute() { public void end(boolean interrupted) { intake.intakeOff(); indexer.off(); - intake.setNoteHeld(true); + RobotState.getInstance().IsNoteHeld = true; } // Returns true when the command should end. @Override public boolean isFinished() { - return intake.isNoteSeen(); + return RobotState.getInstance().isNoteSeen(); } } diff --git a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java index 1e30e68d..6681d8d3 100644 --- a/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java +++ b/src/main/java/frc/robot/commands/NamedCommands/ShootNote.java @@ -4,6 +4,7 @@ package frc.robot.commands.NamedCommands; +import frc.robot.subsystems.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -54,7 +55,7 @@ public void end(boolean interrupted) { timer.stop(); indexer.off(); timer.reset(); - intake.setNoteHeld(false); + RobotState.getInstance().IsNoteHeld = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 3ee5c78b..3c1ab89c 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -220,6 +220,10 @@ public double getHeadingLooped() { public Rotation2d getGyroscopeRotation() { return pigeon.getRotation2d(); } + /** + * + * @return a rotation2D of the angle according to the odometer + */ public Rotation2d getOdometryRotation() { return odometer.getEstimatedPosition().getRotation(); } @@ -313,7 +317,7 @@ public void updateOdometry() { * larger pose shifts will take multiple calls to complete. */ public void updateOdometryWithVision() { - PoseEstimate estimate = limelight.getTrustedPose(getPose(), getLLFOM(APRILTAG_LIMELIGHT2_NAME), getLLFOM(APRILTAG_LIMELIGHT3_NAME)); + PoseEstimate estimate = limelight.getTrustedPose(); if (estimate != null) { boolean doRejectUpdate = false; if(Math.abs(pigeon.getRate()) > 720) { @@ -330,8 +334,6 @@ public void updateOdometryWithVision() { } else { RobotState.getInstance().LimelightsUpdated = false; } - - limelight.updateLoggingWithPoses(); } /** * Set the odometry using the current apriltag estimate, disregarding the pose trustworthyness. @@ -339,7 +341,7 @@ public void updateOdometryWithVision() { * You only need to run this once for it to take effect. */ public void forceUpdateOdometryWithVision() { - PoseEstimate estimate = limelight.getValidPose(); + PoseEstimate estimate = limelight.getTrustedPose(); if (estimate != null) { resetOdometry(estimate.pose); } else { @@ -574,6 +576,7 @@ public void periodic() { m_field.setRobotPose(odometer.getEstimatedPosition()); SmartDashboard.putNumber("Robot Angle", getOdometryRotation().getDegrees()); + RobotState.getInstance().odometerOrientation = getOdometryRotation().getDegrees(); SmartDashboard.putString("Robot Location", getPose().getTranslation().toString()); SmartDashboard.putNumber("calculated speaker angle", calculateSpeakerAngleMoving()); SmartDashboard.putNumber("TESTING robot angle difference", getSpeakerAngleDifference()); @@ -622,37 +625,4 @@ private Pose3d robotToFieldCoordinates(Pose3d robotCoordinates) return new Pose3d(robotCoordinatesX+odometrPoseX,robotCoordinatesY+odometrPosey,robotCoordinatesZ, robotCoordinatesAngle); } - - public double getLLFOM(String limelightName) //larger fom is BAD, and is less trustworthy. - { - //the value we place on each variable in the FOM. Higher value means it will get weighted more in the final FOM - /*These values should be tuned based on how heavily you want a contributer to be favored. Right now, we want the # of tags to be the most important - * with the distance from the tags also being immportant. and the tx and ty should only factor in a little bit, so they have the smallest number. Test this by making sure the two - * limelights give very different robot positions, and see where it decides to put the real robot pose. - */ - double distValue = 6; - double tagCountValue = 7; - double xyValue = 1; - - //numTagsContributer is better when smaller, and is based off of how many april tags the Limelight identifies - double numTagsContributer; - if(limelight.getLLTagCount(limelightName) <= 0){ - numTagsContributer = 0; - }else{ - numTagsContributer = 1/limelight.getLLTagCount(limelightName); - } - //tx and ty contributers are based off where on the limelights screen the april tag is. Closer to the center means the contributer will bea smaller number, which is better. - double centeredTxContributer = Math.abs((limelight.getAprilValues(limelightName).tx))/29.8; //tx gets up to 29.8, the closer to 0 tx is, the closer to the center it is. - double centeredTyContributer = Math.abs((limelight.getAprilValues(limelightName).ty))/20.5; //ty gets up to 20.5 for LL2's and down. LL3's go to 24.85. The closer to 0 ty is, the closer to the center it is. - //the distance contributer gets smaller when the distance is closer, and is based off of how far away the closest tag is - double distanceContributer = (limelight.getClosestTagDist(limelightName)/5); - - // calculates the final FOM by taking the contributors and multiplying them by their values, adding them all together and then dividing by the sum of the values. - double LLFOM = ( - (distValue*distanceContributer)+(tagCountValue*numTagsContributer)+(centeredTxContributer*xyValue)+(centeredTyContributer) - )/distValue+tagCountValue+xyValue+xyValue; - Logger.recordOutput("Vision/LLFOM" + limelightName, LLFOM); - return LLFOM; - } - } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 48c0d835..fa1f78ae 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -83,12 +83,7 @@ public IntakeSubsystem() { intakeSideRight.burnFlash(); //intakeSideRight.getPIDController().setReference(intakeRunSpeed, CANSparkMax.ControlType.kVelocity) } - - if (Preferences.getBoolean("CompBot", true)) { - m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute); - } else { - m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute); - } + m_DistanceSensor = intakeSideLeft.getAnalog(Mode.kAbsolute); DataLog log = DataLogManager.getLog(); motorLogger1 = new MotorLogger(log, "/intake/motor1"); @@ -152,24 +147,6 @@ public void intakeOff() { } } - /** - * uses the distance sensor inside the indexer to tell if there is a note fully - * inside the indexer - * - * @return if the sensor sees something within it's range in front of it - */ - public boolean isNoteSeen() { - return m_DistanceSensor.getVoltage() < 2; - } - - public boolean isNoteHeld() { - return isNoteHeld; - } - - public void setNoteHeld(boolean held) { - isNoteHeld = held; - } - @Override public void periodic() { SmartDashboard.putNumber("voltage sensor output", m_DistanceSensor.getVoltage()); @@ -178,9 +155,7 @@ public void periodic() { motorLogger1.log(intake1); motorLogger2.log(intake2); logDistance.append(m_DistanceSensor.getVoltage()); - logNoteIn.append(isNoteSeen()); - SmartDashboard.putBoolean("is note in", isNoteSeen()); - SmartDashboard.putBoolean("is note held", isNoteHeld()); - RobotState.getInstance().IsNoteHeld = isNoteSeen(); + logNoteIn.append(RobotState.getInstance().isNoteSeen()); + RobotState.getInstance().intakeSensorVoltage = m_DistanceSensor.getVoltage(); } } diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 2faf8765..581ee252 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/Limelight.java @@ -34,8 +34,6 @@ public class Limelight { private static Field2d field1 = new Field2d(); private static Field2d field2 = new Field2d(); - public static Boolean detectorEnabled = false; - public static final AprilTagFieldLayout FIELD_LAYOUT; static { @@ -75,10 +73,6 @@ public static Limelight getInstance() { return limelight; } - public static void useDetectorLimelight(boolean enabled) { - detectorEnabled = enabled; - } - /** * Gets the most recent limelight pose estimate, given that a trustworthy * estimate is @@ -95,19 +89,19 @@ public static void useDetectorLimelight(boolean enabled) { * @return A valid and trustworthy pose. Null if no valid pose. Poses are * prioritized by lowest tagDistance. */ - public PoseEstimate getTrustedPose(Pose2d odometryPose, double LL1FOM, double LL2FOM) { + public PoseEstimate getTrustedPose() { PoseEstimate pose1 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT2_NAME); PoseEstimate pose2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(APRILTAG_LIMELIGHT3_NAME); - - Boolean pose1Trust = isTrustworthy(APRILTAG_LIMELIGHT2_NAME, pose1, odometryPose); - Boolean pose2Trust = isTrustworthy(APRILTAG_LIMELIGHT3_NAME, pose2, odometryPose); + //we aren't using isTrustworthy here becuase as LL readings have gotten more reliable, we care less about tag distance + Boolean pose1Trust = isValid(APRILTAG_LIMELIGHT2_NAME, pose1); + Boolean pose2Trust = isValid(APRILTAG_LIMELIGHT3_NAME, pose2); //if the limelight positions will be merged, let SmartDashboard know! boolean mergingPoses = false; if(pose1Trust&&pose2Trust) { mergingPoses = true;} SmartDashboard.putBoolean("LL poses merged", mergingPoses); if (pose1Trust && pose2Trust) { - return (mergedPose(pose1, pose2, LL1FOM, LL2FOM)); //merge the two positions proportionally based on the closest tag distance + return (mergedPose(pose1, pose2, getLLFOM(APRILTAG_LIMELIGHT2_NAME), getLLFOM(APRILTAG_LIMELIGHT3_NAME))); //merge the two positions proportionally based on the closest tag distance } else if (pose1Trust) { return pose1; } else if (pose2Trust) { @@ -207,6 +201,42 @@ public PoseEstimate getValidPose() { } else return null; } +/** + * larger FOM is bad, and should be used to indicate that this limelight is less trestworthy + * @param limelightName + * @return + */ + public double getLLFOM(String limelightName) //larger fom is BAD, and is less trustworthy. + { + //the value we place on each variable in the FOM. Higher value means it will get weighted more in the final FOM + /*These values should be tuned based on how heavily you want a contributer to be favored. Right now, we want the # of tags to be the most important + * with the distance from the tags also being immportant. and the tx and ty should only factor in a little bit, so they have the smallest number. Test this by making sure the two + * limelights give very different robot positions, and see where it decides to put the real robot pose. + */ + double distValue = 6; + double tagCountValue = 7; + double xyValue = 1; + + //numTagsContributer is better when smaller, and is based off of how many april tags the Limelight identifies + double numTagsContributer; + if(limelight.getLLTagCount(limelightName) <= 0){ + numTagsContributer = 0; + }else{ + numTagsContributer = 1/limelight.getLLTagCount(limelightName); + } + //tx and ty contributers are based off where on the limelights screen the april tag is. Closer to the center means the contributer will bea smaller number, which is better. + double centeredTxContributer = Math.abs((limelight.getAprilValues(limelightName).tx))/29.8; //tx gets up to 29.8, the closer to 0 tx is, the closer to the center it is. + double centeredTyContributer = Math.abs((limelight.getAprilValues(limelightName).ty))/20.5; //ty gets up to 20.5 for LL2's and down. LL3's go to 24.85. The closer to 0 ty is, the closer to the center it is. + //the distance contributer gets smaller when the distance is closer, and is based off of how far away the closest tag is + double distanceContributer = (limelight.getClosestTagDist(limelightName)/5); + + // calculates the final FOM by taking the contributors and multiplying them by their values, adding them all together and then dividing by the sum of the values. + double LLFOM = ( + (distValue*distanceContributer)+(tagCountValue*numTagsContributer)+(centeredTxContributer*xyValue)+(centeredTyContributer) + )/distValue+tagCountValue+xyValue+xyValue; + Logger.recordOutput("Vision/LLFOM" + limelightName, LLFOM); + return LLFOM; + } public LimelightDetectorData getNeuralDetectorValues() { return new LimelightDetectorData( @@ -268,8 +298,6 @@ private boolean isValid(String limelightName, PoseEstimate estimate) { private boolean isTrustworthy(String limelightName, PoseEstimate estimate, Pose2d odometryPose) { Boolean trusted = ( isValid(limelightName, estimate) && - // estimate.pose.getTranslation().getDistance(odometryPose.getTranslation()) < ALLOWABLE_POSE_DIFFERENCE && // Unused - // ((estimate.avgTagDist < MAX_TAG_DISTANCE) || (estimate.tagCount >= 2 && estimate.avgTagDist<6))); // Trust poses when there are two tags, or when the tags are close to the camera. estimate.avgTagDist<7); if (limelightName.equalsIgnoreCase(APRILTAG_LIMELIGHT2_NAME)) { diff --git a/src/main/java/frc/robot/subsystems/RobotState.java b/src/main/java/frc/robot/subsystems/RobotState.java index 8d5dafad..45ba83de 100644 --- a/src/main/java/frc/robot/subsystems/RobotState.java +++ b/src/main/java/frc/robot/subsystems/RobotState.java @@ -8,15 +8,24 @@ public class RobotState { public double ShooterError; public boolean LimelightsUpdated; public boolean lightsReset; + public double intakeSensorVoltage; + public double odometerOrientation; private RobotState() { } - public static RobotState getInstance() { if (instance == null) { instance = new RobotState(); } return instance; } + + public boolean isNoteSeen() { + return intakeSensorVoltage < 2; + } + + public void setNoteHeld(boolean held) { + IsNoteHeld = held; + } }