Algae remover made not tested

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

@@ -15,6 +15,8 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.algae.Algae;
+import frc.robot.subsystems.algae.AlgaeIOTalonFX;
 import frc.robot.subsystems.elevator.Elevator;
 import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
 import frc.robot.subsystems.shooter.Shooter;
@@ -45,9 +47,12 @@ public class RobotContainer {
 
   private final Shooter s_Shooter;
   private final Elevator s_Elevator;
+  private final Algae s_Algae;
+
   public RobotContainer() {
     s_Shooter = new Shooter(new ShooterIOTalonFX());
     s_Elevator = new Elevator(new ElevatorIOTalonFX());
+    s_Algae = new Algae(new AlgaeIOTalonFX());
     configureBindings();
   }
 
@@ -87,12 +92,16 @@ private void configureBindings() {
     joystick.a().whileTrue(s_Shooter.shootTrough()).onFalse(s_Shooter.stop());
 
     joystick.b().onTrue(s_Elevator.goToL1()).onFalse(s_Elevator.stop());
-    joystick.leftBumper().onTrue(s_Elevator.goToL2
-    ()).onFalse(s_Elevator.stop());
+    joystick.leftBumper().onTrue(s_Elevator.goToL2()).onFalse(s_Elevator.stop());
     joystick.rightBumper().onTrue(s_Elevator.goToL3()).onFalse(s_Elevator.stop());
     joystick.leftTrigger().onTrue(s_Elevator.goToL4()).onFalse(s_Elevator.stop());
-    joystick.povUp().whileTrue(s_Elevator.moveUp()).onFalse(s_Elevator.stop());
-    joystick.povDown().whileTrue(s_Elevator.moveDown()).onFalse(s_Elevator.stop());
+
+    joystick.povUp().whileTrue(s_Algae.shootAlgae()).onFalse(s_Algae.stop());
+    joystick.povDown().whileTrue(s_Algae.intake()).onFalse(s_Algae.stop());
+
+    // joystick.povUp().whileTrue(s_Elevator.moveUp()).onFalse(s_Elevator.stop());
+    // joystick.povDown().whileTrue(s_Elevator.moveDown()).onFalse(s_Elevator.stop());
+
 
     // Run SysId routines when holding back/start and X/Y.
     // Note that each routine should be run exactly once in a single log.

src/main/java/frc/robot/subsystems/algae/Algae.java

@@ -0,0 +1,26 @@
+package frc.robot.subsystems.algae;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class Algae extends SubsystemBase {
+    private final AlgaeIO m_io;
+
+    public Algae(AlgaeIO io) {
+        m_io = io;
+      }
+
+    public Command shootAlgae() {
+        return this.run(() -> m_io.setSpeed(-0.1));
+  }
+
+    public Command stop() {
+        return this.run(() -> m_io.setSpeed(0));
+  }
+
+    public Command intake() {
+        return this.run(() -> m_io.setSpeed(1));
+  }
+}
+

src/main/java/frc/robot/subsystems/algae/AlgaeIO.java

@@ -0,0 +1,12 @@
+package frc.robot.subsystems.algae;
+
+public interface AlgaeIO {
+
+    public static class AlgaeIOInputs {
+        public double speed = 0.0;
+      }
+
+      public default void setSpeed(double speed) {}
+
+      public default void setBrakeMode(boolean enableBrakeMode) {}
+    }

src/main/java/frc/robot/subsystems/algae/AlgaeIOTalonFX.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.algae;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+public class AlgaeIOTalonFX implements AlgaeIO {
+
+     private final TalonFX m_algaeMotor;
+
+     public AlgaeIOTalonFX (){
+        m_algaeMotor = new TalonFX(23, "CANivore2");
+
+        final TalonFXConfiguration algaeMotorConfig = new TalonFXConfiguration();
+        algaeMotorConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
+        algaeMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+        m_algaeMotor.getConfigurator().apply(algaeMotorConfig);
+     }
+
+     @Override
+    public void setSpeed(double speed) {
+        m_algaeMotor.setControl(new DutyCycleOut(speed));
+  }
+
+    @Override
+    public void setBrakeMode(boolean enableBrakeMode) {
+    final NeutralModeValue neutralModeValue =
+        enableBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast;
+    m_algaeMotor.setNeutralMode(neutralModeValue);
+  }
+}