diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d684ab9e..c374bbbb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,10 +103,11 @@ public RobotContainer() { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo()); // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo()); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createRealIo()); + intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo(), - IntakeSubsystem.createRealDeploymentSwitch()); + intakeDeployer); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); - intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); + climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); @@ -124,14 +125,15 @@ public RobotContainer() { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo()); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createMockIo()); + intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo(), - IntakeSubsystem.createMockDeploymentSwitch()); + intakeDeployer); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); - intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo()); + // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem( @@ -144,14 +146,15 @@ public RobotContainer() { // tiltSubsystem = new // TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer)); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createSimIo(robotVisualizer)); + intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createSimIo(robotVisualizer)); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer), - IntakeSubsystem.createSimDeploymentSwitch()); + intakeDeployer); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); - intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createSimIo(robotVisualizer)); + // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) @@ -295,7 +298,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Intake", - new SpinIntake(intakeSubsystem)); + new SpinIntake(intakeSubsystem, intakeDeployer)); SmartDashboard.putData( "Start Hopper", @@ -304,9 +307,6 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Spin Feeder", new SpinFeeder(feederSubsystem)); - SmartDashboard.putData( - "Spin Intake", - new SpinIntake(intakeSubsystem)); SmartDashboard.putData( "Start Hopper", diff --git a/src/main/java/frc/robot/commands/intake/SpinIntake.java b/src/main/java/frc/robot/commands/intake/SpinIntake.java index b3dbd04b..3c5b03f8 100644 --- a/src/main/java/frc/robot/commands/intake/SpinIntake.java +++ b/src/main/java/frc/robot/commands/intake/SpinIntake.java @@ -3,16 +3,20 @@ import javax.lang.model.util.ElementScanner14; import frc.robot.constants.Constants; +import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; public class SpinIntake extends LoggableCommand { private final IntakeSubsystem subsystem; + private final IntakeDeployerSubsystem deployer; - public SpinIntake(IntakeSubsystem subsystem) { + public SpinIntake(IntakeSubsystem subsystem, IntakeDeployerSubsystem deployer) { this.subsystem = subsystem; addRequirements(subsystem); + this.deployer = deployer; } @Override @@ -21,7 +25,7 @@ public void initialize() { @Override public void execute() { - if (subsystem.isDeployed()) { + if (deployer.deploymentState == DeploymentState.DOWN) { subsystem.setSpeed(Constants.INTAKE_SPEED); } else { subsystem.setSpeed(0); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 5e009a65..26f1b59b 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -7,18 +7,12 @@ import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.intake.SpinIntake; import frc.robot.constants.Constants; -import frc.robot.utils.logging.input.DigitalInputLoggableInputs; import frc.robot.utils.logging.input.MotorLoggableInputs; -import frc.robot.utils.logging.io.motor.DigitalInputIo; -import frc.robot.utils.logging.io.motor.MockDigitalInputIo; import frc.robot.utils.logging.io.motor.MockSparkMaxIo; -import frc.robot.utils.logging.io.motor.RealDigitalInputIo; import frc.robot.utils.logging.io.motor.RealSparkMaxIo; -import frc.robot.utils.logging.io.motor.SimDigitalInputIo; import frc.robot.utils.logging.io.motor.SimSparkMaxIo; import frc.robot.utils.logging.io.motor.SparkMaxIo; import frc.robot.utils.simulation.MotorSimulator; @@ -28,12 +22,12 @@ public class IntakeSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "IntakeSubsystem"; private final SparkMaxIo io; - private final DigitalInputIo intakeDeploymentSwitch; + private final IntakeDeployerSubsystem deployer; - public IntakeSubsystem(SparkMaxIo io, DigitalInputIo intakeDeploymentSwitch) { + public IntakeSubsystem(SparkMaxIo io, IntakeDeployerSubsystem deployer) { this.io = io; - this.intakeDeploymentSwitch = intakeDeploymentSwitch; - setDefaultCommand(new SpinIntake(this)); + this.deployer = deployer; + setDefaultCommand(new SpinIntake(this, deployer)); } public void setSpeed(double speed) { @@ -47,11 +41,6 @@ public void stopMotors() { @Override public void periodic() { io.periodic(); - intakeDeploymentSwitch.periodic(); - } - - public boolean isDeployed() { - return intakeDeploymentSwitch.isPressed(); } public static SparkMaxIo createMockIo() { @@ -66,30 +55,6 @@ public static SparkMaxIo createSimIo(RobotVisualizer visualizer) { new MotorSimulator(motor, visualizer.getIntakeLigament())); } - public static DigitalInputIo createMockDeploymentSwitch() { - return new MockDigitalInputIo( - LOGGING_NAME + "/DeploymentSwitch", - new DigitalInputLoggableInputs() - ); -} - -public static DigitalInputIo createRealDeploymentSwitch() { - return new RealDigitalInputIo( - LOGGING_NAME + "/DeploymentSwitch", - new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL), - new DigitalInputLoggableInputs() - ); - -} - -public static DigitalInputIo createSimDeploymentSwitch() { - return new SimDigitalInputIo( - LOGGING_NAME + "/DeploymentSwitch", - new DigitalInput(Constants.INTAKE_DIGITAL_INPUT_CHANNEL), - new DigitalInputLoggableInputs() - ); -} - private static SparkMax createMotor() { SparkMax motor = new SparkMax(Constants.INTAKE_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); SparkMaxConfig motorConfig = new SparkMaxConfig(); @@ -102,7 +67,4 @@ private static SparkMax createMotor() { return motor; } - public DigitalInputIo getDeploymentSwitchIo() { - return intakeDeploymentSwitch; - } } diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java index 16aee79f..3daef7c0 100644 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RollerSubsystem.java @@ -21,8 +21,6 @@ import frc.robot.utils.simulation.MotorSimulator; import frc.robot.utils.simulation.RobotVisualizer; -// The Roller subsystem spins the wheel that releases the algae. - public class RollerSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "RollerSubsystem"; private final SparkMaxIo io; diff --git a/src/main/java/frc/robot/subsystems/TiltSubsystem.java b/src/main/java/frc/robot/subsystems/TiltSubsystem.java index d9b0f00b..161837b5 100644 --- a/src/main/java/frc/robot/subsystems/TiltSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TiltSubsystem.java @@ -22,8 +22,6 @@ import frc.robot.utils.simulation.ArmSimulator; import frc.robot.utils.simulation.RobotVisualizer; -// The Tilt subsystem extends and retracts the Algae-Go-Bye-Bye mechanism. - public class TiltSubsystem extends SubsystemBase { public static final String LOGGING_NAME = "TiltSubsystem"; private final SparkMaxIo io;