fix: made simulation somewhat working

flamingchickens1540 · Jan 29, 2024 · b906548 · b906548
1 parent 32361a9
commit b906548
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 7 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -57,11 +57,11 @@ public static class Drivetrain {
     }
     public static class Indexer {
         // TODO: fix these constants
-        public static final int INTAKE_ID = 0;
-        public static final int FEEDER_ID = 0;
-        public static final double FEEDER_KP = 0.0;
-        public static final double FEEDER_KI = 0.0;
-        public static final double FEEDER_KD = 0.0;
+        public static final int INTAKE_ID = 11;
+        public static final int FEEDER_ID = 12;
+        public static final double FEEDER_KP = 0.5;
+        public static final double FEEDER_KI = 0.1;
+        public static final double FEEDER_KD = 0.001;
         public static final double FEEDER_KS = 0.0;
         public static final double FEEDER_KV = 0.0;
         public static final double FEEDER_GEAR_RATIO = 1.0;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
@@ -55,7 +55,7 @@ public Module(ModuleIO io, int index) {
 
     public void periodic() {
         io.updateInputs(inputs);
-        Logger.processInputs("Drive/Module" + index, inputs);
+        Logger.processInputs("Drivetrain/Module" + index, inputs);
 
         // On first cycle, reset relative turn encoder
         // Wait until absolute angle is nonzero in case it wasn't initialized yet

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java
@@ -5,11 +5,18 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.util.LoggedTunableNumber;
+import static org.team1540.robot2024.Constants.Indexer.*;
 
 
 public class Indexer extends SubsystemBase {
     private final IndexerIO indexerIO;
     private final IndexerIOInputsAutoLogged indexerInputs = new IndexerIOInputsAutoLogged();
+    private final boolean TUNING = true;
+    private final LoggedTunableNumber kP = new LoggedTunableNumber("Indexer/kP", FEEDER_KP);
+    private final LoggedTunableNumber kI = new LoggedTunableNumber("Indexer/kI", FEEDER_KI);
+    private final LoggedTunableNumber kD = new LoggedTunableNumber("Indexer/kD", FEEDER_KD);
+
 
     public Indexer(IndexerIO indexerIO) {
         this.indexerIO = indexerIO;
@@ -18,6 +25,11 @@ public Indexer(IndexerIO indexerIO) {
     public void periodic() {
         indexerIO.updateInputs(indexerInputs);
         Logger.processInputs("Indexer", indexerInputs);
+        if (TUNING) {
+            if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
+                indexerIO.configurePID(kP.get(), kI.get(), kD.get());
+            }
+        }
     }
 
     public void setIntakePercent(double percent) {

diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIO.java
@@ -13,6 +13,9 @@ class IndexerIOInputs {
         public double feederCurrentAmps;
         public double feederVelocityRPM;
         public boolean noteInIntake = false;
+        public double setpoint;
+        public double feederVelocityRadPerSec;
+        public double feederPositionError;
     }
 
     /**
@@ -26,4 +29,6 @@ default void setFeederVoltage(double volts) {}
 
     default void setFeederVelocity(double velocity) {}
 
+    default void configurePID(double p, double i, double d) {}
+
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/IndexerIOSim.java
@@ -28,9 +28,12 @@ public void updateInputs(IndexerIOInputs inputs) {
 //        inputs.feederVoltage = feederSim.getBusVoltage() * feederSim.getAppliedOutput();
         inputs.feederVelocityRPM = feederSim.getAngularVelocityRPM();
 //        inputs.noteInIntake = beamBreakSim.getBoolean("Indexer Beam Break").get();
+        inputs.setpoint = feederSimPID.getSetpoint();
+        inputs.feederVelocityRadPerSec = feederSim.getAngularVelocityRadPerSec();
+        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(),  feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec()));
+        feederSim.setState(feederSim.getAngularPositionRad(), feederSim.getAngularVelocityRadPerSec() + feederSimPID.calculate(feederSim.getAngularVelocityRadPerSec()));
 
     }
 
@@ -44,6 +47,11 @@ public void setFeederVelocity(double velocity) {
         feederSimPID.setSetpoint(velocity);
     }
 
+    @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));

diff --git a/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java b/src/main/java/org/team1540/robot2024/util/LoggedTunableNumber.java
@@ -0,0 +1,21 @@
+package org.team1540.robot2024.util;
+
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+public class LoggedTunableNumber extends LoggedDashboardNumber {
+    private double lastHasChangedValue;
+
+    public LoggedTunableNumber(String key, double defaultValue) {
+        super(key, defaultValue);
+        lastHasChangedValue = defaultValue;
+    }
+
+    public boolean hasChanged() {
+        double currentValue = get();
+        if (currentValue != lastHasChangedValue) {
+            lastHasChangedValue = currentValue;
+            return true;
+        }
+        return false;
+    }
+}