diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 7129d954..9403200d 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 7129d95439838a599875a4f6c53159408a5fdb57 +Subproject commit 9403200d5d1161a2c75be560251618af01230b5a diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index e938b926..147c1c91 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -11,6 +11,7 @@ import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.lib.hardware.phoenix6.Phoenix6Inputs; import frc.trigon.robot.constants.RobotConstants; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -59,6 +60,7 @@ public void testInit() { @Override public void simulationPeriodic() { + SimulationFieldHandler.update(); } @Override diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2e6ec610..ee86ec47 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -16,18 +16,17 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; +import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.Swerve; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { - public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(); + public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator(); public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator( CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, - ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION, SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE, CameraConstants.OBJECT_DETECTION_CAMERA ); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index 6a6ffb1e..79abd00c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -7,8 +7,8 @@ import frc.trigon.lib.utilities.flippable.FlippableRotation2d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.misc.objectDetection.ObjectPoseEstimator; import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index e8595304..fce03ab1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -106,7 +106,7 @@ private boolean hasNoTrackedGamePiece() { } private void trackGamePiece() { - distanceFromTrackedGamePiece = RobotContainer.OBJECT_POSE_ESTIMATOR.hasObject() ? + distanceFromTrackedGamePiece = RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects() ? calculateSelfRelativeDistanceFromBestGamePiece() : null; } diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 87bc3bc3..834ee311 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: AHAHAHAHAH diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java similarity index 71% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java index c73bd5ad..6b11032b 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraConstants.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectDetectionConstants.java @@ -1,8 +1,8 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.misc.objectDetection; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -public class ObjectDetectionCameraConstants { +public class ObjectDetectionConstants { public static final int NUMBER_OF_GAME_PIECE_TYPES = SimulatedGamePieceConstants.GamePieceType.values().length; static final double TRACKED_OBJECT_TOLERANCE_METERS = 0.12; -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java new file mode 100644 index 00000000..0be39ae7 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/ObjectPoseEstimator.java @@ -0,0 +1,220 @@ +package frc.trigon.robot.misc.objectDetection; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import org.littletonrobotics.junction.Logger; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.HashSet; +import java.util.Set; + +public class ObjectPoseEstimator extends SubsystemBase { + private final double deletionThresholdSeconds; + private final SimulatedGamePieceConstants.GamePieceType gamePieceType; + private final ObjectDetectionCamera camera; + /** + * Stores the position of each detected object along with the timestamp of when it was detected. + */ + private final HashMap objectPositionsToDetectionTimestamp; + + /** + * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. + * + * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed + * @param gamePieceType the type of game piece to track + * @param camera the camera used for detecting objects + */ + public ObjectPoseEstimator(double deletionThresholdSeconds, + SimulatedGamePieceConstants.GamePieceType gamePieceType, + ObjectDetectionCamera camera) { + this.deletionThresholdSeconds = deletionThresholdSeconds; + this.gamePieceType = gamePieceType; + this.camera = camera; + this.objectPositionsToDetectionTimestamp = new HashMap<>(); + } + + /** + * Updates the object positions based on the camera detected objects. + * Removes objects that have not been detected for a certain time frame, defined in {@link ObjectPoseEstimator#deletionThresholdSeconds}. + */ + @Override + public void periodic() { + updateTrackedObjectsPositions(); + removeOldObjects(); + Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); + } + + /** + * Gets the position of all known objects on the field. + * + * @return a list of Translation2d representing the positions of objects on the field + */ + public ArrayList getObjectsOnField() { + return new ArrayList<>(objectPositionsToDetectionTimestamp.keySet()); + } + + /** + * Removes the closest object to the robot from the list of objects in the pose estimator. + */ + public void removeClosestObjectToRobot() { + final Translation2d closestObject = getClosestObjectToRobot(); + if (closestObject == null) + return; + removeObject(closestObject); + } + + /** + * Removes the closest object to the intake from the list of objects in the pose estimator. + * + * @param intakeTransform the transform of the intake relative to the robot + */ + public void removeClosestObjectToIntake(Transform2d intakeTransform) { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d closestObjectToIntake = getClosestTrackedObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation()); + if (closestObjectToIntake == null) + return; + removeObject(closestObjectToIntake); + } + + /** + * Removes the closest object to a given pose from the list of objects in the pose estimator. + * + * @param fieldRelativePose the pose to which the removed object is closest + */ + public void removeClosestObjectToPose(Pose2d fieldRelativePose) { + removeClosestObjectToPosition(fieldRelativePose.getTranslation()); + } + + /** + * Removes the closest object to a given position from the stored objects in the pose estimator. + * + * @param position the position to which the removed object is closest + */ + public void removeClosestObjectToPosition(Translation2d position) { + final Translation2d closestObject = getClosestTrackedObjectToPosition(position); + if (closestObject == null) + return; + removeObject(closestObject); + } + + /** + * Removes a specific object from the stored objects in the pose estimator. + * Unlike {@link #removeClosestObjectToPosition} which removes the closest object to a given position. + * + * @param objectPosition the position of the object to be removed. Must be the precise position as stored in the pose estimator. + */ + public void removeObject(Translation2d objectPosition) { + objectPositionsToDetectionTimestamp.remove(objectPosition); + } + + /** + * Returns whether any objects are stored in the pose estimator. + * + * @return if there are objects stored in the pose estimator + */ + public boolean hasObjects() { + return !objectPositionsToDetectionTimestamp.isEmpty(); + } + + /** + * Gets the position of the closest object to the robot. + * + * @return the closest object to the robot + */ + public Translation2d getClosestObjectToRobot() { + return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); + } + + private void updateTrackedObjectsPositions() { + final Translation2d[] visibleObjects = camera.getObjectPositionsOnField(gamePieceType); + final HashMap trackedObjectsToUpdatedPositions = new HashMap<>(); + + for (Translation2d visibleObject : visibleObjects) + updateObjectPosition(visibleObject, trackedObjectsToUpdatedPositions); + + applyObjectUpdates(trackedObjectsToUpdatedPositions); + } + + private void applyObjectUpdates(HashMap currentToNewObjectPositions) { + final double currentTimestamp = Timer.getTimestamp(); + + objectPositionsToDetectionTimestamp.keySet().removeAll(currentToNewObjectPositions.keySet()); + currentToNewObjectPositions.values().forEach(object -> objectPositionsToDetectionTimestamp.put(object, currentTimestamp)); + } + + private void updateObjectPosition(Translation2d objectUpdate, HashMap trackedObjectsToUpdatedPositions) { + final Translation2d closestAvailableTrackedObjectToVisibleObject = getClosestAvailableObjectToUpdate(objectUpdate, trackedObjectsToUpdatedPositions); + if (closestAvailableTrackedObjectToVisibleObject == null) { + trackedObjectsToUpdatedPositions.put(objectUpdate, objectUpdate); + return; + } + final Translation2d previousUpdate = trackedObjectsToUpdatedPositions.get(closestAvailableTrackedObjectToVisibleObject); + trackedObjectsToUpdatedPositions.put(closestAvailableTrackedObjectToVisibleObject, objectUpdate); + if (previousUpdate != null) + updateObjectPosition(previousUpdate, trackedObjectsToUpdatedPositions); + } + + private Translation2d getClosestAvailableObjectToUpdate(Translation2d update, HashMap objectsWithUpdates) { + final Set availableObjectsToUpdate = getAvailableObjectsToUpdate(update, objectsWithUpdates); + if (availableObjectsToUpdate == null || availableObjectsToUpdate.isEmpty()) + return null; + return getClosestObjectFromSetToPosition(update, availableObjectsToUpdate); + } + + private Set getAvailableObjectsToUpdate(Translation2d update, HashMap objectsWithUpdates) { + if (objectPositionsToDetectionTimestamp.isEmpty()) + return null; + final Set availableObjects = new HashSet<>(); + for (Translation2d currentObject : objectPositionsToDetectionTimestamp.keySet()) { + final double updateDistanceFromCurrentObject = update.getDistance(currentObject); + if (updateDistanceFromCurrentObject > ObjectDetectionConstants.TRACKED_OBJECT_TOLERANCE_METERS) + continue; + if (!objectsWithUpdates.containsKey(currentObject)) { + availableObjects.add(currentObject); + continue; + } + if (isNewUpdateCloserThanPreviousUpdate(update, objectsWithUpdates.get(currentObject), currentObject)) + availableObjects.add(currentObject); + } + return availableObjects; + } + + private boolean isNewUpdateCloserThanPreviousUpdate(Translation2d newUpdate, Translation2d previousUpdate, Translation2d object) { + return newUpdate.getDistance(object) < previousUpdate.getDistance(object); + } + + private Translation2d getClosestTrackedObjectToPosition(Translation2d position) { + return getClosestObjectFromSetToPosition(position, objectPositionsToDetectionTimestamp.keySet()); + } + + private Translation2d getClosestObjectFromSetToPosition(Translation2d position, Set objects) { + if (objects.isEmpty()) + return null; + Translation2d closestObjectTranslation = null; + double closestObjectDistance = Double.MAX_VALUE; + + for (Translation2d object : objects) { + final double currentObjectDistance = position.getDistance(object); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; + closestObjectTranslation = object; + } + } + return closestObjectTranslation; + } + + private void removeOldObjects() { + objectPositionsToDetectionTimestamp.entrySet().removeIf(entry -> hasObjectExpired(entry.getValue())); + } + + private boolean hasObjectExpired(double timestamp) { + return Timer.getTimestamp() - timestamp > deletionThresholdSeconds; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java similarity index 65% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java index b09d2ec4..19b623a4 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCamera.java @@ -1,13 +1,13 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; -import frc.trigon.lib.hardware.RobotHardwareStats; /** * An object detection camera is a class that represents a camera that detects objects other than apriltags, most likely game pieces. @@ -31,51 +31,52 @@ public void periodic() { } /** - * Calculates the position of the best object on the field from the 3D rotation of the object relative to the camera. + * Calculates the position of the closest object on the field from its 3D rotation relative to the camera. * This assumes the object is on the ground. * Once it is known that the object is on the ground, * one can simply find the transform from the camera to the ground and apply it to the object's rotation. * - * @return the best object's 2D position on the field (z is assumed to be 0) + * @return the closest object's 2D position on the field (z is assumed to be 0) */ - public Translation2d calculateBestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece); final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); if (targetObjectsTranslation.length == 0) return null; - Translation2d bestObjectTranslation = targetObjectsTranslation[0]; + Translation2d closestObjectTranslation = targetObjectsTranslation[0]; + double closestObjectDistanceToRobot = currentRobotTranslation.getDistance(closestObjectTranslation); - for (int i = 1; i < targetObjectsTranslation.length; i++) { - final Translation2d currentObjectTranslation = targetObjectsTranslation[i]; - final double bestObjectDifference = currentRobotTranslation.getDistance(bestObjectTranslation); - final double currentObjectDifference = currentRobotTranslation.getDistance(currentObjectTranslation); - if (currentObjectDifference < bestObjectDifference) - bestObjectTranslation = currentObjectTranslation; + for (Translation2d currentObjectTranslation : targetObjectsTranslation) { + final double currentObjectDistanceToRobot = currentRobotTranslation.getDistance(currentObjectTranslation); + if (currentObjectDistanceToRobot < closestObjectDistanceToRobot) { + closestObjectTranslation = currentObjectTranslation; + closestObjectDistanceToRobot = currentObjectDistanceToRobot; + } } - return bestObjectTranslation; + return closestObjectTranslation; } - public boolean hasTargets(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - return objectDetectionCameraInputs.hasTarget[targetGamePiece.id]; + public boolean hasObject(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + return objectDetectionCameraInputs.hasObject[targetGamePiece.id]; } public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { - final Rotation3d[] visibleObjectsRotations = getTargetObjectsRotations(targetGamePiece); - final Translation2d[] objectsPositionsOnField = new Translation2d[visibleObjectsRotations.length]; + final Rotation3d[] visibleObjectRotations = getObjectsRotations(targetGamePiece); + final Translation2d[] objectPositionsOnField = new Translation2d[visibleObjectRotations.length]; - for (int i = 0; i < visibleObjectsRotations.length; i++) - objectsPositionsOnField[i] = calculateObjectPositionFromRotation(visibleObjectsRotations[i]); + for (int i = 0; i < visibleObjectRotations.length; i++) + objectPositionsOnField[i] = calculateObjectPositionFromRotation(visibleObjectRotations[i]); - Logger.recordOutput("ObjectDetectionCamera/Visible" + targetGamePiece.name(), objectsPositionsOnField); - return objectsPositionsOnField; + Logger.recordOutput("ObjectDetectionCamera/Visible" + targetGamePiece.name(), objectPositionsOnField); + return objectPositionsOnField; } - public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]; } /** - * Calculates the position of the object on the field from the 3D rotation of the object relative to the camera. + * Calculates the position of an object on the field from its 3D rotation relative to the camera. * This assumes the object is on the ground. * Once it is known that the object is on the ground, * one can simply find the transform from the camera to the ground and apply it to the object's rotation. diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java similarity index 58% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java index 4d0f336b..6cf09732 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/ObjectDetectionCameraIO.java @@ -1,6 +1,7 @@ -package frc.trigon.robot.misc.objectdetectioncamera; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera; import edu.wpi.first.math.geometry.Rotation3d; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; import org.littletonrobotics.junction.AutoLog; public class ObjectDetectionCameraIO { @@ -13,15 +14,15 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { @AutoLog public static class ObjectDetectionCameraInputs { /** - * Whether there is at least one target or not for each game piece, by game piece index (type). + * Whether the camera sees at least one object or none of each game piece, by game piece index (type). */ - public boolean[] hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + public boolean[] hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; /** * Stores the Rotation3d of all visible objects. * The first index is the game piece ID (type). - * The second index is the index of the game piece's Rotation3d, with the best object placed first (index 0). + * The second index is the index of the game piece's Rotation3d, with the closest object placed first (index 0). */ - public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + public Rotation3d[][] visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; public double latestResultTimestamp = 0; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java similarity index 76% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 541f10e8..1922cf41 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,10 +1,10 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.util.Units; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -43,26 +43,26 @@ private PhotonPipelineResult getLatestPipelineResult() { } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + inputs.hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, PhotonPipelineResult result) { - final List[] visibleObjectsRotations = new List[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + final List[] visibleObjectsRotations = new List[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) visibleObjectsRotations[i] = new ArrayList<>(); - Arrays.fill(inputs.hasTarget, false); + Arrays.fill(inputs.hasObject, false); inputs.latestResultTimestamp = result.getTimestampSeconds(); for (PhotonTrackedTarget currentTarget : result.getTargets()) { if (currentTarget.getDetectedObjectClassID() == -1) continue; - inputs.hasTarget[currentTarget.getDetectedObjectClassID()] = true; + inputs.hasObject[currentTarget.getDetectedObjectClassID()] = true; visibleObjectsRotations[currentTarget.getDetectedObjectClassID()].add(extractRotation3d(currentTarget)); } - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) inputs.visibleObjectRotations[i] = toArray(visibleObjectsRotations[i]); } @@ -82,4 +82,4 @@ private Rotation3d extractRotation3d(PhotonTrackedTarget target) { Units.degreesToRadians(-target.getYaw()) ); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java similarity index 82% rename from src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java rename to src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index a7bd8346..22f1b8f7 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectDetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -1,12 +1,12 @@ -package frc.trigon.robot.misc.objectdetectioncamera.io; +package frc.trigon.robot.misc.objectDetection.objectdetectioncamera.io; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; +import frc.trigon.robot.misc.objectDetection.ObjectDetectionConstants; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraIO; +import frc.trigon.robot.misc.objectDetection.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePiece; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; @@ -33,14 +33,14 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { final Pose3d cameraPose = new Pose3d(robotPose).plus(robotCenterToCamera); final ArrayList>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose); - boolean hasAnyTarget = false; - for (int i = 0; i < ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { - inputs.hasTarget[i] = !visibleGamePieces[i].isEmpty(); - if (inputs.hasTarget[i]) - hasAnyTarget = true; + boolean hasAnyObject = false; + for (int i = 0; i < ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES; i++) { + inputs.hasObject[i] = !visibleGamePieces[i].isEmpty(); + if (inputs.hasObject[i]) + hasAnyObject = true; } - if (hasAnyTarget) { + if (hasAnyObject) { updateHasNewResultInputs(inputs, visibleGamePieces); return; } @@ -49,15 +49,15 @@ protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) { } private ArrayList>[] calculateAllVisibleGamePieces(Pose3d cameraPose) { - final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; + final ArrayList>[] visibleGamePieces = new ArrayList[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; for (int i = 0; i < visibleGamePieces.length; i++) - visibleGamePieces[i] = calculateVisibleGamePieces(cameraPose, i); + visibleGamePieces[i] = calculateVisibleGamePiecesRotations(cameraPose, i); return visibleGamePieces; } private void updateNoNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs) { - inputs.hasTarget = new boolean[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES]; - inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionCameraConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; + inputs.hasObject = new boolean[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES]; + inputs.visibleObjectRotations = new Rotation3d[ObjectDetectionConstants.NUMBER_OF_GAME_PIECE_TYPES][0]; } private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inputs, ArrayList>[] visibleGamePieces) { @@ -79,19 +79,19 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu * @param objectID the ID of the object to check for visibility * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera */ - private ArrayList> calculateVisibleGamePieces(Pose3d cameraPose, int objectID) { + private ArrayList> calculateVisibleGamePiecesRotations(Pose3d cameraPose, int objectID) { final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); - final ArrayList> visibleTargetObjects = new ArrayList<>(); + final ArrayList> visibleObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { if (currentObject.isScored()) continue; final Rotation3d cameraAngleToObject = calculateCameraAngleToObject(currentObject.getPose(), cameraPose); - if (isWithinFOV(cameraAngleToObject)) - visibleTargetObjects.add(new Pair<>(currentObject, cameraAngleToObject)); + if (isObjectWithinFOV(cameraAngleToObject)) + visibleObjects.add(new Pair<>(currentObject, cameraAngleToObject)); } - return visibleTargetObjects; + return visibleObjects; } private Rotation3d calculateCameraAngleToObject(Pose3d objectPose, Pose3d cameraPose) { @@ -137,7 +137,7 @@ private Rotation2d getPitch(Translation3d vector) { * @param objectRotation the rotation of the object relative to the camera * @return if the object is within the field-of-view of the camera */ - private boolean isWithinFOV(Rotation3d objectRotation) { + private boolean isObjectWithinFOV(Rotation3d objectRotation) { return Math.abs(objectRotation.getZ()) <= CAMERA_HORIZONTAL_FOV.getRadians() / 2 && Math.abs(objectRotation.getY()) <= CAMERA_VERTICAL_FOV.getRadians() / 2; } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java deleted file mode 100644 index b153d755..00000000 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ /dev/null @@ -1,207 +0,0 @@ -package frc.trigon.robot.misc.objectdetectioncamera; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import org.littletonrobotics.junction.Logger; - -import java.util.ArrayList; -import java.util.HashMap; - -public class ObjectPoseEstimator extends SubsystemBase { - private final HashMap knownObjectPositions; - private final ObjectDetectionCamera camera; - private final double deletionThresholdSeconds; - private final SimulatedGamePieceConstants.GamePieceType gamePieceType; - private final double rotationToTranslation; - - /** - * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. - * - * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed - * @param gamePieceType the type of game piece to track - * @param camera the camera used for detecting objects - */ - public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, - SimulatedGamePieceConstants.GamePieceType gamePieceType, - ObjectDetectionCamera camera) { - this.deletionThresholdSeconds = deletionThresholdSeconds; - this.gamePieceType = gamePieceType; - this.camera = camera; - this.knownObjectPositions = new HashMap<>(); - this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; - } - - /** - * Updates the object positions based on the camera detected objects. - * Removes objects that have not been detected for {@link ObjectPoseEstimator#deletionThresholdSeconds}. - */ - @Override - public void periodic() { - updateObjectPositions(); - removeOldObjects(); - Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); - } - - /** - * @return if the pose estimator knows any object positions - */ - public boolean hasObject() { - return !knownObjectPositions.isEmpty(); - } - - /** - * Gets the position of all known objects on the field. - * - * @return a list of Translation2d representing the positions of objects on the field - */ - public ArrayList getObjectsOnField() { - return new ArrayList<>(knownObjectPositions.keySet()); - } - - /** - * Removes the closest object to the robot from the list of objects in the pose estimator. - */ - public void removeClosestObjectToRobot() { - final Translation2d closestObject = getClosestObjectToRobot(); - removeObject(closestObject); - } - - /** - * Removes the closest object to the intake from the list of objects in the pose estimator. - * - * @param intakeTransform the transform of the intake relative to the robot - */ - public void removeClosestObjectToIntake(Transform2d intakeTransform) { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - removeObject(getClosestObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); - } - - /** - * Removes the closest object to a given pose from the list of objects in the pose estimator. - * - * @param fieldRelativePose the pose to which the removed object is closest - */ - public void removeClosestObjectToPose(Pose2d fieldRelativePose) { - final Translation2d closestObject = getClosestObjectToPosition(fieldRelativePose.getTranslation()); - removeObject(closestObject); - } - - public void removeClosestObjectToPosition(Translation2d position) { - final Translation2d closestObject = getClosestObjectToPosition(position); - removeObject(closestObject); - } - - /** - * Removes a specific object from the list of known objects in the pose estimator. - * - * @param objectPosition the position of the object to be removed. Must be the precise position as stored in the pose estimator. - */ - public void removeObject(Translation2d objectPosition) { - knownObjectPositions.remove(objectPosition); - } - - /** - * Gets the position of the closest object on the field from the 3D rotation of the object relative to the camera. - * This assumes the object is on the ground. - * Once it is known that the object is on the ground, - * one can simply find the transform from the camera to the ground and apply it to the object's rotation. - * - * @return the best object's 2D position on the field (z is assumed to be 0) - */ - public Translation2d getClosestObjectToRobot() { - return getClosestObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); - } - - - /** - * Gets the position of the closest object to a given position on the field. - * - * @param position the position to which the returned object is closest - * @return the closest object's position on the field, or null if no objects are known - */ - public Translation2d getClosestObjectToPosition(Translation2d position) { - final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); - if (knownObjectPositions.isEmpty()) - return null; - Translation2d bestObjectTranslation = objectsTranslations[0]; - double closestObjectDistance = position.getDistance(bestObjectTranslation); - - for (int i = 1; i < objectsTranslations.length; i++) { - final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double currentObjectDistance = position.getDistance(currentObjectTranslation); - if (currentObjectDistance < closestObjectDistance) { - closestObjectDistance = currentObjectDistance; - bestObjectTranslation = currentObjectTranslation; - } - } - return bestObjectTranslation; - } - - /** - * Calculates the "distance rating" of an object. - * The "distance rating" is a unit used to calculate the distance between 2 poses. - * It factors in both translation and rotation differences by scaling the units depending on the {@link DistanceCalculationMethod}. - * - * @param objectTranslation the translation of the object on the field - * @param pose the pose to which the distance is measured from - * @return the objects "distance rating" - */ - private double calculateObjectDistanceRating(Translation2d objectTranslation, Pose2d pose) { - final double translationDifference = pose.getTranslation().getDistance(objectTranslation); - final double xDifference = Math.abs(pose.getX() - objectTranslation.getX()); - final double yDifference = Math.abs(pose.getY() - objectTranslation.getY()); - final double rotationDifferenceDegrees = Math.abs(pose.getRotation().getDegrees() - Math.atan2(yDifference, xDifference)); - return translationDifference * rotationToTranslation + rotationDifferenceDegrees * (1 - rotationToTranslation); - } - - private void updateObjectPositions() { - final double currentTimestamp = Timer.getTimestamp(); - for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = new Translation2d(); - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; - } - } - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - removeObject(closestObjectToVisibleObject); - knownObjectPositions.put(visibleObject, currentTimestamp); - } - } - - private void removeOldObjects() { - knownObjectPositions.entrySet().removeIf(entry -> isTooOld(entry.getValue())); - } - - private boolean isTooOld(double timestamp) { - return Timer.getTimestamp() - timestamp > deletionThresholdSeconds; - } - - public enum DistanceCalculationMethod { - ROTATION(0), - TRANSLATION(1), - ROTATION_AND_TRANSLATION(0.1); - - /** - * The ratio of rotation to translation in the distance rating calculation. - * A value of 0 means only rotation is considered, 1 means only translation is considered. - * Values in between are the ratio of rotation to translation in the distance rating calculation. - * For example, a value of 0.1 means that 9 cm of translation is considered equivalent to 1 degree of rotation. - */ - final double rotationToTranslation; - - DistanceCalculationMethod(double rotationToTranslation) { - this.rotationToTranslation = rotationToTranslation; - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 959bf750..c7689a5c 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -4,7 +4,7 @@ import edu.wpi.first.wpilibj.DriverStation; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; +import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java index 7c7a2812..56d35893 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java @@ -1,6 +1,6 @@ package frc.trigon.robot.poseestimation.relativerobotposesource; -import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; +import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations; public class RelativeRobotPoseSourceConstants { public static final StandardDeviations STANDARD_DEVIATIONS = new StandardDeviations(0.0000001, 0.0000001); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java similarity index 93% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index b549a358..eb8355cd 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.VecBuilder; @@ -29,7 +29,7 @@ /** * A class that estimates the robot's pose using WPILib's {@link SwerveDrivePoseEstimator} and {@link SwerveDriveOdometry}. */ -public class PoseEstimator implements AutoCloseable { +public class RobotPoseEstimator implements AutoCloseable { private final SwerveDrivePoseEstimator swerveDrivePoseEstimator = createSwerveDrivePoseEstimator(); private final SwerveDriveOdometry swerveDriveOdometry = createSwerveDriveOdometry(); private final Field2d field = new Field2d(); @@ -38,13 +38,13 @@ public class PoseEstimator implements AutoCloseable { private final boolean shouldUseRelativeRobotPoseSource; /** - * Constructs a new PoseEstimator and sets the relativeRobotPoseSource. + * Constructs a new RobotPoseEstimator and sets the relativeRobotPoseSource. * This constructor enables usage of a relative robot pose source and disables the use of april tags for pose estimation, and instead uses them to reset the relative robot pose source's offset. * * @param relativeRobotPoseSource the relative robot pose source that should be used to update the pose estimator * @param aprilTagCameras the apriltag cameras that should be used to update the relative robot pose source */ - public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { + public RobotPoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; this.relativeRobotPoseSource = relativeRobotPoseSource; this.shouldUseRelativeRobotPoseSource = true; @@ -53,12 +53,12 @@ public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCa } /** - * Constructs a new PoseEstimator. + * Constructs a new RobotPoseEstimator. * This constructor disables the use of a relative robot pose source and instead uses april tags cameras for pose estimation. * * @param aprilTagCameras the cameras that should be used to update the pose estimator */ - public PoseEstimator(AprilTagCamera... aprilTagCameras) { + public RobotPoseEstimator(AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; this.relativeRobotPoseSource = null; this.shouldUseRelativeRobotPoseSource = false; @@ -229,8 +229,8 @@ private boolean isUnderMaximumSpeedForOffsetResetting() { final ChassisSpeeds chassisSpeeds = RobotContainer.SWERVE.getSelfRelativeChassisSpeeds(); final double currentTranslationVelocityMetersPerSecond = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); final double currentThetaVelocityRadiansPerSecond = chassisSpeeds.omegaRadiansPerSecond; - return currentTranslationVelocityMetersPerSecond <= PoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && - currentThetaVelocityRadiansPerSecond <= PoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; + return currentTranslationVelocityMetersPerSecond <= RobotPoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && + currentThetaVelocityRadiansPerSecond <= RobotPoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; } private AprilTagCamera[] getCamerasWithResults() { @@ -280,7 +280,7 @@ private SwerveDrivePoseEstimator createSwerveDrivePoseEstimator() { new Rotation2d(), swerveModulePositions, new Pose2d(), - PoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), + RobotPoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.toMatrix(), VecBuilder.fill(0, 0, 0) ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java similarity index 80% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java index d341b294..74be9b54 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimatorConstants.java @@ -1,6 +1,6 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; -public class PoseEstimatorConstants { +public class RobotPoseEstimatorConstants { public static final double ODOMETRY_FREQUENCY_HERTZ = 250; static final StandardDeviations ODOMETRY_STANDARD_DEVIATIONS = new StandardDeviations(0.003, 0.0002); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java similarity index 95% rename from src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java rename to src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java index bbe11c6c..689e45c0 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/StandardDeviations.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.poseestimation.poseestimator; +package frc.trigon.robot.poseestimation.robotposeestimator; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index f6dae857..9b6a18ee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -9,18 +9,18 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; +import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; import frc.trigon.lib.hardware.phoenix6.Phoenix6SignalThread; import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Signal; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.lib.utilities.flippable.FlippableRotation2d; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; -import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; -import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -32,7 +32,7 @@ public class Swerve extends MotorSubsystem { public Swerve() { setName("Swerve"); - phoenix6SignalThread.setThreadFrequencyHertz(PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + phoenix6SignalThread.setThreadFrequencyHertz(RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index d04cce6d..39093736 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -13,7 +13,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; public class SwerveConstants { @@ -109,7 +109,7 @@ private static void configureGyro() { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond());//IMPORTANT: Leave as lambda expression, method reference will crash code - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } private static void configurePIDControllers() { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java index e6d83a7d..5cbee566 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java @@ -9,7 +9,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import frc.trigon.robot.constants.RobotConstants; -import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; +import frc.trigon.robot.poseestimation.robotposeestimator.RobotPoseEstimatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder; import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -201,7 +201,7 @@ private void configureDriveMotor() { driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } private void configureSteerMotor() { @@ -211,7 +211,7 @@ private void configureSteerMotor() { steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, RobotPoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } private void configureSteerEncoder(double offsetRotations) {