feat: add intake & scoring mode

Author: vox-eternal

src/main/java/frc/team5115/DriverController.java

@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.team5115.Constants.AutoConstants.Side;
 import frc.team5115.commands.DriveCommands;
 import frc.team5115.subsystems.drive.Drivetrain;
 
@@ -16,25 +15,18 @@ public class DriverController {
     private boolean robotRelative = false;
     private boolean slowMode = false;
 
-    public DriverController(
-            int port,
-            Drivetrain drivetrain) {
+    public DriverController(int port, Drivetrain drivetrain) {
         joyDrive = new CommandXboxController(port);
         joyManip = null;
 
         this.drivetrain = drivetrain;
-        
     }
 
-    public DriverController(
-            int drivePort,
-            int manipPort,
-            Drivetrain drivetrain) {
+    public DriverController(int drivePort, int manipPort, Drivetrain drivetrain) {
         joyDrive = new CommandXboxController(drivePort);
         joyManip = new CommandXboxController(manipPort);
 
         this.drivetrain = drivetrain;
-
     }
 
     private Command offsetGyro() {
@@ -46,8 +38,8 @@ public boolean isConnected() {
     }
 
     private void configureSingleMode() {}
+
     private void configureDualMode() {}
-        
 
     public void configureButtonBindings() {
         // drive control

src/main/java/frc/team5115/commands/DriveCommands.java

@@ -13,7 +13,6 @@
 import frc.team5115.subsystems.drive.Drivetrain;
 import frc.team5115.subsystems.intakewheel.IntakeWheel;
 import frc.team5115.subsystems.outtake.Outtake;
-
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
@@ -74,19 +73,22 @@ public static Command joystickDrive(
 
     public static Command modeSwap(boolean intakeMode, IntakeWheel intakeWheel, Arm arm) {
         return Commands.either(
-                Commands.parallel(intakeWheel.intake(), arm.deploy()),
-                Commands.parallel(intakeWheel.stop(), arm.stow()),
+                intakeMode(),
+                scoringMode(),
                 () -> intakeMode);
     }
 
-    public static Command intakeMode(IntakeWheel intakeWheel, Arm arm) {
+    public static Command intakeMode(IntakeWheel intakeWheel, Arm arm, Outtake outtake) {
         return Commands.repeatingSequence(
-                Commands.parallel(intakeWheel.intake(), arm.deploy()),
+                Commands.parallel(intakeWheel.intake(), arm.deploy(), outtake.retract(), outtake.allowExtend(false)),
                 (arm.waitForSensorState(true, Double.POSITIVE_INFINITY)),
-                arm.stow());
+                arm.stow(),
+                arm.waitForSetpoint(5.0),
+                intakeWheel.setSpeed(-1));
     }
 
-    public static Command scoringMode(Outtake outtake) {
-        return 
+    public static Command scoringMode(IntakeWheel intakeWheel, Arm arm, Outtake outtake) {
+        return Commands.sequence(
+                Commands.parallel(arm.stow(), intakeWheel.stop()), outtake.allowExtend(true));
     }
 }

src/main/java/frc/team5115/subsystems/outtake/Outtake.java

@@ -8,6 +8,7 @@
 public class Outtake extends SubsystemBase {
     private final OuttakeIO io;
     private final OuttakeIOInputsAutoLogged inputs = new OuttakeIOInputsAutoLogged();
+    public boolean canExtend;
 
     public Outtake(OuttakeIO io) {
         this.io = io;
@@ -19,11 +20,15 @@ public void periodic() {
         Logger.processInputs(this.getName(), inputs);
     }
 
-    private Command extend() {
+    public Command extend() {
         return Commands.runOnce(() -> io.setPneumatic(true), this);
     }
 
-    private Command retract() {
+    public Command retract() {
         return Commands.runOnce(() -> io.setPneumatic(false), this);
     }
+
+    public Command allowExtend(boolean allowExtend) {
+        return Commands.runOnce(() -> canExtend = allowExtend);
+    }
 }