Algae

src/main/java/frc/robot/Robot.java

@@ -2,11 +2,17 @@
 
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.AlgaeArm;
+import frc.robot.subsystems.AlgaeGripper;
 
 public class Robot extends TimedRobot {
+    private AlgaeArm algaeArm;
+    private AlgaeGripper algaeGripper;
 
     @Override
     public void robotInit() {
+        algaeArm = new AlgaeArm();
+        algaeGripper = new AlgaeGripper();
 
     }
 

src/main/java/frc/robot/RobotMap.java

@@ -3,7 +3,14 @@
 public class RobotMap {
 
     private RobotMap() {}
-
+    public static final int ALGAE_GRIPPER_DIGITALINPUT = 0;
+    public static final int ALGAE_ARM_SWITCH_TOP = 0;
+    public static final int ALGAE_ARM_SWITCH_BOTTOM = 0;
+    public static final int ALGAE_GRIPPER_MOTOR = 0;
+    public static final int ALGEA_ARM_FORWARD_PISTON_FORWARD_CHANNEL = 0;
+    public static final int ALGEA_ARM_FORWARD_PISTON_REVERSE_CHANI = 0;
+    public static final int ALGEA_ARM_BACKWARD_PISTON_FORWARD_CHANNEL = 0;
+    public static final int ALGEA_ARM_BACKWARD_PISTON_REVERSE_CHANI = 0;
     // add constants here
     // public static final type NAME = value;
 }

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 final 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) {
+        algaeGripper.stop();
+    }
+
+
+}

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 final 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 final 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) {
+        algaeGripper.stop();
+    }
+}

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 final 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) {
+        algaeGripper.stop();
+    }
+}

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 final 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,52 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotMap;
+
+public class AlgaeArm extends SubsystemBase {
+    private final DoubleSolenoid solenoid1;
+    private final DoubleSolenoid solenoid2;
+    private final DigitalInput switchTop;
+    private final DigitalInput switchBottom;
+
+    public AlgaeArm(){
+        this.solenoid1 = new DoubleSolenoid(PneumaticsModuleType.REVPH, RobotMap.ALGEA_ARM_FORWARD_PISTON_FORWARD_CHANNEL, RobotMap.ALGEA_ARM_FORWARD_PISTON_REVERSE_CHANI);
+        this.solenoid2 = new DoubleSolenoid(PneumaticsModuleType.REVPH, RobotMap.ALGEA_ARM_BACKWARD_PISTON_FORWARD_CHANNEL, RobotMap.ALGEA_ARM_BACKWARD_PISTON_REVERSE_CHANI);
+        this.switchTop = new DigitalInput(RobotMap.ALGAE_ARM_SWITCH_TOP);
+        this.switchBottom = new DigitalInput(RobotMap.ALGAE_ARM_SWITCH_BOTTOM);
+    }
+
+    public boolean isExtended(){
+        return !(switchTop.get());
+    }
+
+    public boolean isRetracted(){
+        return !(switchBottom.get());
+    }
+
+    public void extend(){
+        solenoid1.set(DoubleSolenoid.Value.kForward);
+        solenoid2.set(DoubleSolenoid.Value.kForward);
+    }
+
+    public void retract(){
+        solenoid1.set(DoubleSolenoid.Value.kReverse);
+        solenoid2.set(DoubleSolenoid.Value.kReverse);
+    }
+
+    public void stop(){
+        solenoid1.set(DoubleSolenoid.Value.kOff);
+        solenoid2.set(DoubleSolenoid.Value.kOff);
+    }
+
+    @Override
+    public void periodic(){
+        SmartDashboard.putBoolean("AlgaeArmExtended", isExtended());
+        SmartDashboard.putBoolean("AlgaeArmRetracted", isRetracted());
+    }
+
+}

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

@@ -0,0 +1,47 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkLowLevel;
+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;
+import frc.robot.RobotMap;
+
+public class AlgaeGripper extends SubsystemBase {
+     private static final double COLLECT_SPEED = 0.8;
+     private static final double RELEASE_SPEED = -0.5;
+     private static final double HOLD_SPEED = 0.2;
+     private final SparkMax motor;
+     private final DigitalInput digitalInput;
+
+
+     public AlgaeGripper(){
+          this.motor = new SparkMax(RobotMap.ALGAE_GRIPPER_MOTOR, SparkLowLevel.MotorType.kBrushless);
+          this.digitalInput = new DigitalInput(RobotMap.ALGAE_GRIPPER_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());
+     }
+}