From 293a00aa15ea5e9fda193c3255686299b6bc1b1a Mon Sep 17 00:00:00 2001 From: Vignesh Balasubramaniam Date: Thu, 19 Sep 2024 15:27:29 -0500 Subject: [PATCH] Update PurpleLib --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 2 +- .../subsystems/drive/DriveSubsystem.java | 54 ++++--------------- .../subsystems/vision/VisionSubsystem.java | 2 - .../subsystems/ShooterSubsystemTest.java | 1 - 5 files changed, 13 insertions(+), 48 deletions(-) diff --git a/build.gradle b/build.gradle index 7ffe353..367ef8b 100644 --- a/build.gradle +++ b/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.+' diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 770f508..e302a54 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 DRIVE_SLIP_RATIO = Units.Percent.of(12.0); + public static final Measure DRIVE_SLIP_RATIO = Units.Percent.of(8.0); public static final double DRIVE_TURN_SCALAR = 90.0; public static final double DRIVE_LOOKAHEAD = 10; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 997e41e..9cdc988 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/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,7 +106,6 @@ public Hardware(NavX2 navx, public static final Measure DRIVE_CURRENT_LIMIT = Units.Amps.of(60.0); public static final Measure> NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60); public static final Measure> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI); - public static final Measure> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(20.0); public static final Measure> VISION_ANGULAR_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(720.0); public static final Measure>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second); public final Measure> DRIVE_MAX_LINEAR_SPEED; @@ -114,12 +113,13 @@ public Hardware(NavX2 navx, // 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 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 ROTATE_TOLERANCE = Units.Degrees.of(2.5); + public static final Measure> AIM_VELOCITY_THRESHOLD = Units.DegreesPerSecond.of(20.0); private static final Measure BLUE_AMP_DIRECTION = Units.Radians.of(-Math.PI / 2); private static final Measure 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(); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 4cf3c0c..6dec459 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/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; diff --git a/src/test/java/frc/robot/subsystems/ShooterSubsystemTest.java b/src/test/java/frc/robot/subsystems/ShooterSubsystemTest.java index 001d606..e0febdc 100644 --- a/src/test/java/frc/robot/subsystems/ShooterSubsystemTest.java +++ b/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;