Rewrite sim motors to support lingering setpoint

Galaxia5987 · Jan 23, 2024 · a2c3f5c · a2c3f5c
1 parent 33ff5f8
commit a2c3f5c
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 33 deletions.
diff --git a/src/main/java/lib/motors/SimMotor.java b/src/main/java/lib/motors/SimMotor.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import java.util.function.DoubleSupplier;
 
 public class SimMotor {
 
@@ -16,7 +17,7 @@ public class SimMotor {
     protected ProfiledPIDController profiledController = null;
 
     protected double lastTimestampSeconds = 0;
-    protected double voltageRequest = 0;
+    protected MotorSetpoint voltageRequest = MotorSetpoint.simpleVoltage(0);
 
     protected final double conversionFactor;
 
@@ -47,8 +48,15 @@ public void setProfiledController(ProfiledPIDController profiledController) {
     }
 
     public void update(double timestampSeconds) {
+        motorSim.setInputVoltage(voltageRequest.getAsDouble());
         motorSim.update(timestampSeconds - lastTimestampSeconds);
-
         lastTimestampSeconds = timestampSeconds;
     }
+
+    protected interface MotorSetpoint extends DoubleSupplier {
+
+        static MotorSetpoint simpleVoltage(double voltage) {
+            return () -> voltage;
+        }
+    }
 }
diff --git a/src/main/java/lib/motors/SparkMaxSim.java b/src/main/java/lib/motors/SparkMaxSim.java
@@ -31,32 +31,35 @@ public void set(double speed) {
     }
 
     public void setInputVoltage(double voltage) {
-        voltageRequest = voltage;
-        motorSim.setInputVoltage(voltageRequest);
+        voltageRequest = MotorSetpoint.simpleVoltage(voltage);
     }
 
     public void setReference(double value, CANSparkMax.ControlType ctrl) {
         setReference(value, ctrl, 0);
     }
 
+    private void setInputVoltage(MotorSetpoint voltage) {
+        voltageRequest = voltage;
+    }
+
     public void setReference(double value, CANSparkMax.ControlType ctrl, double arbFeedforward) {
         switch (ctrl) {
             case kDutyCycle:
                 set(value);
                 break;
             case kPosition:
-                setInputVoltage(controller.calculate(getPosition(), value) + arbFeedforward);
+                setInputVoltage(() -> controller.calculate(getPosition(), value) + arbFeedforward);
                 break;
             case kSmartMotion:
                 setInputVoltage(
-                        profiledController.calculate(getPosition(), value) + arbFeedforward);
+                        () -> profiledController.calculate(getPosition(), value) + arbFeedforward);
                 break;
             case kVelocity:
-                setInputVoltage(controller.calculate(getVelocity(), value) + arbFeedforward);
+                setInputVoltage(() -> controller.calculate(getVelocity(), value) + arbFeedforward);
                 break;
             case kSmartVelocity:
                 setInputVoltage(
-                        profiledController.calculate(getVelocity(), value) + arbFeedforward);
+                        () -> profiledController.calculate(getVelocity(), value) + arbFeedforward);
                 break;
             case kVoltage:
                 setInputVoltage(value);
@@ -68,11 +71,11 @@ public void setReference(double value, CANSparkMax.ControlType ctrl, double arbF
     }
 
     public double getBusVoltage() {
-        return voltageRequest;
+        return voltageRequest.getAsDouble();
     }
 
     public double getAppliedOutput() {
-        return voltageRequest / 12.0;
+        return voltageRequest.getAsDouble() / 12.0;
     }
 
     public double getVelocity() {

diff --git a/src/main/java/lib/motors/TalonFXSim.java b/src/main/java/lib/motors/TalonFXSim.java
@@ -30,53 +30,49 @@ public TalonFXSim(
         super(DCMotor.getFalcon500(numMotors), jKgMetersSquared, gearing, conversionFactor);
     }
 
+    @Override
+    public void update(double timestampSeconds) {
+        super.update(timestampSeconds);
+
+        acceleration.update(getVelocity(), timestampSeconds);
+    }
+
     public void setControl(DutyCycleOut request) {
-        this.setControl(new VoltageOut(request.Output * 12));
+        setControl(new VoltageOut(request.Output * 12));
     }
 
     public void setControl(VoltageOut request) {
-        voltageRequest = request.Output;
-        motorSim.setInputVoltage(voltageRequest);
+        voltageRequest = MotorSetpoint.simpleVoltage(request.Output);
     }
 
     public void setControl(PositionDutyCycle request) {
-        voltageRequest =
-                controller.calculate(motorSim.getAngularPositionRotations(), request.Position);
-        this.setControl(new VoltageOut(voltageRequest + 12 * request.FeedForward));
+        setControl(new PositionVoltage(request.Position).withFeedForward(request.FeedForward * 12));
     }
 
     public void setControl(PositionVoltage request) {
         voltageRequest =
-                controller.calculate(motorSim.getAngularPositionRotations(), request.Position);
-        this.setControl(new VoltageOut(voltageRequest + request.FeedForward));
+                () -> controller.calculate(getPosition(), request.Position) + request.FeedForward;
     }
 
     public void setControl(VelocityDutyCycle request) {
-        voltageRequest =
-                controller.calculate(
-                        Units.rpmToRps(motorSim.getAngularVelocityRPM()), request.Velocity);
-        this.setControl(new VoltageOut(voltageRequest + 12 * request.FeedForward));
+        setControl(new VelocityVoltage(request.Velocity).withFeedForward(request.FeedForward * 12));
     }
 
     public void setControl(VelocityVoltage request) {
         voltageRequest =
-                controller.calculate(
-                        Units.rpmToRps(motorSim.getAngularVelocityRPM()), request.Velocity);
-        this.setControl(new VoltageOut(voltageRequest + request.FeedForward));
+                () -> controller.calculate(getVelocity(), request.Velocity) + request.FeedForward;
     }
 
     public void setControl(MotionMagicDutyCycle request) {
-        voltageRequest =
-                profiledController.calculate(
-                        motorSim.getAngularPositionRotations(), request.Position);
-        this.setControl(new VoltageOut(voltageRequest + 12 * request.FeedForward));
+        setControl(
+                new MotionMagicVoltage(request.Position).withFeedForward(request.FeedForward * 12));
     }
 
     public void setControl(MotionMagicVoltage request) {
         voltageRequest =
-                profiledController.calculate(
-                        motorSim.getAngularPositionRotations(), request.Position);
-        this.setControl(new VoltageOut(voltageRequest + request.FeedForward));
+                () ->
+                        profiledController.calculate(getPosition(), request.Position)
+                                + request.FeedForward;
     }
 
     public double getVelocity() {
@@ -96,6 +92,6 @@ public double getAppliedCurrent() {
     }
 
     public double getAppliedVoltage() {
-        return voltageRequest;
+        return voltageRequest.getAsDouble();
     }
 }