From 5d170226d38576c259397dd7224190ac935f217d Mon Sep 17 00:00:00 2001
From: Derek <derekchen1815@gmail.com>
Date: Thu, 7 Dec 2023 16:33:55 -0800
Subject: [PATCH] More logging added in subsystem

---
 src/main/java/frc/robot/subsystems/IntakeIO.java |  2 ++
 .../java/frc/robot/subsystems/IntakeIOSim.java   | 11 ++++++-----
 .../frc/robot/subsystems/IntakeIOTalonFX.java    | 16 +++++-----------
 .../frc/robot/subsystems/IntakeSubsystem.java    |  2 ++
 4 files changed, 15 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/IntakeIO.java b/src/main/java/frc/robot/subsystems/IntakeIO.java
index 82cdc8a..92c731f 100644
--- a/src/main/java/frc/robot/subsystems/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/IntakeIO.java
@@ -34,6 +34,8 @@ public default void updateInputs(IntakeIOInputs inputs) {}
   /** Run closed loop at the specified velocity. */
   public default void setMotorPower(double power) {}
 
+  public default void setIsCone(boolean isCone) {}
+
   /** Stop in open loop. */
   public default void stop() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/IntakeIOSim.java
index 9456ace..964b927 100644
--- a/src/main/java/frc/robot/subsystems/IntakeIOSim.java
+++ b/src/main/java/frc/robot/subsystems/IntakeIOSim.java
@@ -1,10 +1,11 @@
 package frc.robot.subsystems;
 
-import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 
 public class IntakeIOSim implements IntakeIO {
-  // Note: Intakes have no simulation feature, so this simulation class is only here as a
-  // placeholder. To see an actual simulation, go to
-  // https://github.dev/Mechanical-Advantage/RobotCode2022/blob/main/src/main/java/frc/robot/subsystems and look at the simulations that have been implemented
-  private FlywheelSim sim = new FlywheelSim(null, 0, 0);
+  // Note: Intakes have no simulation feature, so this simulation uses the DC motor sim instead. To
+  // see other implementations of simulations, go to
+  // https://github.dev/Mechanical-Advantage/RobotCode2022/blob/main/src/main/java/frc/robot/subsystems
+  // and look at the simulations that have been implemented
+  private DCMotorSim sim = new DCMotorSim(null, 1, 4.69);
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java
index bf8a6dc..b243902 100644
--- a/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java
@@ -53,18 +53,12 @@ public void setIsCone(boolean isCone) {
     this.isCone = isCone;
   }
 
-  public boolean getIsCone() {
-    return isCone;
-  }
-
-  public double getFilterOutput() {
-    return filterOutput;
-  }
-
   public void updateInputs(IntakeIOInputs inputs) {
-    inputs.currentIntakeMode = inputs.currentIntakeMode.toString();
-    inputs.filterOutput = getFilterOutput();
-    inputs.isCone = getIsCone();
+    inputs.currentIntakeMode =
+        inputs.currentIntakeMode
+            .toString(); // The modes are updated in the subsystem instead of here
+    inputs.filterOutput = filter.calculate(inputs.filterOutput);
+    inputs.isCone = inputs.isCone; // This is also updated in the subsystem
     inputs.motorOutput = getMotorPower();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 9b85bac..69290b6 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -86,6 +86,8 @@ public void intakePeriodic(Modes modes) {
 
   @Override
   public void periodic() {
+    inputs.currentIntakeMode = mode.toString();
+
     if (inputs.isCone && inputs.filterOutput >= Intake.CONE_STATOR_LIMIT) {
       mode = Modes.HOLD;
     } else if (inputs.filterOutput >= Intake.CUBE_STATOR_LIMIT) {