Merge branch 'master' into allianceOriented

src/main/deploy/swerve/controllerproperties.json

@@ -1,8 +1,8 @@
 {
  "angleJoystickRadiusDeadband": 0.5,
  "heading": {
- "p": 0.4,
- "i": 0,
- "d": 0.01
+ "p": 0.625,
+ "i": 0.0475,
+ "d": 0.04
  }
 }

src/main/deploy/swerve/modules/backleft.json

@@ -3,7 +3,7 @@
  "front": -10.875,
  "left": 10.875
  },
- "absoluteEncoderOffset": 335.12688,
+ "absoluteEncoderOffset": 285.90804,
  "drive": {
  "type": "sparkmax",
  "id": 17,

src/main/deploy/swerve/modules/backright.json

@@ -3,7 +3,7 @@
  "front": -10.875,
  "left": -10.875
  },
- "absoluteEncoderOffset": 250.31232,
+ "absoluteEncoderOffset": 338.46696,
  "drive": {
  "type": "sparkmax",
  "id": 20,

src/main/deploy/swerve/modules/frontleft.json

@@ -3,7 +3,7 @@
  "front": 10.875,
  "left": 10.875
  },
- "absoluteEncoderOffset": 37.52928,
+ "absoluteEncoderOffset": 127.52928,
  "drive": {
  "type": "sparkmax",
  "id": 11,

src/main/deploy/swerve/modules/frontright.json

@@ -3,7 +3,7 @@
  "front": 10.875,
  "left": -10.875
  },
