Intake code

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

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.
+    }
+}
+

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));
+    }
+}
+

src/main/java/frc/robot/subsystems/SwerveDrivetrain.java

@@ -1 +1,2 @@
 package frc.robot.subsystems;
+