diff --git a/src/main/java/frc/robot/commands/IntakeModeCommand.java b/src/main/java/frc/robot/commands/IntakeModeCommand.java index 1c91549..3d31b76 100644 --- a/src/main/java/frc/robot/commands/IntakeModeCommand.java +++ b/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 { diff --git a/src/main/java/frc/robot/subsystems/IntakeIO.java b/src/main/java/frc/robot/subsystems/IntakeIO.java index c37658a..31e74ae 100644 --- a/src/main/java/frc/robot/subsystems/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/IntakeIO.java @@ -1,14 +1,15 @@ package frc.robot.subsystems; +import frc.robot.subsystems.IntakeSubsystem.Modes; 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[] {}; + public Modes currentIntakeMode = Modes.OFF; + public double filterOutput = 0.0; + public boolean isCone = false; + public double motorOutput = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java index 3b3a016..68c7ec1 100644 --- a/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/IntakeIOTalonFX.java @@ -16,7 +16,6 @@ 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; @@ -34,36 +33,20 @@ public IntakeIOTalonFX() { 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); } + // @AutoLogOutput 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; } @@ -75,4 +58,6 @@ public boolean getIsCone() { public double getFilterOutput() { return filterOutput; } + + public void updateInputs(IntakeIO inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 4ba0f72..601219d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -9,12 +9,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.Intake; import frc.robot.subsystems.IntakeIO.IntakeIOInputs; -import frc.robot.subsystems.IntakeIOTalonFX.Modes; public class IntakeSubsystem extends SubsystemBase { private final IntakeIO io; private final IntakeIOInputs inputs = new IntakeIOInputs(); private final IntakeIOTalonFX motorIO = new IntakeIOTalonFX(); + private Modes mode; private ShuffleboardTab shuffleboard = Shuffleboard.getTab("Intake Subsystem"); // private final TimeOfFlight coneToF, cubeToF; @@ -23,18 +23,25 @@ public class IntakeSubsystem extends SubsystemBase { public IntakeSubsystem(IntakeIO io) { this.io = io; - shuffleboard.addString("Current mode", () -> motorIO.getMode().toString()); + shuffleboard.addString("Current mode", () -> mode.toString()); shuffleboard.addDouble("filter output", () -> motorIO.getFilterOutput()); shuffleboard.addDouble("motor output", () -> motorIO.getMotorPower()); shuffleboard.addBoolean("is cube intake", () -> motorIO.getIsCone()); } + public enum Modes { + INTAKE, + OUTTAKE, + HOLD, + OFF; + } + public Modes getMode() { - return motorIO.getMode(); + return mode; } - public void setMode(frc.robot.subsystems.IntakeIOTalonFX.Modes hold) { - motorIO.setMode(hold); + public void setMode(Modes mode) { + this.mode = mode; } public void setIsCone(boolean isCone) { @@ -45,7 +52,7 @@ public double getFilterOutput() { return motorIO.getFilterOutput(); } - public void intakePeriodic(frc.robot.subsystems.IntakeIOTalonFX.Modes modes) { + public void intakePeriodic(Modes modes) { switch (modes) { case INTAKE: @@ -80,11 +87,11 @@ public void intakePeriodic(frc.robot.subsystems.IntakeIOTalonFX.Modes modes) { public void periodic() { double filterOutput = motorIO.getFilterOutput(); if (motorIO.getIsCone() && filterOutput >= Intake.CONE_STATOR_LIMIT) { - motorIO.setMode(Modes.HOLD); + mode = Modes.HOLD; } else if (filterOutput >= Intake.CUBE_STATOR_LIMIT) { - motorIO.setMode(Modes.HOLD); + mode = Modes.HOLD; } - intakePeriodic(motorIO.getMode()); + intakePeriodic(mode); } }