- "absoluteEncoderOffset": 202.23648,
+ "absoluteEncoderOffset": 276.24024,
  "drive": {
  "type": "sparkmax",
  "id": 14,

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

@@ -55,6 +55,11 @@ private void configureCommands () {
 
  this.driverOne.x().onTrue(new InstantCommand(this.swerve::zeroGyro, this.swerve));
  this.driverOne.leftBumper().whileTrue(new RepeatCommand(new InstantCommand(this.swerve::lock, this.swerve)));
+
+ /**
+ this.driverOne.leftTrigger().whileTrue(this.swerve.getDriveSysidRoutine());
+ this.driverOne.rightTrigger().whileTrue(this.swerve.getAngleSysidRoutine());
+ */
  }
 
  public Command getAutonomousCommand () { return this.swerve.getAutonomousCommand(); }

src/main/java/frc/robot/commands/swerve/TrackAprilTags.java

@@ -1,5 +1,6 @@
 package frc.robot.commands.swerve;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Limelight;
 import frc.robot.subsystems.Swerve;
@@ -8,7 +9,6 @@ public class TrackAprilTags extends Command {
 
  private final Swerve swerve;
  private final Limelight limelight;
- private double latestTimestamp;
 
  public TrackAprilTags (Swerve swerve, Limelight limelight) {
 
@@ -20,15 +20,12 @@ public TrackAprilTags (Swerve swerve, Limelight limelight) {
  @Override
  public void execute () {
 
- if (this.limelight.getLatestTimestamp() == this.latestTimestamp) { return; }
+ if (!this.limelight.areValidMeasurements()) { return; }
 
  this.swerve.addVisionMeasurement(
  this.limelight.getLatestPose(), 
- this.limelight.getLatestTimestamp(), 
- this.limelight.getLatestRotation()
+ Timer.getFPGATimestamp() - (this.limelight.getLatestDelay() / 1000.0)
  );
-
- this.latestTimestamp = this.limelight.getLatestTimestamp();
  }
 
  @Override

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,49 +14,47 @@
 public class Limelight extends SubsystemBase {
 
  private Pose2d latestPose;
- private Rotation3d latestRotation;
- private double latestTimestamp;
+ private double latestDelay;
+
+ private boolean validMeasurements;
+ private Timer usageTimer = new Timer();
 
  public Limelight () {
 
+ LimelightHelpers.setPipelineIndex("limelight", 0);
  LimelightHelpers.setLEDMode_PipelineControl("limelight");
+
  LimelightHelpers.setCropWindow("limelight", -1, 1, -1, 1);
+
+ this.usageTimer.reset();
+ this.usageTimer.start();
  }
 
  @Override
  public void periodic () {
 
  LimelightResults limelightResults = LimelightHelpers.getLatestResults("limelight");
  Results targetingResults = limelightResults.targetingResults;
+ this.validMeasurements = targetingResults.valid;
 
- double latestTimestamp = targetingResults.timestamp_RIOFPGA_capture;
-
- if (this.latestTimestamp != latestTimestamp) {
+ if (this.validMeasurements) {
 
- if (!DriverStation.getAlliance().isPresent()) {
+ 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.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();
- }
+ this.latestDelay = targetingResults.latency_capture + targetingResults.latency_pipeline;
+ this.usageTimer.restart();
  }
 
- this.latestTimestamp = latestTimestamp;
- double lastUpdated = Timer.getFPGATimestamp() - latestTimestamp;
+ double lastUpdated = this.usageTimer.get();
  SmartDashboard.putString("Limelight Last Updated", (Math.round(lastUpdated * 10) / 10.0) + "s Ago");
 
  if (lastUpdated >= 20) { Alerts.limelightDetections.set(true); }
  else { Alerts.limelightDetections.set(false); }
  }
 
  public Pose2d getLatestPose () { return this.latestPose; }
- public Rotation3d getLatestRotation () { return this.latestRotation; }
- public double getLatestTimestamp () { return this.latestTimestamp; }
+ public double getLatestDelay () { return this.latestDelay; }
+ public boolean areValidMeasurements () { return this.validMeasurements; }
 }

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;
@@ -25,8 +24,10 @@
 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;
 import frc.robot.util.Constants;
 import swervelib.SwerveDrive;
+import swervelib.SwerveDriveTest;
 import swervelib.SwerveModule;
 import swervelib.parser.SwerveDriveConfiguration;
 import swervelib.parser.SwerveParser;
@@ -161,14 +162,40 @@ public void 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); }
  public Command getAutonomousCommand () { return this.autonomousChooser.getSelected(); }
+
+ public Command getDriveSysidRoutine () {
+
+ SysIdRoutine sysIdRoutine = SwerveDriveTest.setDriveSysIdRoutine(
+ Constants.SwerveConstants.DRIVE_SYSID_CONFIG,
+ this, this.swerveDrive,
+ 12.0
+ );
+
+ return SwerveDriveTest.generateSysIdCommand(
+ sysIdRoutine, 
+ Constants.SwerveConstants.DRIVE_SYSID_CONFIG.m_timeout.magnitude(), 
+ Constants.SwerveConstants.DRIVE_SYSID_QUASISTATIC_TIMEOUT, 
+ Constants.SwerveConstants.DRIVE_SYSID_DYNAMIC_TIMEOUT
+ );
+ }
+
+ public Command getAngleSysidRoutine () {
+
+ SysIdRoutine sysIdRoutine = SwerveDriveTest.setAngleSysIdRoutine(
+ Constants.SwerveConstants.ANGLE_SYSID_CONFIG,
+ this, this.swerveDrive
+ );
+
+ return SwerveDriveTest.generateSysIdCommand(
+ sysIdRoutine, 
+ Constants.SwerveConstants.ANGLE_SYSID_CONFIG.m_timeout.magnitude(), 
+ Constants.SwerveConstants.ANGLE_SYSID_QUASISTATIC_TIMEOUT, 
+ Constants.SwerveConstants.ANGLE_SYSID_DYNAMIC_TIMEOUT
+ );
+ }
 }

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

@@ -3,6 +3,8 @@
 import java.util.List;
 
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import swervelib.math.Matter;
 
 public final class Constants {
@@ -38,5 +40,24 @@ public final class SwerveConstants {
 
  public static final double REPLANNING_TOTAL_ERROR = 0.15; // Total Error to Replan Path [m]
  public static final double REPLANNING_ERROR_SPIKE = 0.1; // Spike in Error to Replan Path [m / 20ms]
+
+ public static final Config DRIVE_SYSID_CONFIG = new Config(
+ Units.Volts.of(1.5).per(Units.Second),
+ Units.Volts.of(9.6),
+ Units.Seconds.of(10)
+ );
+
+ public static final Config ANGLE_SYSID_CONFIG = new Config(
+ Units.Volts.of(1.25).per(Units.Second),
+ Units.Volts.of(7.8),
+ Units.Seconds.of(2.5)
+ );
+
+ public static final double DRIVE_SYSID_QUASISTATIC_TIMEOUT = 7.5;
+ public static final double DRIVE_SYSID_DYNAMIC_TIMEOUT = 3.0;
+
+ //8
+ public static final double ANGLE_SYSID_QUASISTATIC_TIMEOUT = 0.1;
+ public static final double ANGLE_SYSID_DYNAMIC_TIMEOUT = 3.0;
  }
 }

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
  "fileName": "PathplannerLib.json",
  "name": "PathplannerLib",
- "version": "2024.2.2",
+ "version": "2024.2.3",
  "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
  "frcYear": "2024",
  "mavenUrls": [
@@ -12,15 +12,15 @@
  {
  "groupId": "com.pathplanner.lib",
  "artifactId": "PathplannerLib-java",
- "version": "2024.2.2"
+ "version": "2024.2.3"
  }
  ],
  "jniDependencies": [],
  "cppDependencies": [
  {
  "groupId": "com.pathplanner.lib",
  "artifactId": "PathplannerLib-cpp",
- "version": "2024.2.2",
+ "version": "2024.2.3",
  "libName": "PathplannerLib",
  "headerClassifier": "headers",
  "sharedLibrary": false,