partial new state setup for drive

Author: vox-eternal

gradlew

@@ -69,9 +69,8 @@ public class RobotContainer {
     private boolean slowMode = false;
     private boolean hasFaults = true;
     private double faultPrintTimeout = 0;
-    private boolean intakeMode = false;
 
-    /** The container for the robot. Contains subsystems, OI devices, and commands. */
+    /** The container for the robot. Contains subsystems, IO devices, and commands. */
     public RobotContainer() {
         AutoConstants.precomputeAlignmentPoses(); // Computes robot starting pose with vision
         switch (Constants.currentMode) {
@@ -284,6 +283,9 @@ private Command offsetGyro() {
 
     public void teleopInit() {
         drivetrain.setTeleopCurrentLimit();
+        arm.sensorTrigger
+        .onTrue(outtake.setLock(true))
+        .onFalse(Commands.sequence(Commands.waitSeconds(1), outtake.setLock(false)));
         // drivetrain.offsetGyro(Rotation2d.fromDegrees(-90));
     }
 

src/main/java/frc/team5115/RobotContainer.java

@@ -12,7 +12,7 @@
 import frc.team5115.subsystems.arm.Arm;
 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;
 
@@ -71,24 +71,7 @@ public static Command joystickDrive(
                 drivetrain);
     }
 
-    public static Command modeSwap(boolean intakeMode, IntakeWheel intakeWheel, Arm arm) {
-        return Commands.either(
-                intakeMode(),
-                scoringMode(),
-                () -> intakeMode);
-    }
-
-    public static Command intakeMode(IntakeWheel intakeWheel, Arm arm, Outtake outtake) {
-        return Commands.repeatingSequence(
-                Commands.parallel(intakeWheel.intake(), arm.deploy(), outtake.retract(), outtake.allowExtend(false)),
-                (arm.waitForSensorState(true, Double.POSITIVE_INFINITY)),
-                arm.stow(),
-                arm.waitForSetpoint(5.0),
-                intakeWheel.setSpeed(-1));
-    }
-
-    public static Command scoringMode(IntakeWheel intakeWheel, Arm arm, Outtake outtake) {
-        return Commands.sequence(
-                Commands.parallel(arm.stow(), intakeWheel.stop()), outtake.allowExtend(true));
+    public Command xferLunite(Arm arm, IntakeWheel intakeWheel) {
+        return Commands.sequence(arm.deploy(), intakeWheel.intake(), arm.waitForSensorState(true, Double.POSITIVE_INFINITY), arm.stow(), intakeWheel.setSpeed(-1), Commands.waitSeconds(1));
     }
 }

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.team5115.Constants;
 import java.util.ArrayList;
 import org.littletonrobotics.junction.Logger;
@@ -18,6 +19,7 @@ public class Arm extends SubsystemBase {
     private final ArmFeedforward feedforward;
     private final PIDController pid;
     private final double ks;
+    public final Trigger sensorTrigger;
 
     public Arm(ArmIO io) {
         this.io = io;
@@ -43,6 +45,8 @@ public Arm(ArmIO io) {
 
         pid.setTolerance(5);
         pid.setSetpoint(75.0);
+
+        sensorTrigger = new Trigger(() -> (inputs.luniteDetected == true));
     }
 
     public void getSparks(ArrayList<SparkMax> sparks) {

src/main/java/frc/team5115/subsystems/arm/Arm.java

@@ -8,7 +8,8 @@
 public class Outtake extends SubsystemBase {
     private final OuttakeIO io;
     private final OuttakeIOInputsAutoLogged inputs = new OuttakeIOInputsAutoLogged();
-    public boolean canExtend;
+    public boolean locked;
+    public boolean lockOverride;
 
     public Outtake(OuttakeIO io) {
         this.io = io;
@@ -21,14 +22,20 @@ public void periodic() {
     }
 
     public Command extend() {
-        return Commands.runOnce(() -> io.setPneumatic(true), this);
+        return Commands.runOnce(
+                () -> {
+                    if (!locked || lockOverride) {
+                        io.setPneumatic(true);
+                    }
+                },
+                this);
     }
 
     public Command retract() {
         return Commands.runOnce(() -> io.setPneumatic(false), this);
     }
 
-    public Command allowExtend(boolean allowExtend) {
-        return Commands.runOnce(() -> canExtend = allowExtend);
+    public Command setLock(boolean lock) {
+        return Commands.runOnce(() -> locked = lock);
     }
 }