Ls simulation yay

src/main/java/frc/robot/Robot.java

@@ -122,6 +122,8 @@ public void robotPeriodic() {
       SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX());
       if(!Constants.TESTBED){
           Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose());
+          Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom());
+          Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose());
         // Puts data on the elastic dashboard
       SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
       SmartDashboard.putBoolean("Hub Active?", hubActive());

src/main/java/frc/robot/RobotContainer.java

@@ -12,11 +12,11 @@
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.commands.AddTunableApriltagReading;
-import frc.robot.commands.AddApriltagReading;
+import frc.robot.commands.*;
 import frc.robot.commands.AddGarbageReading;
 import frc.robot.commands.climber.ClimberDown;
 import frc.robot.commands.climber.ClimberUp;
+import frc.robot.commands.drive.DriveCircle;
 import frc.robot.commands.hopper.SpinHopper;
 import frc.robot.commands.drive.DriveDirectionTime;
 import frc.robot.commands.feeder.SpinFeeder;
@@ -61,10 +61,7 @@
 import java.io.File;
 
 import choreo.auto.AutoFactory;
-import choreo.auto.AutoRoutine;
-import choreo.auto.AutoTrajectory;
 
-import choreo.auto.AutoFactory;
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 
@@ -312,11 +309,13 @@ public void putShuffleboardCommands() {
 
 
 
+                        SmartDashboard.putData("DriveInCircle", new DriveCircle(drivebase, 1,10,15000));
             // TODO: These commands do not REQUIRE the subsystem therefore cannot be used in// production
                         SmartDashboard.putData(
                                         "Intake/Spin Forward",
                                         new InstantCommand(() -> intakeSubsystem.setSpeed(1.0)));
 
+
                         SmartDashboard.putData(
                                         "Intake/Spin Backward",
                                         new InstantCommand(() -> intakeSubsystem.setSpeed(-1.0)));

src/main/java/frc/robot/apriltags/TCPApriltagServer.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import java.io.DataInputStream;
 import java.io.IOException;
+import frc.robot.constants.Constants;
 
 public class TCPApriltagServer extends TCPServer<ApriltagReading> {
 
@@ -20,24 +21,24 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc
         double posY = -1;
         double poseYaw = -1;
         double distanceToTag = -1;
-        double timestamp = -1;
+        double latency = -1;
         int apriltagNumber = -1;
         double now = 0;
         while (posX == -1
                 && posY == -1
                 && poseYaw == -1
                 && distanceToTag == -1
                 && apriltagNumber == -1
-                && timestamp == -1) {
+                && latency == -1) {
             posX = stream.readDouble();
             posY = stream.readDouble();
             poseYaw = stream.readDouble();
             distanceToTag = stream.readDouble();
-            timestamp = stream.readDouble();
+            latency = stream.readDouble();
             apriltagNumber = stream.readInt();
-            now = Timer.getFPGATimestamp() * 1000;
+            now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency;
         }
-        return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now);
+        return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, latency, now);
     }
 
 }

src/main/java/frc/robot/commands/drive/DriveCircle.java

@@ -0,0 +1,44 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.utils.logging.commands.LoggableCommand;
+
+public class DriveCircle extends LoggableCommand {
+    private final SwerveSubsystem drivebase;
+    private final double radiusMeters;
+    private final double angularVelocityRadPerSec;
+    private final double time;
+    private final Timer timer;
+    public DriveCircle(SwerveSubsystem drivebase, double radiusMeters, double angularVelocityRadPerSec, double time) {
+        this.drivebase = drivebase;
+        this.radiusMeters = radiusMeters;
+        this.angularVelocityRadPerSec = angularVelocityRadPerSec;
+        this.time = time;
+        this.timer = new Timer();
+        addRequirements(drivebase);
+    }
+
+    @Override
+    public void initialize() {
+        timer.restart();
+    }
+
+    @Override
+    public void execute() {
+        double tangentialSpeed = radiusMeters * angularVelocityRadPerSec;
+        double yVelocity = -tangentialSpeed;
+        drivebase.drive(new Translation2d(0, yVelocity), angularVelocityRadPerSec, false);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return timer.hasElapsed(time);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        drivebase.drive(new Translation2d(), 0, false);
+    }
+}

