diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4013790..4300e97 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,12 +7,14 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.Transporter.Transporter; - public class RobotContainer { + public static final Intake INTAKE = new Intake(); public static final Transporter TRANSPORTER = new Transporter(); + public RobotContainer() { configureBindings(); } 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 0000000..a561842 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase { + private final TalonSRX intakeMotor = IntakeConstants.MOTOR; + + void stopMotor() { + setTargetDutyCycleOutput(0); + } + + void setTargetDutyCycleOutput(double motorOutput) { + intakeMotor.set(ControlMode.PercentOutput, motorOutput); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeCommands.java b/src/main/java/frc/robot/subsystems/Intake/IntakeCommands.java new file mode 100644 index 0000000..999550e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeCommands.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import frc.robot.RobotContainer; + +public class IntakeCommands { + public static Command getSetTargetStateCommand(IntakeConstants.IntakeState targetState) { + return getSetMotorOutputCommand(targetState.targetMotorOutput); + } + + private static Command getSetMotorOutputCommand(double motorOutput) { + return new StartEndCommand( + () -> RobotContainer.INTAKE.setTargetDutyCycleOutput(motorOutput), + RobotContainer.INTAKE::stopMotor, + RobotContainer.INTAKE + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java new file mode 100644 index 0000000..875ad6e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeConstants.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +public class IntakeConstants { + private static final int MOTOR_ID = 5; + + static final WPI_TalonSRX MOTOR = new WPI_TalonSRX(MOTOR_ID); + + private static final NeutralMode NEUTRAL_MODE = NeutralMode.Brake; + private static final InvertType INVERTED_VALUE = InvertType.None; + private static final double VOLTAGE_COMPENSATION_VALUE = 12; + + static { + configureMotor(); + } + + private static void configureMotor() { + MOTOR.configFactoryDefault(); + + MOTOR.setNeutralMode(NEUTRAL_MODE); + MOTOR.setInverted(INVERTED_VALUE); + MOTOR.enableVoltageCompensation(true); + MOTOR.configVoltageCompSaturation(VOLTAGE_COMPENSATION_VALUE); + } + + public enum IntakeState { + COLLECT(0.5), + EJECT(-0.5), + REST(0); + + final double targetMotorOutput; + + IntakeState(double Output) { + this.targetMotorOutput = Output; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Transporter/Transporter.java b/src/main/java/frc/robot/subsystems/Transporter/Transporter.java index 22ea6ae..31d2b6c 100644 --- a/src/main/java/frc/robot/subsystems/Transporter/Transporter.java +++ b/src/main/java/frc/robot/subsystems/Transporter/Transporter.java @@ -16,5 +16,4 @@ void setTargetPercentageVoltageOutput(double percentageMotorOutput) { rightMotor.set(ControlMode.PercentOutput, percentageMotorOutput); leftMotor.set(ControlMode.PercentOutput, percentageMotorOutput); } -} - +} \ No newline at end of file