From 009d44abbfb06e661fdc749b8624a2177b1a8d59 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:39:05 -0800 Subject: [PATCH] minor updates for module.java and gyroiopigeon2 --- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../robot/subsystems/drive/module/Module.java | 34 +++++++++---------- 2 files changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 7470c01d..b2cebb45 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -57,7 +57,7 @@ public void updateInputs(GyroIOInputs inputs) { inputs.odometryYawPositions = yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) + .map(Rotation2d::fromDegrees) .toArray(Rotation2d[]::new); yawPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 3f4a466a..7dd76355 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -18,10 +18,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Module { @@ -51,22 +49,22 @@ public Module(ModuleIO io) { // Switch constants based on mode (the physics simulator is treated as a // separate robot with different tuning) - switch (Constants.currentMode) { - case REAL, REPLAY -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - m_driveFeedback = new PIDController(0.05, 0.0, 0.0); - m_turnFeedback = new PIDController(7.0, 0.0, 0.0); - } - case SIM -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); - m_driveFeedback = new PIDController(0.1, 0.0, 0.0); - m_turnFeedback = new PIDController(10.0, 0.0, 0.0); - } - default -> { - m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - m_driveFeedback = new PIDController(0.0, 0.0, 0.0); - m_turnFeedback = new PIDController(0.0, 0.0, 0.0); - } + switch (Constants.currentMode) { + case REAL, REPLAY -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); + m_driveFeedback = new PIDController(0.05, 0.0, 0.0); + m_turnFeedback = new PIDController(7.0, 0.0, 0.0); + } + case SIM -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); + m_driveFeedback = new PIDController(0.1, 0.0, 0.0); + m_turnFeedback = new PIDController(10.0, 0.0, 0.0); + } + default -> { + m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); + m_driveFeedback = new PIDController(0.0, 0.0, 0.0); + m_turnFeedback = new PIDController(0.0, 0.0, 0.0); + } } m_turnFeedback.enableContinuousInput(-Math.PI, Math.PI);