Merge pull request #8 from Programming-TRIGON/debugging

levyishai · web-flow · commit 9bc7a693ca21 · 2024-09-03T20:55:22.000+03:00
TorqueCurrent Sim Control Fix
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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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());
     }
 

Original file line number	Diff line number	Diff line change
`@@ -11,6 +11,7 @@ public class ElevatorSimulation extends MotorPhysicsSimulation {`
`11`	`11`	`private final double diameterMeters;`
`12`	`12`
`13`	`13`	`public ElevatorSimulation(DCMotor gearbox, double gearRatio, double carriageMassKilograms, double drumRadiusMeters, double retractedHeightMeters, double maximumHeightMeters, boolean simulateGravity) {`
	`14`	`+ super(gearRatio);`
`14`	`15`	`diameterMeters = drumRadiusMeters + drumRadiusMeters;`
`15`	`16`	`this.retractedHeightMeters = retractedHeightMeters;`
`16`	`17`	`elevatorSimulation = new ElevatorSim(`
`@@ -31,12 +32,12 @@ public double getCurrent() {`
`31`	`32`	`}`
`32`	`33`
`33`	`34`	`@Override`
`34`		`- public double getPositionRotations() {`
	`35`	`+ public double getSystemPositionRotations() {`
`35`	`36`	`return Conversions.distanceToRotations(elevatorSimulation.getPositionMeters() - retractedHeightMeters, diameterMeters);`
`36`	`37`	`}`
`37`	`38`
`38`	`39`	`@Override`
`39`		`- public double getVelocityRotationsPerSecond() {`
	`40`	`+ public double getSystemVelocityRotationsPerSecond() {`
`40`	`41`	`return Conversions.distanceToRotations(elevatorSimulation.getVelocityMetersPerSecond(), diameterMeters);`
`41`	`42`	`}`
`42`	`43`
Original file line number	Diff line number	Diff line change
`@@ -12,10 +12,12 @@ public class FlywheelSimulation extends MotorPhysicsSimulation {`
`12`	`12`	`private double lastPositionRadians = 0;`
`13`	`13`
`14`	`14`	`public FlywheelSimulation(DCMotor gearbox, double gearRatio, double momentOfInertia) {`
	`15`	`+ super(gearRatio);`
`15`	`16`	`flywheelSimulation = new FlywheelSim(gearbox, gearRatio, momentOfInertia);`
`16`	`17`	`}`
`17`	`18`
`18`	`19`	`public FlywheelSimulation(DCMotor gearbox, double gearRatio, double kv, double ka) {`
	`20`	`+ super(gearRatio);`
`19`	`21`	`flywheelSimulation = new FlywheelSim(LinearSystemId.identifyVelocitySystem(kv, ka), gearbox, gearRatio);`
`20`	`22`	`}`
`21`	`23`
`@@ -25,12 +27,12 @@ public double getCurrent() {`
`25`	`27`	`}`
`26`	`28`
`27`	`29`	`@Override`
`28`		`- public double getPositionRotations() {`
	`30`	`+ public double getSystemPositionRotations() {`
`29`	`31`	`return Units.radiansToRotations(lastPositionRadians);`
`30`	`32`	`}`
`31`	`33`
`32`	`34`	`@Override`
`33`		`- public double getVelocityRotationsPerSecond() {`
	`35`	`+ public double getSystemVelocityRotationsPerSecond() {`
`34`	`36`	`return Units.radiansToRotations(flywheelSimulation.getAngularVelocityRadPerSec());`
`35`	`37`	`}`
`36`	`38`
Original file line number	Diff line number	Diff line change
`@@ -10,6 +10,7 @@ public class SimpleMotorSimulation extends MotorPhysicsSimulation {`
`10`	`10`	`private final DCMotorSim motorSimulation;`
`11`	`11`
`12`	`12`	`public SimpleMotorSimulation(DCMotor gearbox, double gearRatio, double momentOfInertia) {`
	`13`	`+ super(gearRatio);`
`13`	`14`	`motorSimulation = new DCMotorSim(gearbox, gearRatio, momentOfInertia);`
`14`	`15`	`}`
`15`	`16`
`@@ -19,12 +20,12 @@ public double getCurrent() {`
`19`	`20`	`}`
`20`	`21`
`21`	`22`	`@Override`
`22`		`- public double getPositionRotations() {`
	`23`	`+ public double getSystemPositionRotations() {`
`23`	`24`	`return Units.radiansToRotations(motorSimulation.getAngularPositionRad());`
`24`	`25`	`}`
`25`	`26`
`26`	`27`	`@Override`
`27`		`- public double getVelocityRotationsPerSecond() {`
	`28`	`+ public double getSystemVelocityRotationsPerSecond() {`
`28`	`29`	`return Units.radiansToRotations(motorSimulation.getAngularVelocityRadPerSec());`
`29`	`30`	`}`
`30`	`31`
Original file line number	Diff line number	Diff line change
`@@ -11,6 +11,7 @@ public class SingleJointedArmSimulation extends MotorPhysicsSimulation {`
`11`	`11`	`private final SingleJointedArmSim armSimulation;`
`12`	`12`
`13`	`13`	`public SingleJointedArmSimulation(DCMotor gearbox, double gearRatio, double armLengthMeters, double armMassKilograms, Rotation2d minimumAngle, Rotation2d maximumAngle, boolean simulateGravity) {`
	`14`	`+ super(gearRatio);`
`14`	`15`	`armSimulation = new SingleJointedArmSim(`
`15`	`16`	`gearbox,`
`16`	`17`	`gearRatio,`
`@@ -29,12 +30,12 @@ public double getCurrent() {`
`29`	`30`	`}`
`30`	`31`
`31`	`32`	`@Override`
`32`		`- public double getPositionRotations() {`
	`33`	`+ public double getSystemPositionRotations() {`
`33`	`34`	`return Units.radiansToRotations(armSimulation.getAngleRads());`
`34`	`35`	`}`
`35`	`36`
`36`	`37`	`@Override`
`37`		`- public double getVelocityRotationsPerSecond() {`
	`38`	`+ public double getSystemVelocityRotationsPerSecond() {`
`38`	`39`	`return Units.radiansToRotations(armSimulation.getVelocityRadPerSec());`
`39`	`40`	`}`
`40`	`41`