Virtual limelight

src/main/java/frc/robot/Constants.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import java.util.Random;
+
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -112,13 +114,54 @@ public static final class VisionConstants {
     // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-robot-localization-megatag2
     public static final int kIMUMode = 0;
 
-    // TODO: Experiment with different std devs in the pose estimator
     public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final class VirtualLimelightConstants {
+      public static final double[] kHWMetricsVirtual = new double[] {-1, -1, -1, -1};
+      public static final double[] kHWMetricsFree = new double[] {-2, -2, -2, -2};
+
+      public static final double kLatency = 0.01;
+
+      public static final int kbotpose_orb_wpiblue_header_size = 11;
+      public static final int kValsPerFiducial = 7;
+
+      public static final int kHeaderOffset_posX =        0;
+      public static final int kHeaderOffset_posY =        1;
+      public static final int kHeaderOffset_posZ =        2;
+      public static final int kHeaderOffset_rotP =        3;
+      public static final int kHeaderOffset_rotR =        4;
+      public static final int kHeaderOffset_rotY =        5;
+      public static final int kHeaderOffset_latency =     6;
+      public static final int kHeaderOffset_tagcount =    7;
+      public static final int kHeaderOffset_tagspan =     8;
+      public static final int kHeaderOffset_tagdistance = 9;
+      public static final int kHeaderOffset_tagarea =     10;
+
+      public static final int kFiducialOffset_id =        0;
+      public static final int kFiducialOffset_tx =        1;
+      public static final int kFiducialOffset_ty =        2;
+      public static final int kFiducialOffset_rot =       3;
+      public static final int kFiducialOffset_distcam =   4;
+      public static final int kFiducialOffset_distrobot = 5;
+      public static final int kFiducialOffset_ambiguity = 6;
+
+      public static final double kMaxTranslationError = 0.0001;
+      public static final double kMaxRotationError = 1; // Degrees
+
+      public static final boolean kSimulateLimelight = false;
+    }
+
     public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.7, 0.7, 999999);
 
     public static final boolean kUseVision = false;
   }
 
+  public static final class SimulationConstants {
+    public static final Random kRandom = new Random();
+
+    public static final double kMaxAngleError = .01;
+    public static final double kMaxDistanceError = .01;
+  }
+
   public static final class ElevatorConstants {
     // TODO: Set motor and distance sensor ports
     public static final int kElevatorMotorPort = 0;

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

@@ -4,9 +4,16 @@
 
 package frc.robot;
 
+
+import edu.wpi.first.net.PortForwarder;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants.VisionConstants;
+import frc.robot.Constants.VisionConstants.VirtualLimelightConstants;
+import frc.robot.utils.VirtualLimelight;
+import frc.robot.utils.VirtualLimelight.Fiducial;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -18,6 +25,7 @@ public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;
+  private VirtualLimelight m_virutalLimelight = null;
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -27,7 +35,32 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
+
+    // close and garbage collect the virtual limelight if it exists
+    if (m_virutalLimelight != null) {
+      m_virutalLimelight.close();
+      m_virutalLimelight = null;
+    }
+
+    // make sure we are not in a real competition
+    if (!DriverStation.isFMSAttached()) {
+      if (Robot.isSimulation() || VirtualLimelightConstants.kSimulateLimelight) {
+        m_virutalLimelight = new VirtualLimelight(VisionConstants.kLimelightName);
+        /*
+         * Create testing fiducials here
+         */
+        m_virutalLimelight.setFiducials(new Fiducial[] {new Fiducial(1, .1)});
+       }
+    }
+
+    if (Robot.isReal()) {
+      for (int port = 5800; port < 5809; port++) {
+        PortForwarder.add(port, "limelight.local", port);
+      }
+    }
+
     m_robotContainer = new RobotContainer();
+
     this.addPeriodic(m_robotContainer::fastPeriodic, Constants.kFastPeriodicPeriod);
   }
 
