diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a65ad18..8174126 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,12 +4,13 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; - +//Imports from WPILib import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.Constants; +import frc.robot.subsystems.IntakeSubsystem; /** @@ -21,6 +22,7 @@ public class RobotContainer { XboxController xboxController; + public final IntakeSubsystem intake = new IntakeSubsystem(); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -30,7 +32,6 @@ public RobotContainer() { configureBindings(); } - /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..30352d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems; + + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +//import frc.robot.Constants; + +public class ElevatorSubsystem extends SubsystemBase { + public ElevatorSubsystem() { + // TODO: Set the default command, if any, for this subsystem by calling setDefaultCommand(command) + // in the constructor or in the robot coordination class, such as RobotContainer. + // Also, you can call addChild(name, sendableChild) to associate sendables with the subsystem + // such as SpeedControllers, Encoders, DigitalInputs, etc. + } +} + diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..a72ff33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems; + +//Imports from WPI Lib +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj.DoubleSolenoid; + +public class IntakeSubsystem extends SubsystemBase { + private final TalonFX intakeKraken; + private final DigitalInput beamBreak; + public final Trigger intakeOccupiedTrigger; + + public class IntakeConstants { + public static final int INTAKE_KRAKEN_ID = 6; + public static final InvertedValue INTAKE_INVERSION = InvertedValue.Clockwise_Positive; + public static final NeutralModeValue INTAKE_NEUTRAL_MODE = NeutralModeValue.Brake; + public static final double INTAKE_POSITION_STATUS_FRAME = 0.05; + public static final double INTAKE_VELOCITY_STATUS_FRAME = 0.01; + } + public IntakeSubsystem() { + intakeKraken = new TalonFX(IntakeConstants.INTAKE_KRAKEN_ID, "canivoreBus"); + var intakeConfigurator = intakeKraken.getConfigurator(); + var configs = new TalonFXConfiguration(); + DoubleSolenoid pistons = new DoubleSolenoid(PneumaticsModuleType.REVPH, 1, 2); + + beamBreak = new DigitalInput(2); + intakeOccupiedTrigger = new Trigger(this::getBeamBreak); + configs.MotorOutput.Inverted = IntakeConstants.INTAKE_INVERSION; + configs.MotorOutput.NeutralMode = IntakeConstants.INTAKE_NEUTRAL_MODE; + //configs.FutureProofConfigs = Constants.TalonFXConstants.TALON_FUTURE_PROOF; + intakeKraken.getRotorVelocity().waitForUpdate(IntakeConstants.INTAKE_VELOCITY_STATUS_FRAME); + intakeKraken.getRotorPosition().waitForUpdate(IntakeConstants.INTAKE_POSITION_STATUS_FRAME); + intakeConfigurator.apply(configs); + } + + @Override + public void periodic() { + SmartDashboard.putData("intake kraken", intakeKraken); + SmartDashboard.putData("intake beam break", beamBreak); + } + private void setIntakeSpeed(double speed) { + intakeKraken.set(speed); + } + + private void stop() { + intakeKraken.stopMotor(); + } + + private Command roll(double vel) { + return startEnd(() -> setIntakeSpeed(vel), this::stop); + } + + public Command rollIn(double vel) { + if(vel < 0){ + //Warning if negative + } + + return roll(Math.abs(vel)); + } + + public Value RampPosition() { + return pistons.get(); + } + + public void toggleRamp() { + pistons.toggle(); + } + + public boolean getBeamBreak() { + return !beamBreak.get();} + + public Command rollOut(double vel) { + if(vel > 0){ + //Warning + } + return roll(-Math.abs(vel)); + } +} + diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java index 270b000..4911142 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java @@ -1 +1,2 @@ package frc.robot.subsystems; +