Algae

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

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeGripper;
+
+public class CollectAlgae extends Command {
+    private AlgaeGripper algaeGripper;
+
+    public CollectAlgae(AlgaeGripper algaeGripper){
+        this.algaeGripper = algaeGripper;
+
+        addRequirements(algaeGripper);
+    }
+
+    @Override
+    public void initialize() {
+       algaeGripper.rotateCollect();
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return algaeGripper.hasAlgae();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+
+
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeArm;
+
+public class ExtendedAlgaeArm extends Command {
+    private AlgaeArm algaeArm;
+
+    public ExtendedAlgaeArm(AlgaeArm algaeArm){
+        this.algaeArm = algaeArm;
+
+        addRequirements(algaeArm);
+    }
+
+    @Override
+    public void initialize() {
+        algaeArm.extend();
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return algaeArm.isExtended();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+
+}

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

@@ -0,0 +1,34 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeGripper;
+
+public class HoldAlgae extends Command {
+    private AlgaeGripper algaeGripper;
+
+    public HoldAlgae(AlgaeGripper algaeGripper){
+        this.algaeGripper = algaeGripper;
+
+        addRequirements(algaeGripper);
+    }
+
+    @Override
+    public void initialize() {
+        algaeGripper.rotateHold();
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+}

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

@@ -0,0 +1,34 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeGripper;
+
+public class ReleaseAlgae extends Command {
+    private AlgaeGripper algaeGripper;
+
+    public ReleaseAlgae(AlgaeGripper algaeGripper){
+        this.algaeGripper = algaeGripper;
+
+        addRequirements(algaeGripper);
+    }
+
+    @Override
+    public void initialize() {
+        algaeGripper.rotateRelease();
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !algaeGripper.hasAlgae();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+}

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

@@ -0,0 +1,34 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeArm;
+
+public class RetractAlgaeArm extends Command {
+    private AlgaeArm algaeArm;
+
+    public RetractAlgaeArm(AlgaeArm algaeArm){
+        this.algaeArm = algaeArm;
+
+        addRequirements(algaeArm);
+    }
+
+    @Override
+    public void initialize() {
+        algaeArm.retract();
+    }
+
+    @Override
+    public void execute() {
+
+    }
+
+    @Override
+    public boolean isFinished() {
+        return algaeArm.isRetracted();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+}

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

@@ -0,0 +1,50 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AlgaeArm extends SubsystemBase {
+    private DoubleSolenoid solenoidLeft;
+    private DoubleSolenoid solenoidRight;
+    private DigitalInput switchLeft;
+    private DigitalInput switchRight;
+
+    public AlgaeArm(DoubleSolenoid solenoidLeft, DoubleSolenoid solenoidRight, DigitalInput switchLeft, DigitalInput switchRight){
+        this.solenoidLeft = solenoidLeft;
+        this.solenoidRight = solenoidRight;
+        this.switchLeft = switchLeft;
+        this.switchRight = switchRight;
+    }
+
+    public boolean isExtended(){
+        return !(switchLeft.get()) && !(switchRight.get());
+    }
+
+    public boolean isRetracted(){
+        return switchLeft.get() && switchRight.get();
+    }
+
+    public void extend(){
+        solenoidLeft.set(DoubleSolenoid.Value.kForward);
+        solenoidRight.set(DoubleSolenoid.Value.kForward);
+    }
+
+    public void retract(){
+        solenoidLeft.set(DoubleSolenoid.Value.kReverse);
+        solenoidRight.set(DoubleSolenoid.Value.kReverse);
+    }
+
+    public void stop(){
+        solenoidLeft.set(DoubleSolenoid.Value.kOff);
+        solenoidRight.set(DoubleSolenoid.Value.kOff);
+    }
+
+    @Override
+    public void periodic(){
+        SmartDashboard.putBoolean("Extended", isExtended());
+        SmartDashboard.putBoolean("Retracted", isRetracted());
+    }
+
+}

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

@@ -0,0 +1,44 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AlgaeGripper extends SubsystemBase {
+     private SparkMax motor;
+     private DigitalInput digitalInput;
+     private static final double COLLECT_SPEED = 0.8;
+     private static final double RELEASE_SPEED = -0.5;
+     private static final double HOLD_SPEED = 0.2;
+
+     public AlgaeGripper(SparkMax motor, DigitalInput digitalInput){
+          this.motor = motor;
+          this.digitalInput = digitalInput;
+     }
+
+     public boolean hasAlgae(){
+          return !digitalInput.get();
+     }
+
+     public void rotateCollect(){
+          motor.set(COLLECT_SPEED);
+     }
+
+     public void rotateRelease(){
+          motor.set(RELEASE_SPEED);
+     }
+
+     public void rotateHold(){
+          motor.set(HOLD_SPEED);
+     }
+
+     public void stop(){
+          motor.stopMotor();
+     }
+
+     @Override
+     public void periodic(){
+          SmartDashboard.putBoolean("HasAlgae", hasAlgae());
+     }
+}