Merge remote-tracking branch 'origin/dev' into feat/wrist

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
     id "com.diffplug.spotless" version "6.13.0"
     id "me.champeau.jmh" version "0.6.8"
 }

src/main/java/frc/robot/Commands/AdvancedIntakeSequence.java

@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.Commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.Subsystems.IntakeSubsystem;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class AdvancedIntakeSequence extends SequentialCommandGroup {
+  /** Creates a new AdvancedIntakeSequence. */
+  public AdvancedIntakeSequence(IntakeSubsystem intakeSubsystem, boolean isCone, boolean isIntake) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    if (isIntake) {
+      addCommands(
+          new IntakeCommand(intakeSubsystem, isCone, isIntake),
+          new StopIntakeMotorCommand(intakeSubsystem));
+    } else {
+      addCommands(
+          new IntakeCommand(intakeSubsystem, isCone, isIntake)
+              .withTimeout(Constants.Intake.UNLOADING_WAIT_TIME),
+          new StopIntakeMotorCommand(intakeSubsystem));
+    }
+  }
+}

src/main/java/frc/robot/Commands/IntakeCommand.java

@@ -37,7 +37,7 @@ public void initialize() {
       intakeSubsystem.SetMotorPower(Constants.Intake.CUBE_UNLOADING_SPEED);
     }
 
-    //set the object type - used in the stoter currents
+    // set the object type - used in the stoter currents
     intakeSubsystem.SetObjectType(isCone);
   }
 
@@ -52,6 +52,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return true;
+    return intakeSubsystem.IsLoaded();
   }
 }

src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java

@@ -36,6 +36,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return true;
   }
 }

src/main/java/frc/robot/Constants.java

@@ -27,6 +27,8 @@ public static class Intake {
 
     public static final double CUBE_STATOR_LIMIT = 90;
     public static final double CONE_STATOR_LIMIT = 70;
+
+    public static final double UNLOADING_WAIT_TIME = 2f;
   }
   public final class Wrist {
     public static final int DRIVER_CONTROLLER_PORT = 0;

src/main/java/frc/robot/RobotContainer.java

@@ -13,6 +13,8 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.SequentialCommands;
 import frc.robot.subsystems.WristSubsystem;
+import frc.robot.Commands.AdvancedIntakeSequence;
+import frc.robot.Subsystems.IntakeSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -76,12 +78,9 @@ public void containerMatchStarting() {
    */
   private void configureButtonBindings() {
     // intake and outtake
-    driverB.rightTrigger().onTrue(new IntakeCommand(intakeSubsystem, false, true));
-    driverB.leftTrigger().onTrue(new IntakeCommand(intakeSubsystem, true, true));
-    driverB.rightBumper().onTrue(new IntakeCommand(intakeSubsystem, false, false));
-    driverB.leftBumper().onTrue(new IntakeCommand(intakeSubsystem, true, false));
-
-    // stop the motors
-    driverB.x().onTrue(new StopIntakeMotorCommand(intakeSubsystem));
+    driverB.rightTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, true));
+    driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true));
+    driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false));
+    driverB.leftBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, false));
   }
 }

src/main/java/frc/robot/Subsystems/IntakeSubsystem.java

@@ -53,8 +53,8 @@ public void SetMotorPower(double motorPower) {
     this.motorPower = motorPower;
   }
 
-  //to set the object as either a cone or a cube
-  //to be called in commands alongside of set motor power
+  // to set the object as either a cone or a cube
+  // to be called in commands alongside of set motor power
   public void SetObjectType(boolean isCone) {
     this.isCone = isCone;
   }
@@ -67,7 +67,7 @@ public void periodic() {
     // set the motor power to the var
     intakeMotor.set(TalonFXControlMode.PercentOutput, motorPower);
 
-    // if we are done loading than set the motor power to 0
-    if (IsLoaded()) SetMotorPower(0);
+    // // if we are done loading than set the motor power to 0
+    // if (IsLoaded()) SetMotorPower(0);
   }
 }