From fff486fe4127fffbc7163dc635dd78f8dc5ca74b Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Thu, 11 Jan 2024 19:40:51 -0500 Subject: [PATCH] fix(swerve): Run drive motor open loop for now. --- src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java | 2 +- src/main/java/frc/robot/swerve/SwerveFactory.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java index de2e3c4..5c9a57e 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java @@ -39,7 +39,7 @@ public void setSetpoint(double velocityMetersPerSecond) { if (velocityMetersPerSecond == 0.0) { talonFX.setControl(new CoastOut()); } else { - talonFX.setControl(calculateVelocityVoltage(velocityMetersPerSecond)); + talonFX.setControl(new VoltageOut(velocityMetersPerSecond / SwerveConstants.MAXIMUM_SPEED * 12)); } } diff --git a/src/main/java/frc/robot/swerve/SwerveFactory.java b/src/main/java/frc/robot/swerve/SwerveFactory.java index 8cd228a..27b4d18 100644 --- a/src/main/java/frc/robot/swerve/SwerveFactory.java +++ b/src/main/java/frc/robot/swerve/SwerveFactory.java @@ -79,8 +79,7 @@ public static TalonFXConfiguration createSteerMotorConfig() { * @return a drive motor. */ public static DriveMotorIO createDriveMotor(SwerveModuleConfig config) { - //if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); - if (Robot.isReal()) return new DriveMotorIOSim(); + if (Robot.isReal()) return new DriveMotorIOTalonFXPID(config.moduleCAN().drive()); return new DriveMotorIOSim(); }