Vision Logic

simgui-ds.json

@@ -88,5 +88,13 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    },
+    {
+      "guid": "Keyboard1"
+    }
   ]
 }

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

@@ -7,6 +7,7 @@
 import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
 
+import frc.robot.constants.GameConstants;
 import frc.robot.utils.diag.Diagnostics;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -122,6 +123,10 @@ public void robotPeriodic() {
       SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX());
       if(!Constants.TESTBED){
           Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose());
+          Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom());
+          if (Constants.currentMode == GameConstants.Mode.SIM) {
+              Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get());
+          }
         // Puts data on the elastic dashboard
       SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
       SmartDashboard.putBoolean("Hub Active?", hubActive());

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

@@ -63,6 +63,9 @@
 //import frc.robot.subsystems.RollerSubsystem;
 //import frc.robot.subsystems.TiltSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster;
+import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster;
+import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
 import frc.robot.utils.logging.io.gyro.RealGyroIo;
 import frc.robot.utils.logging.io.gyro.ThreadedGyro;
 import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU;
@@ -81,6 +84,8 @@
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 
+import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2;
+
 /**
  * This class is where the bulk of the robot should be declared. Since
  * Command-based is a
@@ -118,6 +123,7 @@ public class RobotContainer {
         private AutoFactory autoFactory;
         private static AutoRoutine straightRoutine;
         private static AutoTrajectory straightTrajectory;
+        private final VisionTruster truster = new SquareVisionTruster(visionMeasurementStdDevs2, Constants.VISION_STD_DEV_CONST);
 
     // Replace with CommandPS4Controller or CommandJoystick if needed
         // new CommandXboxController(OperatorConstants.kDriverControllerPort);private
@@ -151,7 +157,7 @@ public RobotContainer() {
 
                                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
-                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null;
+                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
                             controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
 
             }
@@ -169,7 +175,7 @@ public RobotContainer() {
                 // No GyroSubsystem in REPLAY for now
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
-                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null;
+                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
 
             }
@@ -190,7 +196,7 @@ public RobotContainer() {
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
                 controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null;
-                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null;
+                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
             }
 
                         default -> {
@@ -510,7 +516,7 @@ public void putShuffleboardCommands() {
     public Command getAutonomousCommand() {
         return autoChooser.getCommand();
     // return straightRoutine.cmd(straightTrajectory.done());
-                
+
             //    return new ExampleAuto(drivebase, autoFactory);
         }
 
@@ -534,7 +540,7 @@ public SwerveSubsystem getDriveBase() {
         return drivebase;
     }
 
-        public ShootingState getShootingState() {
+    public ShootingState getShootingState() {
                 return shootState;
         }
 }

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

@@ -1,14 +1,64 @@
 package frc.robot.apriltags;
 
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.numbers.N3;
 import frc.robot.constants.Constants;
+import frc.robot.subsystems.ApriltagSubsystem;
+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.subsystems.swervedrive.vision.truster.VisionTruster;
 import frc.robot.utils.Apriltag;
 import frc.robot.utils.logging.io.BaseIoImpl;
+
+import java.util.Optional;
 import java.util.Queue;
+import java.util.Random;
+import java.util.function.Supplier;
+
+import frc.robot.utils.math.ObjectUtils;
 import org.littletonrobotics.junction.Logger;
 
 public class SimApriltagIO extends TCPApriltagIo {
-    public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server) {
+    private final Random random = new Random();
+    private final VisionTruster truster;
+    private final SwerveSubsystem swerveSubsystem;
+    public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, SwerveSubsystem swerveSubsystem) {
         super(name, inputs, server);
+        this.truster = truster;
+        this.swerveSubsystem = swerveSubsystem;
+    }
+    public void simReadings() {
+        if (swerveSubsystem.getSimulationPose().isPresent()) {
+            Pose2d pose = swerveSubsystem.getSimulationPose().get();
+            for (Apriltag tag : Apriltag.values()) {
+                Pose3d cameraPos = new Pose3d(pose).transformBy(Constants.ROBOT_TO_CAMERA);
+                if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) {
+                    Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation();
+                    double distance = adjPose2.getNorm();
+                    double distanceTimesCosIncidenceAngle = adjPose2.getX();
+                    if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
+                        VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0);
+                        Vector<N3> stdDevs = truster.calculateTrust(measurement, cameraPos);
+                        double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0);
+                        double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1);
+                        double readingYaw = pose.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)) {
+                            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));
+                        }
+                    }
+                }
+            }
+        }
+    }
+    @Override
+    public void periodic() {
+        super.periodic();
+        simReadings();
     }
 }

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,6 +1,11 @@
 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 swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -116,7 +121,7 @@ public enum Mode {
     public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState
     public static final double ANGLER_LIMIT_SPEED = 0.2;
 
-    
+
  // turret (pan angle) PID
     public static final double TURRET_P = .5;
     public static final double TURRET_I = 0.000001;
@@ -158,6 +163,20 @@ public enum Mode {
     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 double VISION_STD_THRESHOLD = 0.25;
     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 FIELD_LENGTH = 16.5; //TODO: Change Later
+    public static final double FIELD_WIDTH = 8.1; //TODO: Change Later
+    // Vision
+    public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0));
+    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 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 MAX_VISION_DISTANCE_SIMULATION = 3.5;
+    public static final double AVERAGE_PIR_LATENCY = 20; //ms
+    public static final double VISION_SMOOTHER = 100.0;
+    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

@@ -1,12 +1,26 @@
 package frc.robot.subsystems;
 
 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.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.subsystems.swervedrive.vision.truster.VisionTruster;
+import frc.robot.utils.Apriltag;
 import frc.robot.utils.logging.io.BaseIoImpl;
+import frc.robot.utils.math.ObjectUtils;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.Random;
+import java.util.function.Supplier;
 
 public class ApriltagSubsystem extends SubsystemBase {
 
@@ -15,10 +29,11 @@ public class ApriltagSubsystem extends SubsystemBase {
     private final PoseEstimator estimator;
     private final SwerveSubsystem drivebase;
 
-    public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) {
+
+    public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase, VisionTruster truster) {
         this.drivebase = drivebase;
         this.io = io;
-       estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this);
+        estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this, truster);
     }
 
     public static ApriltagIO createRealIo() {
@@ -30,8 +45,8 @@ public static ApriltagIO createMockIo() {
         return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs());
     }
 
-    public static ApriltagIO createSimIo() {
-        return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all
+    public static ApriltagIO createSimIo(VisionTruster truster, SwerveSubsystem drivebase) {
+        return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase); // port doesnt matter at all
     }
     // This is used to inject april tag readings manually and will pretty much only be used for simulation.
     public void addSimReading(ApriltagReading reading) {
@@ -40,7 +55,6 @@ public void addSimReading(ApriltagReading reading) {
     public ApriltagIO getIO(){
         return io;
     }
-
     @Override
     public void periodic() {
         estimator.updateVision();