Add conversion factor to motor sim

Galaxia5987 · Jan 22, 2024 · ba1a817 · ba1a817
1 parent 98f2ba0
commit ba1a817
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 23 deletions.
diff --git 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
@@ -18,12 +18,24 @@ public class SimMotor {
     protected double lastTimestampSeconds = 0;
     protected double voltageRequest = 0;
 
-    public SimMotor(LinearSystem<N2, N1, N2> model, DCMotor motor, double gearing) {
+    protected final double conversionFactor;
+
+    public SimMotor(
+            LinearSystem<N2, N1, N2> 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
@@ -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<N2, N1, N2> model, int numMotors, double gearing) {
-        super(model, DCMotor.getNEO(numMotors), gearing);
+    public SparkMaxSim(
+            LinearSystem<N2, N1, N2> 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
@@ -12,12 +12,17 @@ public class TalonFXSim extends SimMotor {
 
     private final Derivative acceleration = new Derivative();
 
-    public TalonFXSim(LinearSystem<N2, N1, N2> model, int numMotors, double gearing) {
-        super(model, DCMotor.getFalcon500(numMotors), gearing);
+    public TalonFXSim(
+            LinearSystem<N2, N1, N2> 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() {