Fixed Limelight Rotation Reporting

FIRST1939 · Feb 17, 2024 · c72179d · c72179d
1 parent 4a3bd63
commit c72179d
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 31 deletions.
diff --git a/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java b/src/main/java/frc/robot/commands/swerve/TrackAprilTags.java
@@ -24,8 +24,7 @@ public void execute () {
 
  this.swerve.addVisionMeasurement(
  this.limelight.getLatestPose(), 
- Timer.getFPGATimestamp() - (this.limelight.getLatestDelay() / 1000.0), 
- this.limelight.getLatestRotation()
+ Timer.getFPGATimestamp() - (this.limelight.getLatestDelay() / 1000.0)
  );
  }
 

diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -1,7 +1,6 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
@@ -15,7 +14,6 @@
 public class Limelight extends SubsystemBase {
 
  private Pose2d latestPose;
- private Rotation3d latestRotation;
  private double latestDelay;
 
  private boolean validMeasurements;
@@ -41,19 +39,9 @@ public void periodic () {
 
  if (this.validMeasurements) {
 
- if (!DriverStation.getAlliance().isPresent()) {
-
- this.latestPose = targetingResults.getBotPose2d();
- this.latestRotation = targetingResults.getBotPose3d().getRotation();
- } else if (DriverStation.getAlliance().get() == Alliance.Red) { 
-
- this.latestPose = targetingResults.getBotPose2d_wpiRed(); 
- this.latestRotation = targetingResults.getBotPose3d_wpiRed().getRotation();
- } else { 
-
- this.latestPose = targetingResults.getBotPose2d_wpiBlue(); 
- this.latestRotation = targetingResults.getBotPose3d_wpiBlue().getRotation();
- }
+ if (!DriverStation.getAlliance().isPresent()) { this.latestPose = targetingResults.getBotPose2d(); } 
+ else if (DriverStation.getAlliance().get() == Alliance.Red) { this.latestPose = targetingResults.getBotPose2d_wpiRed(); } 
+ else { this.latestPose = targetingResults.getBotPose2d_wpiBlue(); }
 
  this.latestDelay = targetingResults.latency_capture + targetingResults.latency_pipeline;
  this.usageTimer.restart();
@@ -67,8 +55,6 @@ public void periodic () {
  }
 
  public Pose2d getLatestPose () { return this.latestPose; }
- public Rotation3d getLatestRotation () { return this.latestRotation; }
  public double getLatestDelay () { return this.latestDelay; }
-
  public boolean areValidMeasurements () { return this.validMeasurements; }
 }
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
@@ -11,7 +11,6 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.util.sendable.Sendable;
@@ -149,12 +148,7 @@ public void driveHeadingLock (Translation2d translation, Rotation2d rotation) {
  public void zeroGyro () { this.swerveDrive.zeroGyro(); }
  public void resetOdometry (Pose2d pose) { this.swerveDrive.resetOdometry(pose); }
  public void setChassisSpeeds (ChassisSpeeds chassisSpeeds) { this.swerveDrive.drive(chassisSpeeds); }
-
- public void addVisionMeasurement (Pose2d pose2d, double timestamp, Rotation3d rotation3d) { 
-
- this.swerveDrive.addVisionMeasurement(pose2d, timestamp);
- this.swerveDrive.setGyroOffset(rotation3d);
- }
+ public void addVisionMeasurement (Pose2d pose2d, double timestamp) { this.swerveDrive.addVisionMeasurement(pose2d, timestamp); }
 
  public void lock () { this.swerveDrive.lockPose(); }
  public void setBrakeMode (boolean brake) { this.swerveDrive.setMotorIdleMode(brake); }

diff --git a/src/main/java/frc/robot/util/BuildConstants.java b/src/main/java/frc/robot/util/BuildConstants.java
@@ -7,12 +7,12 @@ public final class BuildConstants {
  public static final String MAVEN_GROUP = "";
  public static final String MAVEN_NAME = "YAGSL";
  public static final String VERSION = "unspecified";
- public static final int GIT_REVISION = 14;
- public static final String GIT_SHA = "9a70f19e54c69d74958968d48748a6cf04f2190e";
- public static final String GIT_DATE = "2024-02-12 19:39:11 CST";
+ public static final int GIT_REVISION = 15;
+ public static final String GIT_SHA = "4a3bd63fb04d90379c01b57778a0605f20fdf2ab";
+ public static final String GIT_DATE = "2024-02-15 17:54:09 CST";
  public static final String GIT_BRANCH = "master";
- public static final String BUILD_DATE = "2024-02-15 17:43:24 CST";
- public static final long BUILD_UNIX_TIME = 1708040604758L;
+ public static final String BUILD_DATE = "2024-02-17 14:12:06 CST";
+ public static final long BUILD_UNIX_TIME = 1708200726277L;
  public static final int DIRTY = 1;
 
  private BuildConstants(){}