src/main/java/frc/robot/constants/GameConstants.java

@@ -1,10 +1,19 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.utils.logging.LoggedTunableNumber;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
+import static edu.wpi.first.units.Units.Radians;
+
 public class GameConstants {
 
     public enum Mode {
@@ -145,7 +154,21 @@ public enum Mode {
     public static final int SHIFT_4_START = 55;
     public static final int ENDGAME_START = 30;
 
-    public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed)
+    public static final LoggedTunableNumber VISION_SMOOTHER = new LoggedTunableNumber("VISION_SMOOTHER", 1.0);
+    public static final LoggedTunableNumber VISION_CONSISTENCY_THRESHOLD = new LoggedTunableNumber("VISION_CONSISTENCY_THRESHOLD", 100); //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed)
+    public static final LoggedTunableNumber VISION_STD_THRESHOLD = new LoggedTunableNumber("VISION_STD_THRESHOLD", 0.25);
+    public static final LoggedTunableNumber MAX_VISION_DISTANCE = new LoggedTunableNumber("MAX_VISION_DISTANCE", 6);
     public static final boolean ENABLE_VISION = true;
     public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back)
+
+    public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later
+    public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later
+    public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change Later
+    public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later
+    public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later
+    public static final double AVERAGE_PIR_LATENCY = 20; //ms
+    public static final Vector<N3> STATE_STD_DEVS = VecBuilder.fill(0.1,0.1,0.1); // TODO: Change Later
+    public static final Vector<N3> INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later
+    public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later
+
 }

src/main/java/frc/robot/subsystems/ApriltagSubsystem.java

@@ -2,27 +2,47 @@
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.VecBuilder;
+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.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.apriltags.*;
+import frc.robot.constants.Constants;
+import frc.robot.constants.GameConstants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator;
+import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
+import frc.robot.utils.Apriltag;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator;
 import frc.robot.utils.logging.io.BaseIoImpl;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.function.Supplier;
+import java.util.Random;
 
 public class ApriltagSubsystem extends SubsystemBase {
 
     public static final String LOGGING_NAME = "ApriltagSubsystem";
     private final ApriltagIO io;
     private final PoseEstimator estimator;
     private final SwerveSubsystem drivebase;
+    private final Supplier<Pose2d> robotPoseSupplier;
+    private final Random random = new Random();
 
     public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) {
         this.drivebase = drivebase;
         this.io = io;
        estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this);
+       robotPoseSupplier = drivebase::getSimulationPose;
     }
 
     public static ApriltagIO createRealIo() {
-        
+
         return new TCPApriltagIo(LOGGING_NAME, new ApriltagInputs());
     }
 
@@ -40,11 +60,40 @@ public void addSimReading(ApriltagReading reading) {
     public ApriltagIO getIO(){
         return io;
     }
+    public Pose2d getSimPose() {
+        return drivebase.getSimulationPose();
+    }
 
     @Override
     public void periodic() {
-        estimator.updateVision();
         estimator.updatePosition(drivebase.getOdom());
+        estimator.updateVision();
         io.periodic();
+        if (Constants.currentMode == Constants.Mode.SIM) {
+            simReadings();
+        }
+    }
+    public void simReadings() {
+        for (Apriltag tag: Apriltag.values()) {
+            Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA);
+            if (tag.canSee(cameraPos,Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) {
+                Pose3d adjPose = tag.getPose().relativeTo(cameraPos);
+                double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm());
+                double distance = tag.getTranslation().getDistance(cameraPos.getTranslation());
+                if (distance/cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) {
+                    VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance,0);
+                    Vector<N3> stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos);
+                    double readingX = robotPoseSupplier.get().getX()+ random.nextGaussian()*stdDevs.get(0);
+                    double readingY = robotPoseSupplier.get().getY()+ random.nextGaussian()*stdDevs.get(1);
+                    double readingYaw = robotPoseSupplier.get().getRotation().getDegrees()+ random.nextGaussian()*stdDevs.get(2);
+                    Pose2d readingPos = new Pose2d(readingX,readingY,Rotation2d.fromDegrees(readingYaw));
+                    distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());
+                    if(BasicVisionFilter.inBounds(readingPos)) {
+                        io.addReading(new ApriltagReading(readingX, readingY, readingYaw,
+                                distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0));
+                    }
+                }
+            }
+        }
     }
 }

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -9,20 +9,26 @@
 import static edu.wpi.first.units.Units.Meter;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 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.wpilibj.smartdashboard.SmartDashboard;
 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.utils.logging.io.gyro.ThreadedGyro;
