fix: made simulations work correctly(?)

Author: autumnwilkes

src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java

@@ -26,7 +26,7 @@ public void periodic() {
         indexerIO.updateInputs(indexerInputs);
         Logger.processInputs("Indexer", indexerInputs);
         if (TUNING) {
-            if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
+            if (kP.hasChanged(hashCode()) || kI.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
                 indexerIO.configurePID(kP.get(), kI.get(), kD.get());
             }
         }

src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java

@@ -13,8 +13,7 @@ class IndexerIOInputs {
         public double feederCurrentAmps;
         public double feederVelocityRPM;
         public boolean noteInIntake = false;
-        public double setpoint;
-        public double feederVelocityRadPerSec;
+        public double setpointRPM;
         public double feederPositionError;
     }
 

src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java

@@ -16,44 +16,51 @@ public class IndexerIOSim implements IndexerIO {
     private final DCMotorSim feederSim = new DCMotorSim(DCMotor.getNEO(1), FEEDER_GEAR_RATIO,0.025);
 //    private final SimDeviceSim beamBreakSim = new SimDeviceSim("Indexer Beam Break");
     private final PIDController feederSimPID = new PIDController(FEEDER_KP, FEEDER_KI,FEEDER_KD);
+    private boolean isClosedLoop = true;
+    private double intakeVoltage = 0.0;
+    private double feederVoltage = 0.0;
+    private double feederVelocityRPS = 0.0;
 
     @Override
     public void updateInputs(IndexerIOInputs inputs) {
+        if (isClosedLoop) {
+            feederVoltage = MathUtil.clamp(feederSimPID.calculate(feederSim.getAngularVelocityRPM() / 60, feederVelocityRPS), -12.0, 12.0);
+        }
+        intakeSim.setInputVoltage(intakeVoltage);
+        feederSim.setInputVoltage(feederVoltage);
         intakeSim.update(Constants.LOOP_PERIOD_SECS);
         feederSim.update(Constants.LOOP_PERIOD_SECS);
         inputs.intakeCurrentAmps = intakeSim.getCurrentDrawAmps();
-//        inputs.intakeVoltage = intakeSim.getBusVoltage() * intakeSim.getAppliedOutput();
+        inputs.intakeVoltage = intakeVoltage;
         inputs.intakeVelocityRPM = intakeSim.getAngularVelocityRPM();
         inputs.feederCurrentAmps = feederSim.getCurrentDrawAmps();
-//        inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput();
+        inputs.feederVoltage = feederVoltage;
         inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
 //        inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
-        inputs.setpoint = feederSimPID.getSetpoint();
-        inputs.feederVelocityRadPerSec = feederSim.getAngularVelocityRadPerSec();
+        inputs.setpointRPM = feederSimPID.getSetpoint() * 60;
         inputs.feederPositionError = feederSimPID.getPositionError();
-
-        // this is a very funny line of code, and absolutely does not belong here, but I don't know how to do this otherwise
-        feederSim.setState(feederSim.getAngularPositionRad(), feederSim.getAngularVelocityRadPerSec() + feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec()));
-
     }
 
     @Override
     public void setIntakeVoltage(double volts) {
-        intakeSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
-    }
-
-    @Override
-    public void setFeederVelocity(double velocity) {
-        feederSimPID.setSetpoint(velocity);
+        intakeVoltage = MathUtil.clamp(volts, -12.0, 12.0);
     }
 
     @Override
     public void configurePID(double p, double i, double d) {
         feederSimPID.setPID(p, i, d);
     }
 
-    @Override
     public void setFeederVoltage(double volts) {
-        feederSim.setInputVoltage(MathUtil.clamp(volts, -12, 12));
+        isClosedLoop = false;
+        feederVoltage = MathUtil.clamp(volts, -12.0, 12.0);
+    }
+
+
+    public void setFeederVelocity(double velocity) {
+        isClosedLoop = true;
+        feederSimPID.reset();
+        feederVelocityRPS = velocity / 60;
     }
+
 }

src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSparkMax.java

@@ -2,7 +2,6 @@
 
 import com.revrobotics.*;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DigitalInput;
 
 import static org.team1540.robot2024.Constants.Indexer.*;
@@ -14,6 +13,7 @@ public class IndexerIOSparkMax implements IndexerIO {
     private final DigitalInput indexerBeamBreak = new DigitalInput(0);
     private final SparkPIDController feederPID;
     private final SimpleMotorFeedforward feederFF = new SimpleMotorFeedforward(FEEDER_KS, FEEDER_KV);
+    private double setpointRPM;
 
 
     public IndexerIOSparkMax() {
@@ -29,9 +29,6 @@ public IndexerIOSparkMax() {
         feederPID.setP(FEEDER_KP, 0);
         feederPID.setI(FEEDER_KI, 0);
         feederPID.setD(FEEDER_KD, 0);
-
-
-
     }
 
     @Override
@@ -43,6 +40,8 @@ public void updateInputs(IndexerIOInputs inputs) {
         inputs.feederVoltage = feederMotor.getBusVoltage() * feederMotor.getAppliedOutput();
         inputs.feederVelocityRPM = feederMotor.getEncoder().getVelocity();
         inputs.noteInIntake = indexerBeamBreak.get();
+        inputs.setpointRPM = setpointRPM;
+        inputs.feederPositionError = setpointRPM - feederMotor.getEncoder().getVelocity();
     }
 
     @Override
@@ -57,8 +56,9 @@ public void setFeederVoltage(double volts) {
 
     @Override
     public void setFeederVelocity(double velocity) {
+        setpointRPM = velocity;
         feederPID.setReference(
-                Units.radiansPerSecondToRotationsPerMinute(velocity) * FEEDER_GEAR_RATIO,
+                velocity / FEEDER_GEAR_RATIO, // Should this be multiplied?
                 CANSparkBase.ControlType.kVelocity,
                 0,
                 feederFF.calculate(velocity),