IntakeCommand Refactor

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

@@ -20,11 +20,14 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IOConstants;
-import frc.robot.commands.IntakeCommand;
+import frc.robot.commands.DiskIntakeCommand;
+import frc.robot.commands.ExtendIntakeCommand;
+import frc.robot.commands.RetractIntakeCommand;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -115,7 +118,11 @@ private void configureBindings() {
 
     new Trigger(() -> {
       return m_driverController.getRightTriggerAxis() > 0.5;
-    }).whileTrue(new IntakeCommand(m_intakeSubsystem));
+    }).whileTrue(
+      new SequentialCommandGroup(
+        new ExtendIntakeCommand(m_intakeSubsystem), 
+        new DiskIntakeCommand(m_intakeSubsystem), 
+        new RetractIntakeCommand(m_intakeSubsystem)));
   }
 
   /**

src/main/java/frc/robot/commands/IntakeCommand.java → src/main/java/frc/robot/commands/DiskIntakeCommand.java

@@ -4,40 +4,48 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
 
-public class IntakeCommand extends Command {
+public class DiskIntakeCommand extends Command {
   private IntakeSubsystem m_intakeSubsystem;
 
+  boolean haveDisk = false;
+
+  private Timer m_timer = new Timer();
+
   /** Creates a new intakeCommand. */
-  public IntakeCommand(IntakeSubsystem subsystem) {
+  public DiskIntakeCommand(IntakeSubsystem subsystem) {
     m_intakeSubsystem = subsystem;
     addRequirements(m_intakeSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_intakeSubsystem.armExtend();
     m_intakeSubsystem.intake();
+    m_timer.reset();
+    m_timer.start();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-
+    if (m_intakeSubsystem.getDistanceSensor() < 6){
+      haveDisk = true;
+    }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    m_intakeSubsystem.armRetract();
     m_intakeSubsystem.stopIntake();
   }
 
+  // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return haveDisk || m_timer.get() > 15;
   }
 }

src/main/java/frc/robot/commands/ExtendIntakeCommand.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.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class ExtendIntakeCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+
+  /** Creates a new intakeCommand. */
+  public ExtendIntakeCommand(IntakeSubsystem subsystem) {
+    m_intakeSubsystem = subsystem;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_intakeSubsystem.armExtend();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_intakeSubsystem.atSetpoint();
+  }
+}

src/main/java/frc/robot/commands/RetractIntakeCommand.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.Command;
+import frc.robot.subsystems.IntakeSubsystem;
+
+public class RetractIntakeCommand extends Command {
+  private IntakeSubsystem m_intakeSubsystem;
+
+  /** Creates a new intakeCommand. */
+  public RetractIntakeCommand(IntakeSubsystem subsystem) {
+    m_intakeSubsystem = subsystem;
+    addRequirements(m_intakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_intakeSubsystem.armRetract();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return m_intakeSubsystem.atSetpoint();
+  }
+}

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

@@ -14,10 +14,10 @@
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Constants.IntakeConstants;
 
 public class IntakeSubsystem extends SubsystemBase {
-  private boolean deployed = false;
   private boolean haveNote = false;
 
   private CANSparkFlex m_intakeMotor = new CANSparkFlex(IntakeConstants.kIntakeMotorID, MotorType.kBrushless);
@@ -51,28 +51,24 @@ public IntakeSubsystem() {
 
   public void armExtend() {
     m_armPID.setSetpoint(IntakeConstants.kIntakeLoweredAngle);
-
-    stopIntake();
-
-    deployed = true;
   }
 
   public void armRetract() {
     m_armPID.setSetpoint(IntakeConstants.kIntakeRaisedAngle);
+  }
 
-    stopIntake();
-
-    deployed = false;
+  public boolean atSetpoint(){
+    return m_armPID.atSetpoint();
   }
 
   public void intake() {
-    if (deployed && !haveNote) {
+    if (!haveNote) {
       m_intakeSpeed = IntakeConstants.kIntakeSpeed;
     }
   }
 
   public void outake() {
-    if (!deployed && haveNote) {
+    if (haveNote) {
       m_intakeSpeed = -IntakeConstants.kIntakeSpeed;
     }
   }
@@ -91,24 +87,19 @@ public double getDistanceSensor() {
 
   @Override
   public void periodic() {
-    haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;
-
-    // If we have a note and the arm is deployed, automatically bring it back in
-    if (haveNote && deployed) {
-      stopIntake();
-      armRetract();
-    }
+    haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold ? true : false;
 
     // m_armMotor.set(m_armPID.calculate(m_armEncoder.getDistance()));
     // m_intakeMotor.set(m_intakeSpeed);
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
-    SmartDashboard.putBoolean("Arm Deployed?", deployed);
     SmartDashboard.putBoolean("Have Note?", haveNote);
     SmartDashboard.putNumber("pid output", m_armPID.calculate(m_armEncoder.getDistance()));
   }
 
   public boolean readyToShoot() {
-    return haveNote && !deployed && m_armPID.atSetpoint();
+    return haveNote && 
+        m_armPID.getSetpoint() == IntakeConstants.kIntakeRaisedAngle && 
+        m_armPID.atSetpoint();
   }
 }