@@ -44,12 +77,17 @@ public void robotPeriodic() {
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
+    if (m_virutalLimelight != null) {
+      m_virutalLimelight.update(m_robotContainer.getDriveSubsystem().getTruePose());
+    }
     CommandScheduler.getInstance().run();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+
+  }
 
   @Override
   public void disabledPeriodic() {}

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
+
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -33,6 +34,10 @@ public class RobotContainer {
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
 
+  public DriveSubsystem getDriveSubsystem(){
+    return m_robotDrive;
+  }
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
@@ -122,7 +127,7 @@ public Command getAutonomousCommand() {
     // An example command will be run in autonomous
     return null;
   }
-
+  
   /**
    * This periodic loop runs every 10ms (100Hz)
    * 

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

@@ -8,11 +8,8 @@
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -23,8 +20,10 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.VisionConstants;
-import frc.robot.Constants;
+import frc.robot.utils.SimulatedEstimator;
+import frc.robot.utils.IEstimatorWrapper;
 import frc.robot.utils.LimelightHelpers;
+import frc.robot.utils.RealEstimator;
 import frc.robot.Robot;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -52,9 +51,6 @@ public class DriveSubsystem extends SubsystemBase {
       DriveConstants.kRearRightTurningEncoderPort,
       DriveConstants.kRearRightDriveMotorReversed);
 
-  private final AHRS m_gyro = new AHRS(NavXComType.kMXP_SPI);
-  private double m_gyroAngle;
-
   private final Timer m_headingCorrectionTimer = new Timer();
   private final PIDController m_headingCorrectionPID = new PIDController(DriveConstants.kPHeadingCorrectionController,
       0, 0);
@@ -65,16 +61,22 @@ public class DriveSubsystem extends SubsystemBase {
       m_rearRight.getPosition()
   };
 
-  private SwerveModuleState[] m_desiredStates;
+  private final IEstimatorWrapper m_poseEstimator;
+
 
-  private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics,
-      m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
-      VisionConstants.kVisionSTDDevs);
+  private SwerveModuleState[] m_desiredStates;
 
   private final Field2d m_field = new Field2d();
 
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    if (Robot.isReal()) {
+      m_poseEstimator = new RealEstimator(DriveConstants.kDriveKinematics, m_swerveModulePositions, new AHRS(NavXComType.kMXP_SPI));
+    }
+    else {
+      m_poseEstimator = new SimulatedEstimator(DriveConstants.kDriveKinematics, m_swerveModulePositions);
+    }
+
     this.zeroHeading();
     this.resetOdometry(new Pose2d());
     SmartDashboard.putData("Field", m_field);
@@ -111,20 +113,21 @@ public void periodic() {
         m_rearRight.getPosition()
     };
 
-    m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
-        m_swerveModulePositions);
+    m_poseEstimator.update(
+        m_swerveModulePositions, m_desiredStates);
 
     boolean limelightReal = LimelightHelpers.getLatency_Pipeline(VisionConstants.kLimelightName) != 0.0;
-    if (VisionConstants.kUseVision && Robot.isReal() && limelightReal) {
+
+    if (VisionConstants.kUseVision) {
       // Update LimeLight with current robot orientation
-      LimelightHelpers.SetRobotOrientation(VisionConstants.kLimelightName, m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0);
+      LimelightHelpers.SetRobotOrientation(VisionConstants.kLimelightName, m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), Math.toDegrees(m_poseEstimator.getGyroRate()), 0.0, 0.0, 0.0, 0.0);
 
       // Get the pose estimate
       LimelightHelpers.PoseEstimate limelightMeasurement = LimelightHelpers
           .getBotPoseEstimate_wpiBlue_MegaTag2(VisionConstants.kLimelightName);
 
       // Add it to your pose estimator if it is a valid measurement
-      if (limelightMeasurement != null && limelightMeasurement.tagCount != 0 && m_gyro.getRate() < 720) {
+      if (limelightMeasurement != null && limelightMeasurement.tagCount != 0 && m_poseEstimator.getGyroRate() < 720) {
         m_poseEstimator.addVisionMeasurement(
             limelightMeasurement.pose,
             limelightMeasurement.timestampSeconds);
@@ -133,7 +136,7 @@ public void periodic() {
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
-    SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
+    SmartDashboard.putNumber("gyro angle", m_poseEstimator.getGyroAngle().getDegrees());
     SmartDashboard.putNumber("odometryX", m_poseEstimator.getEstimatedPosition().getX());
     SmartDashboard.putNumber("odometryY", m_poseEstimator.getEstimatedPosition().getY());
     SmartDashboard.putBoolean("Limelight isreal", limelightReal);
@@ -167,6 +170,15 @@ public Pose2d getPose() {
     return m_poseEstimator.getEstimatedPosition();
   }
 
+  /**
+   * Gets the true simulated position.
+   * Should only be used by simulation support code.
+   * @return The true simulated pose. new Pose2d() for real robots 
+   */
+  public Pose2d getTruePose() {
+    return m_poseEstimator.getTruePosition();
+  }
+
   /**
    * Method to drive the robot using joystick info.
    *
@@ -199,7 +211,7 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
     // adjustments to
     double calculatedRotation = rotation;
 
-    double currentAngle = MathUtil.angleModulus(m_gyro.getRotation2d().getRadians());
+    double currentAngle = MathUtil.angleModulus(m_poseEstimator.getGyroAngle().getRadians());
 
     // If we are not translating or if not enough time has passed since the last
     // time we rotated
@@ -218,7 +230,7 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
     m_desiredStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative
             ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
-                Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
+                m_poseEstimator.getGyroAngle())
             : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
   }
 
@@ -229,7 +241,23 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
    */
   public void resetOdometry(Pose2d pose) {
     m_poseEstimator.resetPosition(
-        Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
+        new SwerveModulePosition[] {
+            m_frontLeft.getPosition(),
+            m_frontRight.getPosition(),
+            m_rearLeft.getPosition(),
+            m_rearRight.getPosition()
+        },
+        pose);
+  }
+
+  /**
+   * Sets the simulated true pose
+   * Should only be used by simulation support code
+   * Does nothing when called from a real robot
+   * @param pose
+   */
+  public void resetTrueOdometry(Pose2d pose) {
+    m_poseEstimator.resetTruePosition(
         new SwerveModulePosition[] {
             m_frontLeft.getPosition(),
             m_frontRight.getPosition(),
@@ -241,8 +269,7 @@ public void resetOdometry(Pose2d pose) {
 
   /** Zeroes the heading of the robot. */
   public void zeroHeading() {
-    m_gyro.reset();
-    m_gyroAngle = 0;
+    m_poseEstimator.resetGyro();
   }
 
   public void addVisionMeasurement(Pose2d pose, double timestamp) {
@@ -257,10 +284,5 @@ public void fastPeriodic() {
     m_frontRight.setDesiredState(m_desiredStates[1]);
     m_rearLeft.setDesiredState(m_desiredStates[2]);
     m_rearRight.setDesiredState(m_desiredStates[3]);
-
-    // Takes the integral of the rotation speed to find the current angle for the
-    // simulator
-    m_gyroAngle += DriveConstants.kDriveKinematics.toChassisSpeeds(m_desiredStates).omegaRadiansPerSecond
-        * Constants.kFastPeriodicPeriod;
   }
-}
+}