diff --git a/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/SimulationTalonFXIO.java b/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/SimulationTalonFXIO.java index b544806f..d623f055 100644 --- a/src/main/java/org/trigon/hardware/phoenix6/talonfx/io/SimulationTalonFXIO.java +++ b/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); } diff --git a/src/main/java/org/trigon/hardware/simulation/ElevatorSimulation.java b/src/main/java/org/trigon/hardware/simulation/ElevatorSimulation.java index 18b0de48..68a57822 100644 --- a/src/main/java/org/trigon/hardware/simulation/ElevatorSimulation.java +++ b/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); } diff --git a/src/main/java/org/trigon/hardware/simulation/FlywheelSimulation.java b/src/main/java/org/trigon/hardware/simulation/FlywheelSimulation.java index 1f28680c..650339c6 100644 --- a/src/main/java/org/trigon/hardware/simulation/FlywheelSimulation.java +++ b/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()); } diff --git a/src/main/java/org/trigon/hardware/simulation/MotorPhysicsSimulation.java b/src/main/java/org/trigon/hardware/simulation/MotorPhysicsSimulation.java index 134ab656..ab661058 100644 --- a/src/main/java/org/trigon/hardware/simulation/MotorPhysicsSimulation.java +++ b/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); diff --git a/src/main/java/org/trigon/hardware/simulation/SimpleMotorSimulation.java b/src/main/java/org/trigon/hardware/simulation/SimpleMotorSimulation.java index 9f74c994..ab03df11 100644 --- a/src/main/java/org/trigon/hardware/simulation/SimpleMotorSimulation.java +++ b/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()); } diff --git a/src/main/java/org/trigon/hardware/simulation/SingleJointedArmSimulation.java b/src/main/java/org/trigon/hardware/simulation/SingleJointedArmSimulation.java index da7bef9a..4907808a 100644 --- a/src/main/java/org/trigon/hardware/simulation/SingleJointedArmSimulation.java +++ b/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()); }