Update PurpleLib

build.gradle

@@ -97,7 +97,7 @@ dependencies {
   nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
   simulationRelease wpi.sim.enableRelease()
 
-  implementation 'com.github.lasarobotics:PurpleLib:98dd4bf354'
+  implementation 'com.github.lasarobotics:PurpleLib:19c3227c08'
 
   implementation 'org.apache.commons:commons-math3:3.+'
 

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

@@ -118,7 +118,7 @@ public static class AutoNames {
   public static class Drive {
     public static final DriveWheel DRIVE_WHEEL = new DriveWheel(Units.Inches.of(3.0), Units.Value.of(0.9), Units.Value.of(0.8));
     public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(8.0, 0.0, 0.3, 0.0, 0.0);
-    public static final Measure<Dimensionless> DRIVE_SLIP_RATIO = Units.Percent.of(12.0);
+    public static final Measure<Dimensionless> DRIVE_SLIP_RATIO = Units.Percent.of(8.0);
     public static final double DRIVE_TURN_SCALAR = 90.0;
     public static final double DRIVE_LOOKAHEAD = 10;
 

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

@@ -5,7 +5,6 @@
 package frc.robot.subsystems.drive;
 
 import java.util.Optional;
-import java.util.concurrent.ThreadLocalRandom;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
@@ -19,6 +18,7 @@
 import org.lasarobotics.drive.SwervePoseEstimatorService;
 import org.lasarobotics.drive.ThrottleMap;
 import org.lasarobotics.hardware.kauailabs.NavX2;
+import org.lasarobotics.hardware.kauailabs.NavX2Sim;
 import org.lasarobotics.hardware.revrobotics.Spark.MotorKind;
 import org.lasarobotics.led.LEDStrip.Pattern;
 import org.lasarobotics.led.LEDSubsystem;
@@ -106,20 +106,20 @@ public Hardware(NavX2 navx,
   public static final Measure<Current> DRIVE_CURRENT_LIMIT = Units.Amps.of(60.0);
   public static final Measure<Velocity<Angle>> NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60);
   public static final Measure<Velocity<Angle>> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI);
-  public static final Measure<Velocity<Angle>> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(20.0);
   public static final Measure<Velocity<Angle>> VISION_ANGULAR_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(720.0);
   public static final Measure<Velocity<Velocity<Angle>>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second);
   public final Measure<Velocity<Distance>> DRIVE_MAX_LINEAR_SPEED;
   public final Measure<Velocity<Velocity<Distance>>> DRIVE_AUTO_ACCELERATION;
 
   // Other settings
   private static final double TIP_THRESHOLD = 35.0;
-  private static final double TOLERANCE = 2.5;
   private static final double BALANCED_THRESHOLD = 10.0;
   private static final double AIM_VELOCITY_COMPENSATION_FUDGE_FACTOR = 0.1;
   private static final Matrix<N3, N1> ODOMETRY_STDDEV = VecBuilder.fill(0.03, 0.03, Math.toRadians(1.0));
   private static final PIDConstants AUTO_AIM_PID = new PIDConstants(12.0, 0.0, 0.1, 0.0, 0.0, GlobalConstants.ROBOT_LOOP_PERIOD);
   private static final TrapezoidProfile.Constraints AIM_PID_CONSTRAINT = new TrapezoidProfile.Constraints(2160.0, 4320.0);
+  private static final Measure<Angle> ROTATE_TOLERANCE = Units.Degrees.of(2.5);
+  public static final Measure<Velocity<Angle>> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(20.0);
 
   private static final Measure<Angle> BLUE_AMP_DIRECTION = Units.Radians.of(-Math.PI / 2);
   private static final Measure<Angle> BLUE_SOURCE_DIRECTION = Units.Radians.of(-1.060 + Math.PI);
@@ -173,6 +173,7 @@ public Hardware(NavX2 navx,
   private MAXSwerveModule m_lRearModule;
   private MAXSwerveModule m_rRearModule;
 
+  private NavX2Sim m_navxSim;
 
   private ControlCentricity m_controlCentricity;
   private ChassisSpeeds m_desiredChassisSpeeds;
@@ -211,6 +212,7 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     this.m_rFrontModule = drivetrainHardware.rFrontModule;
     this.m_lRearModule = drivetrainHardware.lRearModule;
     this.m_rRearModule = drivetrainHardware.rRearModule;
+    this.m_navxSim = new NavX2Sim();
     this.m_controlCentricity = controlCentricity;
     this.m_throttleMap = new ThrottleMap(throttleInputCurve, DRIVE_MAX_LINEAR_SPEED, deadband);
     this.m_rotatePIDController = new RotatePIDController(turnInputCurve, pidf, turnScalar, deadband, lookAhead);
@@ -230,8 +232,8 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     m_navx.reset();
 
     // Setup rotate PID
-    m_rotatePIDController.setTolerance(TOLERANCE);
-    m_rotatePIDController.setSetpoint(getAngle().in(Units.Degrees));
+    m_rotatePIDController.setTolerance(ROTATE_TOLERANCE, AIM_VELOCITY_THRESHOLD);
+    m_rotatePIDController.setSetpoint(getAngle());
 
     // Define drivetrain kinematics
     m_kinematics = new SwerveDriveKinematics(m_lFrontModule.getModuleCoordinate(),
@@ -266,11 +268,11 @@ public DriveSubsystem(Hardware drivetrainHardware, PIDConstants pidf, ControlCen
     // Setup auto-aim PID controller
     m_autoAimPIDControllerFront = new ProfiledPIDController(AUTO_AIM_PID.kP, 0.0, AUTO_AIM_PID.kD, AIM_PID_CONSTRAINT, AUTO_AIM_PID.period);
     m_autoAimPIDControllerFront.enableContinuousInput(-180.0, +180.0);
-    m_autoAimPIDControllerFront.setTolerance(TOLERANCE);
+    m_autoAimPIDControllerFront.setTolerance(ROTATE_TOLERANCE.in(Units.Degrees));
     m_autoAimPIDControllerFront.setIZone(AUTO_AIM_PID.kIZone);
     m_autoAimPIDControllerBack = new ProfiledPIDController(AUTO_AIM_PID.kP, 0.0, AUTO_AIM_PID.kD, AIM_PID_CONSTRAINT, AUTO_AIM_PID.period);
     m_autoAimPIDControllerBack.enableContinuousInput(-180.0, +180.0);
-    m_autoAimPIDControllerBack.setTolerance(TOLERANCE);
+    m_autoAimPIDControllerBack.setTolerance(ROTATE_TOLERANCE.in(Units.Degrees));
     m_autoAimPIDControllerBack.setIZone(AUTO_AIM_PID.kIZone);
 
     // Initialise other variables
@@ -689,22 +691,6 @@ private void autoDefense(double xRequest, double yRequest, double rotateRequest)
     );
   }
 
-  /**
-   * Aim robot by given angle
-   * @param angle Desired angle in degrees
-   */
-  private void aimAtAngle(double angle) {
-    double rotateOutput = m_rotatePIDController.calculate(getAngle().in(Units.Degrees), getAngle().in(Units.Degrees) + angle);
-
-    drive(
-      m_controlCentricity,
-      Units.MetersPerSecond.of(0),
-      Units.MetersPerSecond.of(0),
-      Units.DegreesPerSecond.of(rotateOutput),
-      getInertialSpeeds()
-    );
-  }
-
   /**
    * Call this repeatedly to drive using PID during teleoperation
    * @param xRequest Desired X axis (forward) speed [-1.0, +1.0]
@@ -820,17 +806,8 @@ public void periodic() {
   @Override
   public void simulationPeriodic() {
     // This method will be called once per scheduler run in simulation
-    double randomNoise = ThreadLocalRandom.current().nextDouble(0.9, 1.0);
-    m_navx.getInputs().xVelocity = Units.MetersPerSecond.of(m_desiredChassisSpeeds.vxMetersPerSecond * randomNoise);
-    m_navx.getInputs().yVelocity = Units.MetersPerSecond.of(m_desiredChassisSpeeds.vyMetersPerSecond * randomNoise);
-    m_navx.getInputs().yawRate = Units.RadiansPerSecond.of(m_desiredChassisSpeeds.omegaRadiansPerSecond * randomNoise);
-
-    int yawDriftDirection = ThreadLocalRandom.current().nextDouble(1.0) < 0.5 ? -1 : +1;
-    double angle = m_navx.getSimAngle() - Math.toDegrees(m_desiredChassisSpeeds.omegaRadiansPerSecond * randomNoise) * GlobalConstants.ROBOT_LOOP_PERIOD
-                   + (NAVX2_YAW_DRIFT_RATE.in(Units.DegreesPerSecond) * GlobalConstants.ROBOT_LOOP_PERIOD * yawDriftDirection);
-    m_navx.setSimAngle(angle);
+    m_navxSim.update(getPose().getRotation(), m_desiredChassisSpeeds, m_controlCentricity);
 
-    //updatePose();
     smartDashboard();
     logOutputs();
   }
@@ -975,15 +952,6 @@ public Command aimAtPointCommand(Translation2d point, boolean reversed, boolean
     return aimAtPointCommand(() -> 0.0, () -> 0.0, () -> 0.0, () -> point, reversed, velocityCorrection);
   }
 
-  /**
-   * Change robot aim by desired angle
-   * @param angleRequestSupplier
-   * @return Command that aims robot
-   */
-  public Command aimAtAngleCommand(DoubleSupplier angleRequestSupplier) {
-    return run(() -> aimAtAngle(angleRequestSupplier.getAsDouble()));
-  }
-
   /**
    *
    * @param xRequestSupplier
@@ -1118,7 +1086,7 @@ public Command goToPoseCommand(PurplePathPose goal) {
    * Reset DriveSubsystem turn PID
    */
   public void resetRotatePID() {
-    m_rotatePIDController.setSetpoint(getAngle().in(Units.Degrees));
+    m_rotatePIDController.setSetpoint(getAngle());
     m_rotatePIDController.reset();
   }
 

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -11,7 +11,6 @@
 import java.util.concurrent.atomic.AtomicReference;
 import java.util.function.Supplier;
 
-import org.lasarobotics.utils.GlobalConstants;
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.simulation.VisionSystemSim;
 
@@ -27,7 +26,6 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.subsystems.vision.AprilTagCamera.AprilTagCameraResult;

src/test/java/frc/robot/subsystems/ShooterSubsystemTest.java

@@ -25,7 +25,6 @@
 import org.mockito.ArgumentMatchers;
 
 import com.revrobotics.CANSparkBase.ControlType;
-import com.revrobotics.SparkPIDController.ArbFFUnits;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.Angle;