diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 737fb59..8ac8852 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,7 @@ package frc.robot; + import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoSource; @@ -138,11 +139,11 @@ public void robotPeriodic() { // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); + if (Constants.currentMode == Constants.Mode.SIM) { + Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); + } } - - // Puts data on the elastic dashboard - SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); - SmartDashboard.putBoolean("Hub Active?", hubActive()); } SmartDashboard.putString("Selected Action", robotContainer.getAutoChooser().getCommandDescription()); diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index bcc969b..d66581b 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -49,7 +49,7 @@ public void simReadings() { distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); if (BasicVisionFilter.inBounds(readingPos)) { addReading(new ApriltagReading(readingX, readingY, readingYaw, - distance, 0, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, 0, Logger.getTimestamp() / 1000.0)); + distance,0,tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); } } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d93ac3e..9f1dde9 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 92b362f..6053afc 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -13,16 +13,21 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.constants.Constants; +import frc.robot.constants.GameConstants; import frc.robot.utils.logging.io.gyro.ThreadedGyro; +import org.littletonrobotics.junction.Logger; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -38,6 +43,8 @@ import java.io.File; import java.util.Arrays; import java.util.Optional; +import java.util.concurrent.ConcurrentLinkedDeque; +import java.util.Optional; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -49,6 +56,10 @@ public class SwerveSubsystem extends SubsystemBase { */ private final SwerveDrive swerveDrive; private Vector variance = VecBuilder.fill(0.1,0.1,0.1); + private SwerveDriveOdometry rawOdometry; + private final ConcurrentLinkedDeque poseError = new ConcurrentLinkedDeque<>(); + private record PoseErrorRecord(double timestamp, double error) { + } /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, @@ -83,7 +94,14 @@ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { swerveDrive.setModuleEncoderAutoSynchronize(Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE, Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - + if (Constants.currentMode==Constants.simMode) { + rawOdometry = new SwerveDriveOdometry( + swerveDrive.kinematics, + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions(), + startingPose); + } + } /** @@ -105,6 +123,17 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + if (Constants.currentMode == GameConstants.Mode.SIM) { + double currentTime = Logger.getTimestamp() / 1000000.0; + double oneSecondAgo = currentTime - 1.0; + poseError.removeIf(record -> record.timestamp < oneSecondAgo); + poseError.add(new PoseErrorRecord(currentTime, getError())); + Logger.recordOutput("AveragePoseError", getAverageError()); + rawOdometry.update( + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions() + ); + } } @Override @@ -282,8 +311,16 @@ public SwerveDriveKinematics getKinematics() { * * @param initialHolonomicPose The pose to set the odometry to */ + // might be broken public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); + if (Constants.currentMode==Constants.simMode) { + SwerveModulePosition[] modules = new SwerveModulePosition[4]; + for (int i=0; i<4; i++) { + modules[i] = new SwerveModulePosition(); + } + rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); + } } /** @@ -294,13 +331,23 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public double getError() { + return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation())); + } + public double getAverageError(){ + return poseError.stream().mapToDouble(record -> record.error).average().orElse(0); + } public Optional getSimulationPose() { return swerveDrive.getSimulationDriveTrainPose(); } // Todo: fix to only get odomtry public Pose2d getOdom() { - return swerveDrive.getPose(); + if (Constants.currentMode==Constants.simMode) { + return rawOdometry.getPoseMeters(); + } else { + return getPose(); + } } /** diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 0d04e40..4452c1d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -2,9 +2,11 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.GameConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; import frc.robot.subsystems.swervedrive.vision.truster.PoseDeviation; @@ -12,10 +14,10 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; -import java.util.ArrayList; -import java.util.LinkedHashMap; -import java.util.List; -import java.util.Map; +import java.util.*; +import java.util.concurrent.ConcurrentLinkedDeque; + +import frc.robot.utils.Apriltag; import org.littletonrobotics.junction.Logger; /** @@ -53,29 +55,32 @@ public FilterablePoseManager( @Override public void processQueue() { + List validMeasurementsPose = new ArrayList<>(); + List invalidMeasurementsPose = new ArrayList<>(); + LinkedHashMap filteredData = - filter.filter(visionMeasurementQueue); + filter.filter(visionMeasurementQueue); visionMeasurementQueue.clear(); - List validMeasurements = new ArrayList<>(); - List invalidMeasurements = new ArrayList<>(); - for (Map.Entry entry : filteredData.entrySet()) { - VisionMeasurement v = entry.getKey(); - FilterResult r = entry.getValue(); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + FilterResult r = filterEntry.getValue(); switch (r) { case ACCEPTED -> { setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurements.add(v.measurement()); + validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); + } case NOT_PROCESSED -> visionMeasurementQueue.add(v); case REJECTED -> { - invalidMeasurements.add(v.measurement()); + invalidMeasurementsPose.add(v.measurement()); } } } - Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new)); - Logger.recordOutput( - "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new)); +// Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); +// Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/numberAccepted", validMeasurementsPose.size()); + Logger.recordOutput("Apriltag/numberRejected", invalidMeasurementsPose.size()); } public VisionTruster getVisionTruster() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index edd9af3..c4913a8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.Apriltag; /** * @param measurement estimated robot position (meters) calculated from apriltag tag what tag produced the @@ -8,5 +9,4 @@ * @param distanceFromTag distance (meters) estimated robot pose was from the tag * @param timeOfMeasurement time when the pose was measured (seconds) */ -public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} +public record VisionMeasurement(Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 8b13445..badbe99 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -82,7 +82,6 @@ public enum Apriltag { THIRTY_ONE(0.32, 147.47, 21.75),//Tower, Blue z rotation:0 THIRTY_TWO(0.32, 164.47, 21.75);//Tower, Blue z rotation:0 */ - public record TagPose(Apriltag tag, Pose3d pose) {} private final double xMeters; private final double yMeters; private final double zMeters; @@ -133,7 +132,4 @@ public Pose3d getPose() { public int number() { return ordinal()+1; } - public TagPose getTagInfo() { - return new TagPose(this, pose); - } } \ No newline at end of file