diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java index cda0fbd6..50e8484e 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSparkMax.java @@ -125,7 +125,8 @@ public ModuleIOSparkMax(int index) { @Override public void updateInputs(ModuleIOInputs inputs) { inputs.setDrivePositionRad(Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO); - inputs.setDriveVelocityRadPerSec(Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO); + inputs.setDriveVelocityRadPerSec( + Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO); inputs.setDriveAppliedVolts(driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage()); inputs.setDriveCurrentAmps(new double[] {driveSparkMax.getOutputCurrent()}); diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 77a5dbf5..62e59b6b 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -168,15 +168,19 @@ public void updateInputs(ModuleIOInputs inputs) { m_turnAppliedVolts, m_turnCurrent); - inputs.setDrivePositionRad(Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); - inputs.setDriveVelocityRadPerSec(Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDrivePositionRad( + Units.rotationsToRadians(m_drivePosition.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); + inputs.setDriveVelocityRadPerSec( + Units.rotationsToRadians(m_driveVelocity.getValueAsDouble()) / m_moduleConstants.DRIVE_GEAR_RATIO()); inputs.setDriveAppliedVolts(m_driveAppliedVolts.getValueAsDouble()); inputs.setDriveCurrentAmps(new double[] {m_driveCurrent.getValueAsDouble()}); inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble()) .minus(m_moduleConstants.ENCODER_OFFSET())); - inputs.setTurnPosition(Rotation2d.fromRotations((m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO()))); - inputs.setTurnVelocityRadPerSec(Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); + inputs.setTurnPosition(Rotation2d.fromRotations(( + m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO()))); + inputs.setTurnVelocityRadPerSec( + Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO()); inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble()); inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); diff --git a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java index 2328dfb1..2a6bde0a 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/module/PhoenixOdometryThread.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.subsystems.drive.DriveSubsystem; import java.util.ArrayList; @@ -93,7 +94,7 @@ public void run() { } } } catch (InterruptedException e) { - e.printStackTrace(); + DriverStation.reportError(e.toString(), true); } finally { signalsLock.unlock(); }