diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 65d4fce0..eeefec16 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,14 @@ public static final class ElevatorWrist { public static final int TALON_ID = -1; public static final int NEO_ID = -1; } + + /** + * Intake and indexer motor constants + */ + public static final class Intake { + public static final int INTAKE_MOTOR_ID = -1; + public static final int INDEXER_MOTOR_ID = -1; + } } /** @@ -64,6 +72,4 @@ public static final class ElevatorWristConstants { */ public static final class Pneumatics { } - - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 62bc61b2..e5c9f730 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,9 @@ import frc.robot.subsystems.drive.Drivetrain; import frc.robot.subsystems.drive.DrivetrainIO; import frc.robot.subsystems.drive.DrivetrainVictorSP; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOFalcon; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -29,6 +32,7 @@ public class RobotContainer { /* Subsystems */ private Drivetrain drivetrain; + private Intake intake; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -39,12 +43,14 @@ public RobotContainer(RobotRunType runtimeType) { switch (runtimeType) { case kReal: drivetrain = new Drivetrain(new DrivetrainVictorSP()); + intake = new Intake(new IntakeIOFalcon()); break; case kSimulation: // drivetrain = new Drivetrain(new DrivetrainSim() {}); break; default: drivetrain = new Drivetrain(new DrivetrainIO() {}); + intake = new Intake(new IntakeIO() {}); } // Configure the button bindings configureButtonBindings(); @@ -56,7 +62,9 @@ public RobotContainer(RobotRunType runtimeType) { * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() {} + private void configureButtonBindings() { + operator.a().whileTrue(intake.runIntakeMotor()); + } /** * Gets the user's selected autonomous command. diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 00000000..4660418a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Intake Subsystem + */ +public class Intake extends SubsystemBase { + private IntakeIO io; + private IntakeInputsAutoLogged intakeAutoLogged = new IntakeInputsAutoLogged(); + + public Intake(IntakeIO io) { + this.io = io; + io.updateInputs(intakeAutoLogged); + } + + @Override + public void periodic() { + io.updateInputs(intakeAutoLogged); + Logger.processInputs("Intake", intakeAutoLogged); + } + + public void setIntakeMotor(double percentage) { + Logger.recordOutput("/Intake/Intake Percentage", percentage); + io.setIntakeMotorPercentage(percentage); + } + + public void setIndexerMotor(double percentage) { + Logger.recordOutput("/Intake/Indexer Percentage", percentage); + io.setIndexerMotorPercentage(percentage); + } + + public boolean getSensorStatus() { + return intakeAutoLogged.sensorStatus; + } + + /** + * Command to run the intake motor and indexer until the sensor trips + * + * @return {@link Command} to run the intake and indexer motors + */ + public Command runIntakeMotor() { + return Commands.startEnd(() -> { + setIntakeMotor(0.5); + setIndexerMotor(0.5); + }, () -> { + setIntakeMotor(0); + setIndexerMotor(0); + }, this).until(() -> getSensorStatus()).unless(() -> getSensorStatus()); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 00000000..af0d4bda --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +/** + * Intake IO Interface + */ +public interface IntakeIO { + + /** + * Intake Inputs to Log + */ + @AutoLog + public static class IntakeInputs { + public double intakeSupplyVoltage; + public double indexerSupplyVoltage; + public double intakeMotorVoltage; + public double indexerMotorVoltage; + public double intakeAmps; + public double indexerAmps; + public double intakeRPM; + public double indexerRPM; + public boolean sensorStatus; + } + + public default void updateInputs(IntakeInputs inputs) {} + + public default void setIntakeMotorPercentage(double percent) {} + + public default void setIndexerMotorPercentage(double percent) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java new file mode 100644 index 00000000..bb6acba4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOFalcon.java @@ -0,0 +1,47 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.Constants; + +/** + * Intake IO Layer with real motors and sensors + */ +public class IntakeIOFalcon implements IntakeIO { + + private final TalonFX intakeMotor = + new TalonFX(Constants.Motors.Intake.INTAKE_MOTOR_ID, "canivore"); + private final TalonFX indexerMotor = + new TalonFX(Constants.Motors.Intake.INDEXER_MOTOR_ID, "canivore"); + + private final DutyCycleOut intakeDutyCycleOut = new DutyCycleOut(0); + private final DutyCycleOut indexerDutyCycleOut = new DutyCycleOut(0); + + /** + * Intake IO Layer with real motors and sensors + */ + public IntakeIOFalcon() {} + + @Override + public void updateInputs(IntakeInputs inputs) { + inputs.intakeSupplyVoltage = intakeMotor.getSupplyVoltage().getValueAsDouble(); + inputs.intakeMotorVoltage = intakeMotor.getMotorVoltage().getValueAsDouble(); + inputs.intakeAmps = intakeMotor.getStatorCurrent().getValueAsDouble(); + inputs.intakeRPM = intakeMotor.getVelocity().getValueAsDouble(); + inputs.indexerSupplyVoltage = indexerMotor.getSupplyVoltage().getValueAsDouble(); + inputs.indexerMotorVoltage = indexerMotor.getMotorVoltage().getValueAsDouble(); + inputs.indexerAmps = indexerMotor.getStatorCurrent().getValueAsDouble(); + inputs.indexerRPM = indexerMotor.getVelocity().getValueAsDouble(); + inputs.sensorStatus = false; + } + + @Override + public void setIntakeMotorPercentage(double percent) { + intakeMotor.setControl(intakeDutyCycleOut.withOutput(percent)); + } + + @Override + public void setIndexerMotorPercentage(double percent) { + indexerMotor.setControl(indexerDutyCycleOut.withOutput(percent)); + } +}