+import org.littletonrobotics.junction.Logger;
 import swervelib.SwerveController;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
@@ -37,17 +43,21 @@
 
 import java.io.File;
 import java.util.Arrays;
+import java.util.concurrent.ConcurrentLinkedDeque;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import static edu.wpi.first.units.Units.Meter;
-
 public class SwerveSubsystem extends SubsystemBase {
     /**
      * Swerve drive object.
      */
     private final SwerveDrive swerveDrive;
-    private Vector<N3> variance = VecBuilder.fill(0.1,0.1,0.1);
+    private Vector<N3> variance = Constants.INITIAL_VISION_STD_DEVS;
+    private final Field2d rawOdomField = new Field2d();
+    private SwerveDriveOdometry rawOdometry;
+    private final ConcurrentLinkedDeque<PoseErrorRecord> 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,
@@ -86,7 +96,12 @@ 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
-
+        rawOdometry = new SwerveDriveOdometry(
+                swerveDrive.kinematics,
+                swerveDrive.getOdometryHeading(),
+                swerveDrive.getModulePositions(),
+                startingPose
+        );
     }
 
     /**
@@ -108,6 +123,17 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig
     public void periodic() {
         //add vision pose here
         //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d()));
+        rawOdometry.update(
+                swerveDrive.getOdometryHeading(),
+                swerveDrive.getModulePositions()
+        );
+
+        rawOdomField.setRobotPose(rawOdometry.getPoseMeters());
+        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());
     }
 
     @Override
@@ -285,8 +311,14 @@ public SwerveDriveKinematics getKinematics() {
      *
      * @param initialHolonomicPose The pose to set the odometry to
      */
+    // Might be broken
     public void resetOdometry(Pose2d initialHolonomicPose) {
         swerveDrive.resetOdometry(initialHolonomicPose);
+        SwerveModulePosition[] modules = new SwerveModulePosition[4];
+        for (int i=0; i<4; i++) {
+            modules[i] = new SwerveModulePosition();
+        }
+        rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose);
     }
 
     /**
@@ -297,9 +329,22 @@ public void resetOdometry(Pose2d initialHolonomicPose) {
     public Pose2d getPose() {
         return swerveDrive.getPose();
     }
+    public double getError() {
+        return getPose().getTranslation().getDistance((getSimulationPose().getTranslation()));
+    }
+    public double getAverageError(){
+        return poseError.stream().mapToDouble(record -> record.error).average().orElse(0);
+    }
+    public Pose3d getCameraPose() {
+        return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA);
+    }
+
+    public Pose2d getSimulationPose() {
+        return swerveDrive.getSimulationDriveTrainPose().orElse(new Pose2d());
+    }
     // Todo: fix to only get odomtry
     public Pose2d getOdom() {
-        return swerveDrive.getPose();
+        return rawOdometry.getPoseMeters();
     }
 
     /**
@@ -470,7 +515,7 @@ public SwerveDrive getSwerveDrive() {
         return swerveDrive;
     }
     public void setVariance(Vector<N3> variance){
-        this.variance = variance;
+        this.variance = variance.times(Constants.VISION_SMOOTHER.get());
     }
     public void addVisionMeasurement(Pose2d pose, double visionTimestamp){
         swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance);