remove second motor and...

refactor methods to call a setCommand method
Patribots4738 · Jan 26, 2024 · 3a54a5c · 3a54a5c
1 parent a6b1ce7
commit 3a54a5c
Showing 1 changed file with 12 additions and 33 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -8,59 +8,38 @@
 import frc.robot.util.Constants.IntakeConstants;
 
 public class Intake extends SubsystemBase {
-    private final Neo topIntake;
-    private final Neo bottomIntake;
+    private final Neo intakeMotor;
 
     public Intake() {
-        topIntake = new Neo(IntakeConstants.TOP_INTAKE_CAN_ID);
-        bottomIntake = new Neo(IntakeConstants.BOTTOM_INTAKE_CAN_ID);
+        intakeMotor = new Neo(IntakeConstants.TOP_INTAKE_CAN_ID);
         configMotors();
     }
 
     public void configMotors() {
-        bottomIntake.setSmartCurrentLimit(IntakeConstants.INTAKE_CURRENT_LIMIT_AMPS);
+        intakeMotor.setSmartCurrentLimit(IntakeConstants.INTAKE_CURRENT_LIMIT_AMPS);
         // See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
-        bottomIntake.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
-        bottomIntake.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535);
-        bottomIntake.setInverted(false); // TODO: is this correct?
-
-        topIntake.setSmartCurrentLimit(IntakeConstants.INTAKE_CURRENT_LIMIT_AMPS);
-        // See https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
-        topIntake.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
-        topIntake.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535);
-        topIntake.setInverted(false); // TODO: is this correct?
-
-        // sets brake mode
-        topIntake.setBrakeMode();
-        bottomIntake.setBrakeMode();
+        intakeMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535);
+        intakeMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 65535);
+    }
 
-        // TODO: should we slave the motors together?
+    public Command setCommand(double desiredSpeed) {
+        return runOnce(() -> intakeMotor.set(desiredSpeed));
     }
 
     public Command inCommand() {
-        return runOnce(() -> {
-            topIntake.set(IntakeConstants.INTAKE_SPEED);
-            bottomIntake.set(IntakeConstants.INTAKE_SPEED);
-        });
+        return setCommand(IntakeConstants.INTAKE_SPEED);
     }
 
     public Command outCommand() {
-        return runOnce(() -> {
-            topIntake.set(IntakeConstants.OUTTAKE_SPEED);
-            bottomIntake.set(IntakeConstants.OUTTAKE_SPEED);
-        });
+        return setCommand(IntakeConstants.OUTTAKE_SPEED);
     }
 
     public Command stop() {
-        return runOnce(() -> {
-            topIntake.set(IntakeConstants.STOP_SPEED);
-            bottomIntake.set(IntakeConstants.STOP_SPEED);
-        });
+        return setCommand(IntakeConstants.STOP_SPEED);
     }
 
     public Trigger hasGamePieceTrigger() {
-        return new Trigger(() -> topIntake.getOutputCurrent() > IntakeConstants.HAS_PIECE_CURRENT_THRESHOLD
-                || bottomIntake.getOutputCurrent() > IntakeConstants.HAS_PIECE_CURRENT_THRESHOLD);
+        return new Trigger(() -> intakeMotor.getOutputCurrent() > IntakeConstants.HAS_PIECE_CURRENT_THRESHOLD);
     }
 
 }