Merge branch 'algaeintake'

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

@@ -0,0 +1,15 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
+
+public class IntakeSetPivotCommand {
+    private final AlgaeIntakeSubsystem intakeSubsystem;
+
+    public IntakeSetPivotCommand(AlgaeIntakeSubsystem subsystem) {
+        this.intakeSubsystem = subsystem;
+    }
+
+    public void initialize() { intakeSubsystem.setPivotToVoltage(); }
+
+    public boolean isFinished() { return true; }
+}

src/main/java/frc/robot/commands/IntakePickupCommand.java → src/main/java/frc/robot/commands/IntakeSetRollersCommand.java

@@ -2,16 +2,16 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.AlgaeIntakeSubsystem.AlgaeIntakeSubsystem;
 
-public class IntakePickupCommand extends Command {
+public class IntakeSetRollersCommand extends Command {
     private final AlgaeIntakeSubsystem intakeSubsystem;
 
-    public IntakePickupCommand(AlgaeIntakeSubsystem intakeSubsystem) {
+    public IntakeSetRollersCommand(AlgaeIntakeSubsystem intakeSubsystem) {
         this.intakeSubsystem = intakeSubsystem;
         addRequirements(intakeSubsystem);
     }
 
     public void initialize() {
-        intakeSubsystem.pickup();
+        intakeSubsystem.setRollersToVoltage();
     }
 
     public boolean isFinished() {

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

@@ -1,18 +1,25 @@
 package frc.robot.subsystems.AlgaeIntakeSubsystem;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakeConstants;
 
 public class AlgaeIntakeSubsystem extends SubsystemBase {
     private final IntakeIO intake;
-
+    private final IntakeIO.IntakeInputsAutoLogged inputs = new IntakeIO.IntakeInputsAutoLogged();
     public AlgaeIntakeSubsystem(IntakeIO intake){
         this.intake = intake;
     }
 
-    public void pickup() {
-        intake.pivotDown();
-        intake.pivotUp();
+    public void setPivotToVoltage() {
+        intake.setPivotVoltage(IntakeConstants.pivotVoltsTarget);
+    }
+
+    public void setRollersToVoltage() {
+        intake.setRollersVoltage(IntakeConstants.rollerVoltsTarget);
     }
 
+    public void periodic() {
+        intake.updateInputs(inputs);
+    }
 
 }

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIO.java

@@ -16,6 +16,8 @@ public class IntakeInputsAutoLogged {
     public default void setRunning(boolean state) {}
     public default void pivotDown() {}
     public default void pivotUp() {}
+    public default void setPivotVoltage(double volts) {}
+    public default void setRollersVoltage(double volts) {}
     public default boolean getIsRunning() { return false; }
     public default boolean coralInside() { return false; }
     public default void updateInputs(IntakeInputsAutoLogged inputs) {}

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSim.java

@@ -61,6 +61,16 @@ public void pivotUp() {
         pivotWheel.setInputVoltage(IntakeConstants.pivotVoltsTarget);
     }
 
+    @Override
+    public void setPivotVoltage(double volts) {
+        pivotWheel.setInputVoltage(volts);
+    }
+
+    @Override
+    public void setRollersVoltage(double volts) {
+        rollers.setInputVoltage(volts);
+    }
+
     @Override
     public boolean coralInside() {
         return intakeSim.getGamePiecesAmount() != 0;

src/main/java/frc/robot/subsystems/AlgaeIntakeSubsystem/IntakeIOSparkMax.java

@@ -40,6 +40,16 @@ public void pivotUp() {
         setRunning(false);
     }
 
+    @Override
+    public void setPivotVoltage(double volts) {
+        pivot.setVoltage(volts);
+    }
+
+    @Override
+    public void setRollersVoltage(double volts) {
+        rollers.setVoltage(volts);
+    }
+
     @Override
     public void updateInputs(IntakeInputsAutoLogged inputs) {
         inputs.isRunning = isRunning;