diff --git a/src/main/java/frc/robot/swerve/ModuleIOSim.java b/src/main/java/frc/robot/swerve/ModuleIOSim.java index 67bc4c2d..72b05f55 100644 --- a/src/main/java/frc/robot/swerve/ModuleIOSim.java +++ b/src/main/java/frc/robot/swerve/ModuleIOSim.java @@ -30,13 +30,15 @@ public ModuleIOSim() { new TalonFXSim( 1, 1 / SwerveConstants.DRIVE_REDUCTION, - SwerveConstants.DRIVE_MOTOR_MOMENT_OF_INERTIA); + SwerveConstants.DRIVE_MOTOR_MOMENT_OF_INERTIA, + 1); angleMotor = new TalonFXSim( 1, 1 / SwerveConstants.ANGLE_REDUCTION, - SwerveConstants.ANGLE_MOTOR_MOMENT_OF_INERTIA); + SwerveConstants.ANGLE_MOTOR_MOMENT_OF_INERTIA, + 1); velocityController = new PIDController( @@ -63,18 +65,18 @@ public void updateInputs(SwerveModuleInputs inputs) { inputs.driveMotorAppliedVoltage = driveMotor.getAppliedVoltage(); inputs.driveMotorVelocity = Units.rpsToMetersPerSecond( - driveMotor.getRotorVelocity(), SwerveConstants.WHEEL_DIAMETER / 2); + driveMotor.getVelocity(), SwerveConstants.WHEEL_DIAMETER / 2); currentVelocity = inputs.driveMotorVelocity; inputs.driveMotorVelocitySetpoint = velocitySetpoint; inputs.angleMotorAppliedVoltage = angleMotor.getAppliedVoltage(); - inputs.angleMotorVelocity = angleMotor.getRotorVelocity(); + inputs.angleMotorVelocity = angleMotor.getVelocity(); inputs.angleSetpoint = angleSetpoint; inputs.angle = Utils.normalize(currentAngle); inputs.moduleDistance = Units.rpsToMetersPerSecond( - driveMotor.getRotorPosition(), SwerveConstants.WHEEL_DIAMETER / 2); + driveMotor.getPosition(), SwerveConstants.WHEEL_DIAMETER / 2); inputs.moduleState = getModuleState(); if (hasPIDChanged(SwerveConstants.PID_VALUES)) updatePID(); @@ -125,7 +127,7 @@ public SwerveModuleState getModuleState() { public SwerveModulePosition getModulePosition() { return new SwerveModulePosition( Units.rpsToMetersPerSecond( - driveMotor.getRotorPosition(), SwerveConstants.WHEEL_DIAMETER / 2), + driveMotor.getPosition(), SwerveConstants.WHEEL_DIAMETER / 2), currentAngle); } diff --git a/src/main/java/lib/motors/SimMotor.java b/src/main/java/lib/motors/SimMotor.java index f42e272d..b510eff3 100644 --- a/src/main/java/lib/motors/SimMotor.java +++ b/src/main/java/lib/motors/SimMotor.java @@ -18,12 +18,24 @@ public class SimMotor { protected double lastTimestampSeconds = 0; protected double voltageRequest = 0; - public SimMotor(LinearSystem model, DCMotor motor, double gearing) { + protected final double conversionFactor; + + public SimMotor( + LinearSystem model, + DCMotor motor, + double gearing, + double conversionFactor) { this.motorSim = new DCMotorSim(model, motor, gearing); + this.conversionFactor = conversionFactor / gearing; } - public SimMotor(DCMotor motor, double jKgMetersSquared, double gearing) { - this(LinearSystemId.createDCMotorSystem(motor, jKgMetersSquared, gearing), motor, gearing); + public SimMotor( + DCMotor motor, double jKgMetersSquared, double gearing, double conversionFactor) { + this( + LinearSystemId.createDCMotorSystem(motor, jKgMetersSquared, gearing), + motor, + gearing, + conversionFactor); } public void setController(PIDController controller) { diff --git a/src/main/java/lib/motors/SparkMaxSim.java b/src/main/java/lib/motors/SparkMaxSim.java index aad24ad5..ff552500 100644 --- a/src/main/java/lib/motors/SparkMaxSim.java +++ b/src/main/java/lib/motors/SparkMaxSim.java @@ -8,12 +8,17 @@ public class SparkMaxSim extends SimMotor { - public SparkMaxSim(int numMotors, double gearing, double jKgMetersSquared) { - super(DCMotor.getNEO(numMotors), jKgMetersSquared, gearing); + public SparkMaxSim( + int numMotors, double gearing, double jKgMetersSquared, double conversionFactor) { + super(DCMotor.getNEO(numMotors), jKgMetersSquared, gearing, conversionFactor); } - public SparkMaxSim(LinearSystem model, int numMotors, double gearing) { - super(model, DCMotor.getNEO(numMotors), gearing); + public SparkMaxSim( + LinearSystem model, + int numMotors, + double gearing, + double conversionFactor) { + super(model, DCMotor.getNEO(numMotors), gearing, conversionFactor); } public void set(double speed) { @@ -66,11 +71,11 @@ public double getAppliedOutput() { } public double getVelocity() { - return motorSim.getAngularVelocityRPM(); + return motorSim.getAngularVelocityRPM() * conversionFactor; } public double getPosition() { - return motorSim.getAngularPositionRotations(); + return motorSim.getAngularPositionRotations() * conversionFactor; } public double getOutputCurrent() { diff --git a/src/main/java/lib/motors/TalonFXSim.java b/src/main/java/lib/motors/TalonFXSim.java index aa8d701e..bfd352e4 100644 --- a/src/main/java/lib/motors/TalonFXSim.java +++ b/src/main/java/lib/motors/TalonFXSim.java @@ -12,12 +12,17 @@ public class TalonFXSim extends SimMotor { private final Derivative acceleration = new Derivative(); - public TalonFXSim(LinearSystem model, int numMotors, double gearing) { - super(model, DCMotor.getFalcon500(numMotors), gearing); + public TalonFXSim( + LinearSystem model, + int numMotors, + double gearing, + double conversionFactor) { + super(model, DCMotor.getFalcon500(numMotors), gearing, conversionFactor); } - public TalonFXSim(int numMotors, double gearing, double jKgMetersSquared) { - super(DCMotor.getFalcon500(numMotors), jKgMetersSquared, gearing); + public TalonFXSim( + int numMotors, double gearing, double jKgMetersSquared, double conversionFactor) { + super(DCMotor.getFalcon500(numMotors), jKgMetersSquared, gearing, conversionFactor); } public void setControl(DutyCycleOut request) { @@ -69,12 +74,12 @@ public void setControl(MotionMagicVoltage request) { this.setControl(new VoltageOut(voltageRequest + request.FeedForward)); } - public double getRotorVelocity() { - return Units.rpmToRps(motorSim.getAngularVelocityRPM()); + public double getVelocity() { + return Units.rpmToRps(motorSim.getAngularVelocityRPM()) * conversionFactor; } - public double getRotorPosition() { - return motorSim.getAngularPositionRotations(); + public double getPosition() { + return motorSim.getAngularPositionRotations() * conversionFactor; } public double getAcceleration() {