TorqueCurrent Sim Control Fix

src/main/java/org/trigon/hardware/phoenix6/talonfx/io/SimulationTalonFXIO.java

@@ -26,8 +26,8 @@ public void updateMotor() {
             return;
         physicsSimulation.setInputVoltage(motorSimState.getMotorVoltage());
         physicsSimulation.updateMotor();
-        motorSimState.setRawRotorPosition(physicsSimulation.getPositionRotations());
-        motorSimState.setRotorVelocity(physicsSimulation.getVelocityRotationsPerSecond());
+        motorSimState.setRawRotorPosition(physicsSimulation.getRotorPositionRotations());
+        motorSimState.setRotorVelocity(physicsSimulation.getRotorVelocityRotationsPerSecond());
     }
 
     @Override
@@ -43,9 +43,11 @@ protected void setPosition(double positionRotations) {
     @Override
     public void applyConfiguration(TalonFXConfiguration configuration) {
         configuration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-        configuration.Feedback.SensorToMechanismRatio = 1;
-        configuration.Feedback.RotorToSensorRatio = 1;
         configuration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor;
+        if (configuration.Feedback.RotorToSensorRatio != 1.0) {
+            configuration.Feedback.SensorToMechanismRatio *= configuration.Feedback.RotorToSensorRatio;
+            configuration.Feedback.RotorToSensorRatio = 1.0;
+        }
         talonFX.getConfigurator().apply(configuration);
     }
 

src/main/java/org/trigon/hardware/simulation/ElevatorSimulation.java

@@ -11,6 +11,7 @@ public class ElevatorSimulation extends MotorPhysicsSimulation {
     private final double diameterMeters;
 
     public ElevatorSimulation(DCMotor gearbox, double gearRatio, double carriageMassKilograms, double drumRadiusMeters, double retractedHeightMeters, double maximumHeightMeters, boolean simulateGravity) {
+        super(gearRatio);
         diameterMeters = drumRadiusMeters + drumRadiusMeters;
         this.retractedHeightMeters = retractedHeightMeters;
         elevatorSimulation = new ElevatorSim(
@@ -31,12 +32,12 @@ public double getCurrent() {
     }
 
     @Override
-    public double getPositionRotations() {
+    public double getSystemPositionRotations() {
         return Conversions.distanceToRotations(elevatorSimulation.getPositionMeters() - retractedHeightMeters, diameterMeters);
     }
 
     @Override
-    public double getVelocityRotationsPerSecond() {
+    public double getSystemVelocityRotationsPerSecond() {
         return Conversions.distanceToRotations(elevatorSimulation.getVelocityMetersPerSecond(), diameterMeters);
     }
 

src/main/java/org/trigon/hardware/simulation/FlywheelSimulation.java

@@ -12,10 +12,12 @@ public class FlywheelSimulation extends MotorPhysicsSimulation {
     private double lastPositionRadians = 0;
 
     public FlywheelSimulation(DCMotor gearbox, double gearRatio, double momentOfInertia) {
+        super(gearRatio);
         flywheelSimulation = new FlywheelSim(gearbox, gearRatio, momentOfInertia);
     }
 
     public FlywheelSimulation(DCMotor gearbox, double gearRatio, double kv, double ka) {
+        super(gearRatio);
         flywheelSimulation = new FlywheelSim(LinearSystemId.identifyVelocitySystem(kv, ka), gearbox, gearRatio);
     }
 
@@ -25,12 +27,12 @@ public double getCurrent() {
     }
 
     @Override
-    public double getPositionRotations() {
+    public double getSystemPositionRotations() {
         return Units.radiansToRotations(lastPositionRadians);
     }
 
     @Override
-    public double getVelocityRotationsPerSecond() {
+    public double getSystemVelocityRotationsPerSecond() {
         return Units.radiansToRotations(flywheelSimulation.getAngularVelocityRadPerSec());
     }
 

src/main/java/org/trigon/hardware/simulation/MotorPhysicsSimulation.java

@@ -4,11 +4,25 @@
  * An abstract class to simulate the physics of a motor.
  */
 public abstract class MotorPhysicsSimulation {
+    private final double gearRatio;
+
+    MotorPhysicsSimulation(double gearRatio) {
+        this.gearRatio = gearRatio;
+    }
+
+    public double getRotorPositionRotations() {
+        return getSystemPositionRotations() * gearRatio;
+    }
+
+    public double getRotorVelocityRotationsPerSecond() {
+        return getSystemVelocityRotationsPerSecond() * gearRatio;
+    }
+
     public abstract double getCurrent();
 
-    public abstract double getPositionRotations();
+    public abstract double getSystemPositionRotations();
 
-    public abstract double getVelocityRotationsPerSecond();
+    public abstract double getSystemVelocityRotationsPerSecond();
 
     public abstract void setInputVoltage(double voltage);
 

src/main/java/org/trigon/hardware/simulation/SimpleMotorSimulation.java

@@ -10,6 +10,7 @@ public class SimpleMotorSimulation extends MotorPhysicsSimulation {
     private final DCMotorSim motorSimulation;
 
     public SimpleMotorSimulation(DCMotor gearbox, double gearRatio, double momentOfInertia) {
+        super(gearRatio);
         motorSimulation = new DCMotorSim(gearbox, gearRatio, momentOfInertia);
     }
 
@@ -19,12 +20,12 @@ public double getCurrent() {
     }
 
     @Override
-    public double getPositionRotations() {
+    public double getSystemPositionRotations() {
         return Units.radiansToRotations(motorSimulation.getAngularPositionRad());
     }
 
     @Override
-    public double getVelocityRotationsPerSecond() {
+    public double getSystemVelocityRotationsPerSecond() {
         return Units.radiansToRotations(motorSimulation.getAngularVelocityRadPerSec());
     }
 

src/main/java/org/trigon/hardware/simulation/SingleJointedArmSimulation.java

@@ -11,6 +11,7 @@ public class SingleJointedArmSimulation extends MotorPhysicsSimulation {
     private final SingleJointedArmSim armSimulation;
 
     public SingleJointedArmSimulation(DCMotor gearbox, double gearRatio, double armLengthMeters, double armMassKilograms, Rotation2d minimumAngle, Rotation2d maximumAngle, boolean simulateGravity) {
+        super(gearRatio);
         armSimulation = new SingleJointedArmSim(
                 gearbox,
                 gearRatio,
@@ -29,12 +30,12 @@ public double getCurrent() {
     }
 
     @Override
-    public double getPositionRotations() {
+    public double getSystemPositionRotations() {
         return Units.radiansToRotations(armSimulation.getAngleRads());
     }
 
     @Override
-    public double getVelocityRotationsPerSecond() {
+    public double getSystemVelocityRotationsPerSecond() {
         return Units.radiansToRotations(armSimulation.getVelocityRadPerSec());
     }