diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index 78470e3..e1fcd85 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -13,6 +13,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -201,6 +202,16 @@ public ModuleConstants getBackRightModuleConstants() { return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.205)); } + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + @Override public int getGyroID() { return 0; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java index f738ca2..da3f0a6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/CompBotSwerveConstants.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Mass; import frc.robot.components.camera.Camera.CameraConstants; @@ -129,6 +130,16 @@ public TalonFXConfiguration getTurnConfig(int cancoderID) { throw new UnsupportedOperationException("Unimplemented method 'getTurnConfiguration'"); } + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX44; + } + @Override public CANcoderConfiguration getCancoderConfig(Rotation2d cancoderOffset) { // TODO Auto-generated method stub diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java index 0ec6f5c..4643b9b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SeahorseSwerveConstants.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -57,6 +58,16 @@ public int getGyroID() { return 0; } + @Override + public MotorType getDriveMotorType() { + return MotorType.KrakenX60; + } + + @Override + public MotorType getTurnMotorType() { + return MotorType.KrakenX60; + } + public Pigeon2Configuration getGyroConfig() { Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = -0.002175945555791259; diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java index c97139b..e854fd6 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/SwerveConstants.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation2d; @@ -136,6 +137,20 @@ public double getWheelRadiusMeters() { */ public abstract TalonFXConfiguration getTurnConfig(int cancoderID); + /** + * The motor type of the swerve turn motors (i.e. x44 vs x60) + * + * @return + */ + public abstract MotorType getTurnMotorType(); + + /** + * The motor type of the swerve drive motors (i.e. x44 vs x60) + * + * @return + */ + public abstract MotorType getDriveMotorType(); + /** * The configuration for a swerve-module cancoder * diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index dad28f6..4ba4d8c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -20,10 +20,12 @@ public ModuleIOSim( // using Async odo) this.simulation = simulation; this.simulation.useDriveMotorController( - new MaplePhoenixUtil.TalonFXMotorControllerSim(driveTalon, true)); + new MaplePhoenixUtil.TalonFXMotorControllerSim( + driveTalon, SwerveSubsystem.SWERVE_CONSTANTS.getDriveMotorType(), true)); this.simulation.useSteerMotorController( new MaplePhoenixUtil.TalonFXMotorControllerWithRemoteCancoderSim( turnTalon, + SwerveSubsystem.SWERVE_CONSTANTS.getTurnMotorType(), SwerveSubsystem.SWERVE_CONSTANTS.getTurnMotorInverted(), cancoder, false, diff --git a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java index 2fda1f0..a7d3f98 100644 --- a/src/main/java/frc/robot/utils/MaplePhoenixUtil.java +++ b/src/main/java/frc/robot/utils/MaplePhoenixUtil.java @@ -20,6 +20,7 @@ import com.ctre.phoenix6.sim.CANcoderSimState; import com.ctre.phoenix6.sim.ChassisReference; import com.ctre.phoenix6.sim.TalonFXSimState; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -36,10 +37,11 @@ public static class TalonFXMotorControllerSim implements SimulatedMotorControlle private final TalonFXSimState talonFXSimState; - public TalonFXMotorControllerSim(TalonFX talonFX, boolean motorInverted) { + public TalonFXMotorControllerSim(TalonFX talonFX, MotorType motorType, boolean motorInverted) { this.id = instances++; this.talonFXSimState = talonFX.getSimState(); + this.talonFXSimState.setMotorType(motorType); talonFXSimState.Orientation = motorInverted ? ChassisReference.Clockwise_Positive @@ -66,11 +68,12 @@ public static class TalonFXMotorControllerWithRemoteCancoderSim public TalonFXMotorControllerWithRemoteCancoderSim( TalonFX talonFX, + MotorType motorType, boolean motorInverted, CANcoder cancoder, boolean encoderInverted, Angle encoderOffset) { - super(talonFX, motorInverted); + super(talonFX, motorType, motorInverted); this.remoteCancoderSimState = cancoder.getSimState(); this.remoteCancoderSimState.Orientation = encoderInverted