diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c866910..b25b4935 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,9 +25,9 @@ import frc.robot.subsystems.swerve.moduleIO.ModuleInterface; import frc.robot.subsystems.swerve.moduleIO.PhysicalModule; import frc.robot.subsystems.swerve.moduleIO.SimulatedModule; +import frc.robot.subsystems.vision.PhysicalVision; import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOReal; +import frc.robot.subsystems.vision.VisionInterface; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -67,7 +67,7 @@ public RobotContainer() { new PhysicalModule(SwerveConstants.moduleConfigs[1]), new PhysicalModule(SwerveConstants.moduleConfigs[2]), new PhysicalModule(SwerveConstants.moduleConfigs[3])); - visionSubsystem = new Vision(new VisionIOReal()); + visionSubsystem = new Vision(new PhysicalVision()); } case SIM -> { @@ -106,7 +106,7 @@ public RobotContainer() { new SimulatedModule(swerveDriveSimulation.getModules()[3])); // TODO: add sim impl - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = new Vision(new VisionInterface() {}); SimulatedField.getInstance().resetFieldForAuto(); resetFieldAndOdometryForAuto( @@ -114,7 +114,7 @@ public RobotContainer() { } default -> { - visionSubsystem = new Vision(new VisionIO() {}); + visionSubsystem = new Vision(new VisionInterface() {}); /* Replayed robot, disable IO implementations */ /* physics simulations are also not needed */ diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 36f0e305..25c76443 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -4,12 +4,12 @@ import org.littletonrobotics.junction.Logger; public class Elevator extends SubsystemBase { - private final ElevatorIO io; - private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private final ElevatorInterface elevatorInterface; + private final ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged(); - public Elevator(ElevatorIO io) { + public Elevator(ElevatorInterface elevatorInterface) { super("Elevator"); - this.io = io; + this.elevatorInterface = elevatorInterface; } /** @@ -19,7 +19,7 @@ public Elevator(ElevatorIO io) { */ public void setElevatorPosition(double position) { Logger.recordOutput("Elevator", position); - io.setElevatorPosition(position); + elevatorInterface.setElevatorPosition(position); } /** @@ -28,12 +28,12 @@ public void setElevatorPosition(double position) { * @param speed output value from -1.0 to 1.0 */ public void setElevatorSpeed(double speed) { - io.setElevatorSpeed(speed); + elevatorInterface.setElevatorSpeed(speed); } @Override public void periodic() { - io.updateInputs(inputs); + elevatorInterface.updateInputs(inputs); Logger.processInputs("Elevator", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java similarity index 85% rename from src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java rename to src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java index cfb9dc1f..6a2f6a7d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -2,9 +2,9 @@ import org.littletonrobotics.junction.AutoLog; -public interface ElevatorIO { +public interface ElevatorInterface { @AutoLog - public static class ElevatorIOInputs { + public static class ElevatorInputs { public boolean isConnected = false; public double leaderMotorPosition = 0.0; @@ -18,7 +18,7 @@ public static class ElevatorIOInputs { public double followerMotorCurrentAmps = 0.0; } - public default void updateInputs(ElevatorIOInputs inputs) {} + public default void updateInputs(ElevatorInputs inputs) {} public default double getElevatorPosition() { return 0.0; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java similarity index 96% rename from src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java rename to src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index e317aa1f..29c86bac 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -17,8 +17,9 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.HardwareConstants; +import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs; -public class ElevatorIOTalonFX implements ElevatorIO { +public class PhysicalElevator implements ElevatorInterface { private final TalonFX leaderElevatorMotor; private final TalonFX followerElevatorMotor; @@ -36,7 +37,7 @@ public class ElevatorIOTalonFX implements ElevatorIO { StatusSignal followerAppliedVolts; StatusSignal followerCurrentAmps; - public ElevatorIOTalonFX() { + public PhysicalElevator() { leaderElevatorMotor = new TalonFX(0); followerElevatorMotor = new TalonFX(0); @@ -106,7 +107,7 @@ public ElevatorIOTalonFX() { } @Override - public void updateInputs(ElevatorIOInputs inputs) { + public void updateInputs(ElevatorInputs inputs) { inputs.isConnected = BaseStatusSignal.isAllGood( leaderPosition, diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java similarity index 94% rename from src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java rename to src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index 56e83c86..935be464 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -9,13 +9,13 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.ElevatorSim; -public class ElevatorIOSim implements ElevatorIO { +public class SimulatedElevator implements ElevatorInterface { ElevatorSim elevatorSim; LinearSystem elevatorSystem; private double elevatorAppliedVolts = 0.0; private final PIDController elevatorPID = new PIDController(0.0, 0.0, 0.0); - public ElevatorIOSim() { + public SimulatedElevator() { this.elevatorSystem = LinearSystemId.createElevatorSystem(DCMotor.getFalcon500(2), 0, 0, 0); this.elevatorSim = new ElevatorSim( @@ -28,7 +28,7 @@ public ElevatorIOSim() { } @Override - public void updateInputs(ElevatorIOInputs inputs) { + public void updateInputs(ElevatorInputs inputs) { elevatorSim.update(0.02); inputs.isConnected = true; diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 4431710c..19214474 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -4,11 +4,11 @@ import org.littletonrobotics.junction.Logger; public class Indexer extends SubsystemBase { - private IndexerIO io; - private IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + private IndexerInterface indexerInterface; + private IndexerInterfaceInputsAutoLogged inputs = new IndexerInterfaceInputsAutoLogged(); - public Indexer(IndexerIO io) { - this.io = io; + public Indexer(IndexerInterface indexerInterface) { + this.indexerInterface = indexerInterface; } /** @@ -17,18 +17,18 @@ public Indexer(IndexerIO io) { * @param speed The desired speed (-1.0 to 1.0) */ public void setIndexerSpeed(double speed) { - io.setSpeed(speed); + indexerInterface.setSpeed(speed); Logger.recordOutput("Indexer", speed); } /** Stops the motor */ public void stop() { - io.stop(); + indexerInterface.stop(); } @Override public void periodic() { - io.updateInputs(inputs); + indexerInterface.updateInputs(inputs); Logger.processInputs("Indexer", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerInterface.java similarity index 79% rename from src/main/java/frc/robot/subsystems/indexer/IndexerIO.java rename to src/main/java/frc/robot/subsystems/indexer/IndexerInterface.java index 3b06c92f..0058bab9 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerInterface.java @@ -2,9 +2,9 @@ import org.littletonrobotics.junction.AutoLog; -public interface IndexerIO { +public interface IndexerInterface { @AutoLog - public static class IndexerIOInputs { + public static class IndexerInputs { public boolean isConnected = false; public double indexerVelocity = 0.0; public double indexerAppliedVolts = 0.0; @@ -13,7 +13,7 @@ public static class IndexerIOInputs { public double indexerSupplyCurrentAmps = 0.0; } - default void updateInputs(IndexerIOInputs inputs) {} + default void updateInputs(IndexerInputs inputs) {} default boolean isIndexing() { return false; diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java b/src/main/java/frc/robot/subsystems/indexer/PhysicalIndexer.java similarity index 95% rename from src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java rename to src/main/java/frc/robot/subsystems/indexer/PhysicalIndexer.java index bb2c0781..9b838a5b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/indexer/PhysicalIndexer.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.HardwareConstants; -public class IndexerIOTalonFX implements IndexerIO { +public class PhysicalIndexer implements IndexerInterface { private final TalonFX indexerMotor = new TalonFX(0); private final StatusSignal indexerVelocity; @@ -21,7 +21,7 @@ public class IndexerIOTalonFX implements IndexerIO { private final StatusSignal indexerSupplyCurrentAmps; private final StatusSignal indexerTemp; - public IndexerIOTalonFX() { + public PhysicalIndexer() { TalonFXConfiguration indexerConfiguration = new TalonFXConfiguration(); indexerConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; indexerConfiguration.MotorOutput.DutyCycleNeutralDeadband = @@ -52,7 +52,7 @@ public IndexerIOTalonFX() { } @Override - public void updateInputs(IndexerIOInputs inputs) { + public void updateInputs(IndexerInputs inputs) { inputs.indexerAppliedVolts = indexerAppliedVolts.getValueAsDouble(); inputs.indexerStatorCurrentAmps = indexerStatorCurrentAmps.getValueAsDouble(); inputs.indexerSupplyCurrentAmps = indexerSupplyCurrentAmps.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/SimulatedIndexer.java similarity index 86% rename from src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java rename to src/main/java/frc/robot/subsystems/indexer/SimulatedIndexer.java index 919ce350..418b5526 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java +++ b/src/main/java/frc/robot/subsystems/indexer/SimulatedIndexer.java @@ -3,12 +3,12 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; -public class IndexerIOSim implements IndexerIO { +public class SimulatedIndexer implements IndexerInterface { private final DCMotorSim indexerSim = new DCMotorSim(null, DCMotor.getFalcon500(1), 0); private double indexerAppliedVolts = 0.0; - public IndexerIOSim(IndexerIOInputs inputs) { + public SimulatedIndexer(IndexerInputs inputs) { indexerSim.update(0.02); inputs.isConnected = true; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 441025e0..18297d46 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,11 +5,11 @@ import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { - private IntakeIO io; - private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private IntakeInterface intakeInterface; + private IntakeInterfaceInputsAutoLogged inputs = new IntakeInterfaceInputsAutoLogged(); - public Intake(IntakeIO io) { - this.io = io; + public Intake(IntakeInterface intakeInterface) { + this.intakeInterface = intakeInterface; } /** @@ -19,7 +19,7 @@ public Intake(IntakeIO io) { */ public void setPivotAngle(double angle) { double angleRots = Units.degreesToRotations(angle); - io.setPivotPosition(angleRots); + intakeInterface.setPivotPosition(angleRots); Logger.recordOutput("OTBIntake/Pivot", angleRots); } @@ -29,21 +29,21 @@ public void setPivotAngle(double angle) { * @param speed intake roller speed (-1.0 to 1.0) */ public void setIntakeSpeed(double speed) { - io.setIntakeSpeed(speed); + intakeInterface.setIntakeSpeed(speed); Logger.recordOutput("OTBIntake/Intake", speed); } public void setPivotSpeed(double speed) { - io.setPivotSpeed(speed); + intakeInterface.setPivotSpeed(speed); } public void getPivotPosition() { - io.getPivotPosition(); + intakeInterface.getPivotPosition(); } @Override public void periodic() { - io.updateInputs(inputs); + intakeInterface.updateInputs(inputs); Logger.processInputs("OTBIntake", inputs); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java similarity index 86% rename from src/main/java/frc/robot/subsystems/intake/IntakeIO.java rename to src/main/java/frc/robot/subsystems/intake/IntakeInterface.java index cbfa74d6..55eb5ce4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeInterface.java @@ -2,9 +2,9 @@ import org.littletonrobotics.junction.AutoLog; -public interface IntakeIO { +public interface IntakeInterface { @AutoLog - public static class IntakeIOInputs { + public static class IntakeInputs { public boolean isConnected = true; // intake motor @@ -21,7 +21,7 @@ public static class IntakeIOInputs { public double pivotCurrentAmps = 0.0; } - default void updateInputs(IntakeIOInputs inputs) {} + default void updateInputs(IntakeInputs inputs) {} default void setPivotPosition(double position) {} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java similarity index 97% rename from src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java rename to src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java index ff6aa355..64514c2c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.Constants.HardwareConstants; -public class IntakeIOTalonFX implements IntakeIO { +public class PhysicalIntake implements IntakeInterface { private final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID); private final TalonFX leaderPivotMotor = new TalonFX(IntakeConstants.LEFT_INTAKE_PIVOT_MOTOR_ID); private final TalonFX followerPivotMotor = @@ -24,7 +24,7 @@ public class IntakeIOTalonFX implements IntakeIO { private final StatusSignal pivotPosition; private final StatusSignal pivotVelocity; - public IntakeIOTalonFX() { + public PhysicalIntake() { TalonFXConfiguration intakeConfig = new TalonFXConfiguration(); intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; @@ -82,7 +82,7 @@ public IntakeIOTalonFX() { } @Override - public void updateInputs(IntakeIOInputs inputs) { + public void updateInputs(IntakeInputs inputs) { inputs.intakeVelocity = intakeVelocity.getValueAsDouble(); inputs.pivotPosition = pivotPosition.getValueAsDouble(); inputs.pivotVelocity = pivotVelocity.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java similarity index 93% rename from src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java rename to src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java index ca79778e..849c8ebb 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/SimulatedIntake.java @@ -6,9 +6,8 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; -import frc.robot.subsystems.intake.IntakeIO.IntakeIOInputs; -public class IntakeIOSim implements IntakeIO { +public class SimulatedIntake implements IntakeInterface { DCMotorSim intakeSim = new DCMotorSim(null, DCMotor.getFalcon500(1), 0); SingleJointedArmSim pivotSim = new SingleJointedArmSim( @@ -22,7 +21,7 @@ public class IntakeIOSim implements IntakeIO { private double intakeAppliedVolts = 0.0; private double pivotAppliedVolts = 0.0; - public IntakeIOSim(IntakeIOInputs inputs) { + public SimulatedIntake(IntakeInputs inputs) { intakeSim.update(0.02); pivotSim.update(0.02); diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/pivot/PhysicalPivot.java similarity index 98% rename from src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java rename to src/main/java/frc/robot/subsystems/pivot/PhysicalPivot.java index 908f7c76..197d89f3 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/pivot/PhysicalPivot.java @@ -20,7 +20,7 @@ import frc.robot.Constants.HardwareConstants; /** Add your docs here. */ -public class PivotIOTalonFX implements PivotIO { +public class PhysicalPivot implements PivotInterface { private final TalonFX leaderPivotMotor; private final TalonFX followerPivotMotor; @@ -45,7 +45,7 @@ public class PivotIOTalonFX implements PivotIO { private final VoltageOut voltageControl = new VoltageOut(0); - public PivotIOTalonFX() { + public PhysicalPivot() { leaderPivotMotor = new TalonFX(PivotConstants.LEADER_PIVOT_MOTOR_ID); followerPivotMotor = new TalonFX(PivotConstants.FOLLOWER_PIVOT_MOTOR_ID); pivotEncoder = new CANcoder(PivotConstants.PIVOT_ENCODER_ID); @@ -115,7 +115,7 @@ public PivotIOTalonFX() { } @Override - public void updateInputs(PivotIOInputs inputs) { + public void updateInputs(PivotInputs inputs) { inputs.leaderPosition = leaderPivotMotor.getPosition().getValueAsDouble(); inputs.leaderVelocity = leaderPivotMotor.getVelocity().getValueAsDouble(); inputs.leaderAppliedVolts = leaderPivotMotor.getMotorVoltage().getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java index 0b6faac1..633ceace 100644 --- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ b/src/main/java/frc/robot/subsystems/pivot/Pivot.java @@ -1,22 +1,20 @@ package frc.robot.subsystems.pivot; -import static edu.wpi.first.units.Units.*; - import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.extras.interpolators.SingleLinearInterpolator; import org.littletonrobotics.junction.Logger; public class Pivot extends SubsystemBase { - private final PivotIO io; - private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + private final PivotInterface pivotInterface; + private final PivotInterfaceInputsAutoLogged inputs = new PivotInterfaceInputsAutoLogged(); private final SingleLinearInterpolator speakerAngleLookupValues; private final SingleLinearInterpolator speakerOverDefenseAngleLookupValues; private final SingleLinearInterpolator passAngleLookupValues; /** Creates a new Pivot. */ - public Pivot(PivotIO io) { - this.io = io; + public Pivot(PivotInterface pivotInterface) { + this.pivotInterface = pivotInterface; speakerAngleLookupValues = new SingleLinearInterpolator(PivotConstants.SPEAKER_PIVOT_POSITION); speakerOverDefenseAngleLookupValues = new SingleLinearInterpolator(PivotConstants.SPEAKER_OVER_DEFENSE_PIVOT_POSITION); @@ -29,7 +27,7 @@ public Pivot(PivotIO io) { * @param angle the desired angle in rotations */ public void setPivotAngle(double angle) { - io.setPivotAngle(angle); + pivotInterface.setPivotAngle(angle); } /** @@ -70,7 +68,7 @@ public void setPivotFromSpeakerDistanceOverDefense(double speakerDistance) { * @param output output value from -1.0 to 1.0 */ public void setPivotSpeed(double output) { - io.setPivotSpeed(output); + pivotInterface.setPivotSpeed(output); } /** @@ -79,7 +77,7 @@ public void setPivotSpeed(double output) { * @return pivot error between desired and actual state in rotations */ public boolean isPivotWithinAcceptableError() { - return io.isPivotWithinAcceptableError(); + return pivotInterface.isPivotWithinAcceptableError(); } /** @@ -88,7 +86,7 @@ public boolean isPivotWithinAcceptableError() { * @return angle of pivot in rotations */ public double getAngle() { - return io.getAngle(); + return pivotInterface.getAngle(); } /** @@ -97,7 +95,7 @@ public double getAngle() { * @return the target angle */ public double getPivotTarget() { - return io.getPivotTarget(); + return pivotInterface.getPivotTarget(); } /** @@ -106,12 +104,12 @@ public double getPivotTarget() { * @param volts the voltage */ public void runVolts(double volts) { - io.setVoltage(volts); + pivotInterface.setVoltage(volts); } @Override public void periodic() { - io.updateInputs(inputs); + pivotInterface.updateInputs(inputs); Logger.processInputs("Pivot", inputs); // This method will be called once per scheduler run } @@ -119,6 +117,6 @@ public void periodic() { @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation - io.updateInputs(inputs); + pivotInterface.updateInputs(inputs); } } diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotInterface.java similarity index 89% rename from src/main/java/frc/robot/subsystems/pivot/PivotIO.java rename to src/main/java/frc/robot/subsystems/pivot/PivotInterface.java index 5492ae44..f9858481 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/pivot/PivotInterface.java @@ -2,9 +2,9 @@ import org.littletonrobotics.junction.AutoLog; -public interface PivotIO { +public interface PivotInterface { @AutoLog - public static class PivotIOInputs { + public static class PivotInputs { public boolean leaderMotorConnected = true; public boolean followerMotorConnected = true; public double leaderPosition = 0.0; @@ -22,12 +22,7 @@ public static class PivotIOInputs { public double followerTempCelsius = 0.0; } - /** - * Updates Inputs - * - * @param inputs inputs for logging - */ - public default void updateInputs(PivotIOInputs inputs) {} + public default void updateInputs(PivotInputs inputs) {} /** * Gets the angle of the pivot diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/SimulatedPivot.java similarity index 96% rename from src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java rename to src/main/java/frc/robot/subsystems/pivot/SimulatedPivot.java index 10a2dc67..a9762c68 100644 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/pivot/SimulatedPivot.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import frc.robot.Constants.HardwareConstants; -public class PivotIOSim implements PivotIO { +public class SimulatedPivot implements PivotInterface { private final double pivotGearing = PivotConstants.PIVOT_GEAR_RATIO; private final double pivotMass = PivotConstants.PIVOT_MASS; private final double pivotLength = PivotConstants.PIVOT_LENGTH; @@ -37,7 +37,7 @@ public class PivotIOSim implements PivotIO { * @param inputs inputs for logging */ @Override - public void updateInputs(PivotIOInputs inputs) { + public void updateInputs(PivotInputs inputs) { pivotSim.update(HardwareConstants.TIMEOUT_S); inputs.leaderPosition = Units.radiansToRotations(pivotSim.getAngleRads()); diff --git a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java index ba81cd8f..b3a4d8fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/shooter/Flywheel.java @@ -4,13 +4,13 @@ import org.littletonrobotics.junction.Logger; public class Flywheel extends SubsystemBase { - private final FlywheelIO io; - private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); + private final FlywheelInterface flywheelInterface; + private final FlywheelInputsAutoLogged inputs = new FlywheelInputsAutoLogged(); // private final SimpleMotorFeedforward ffModel; - public Flywheel(FlywheelIO io) { - this.io = io; + public Flywheel(FlywheelInterface flywheelInterface) { + this.flywheelInterface = flywheelInterface; } /** @@ -19,7 +19,7 @@ public Flywheel(FlywheelIO io) { * @param desiredVoltage */ public void setFlywheelVoltage(double desiredVoltage) { - io.setVoltage(desiredVoltage); // /io calls the functions + flywheelInterface.setVoltage(desiredVoltage); // /flywheelInterface calls the functions Logger.recordOutput("Flywheel/voltage", desiredVoltage); } @@ -29,7 +29,7 @@ public void setFlywheelVoltage(double desiredVoltage) { * @param desiredRPM desired velocity in RPM */ public void setFlywheelVelocity(double desiredRPM) { - io.setVelocity(desiredRPM); + flywheelInterface.setVelocity(desiredRPM); Logger.recordOutput("Flywheel/RPM", desiredRPM); } @@ -43,20 +43,20 @@ public boolean hasNote() { * @param speed desired speed in rotations/sec */ public void setRollerSpeed(double speed) { - io.setRollerSpeed(speed); + flywheelInterface.setRollerSpeed(speed); Logger.recordOutput("Roller/DutyCycleOut", speed); } @Override public void periodic() { // This method will be called once per scheduler run - io.updateInputs(inputs); + flywheelInterface.updateInputs(inputs); Logger.processInputs("FlywheelSubsystem", inputs); } @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation - io.updateInputs(inputs); + flywheelInterface.updateInputs(inputs); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelInterface.java similarity index 83% rename from src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java rename to src/main/java/frc/robot/subsystems/shooter/FlywheelInterface.java index a0ac97f5..8e3254ab 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelInterface.java @@ -2,9 +2,9 @@ import org.littletonrobotics.junction.AutoLog; -public interface FlywheelIO { +public interface FlywheelInterface { @AutoLog - public static class FlywheelIOInputs { // variables that we want to log + public static class FlywheelInputs { // variables that we want to log public double positionRotations = 0.0; // positions in radians | convert to rpms public double velocityRPM = 0.0; public double appliedVolts = 0.0; @@ -13,7 +13,7 @@ public static class FlywheelIOInputs { // variables that we want to log } /** Updates the set of loggable inputs. */ - public default void updateInputs(FlywheelIOInputs inputs) {} + public default void updateInputs(FlywheelInputs inputs) {} /** Run open loop at the specified voltage. */ public default void setVoltage(double volts) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/PhysicalFlywheel.java similarity index 95% rename from src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java rename to src/main/java/frc/robot/subsystems/shooter/PhysicalFlywheel.java index 030a5ca0..629374ed 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/PhysicalFlywheel.java @@ -15,8 +15,9 @@ import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants.HardwareConstants; -public class FlywheelIOTalonFX - implements FlywheelIO { // FlywheelIOTalonFX makes Advantagekit log physical hardware movement +public class PhysicalFlywheel + implements FlywheelInterface { // FlywheelIOTalonFX makes Advantagekit log physical hardware + // movement private final TalonFX leftFlywheelMotor = new TalonFX(ShooterConstants.LEFT_FLYWHEEL_MOTOR_ID); // Leader=left motor private final TalonFX rightFlywheelMotor = @@ -38,7 +39,7 @@ public class FlywheelIOTalonFX // advantagekit log variables - public FlywheelIOTalonFX() { // Object to set different flywheel configs + public PhysicalFlywheel() { // Object to set different flywheel configs TalonFXConfiguration flywheelConfig = new TalonFXConfiguration(); flywheelConfig.CurrentLimits.SupplyCurrentLimit = ShooterConstants @@ -95,7 +96,7 @@ public FlywheelIOTalonFX() { // Object to set different flywheel configs */ @Override public void updateInputs( - FlywheelIOInputs inputs) { // gets current values for motors and puts them into a log + FlywheelInputs inputs) { // gets current values for motors and puts them into a log BaseStatusSignal.refreshAll( leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); inputs.positionRotations = diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/SimulatedFlywheel.java similarity index 90% rename from src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java rename to src/main/java/frc/robot/subsystems/shooter/SimulatedFlywheel.java index e4bc56be..e47ca06e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/SimulatedFlywheel.java @@ -7,8 +7,9 @@ import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; -public class FlywheelIOSim - implements FlywheelIO { // FlywheelIOSim makes Advantage kit log simulated movements using +public class SimulatedFlywheel + implements FlywheelInterface { // FlywheelIOSim makes Advantage kit log simulated movements + // using // physics private FlywheelSim flywheelSim = new FlywheelSim(null, DCMotor.getKrakenX60(2), 0); private DIOSim noteSensorSim = new DIOSim(new DigitalInput(ShooterConstants.NOTE_SENSOR_ID)); @@ -22,7 +23,7 @@ public class FlywheelIOSim private double appliedVolts = 0.0; @Override - public void updateInputs(FlywheelIOInputs inputs) { + public void updateInputs(FlywheelInputs inputs) { flywheelSim.update(0.02); inputs.positionRotations = 0.0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java similarity index 99% rename from src/main/java/frc/robot/subsystems/vision/VisionIOReal.java rename to src/main/java/frc/robot/subsystems/vision/PhysicalVision.java index aa4405d8..12f46414 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java @@ -14,7 +14,7 @@ import java.util.concurrent.TimeUnit; import java.util.concurrent.atomic.AtomicBoolean; -public class VisionIOReal implements VisionIO { +public class PhysicalVision implements VisionInterface { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; @@ -28,7 +28,7 @@ public class VisionIOReal implements VisionIO { */ private PoseEstimate[] limelightEstimates; - public VisionIOReal() { + public PhysicalVision() { limelightEstimates = new PoseEstimate[3]; for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { limelightThreads.put(limelightNumber, new AtomicBoolean(true)); @@ -37,7 +37,7 @@ public VisionIOReal() { } @Override - public void updateInputs(VisionIOInputs inputs) { + public void updateInputs(VisionInputs inputs) { inputs.camerasAmount = limelightEstimates.length; inputs.cameraConnected = true; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java similarity index 100% rename from src/main/java/frc/robot/subsystems/vision/VisionIOSim.java rename to src/main/java/frc/robot/subsystems/vision/SimulatedVision.java diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 72a4df2d..493763df 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -6,49 +6,49 @@ import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { - private final VisionIO visionIO; - private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged(); + private final VisionInterface visionInterface; + private final VisionInputsAutoLogged inputs = new VisionInputsAutoLogged(); - public Vision(VisionIO visionIO) { + public Vision(VisionInterface visionInterface) { // Initializing Fields - this.visionIO = visionIO; + this.visionInterface = visionInterface; } @Override public void periodic() { // Updates limelight inputs - visionIO.updateInputs(inputs); - Logger.processInputs(visionIO.getLimelightName(0), inputs); + visionInterface.updateInputs(inputs); + Logger.processInputs(visionInterface.getLimelightName(0), inputs); } // Add methods to support DriveCommand public int getNumberOfAprilTags(int limelightNumber) { - return visionIO.getNumberOfAprilTags(limelightNumber); + return visionInterface.getNumberOfAprilTags(limelightNumber); } public double getLimelightAprilTagDistance(int limelightNumber) { - return visionIO.getLimelightAprilTagDistance(limelightNumber); + return visionInterface.getLimelightAprilTagDistance(limelightNumber); } public double getTimeStampSeconds(int limelightNumber) { - return visionIO.getTimeStampSeconds(limelightNumber); + return visionInterface.getTimeStampSeconds(limelightNumber); } public double getLatencySeconds(int limelightNumber) { - return visionIO.getLatencySeconds(limelightNumber); + return visionInterface.getLatencySeconds(limelightNumber); } public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { - visionIO.setHeadingInfo(headingDegrees, headingRateDegrees); + visionInterface.setHeadingInfo(headingDegrees, headingRateDegrees); } @AutoLogOutput(key = "Vision/Has Targets") public boolean canSeeAprilTags(int limelightNumber) { - return visionIO.canSeeAprilTags( + return visionInterface.canSeeAprilTags( limelightNumber); // Assuming we're checking the shooter limelight } public Pose2d getPoseFromAprilTags(int limelightNumber) { - return visionIO.getPoseFromAprilTags(limelightNumber); + return visionInterface.getPoseFromAprilTags(limelightNumber); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java similarity index 89% rename from src/main/java/frc/robot/subsystems/vision/VisionIO.java rename to src/main/java/frc/robot/subsystems/vision/VisionInterface.java index 668cfeb0..f7f7f849 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java @@ -3,9 +3,9 @@ import edu.wpi.first.math.geometry.Pose2d; import org.littletonrobotics.junction.AutoLog; -public interface VisionIO { +public interface VisionInterface { @AutoLog - class VisionIOInputs { + class VisionInputs { public boolean cameraConnected = false; public double latency = 0.0; public double fiducialMarksID = 0.0; @@ -14,7 +14,7 @@ class VisionIOInputs { public int targetsCount = 0; } - default void updateInputs(VisionIOInputs inputs) {} + default void updateInputs(VisionInputs inputs) {} default String getLimelightName(int limelightNumber) { return "";