Attempt at implementing io logging for subsystems

Only intake is implemented like how the examples show. The implementations for the elevator and drivebase are incorrect and likely will not work

src/main/java/frc/robot/autonomous/commands/N6_1Cone.java

@@ -4,18 +4,13 @@
 
 package frc.robot.autonomous.commands;
 
-import com.pathplanner.lib.PathPlannerTrajectory;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
 import frc.robot.Constants.Elevator;
 import frc.robot.commands.ElevatorPositionCommand;
-import frc.robot.commands.EngageCommand;
-import frc.robot.commands.FollowTrajectoryCommand;
 import frc.robot.commands.IntakeModeCommand;
 import frc.robot.commands.ScoreCommand;
 import frc.robot.commands.SetZeroModeCommand;
-import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.Modes;
@@ -24,9 +19,7 @@
 
 public class N6_1Cone extends SequentialCommandGroup {
   /** Creates a new N2MobilityEngage. */
-  public N6_1Cone(
-      IntakeSubsystem intakeSubsystem,
-      ElevatorSubsystem elevatorSubsystem) {
+  public N6_1Cone(IntakeSubsystem intakeSubsystem, ElevatorSubsystem elevatorSubsystem) {
 
     addCommands(
         new SetZeroModeCommand(elevatorSubsystem)

src/main/java/frc/robot/commands/DefaultDriveCommand.java

@@ -4,7 +4,6 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.DrivebaseSubsystem;

src/main/java/frc/robot/commands/IntakeModeCommand.java

@@ -6,8 +6,8 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants.Intake;
+import frc.robot.subsystems.IntakeIOTalonFX.Modes;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.Modes;
 import java.util.function.BooleanSupplier;
 
 public class IntakeModeCommand extends CommandBase {
@@ -17,7 +17,8 @@ public class IntakeModeCommand extends CommandBase {
   private boolean isGroundIntake;
 
   /** Creates a new IntakeCommand. */
-  public IntakeModeCommand(IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone, boolean isGroundIntake) {
+  public IntakeModeCommand(
+      IntakeSubsystem intakeSubsystem, Modes mode, BooleanSupplier isCone, boolean isGroundIntake) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.intakeSubsystem = intakeSubsystem;
     this.mode = mode;
@@ -60,12 +61,12 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if(!isGroundIntake) {
+    if (!isGroundIntake) {
       if (intakeSubsystem.getMode() == Modes.HOLD || intakeSubsystem.getMode() == Modes.OFF) {
         return true;
       }
     } else {
-      if(intakeSubsystem.getFilterOutput() > Intake.GROUND_CUBE_STATOR_LIMIT) {
+      if (intakeSubsystem.getFilterOutput() > Intake.GROUND_CUBE_STATOR_LIMIT) {
         return true;
       }
     }

src/main/java/frc/robot/subsystems/DrivebaseSubsystemIO.java

@@ -46,6 +46,6 @@ public static class DrivebaseSubsystemIOInputs {
   public default void updateInputs(DrivebaseSubsystemIOInputs inputs) {}
 
   /** Run open loop at the specified voltage. */
-  public default void setVoltage(
-      double frontLeftVolts, double backLeftVolts, double frontRightVolts, double backRightVolts) {}
+  public default void setMotorPower(
+      double frontLeft, double backLeft, double frontRight, double backRight) {}
 }

src/main/java/frc/robot/subsystems/ElevatorIO.java

@@ -4,15 +4,29 @@
 
 public interface ElevatorIO {
   @AutoLog
-  public static class FlywheelIOInputs {
-    public double positionRad = 0.0;
-    public double velocityRadPerSec = 0.0;
-    public double appliedVolts = 0.0;
-    public double[] currentAmps = new double[] {};
+  public static class ElevatorIOInputs {
+
+    public double currentExtension = 0.0;
+    public double targetExtension = 0.0;
+    public double currentWristAngle = 0.0;
+    public double targetAngle = 0.0;
+    public double elevatorPercentControl = 0.0;
+    public double wristPercentControl = 0.0;
+    private double elevatorFilterOutput = 0.0;
+    private double wristFilterOutput = 0.0;
+
+    public boolean elevatorZeroed = false;
+    public boolean wristZeroed = false;
+
+    public enum Modes {
+      PERCENT_CONTROL,
+      POSITION_CONTROL,
+      ZERO
+    }
   }
 
   /** Updates the set of loggable inputs. */
-  public default void updateInputs(FlywheelIOInputs inputs) {}
+  public default void updateInputs(ElevatorIOInputs inputs) {}
 
   /** Run closed loop at the specified velocity. */
   public default void setVelocity(double velocityRadPerSec, double ffVolts) {}

src/main/java/frc/robot/subsystems/ElevatorSubsystem.java

@@ -22,6 +22,9 @@
 /** Add your docs here. */
 public class ElevatorSubsystem extends SubsystemBase {
 
+  private final ElevatorIO io;
+  private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
+
   public enum Modes {
     PERCENT_CONTROL,
     POSITION_CONTROL,
@@ -66,7 +69,9 @@ public enum Modes {
 
   public static record ElevatorState(double extension, double angle) {}
 
-  public ElevatorSubsystem() {
+  public ElevatorSubsystem(ElevatorIO io) {
+
+    this.io = io;
 
     currentMode = Modes.ZERO;
 
@@ -87,15 +92,15 @@ public ElevatorSubsystem() {
     leftMotor.setSelectedSensorPosition(0);
     wristMotor.setSelectedSensorPosition(0);
 
-    currentExtension = 0.0;
-    targetExtension = 0.0;
-    currentWristAngle = 0.0;
-    targetAngle = 0.0;
-    elevatorPercentControl = 0.0;
-    wristPercentControl = 0.0;
+    currentExtension = inputs.currentExtension;
+    targetExtension = inputs.targetExtension;
+    currentWristAngle = inputs.currentWristAngle;
+    targetAngle = inputs.targetAngle;
+    elevatorPercentControl = inputs.elevatorPercentControl;
+    wristPercentControl = inputs.wristPercentControl;
 
-    elevatorZeroed = false;
-    wristZeroed = false;
+    elevatorZeroed = inputs.elevatorZeroed;
+    wristZeroed = inputs.wristZeroed;
 
     rightMotor.configFactoryDefault();
     leftMotor.configFactoryDefault();

src/main/java/frc/robot/subsystems/IntakeIO.java

@@ -0,0 +1,22 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public double currentExtension = 0.0;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+    public double[] currentAmps = new double[] {};
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(IntakeIOInputs inputs) {}
+
+  /** Run closed loop at the specified velocity. */
+  public default void setMotorPower(double power) {}
+
+  /** Stop in open loop. */
+  public default void stop() {}
+}

src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class IntakeIOTalonFX extends SubsystemBase {
+  private TalonFX intakeMotor;
+  private ShuffleboardTab shuffleboard = Shuffleboard.getTab("Intake Subsystem");
+  private Modes currentIntakeMode;
+  private LinearFilter filter;
+  private double filterOutput;
+  private boolean isCone;
+  // private final TimeOfFlight coneToF, cubeToF;
+
+  /** Creates a new IntakeIOTalonFX. */
+  public IntakeIOTalonFX() {
+    intakeMotor = new TalonFX(Constants.Intake.Ports.INTAKE_MOTOR_PORT);
+
+    intakeMotor.configFactoryDefault();
+    intakeMotor.clearStickyFaults();
+
+    intakeMotor.setNeutralMode(NeutralMode.Brake);
+    intakeMotor.setInverted(true);
+
+    filter = LinearFilter.movingAverage(30);
+
+    currentIntakeMode = Modes.OFF;
+
+    filterOutput = 0.0;
+
+    isCone = false;
+  }
+
+  public enum Modes {
+    INTAKE,
+    OUTTAKE,
+    HOLD,
+    OFF;
+  }
+
+  public void setMotorPower(double power) {
+    intakeMotor.set(TalonFXControlMode.PercentOutput, power);
+  }
+
+  public double getMotorPower() {
+    return intakeMotor.getMotorOutputPercent();
+  }
+
+  public Modes getMode() {
+    return currentIntakeMode;
+  }
+
+  public void setMode(Modes hold) {
+    currentIntakeMode = hold;
+  }
+
+  public void setIsCone(boolean isCone) {
+    this.isCone = isCone;
+  }
+
+  public boolean getIsCone() {
+    return isCone;
+  }
+
+  public double getFilterOutput() {
+    return filterOutput;
+  }
+}