created seperate commands and methods for coral and alage to command

src/main/java/frc/robot/commands/AcquireCommand.java → src/main/java/frc/robot/commands/AcquireAlgaeCommand.java

@@ -3,22 +3,22 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Acquisition;
 
-public class AcquireCommand extends Command {
+public class AcquireAlgaeCommand extends Command {
 
     private Acquisition acquisition = Acquisition.getInstance();
 
-    public AcquireCommand() {
+    public AcquireAlgaeCommand() {
         this.addRequirements(acquisition);
     }
 
     @Override
     public void execute() {
-        acquisition.acquire();
+        acquisition.acquireAlgae();
     }
 
     @Override
     public boolean isFinished() {
-        return acquisition.getAlgaeSwitch() || acquisition.getBottomLaser();
+        return false;
     }
 
     @Override

src/main/java/frc/robot/commands/AcquireCoralCommand.java

@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Acquisition;
+
+public class AcquireCoralCommand extends Command {
+
+    private Acquisition acquisition = Acquisition.getInstance();
+
+    public AcquireCoralCommand() {
+        this.addRequirements(acquisition);
+    }
+
+    @Override
+    public void execute() {
+        acquisition.acquireCoral();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return acquisition.getBottomLaser();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        acquisition.stopAcquisition();
+    }
+
+}

src/main/java/frc/robot/commands/DisposeAlgaeCommand.java

@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Acquisition;
+
+public class DisposeAlgaeCommand extends Command {
+    private Acquisition acquisition = Acquisition.getInstance();
+    private double secondsToDispose;
+    private Timer timer = new Timer();
+
+    public DisposeAlgaeCommand() {
+        this(0.0);
+    }
+
+    public DisposeAlgaeCommand(double secondsToDispose) {
+        this.secondsToDispose = secondsToDispose;
+        super.addRequirements(acquisition);
+    }
+
+    @Override
+    public void execute() {
+        timer.restart();
+        acquisition.disposeAlgae();
+    }
+
+    @Override
+    public boolean isFinished() {
+        // if algae limit switch is not pressed and set time has passed stop the command
+        if (!acquisition.getAlgaeSwitch()) {
+            return secondsToDispose != 0 && timer.get() >= secondsToDispose;
+        }
+
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        acquisition.stopAcquisition();
+    }
+}

src/main/java/frc/robot/commands/DisposeCommand.java → src/main/java/frc/robot/commands/DisposeCoralCommand.java

@@ -4,35 +4,32 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Acquisition;
 
-public class DisposeCommand extends Command {
+public class DisposeCoralCommand extends Command {
     private Acquisition acquisition = Acquisition.getInstance();
     private double secondsToDispose;
     private Timer timer = new Timer();
 
-    public DisposeCommand() {
+    public DisposeCoralCommand() {
         this(0.0);
     }
 
-    public DisposeCommand(double secondsToDispose) {
+    public DisposeCoralCommand(double secondsToDispose) {
         this.secondsToDispose = secondsToDispose;
         super.addRequirements(acquisition);
     }
 
     @Override
     public void execute() {
         timer.restart();
-        acquisition.dispose();
+        acquisition.disposeCoral();
     }
 
     @Override
     public boolean isFinished() {
-        // if neither acquisition laser returns true, move on to checking the algae
-        // switch. command is finished if all three sensors return false.
+        // if neither acquisition laser returns true command is finished
 
         if (!(acquisition.getTopLaser() || acquisition.getBottomLaser())) {
-            if (!acquisition.getAlgaeSwitch()) {
-                return secondsToDispose == 0.0 ? false : timer.get() >= secondsToDispose;
-            }
+            return secondsToDispose != 0.0 && timer.get() >= secondsToDispose;
         }
 
         return false;

src/main/java/frc/robot/constants/AcquisitionConstants.java

@@ -4,6 +4,8 @@ public class AcquisitionConstants {
     public static final int kTopLaserPort = 0;
     public static final int kBottomLaserPort = 10;
     public static final int kAcquisitionMotorPort = 9;
-    public static final double kAcquireSpeed = 1.0;
-    public static final double kDisposeSpeed = -1.0;
+    public static final double kAcquireCoralSpeed = 1.0;
+    public static final double kAcquireAlgaeSpeed = 1.0;
+    public static final double kDisposeCoralSpeed = -1.0;
+    public static final double kDisposeAlgaeSpeed = -1.0;
 }

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

@@ -5,10 +5,9 @@
 import com.revrobotics.spark.SparkFlex;
 import com.revrobotics.spark.SparkLimitSwitch;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkFlexConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkFlexConfig;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AcquisitionConstants;
@@ -39,12 +38,20 @@ public static Acquisition getInstance() {
         return instance;
     }
 
-    public void acquire() {
-        this.acquisitionMotor.set(AcquisitionConstants.kAcquireSpeed);
+    public void acquireCoral() {
+        this.acquisitionMotor.set(AcquisitionConstants.kAcquireCoralSpeed);
+    }
+
+    public void acquireAlgae() {
+        this.acquisitionMotor.set(AcquisitionConstants.kAcquireAlgaeSpeed);
+    }
+
+    public void disposeCoral() {
+        this.acquisitionMotor.set(AcquisitionConstants.kDisposeCoralSpeed);
     }
 
-    public void dispose() {
-        this.acquisitionMotor.set(AcquisitionConstants.kDisposeSpeed);
+    public void disposeAlgae() {
+        this.acquisitionMotor.set(AcquisitionConstants.kDisposeAlgaeSpeed);
     }
 
     public void stopAcquisition() {