diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index d130219..c66b0d9 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -106,13 +106,6 @@ public SwerveDrive( gyroDisconnectedAlert.set(false); } - /** Updates the pose estimator with the pose calculated from the swerve modules. */ - // public void addPoseEstimatorSwerveMeasurement() { - // for (int timestampIndex = 0; - // timestampIndex < odometryThreadInputs.measurementTimeStamps.length; - // timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex); - // } - /* * Updates the pose estimator with the pose calculated from the april tags. How much it * contributes to the pose estimation is set by setPoseEstimatorVisionConfidence. @@ -140,6 +133,7 @@ public void setPoseEstimatorVisionConfidence( VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation)); } + @Override public void periodic() { final double t0 = TimeUtil.getRealTimeSeconds(); fetchOdometryInputs(); @@ -183,6 +177,7 @@ private void fetchOdometryInputs() { odometryThread.unlockOdometry(); } + /** Runs the SwerveModules periodic methods */ private void modulesPeriodic() { for (SwerveModule module : swerveModules) module.periodic(); } @@ -289,23 +284,6 @@ public void setModuleStates(SwerveModuleState[] desiredStates) { * * @param timestampIndex index of the timestamp to sample the pose at */ - // private void addPoseEstimatorSwerveMeasurement(int timestampIndex) { - // final SwerveModulePosition[] modulePositions = getModulePositions(), - // moduleDeltas = getModulesDelta(modulePositions); - - // if (gyroInputs.isConnected) { - // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex]; - // } else { - // Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas); - // rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - // } - - // poseEstimator.updateWithTime( - // odometryThreadInputs.measurementTimeStamps[timestampIndex], - // rawGyroRotation, - // modulePositions); - // } - public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex final SwerveModulePosition[] modulePositions = getModulePositions(), moduleDeltas = getModulesDelta(modulePositions); @@ -324,21 +302,9 @@ public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex } /** - * Gets the modules positions, sampled at the indexed timestamp. - * - * @param timestampIndex the timestamp index to sample. - * @return a list of SwerveModulePosition, containing relative drive position and absolute turn - * rotation at the sampled timestamp. + * @param freshModulesPosition Latest module positions + * @return The change of the module positions between the current and last update */ - // private SwerveModulePosition[] getModulesPosition(int timestampIndex) { - // SwerveModulePosition[] swerveModulePositions = new - // SwerveModulePosition[swerveModules.length]; - // for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) - // swerveModulePositions[moduleIndex] = - // swerveModules[moduleIndex].getOdometryPositions()[timestampIndex]; - // return swerveModulePositions; - // } - private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) { SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length]; for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 033af94..342ea77 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -32,6 +32,7 @@ public SwerveModule(ModuleInterface io, String name) { CommandScheduler.getInstance().unregisterSubsystem(this); } + /** Updates the module's odometry inputs. */ public void updateOdometryInputs() { io.updateInputs(inputs); Logger.processInputs("Drive/Module-" + name, inputs); @@ -39,20 +40,20 @@ public void updateOdometryInputs() { } @Override - public void periodic() { - // updateOdometryPositions(); - Logger.recordOutput("Drive/current turn angle", getTurnRotation().getRotations()); - } + public void periodic() {} + /** Sets the drive voltage of the module. */ public void setVoltage(Voltage volts) { io.setDriveVoltage(volts); io.setTurnVoltage(Volts.zero()); } + /** Gets the drive voltage of the module. */ public double getDriveVoltage() { return inputs.driveAppliedVolts; } + /** Sets the drive velocity of the module. */ public double getCharacterizationVelocity() { return inputs.driveVelocity; } diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index 311f322..2c07913 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -297,6 +297,12 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { this.headingRateDegreesPerSecond = headingRateDegrees; } + /** + * Checks if the specified limelight is connected + * + * @param limelight The limelight to check + * @return True if the limelight network table contains the key "tv" + */ public boolean isLimelightConnected(Limelight limelight) { NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName()); return limelightTable.containsKey("tv"); @@ -403,7 +409,6 @@ public void stopLimelightThread(Limelight limelight) { } /** Shuts down all the threads. */ - // @Override public void endAllThreads() { threadManager.shutdownAllThreads(); } diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java index a4e4677..3a82e3b 100644 --- a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java +++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java @@ -106,6 +106,13 @@ public void updateInputs(VisionInputs inputs) { } } + /** + * Writes photonvision results to the limelight network table + * + * @param results The simulated photonvision pose estimation results + * @param table The Limelight table to write to + * @param limelight The Limelight to write for + */ private void writeToTable( List results, NetworkTable table, Limelight limelight) { // write to ll table @@ -159,7 +166,13 @@ private void writeToTable( } } - public PhotonCamera getSimulationCamera(Limelight limelight) { + /** + * Gets the PhotonCamera for the given Limelight + * + * @param limelight The Limelight to get the camera for + * @return A PhotonCamera object for the given Limelight + */ + private PhotonCamera getSimulationCamera(Limelight limelight) { return switch (limelight) { case SHOOTER -> VisionConstants.SHOOTER_CAMERA; case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA; @@ -168,7 +181,13 @@ public PhotonCamera getSimulationCamera(Limelight limelight) { }; } - public NetworkTable getLimelightTable(Limelight limelight) { + /** + * Gets the Limelight network table + * + * @param limelight The Limelight to get the table for + * @return The network table of the Limelight + */ + private NetworkTable getLimelightTable(Limelight limelight) { return switch (limelight) { case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName()); case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName()); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java index baba642..08c77cb 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -23,34 +23,72 @@ class VisionInputs { public double[] limelightTimestamps = new double[Limelight.values().length]; } + /** + * Updates the inputs for the vision subsystem using VisionInputs + * + * @param inputs the inputs to update + */ default void updateInputs(VisionInputs inputs) {} + /** + * @param limelight The Limelight to retrieve the latency data from + * @return The current latency of the Limelight + */ default double getLatencySeconds(Limelight limelight) { return 0.0; } + /** + * @param limelight The Limelight to retrieve the timestamp data from + * @return The current timestamp of the Limelight + */ default double getTimeStampSeconds(Limelight limelight) { return 0.0; } + /** + * @param limelight The Limelight to retrieve the target data from + * @return Whether the Limelight can see any targets, and if it is within its field of view + */ default boolean canSeeAprilTags(Limelight limelight) { return false; } + /** + * @param limelight The Limelight to retrieve the distance data from + * @return The current April Tag distance from the Limelight + */ default double getLimelightAprilTagDistance(Limelight limelight) { return 0.0; } + /** + * @param limelight The Limelight to retrieve the data from + * @return The current number of April Tags of the Limelight + */ default int getNumberOfAprilTags(Limelight limelight) { return 0; } + /** + * @param limelight The Limelight to retrieve the pose data from + * @return The current pose of the Limelight + */ default Pose2d getPoseFromAprilTags(Limelight limelight) { return null; } + /** + * Sets the heading information of the robot, used with MT2 + * + * @param headingDegrees The heading of the robot in degrees + * @param headingRateDegrees The rate of change of the heading of the robot in degrees + */ default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {} + /** + * @return The last seen pose of any Limelight that most recently saw a target + */ default Pose2d getLastSeenPose() { return null; }