add intake subsystem

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,6 +7,7 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
@@ -21,6 +22,7 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final Intake intake = Intake.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class IntakeAcquire extends Command{
+
+    private Intake intake;
+
+    public IntakeAcquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return intake.hasNote();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeAcquireForever extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeAcquireForever() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java

@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeDeacquire extends InstantCommand {
+    private Intake intake;
+
+    public IntakeDeacquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.deacquire();
+    }
+}

src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java

@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+public class IntakeStop extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeStop() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.stop();
+    }
+}

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -24,6 +24,10 @@ public interface Motors {
 
     /** Classes to store all of the values a motor needs */
 
+    public interface Intake {
+        CANSparkMaxConfig MOTOR_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 80, 0.1);
+    }
+
     public static class TalonSRXConfig {
         public final boolean INVERTED;
         public final NeutralMode NEUTRAL_MODE;

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -12,4 +12,9 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Intake {
+        int MOTOR = 40;
+        int SENSOR = 1;
+    }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -5,7 +5,6 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
 /*-
@@ -14,4 +13,14 @@
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-public interface Settings {}
+public interface Settings {
+    public interface Intake {
+        public interface Detection {
+            SmartNumber TRIGGER_TIME = new SmartNumber("Intake/Trigger Debounce Time", 0.05);
+            SmartNumber STALL_TIME = new SmartNumber("Intake/Stall Debounce Time", .05);
+            SmartNumber STALL_CURRENT = new SmartNumber("Intake/Stall Current", 40);
+        }
+        SmartNumber ACQUIRE_SPEED = new SmartNumber("Intake/Acquire", 1);
+        SmartNumber DEACQUIRE_SPEED = new SmartNumber("Intake/Deacquire", -1);
+    }
+}

src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java

@@ -0,0 +1,23 @@
+package com.stuypulse.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Intake extends SubsystemBase {
+
+    private static final Intake instance;
+
+    static {
+        instance = new IntakeImpl();
+    }
+
+    public static Intake getInstance() {
+        return instance;
+    }
+
+    public abstract void acquire();
+    public abstract void deacquire();
+    public abstract void stop();
+
+    public abstract boolean hasNote();
+
+}

src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java

@@ -0,0 +1,76 @@
+package com.stuypulse.robot.subsystems.intake;
+
+import com.stuypulse.robot.constants.Motors;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class IntakeImpl extends Intake {
+
+    private final CANSparkMax motor;
+    private final DigitalInput sensor;
+
+    private final BStream triggered;
+    private final BStream stalling;
+
+    public IntakeImpl() {
+        motor = new CANSparkMax(Ports.Intake.MOTOR, MotorType.kBrushless);
+        sensor = new DigitalInput(Ports.Intake.SENSOR);
+
+        Motors.Intake.MOTOR_CONFIG.configure(motor);
+
+        triggered = BStream.create(sensor).not().filtered(new BDebounce.Both(Settings.Intake.Detection.TRIGGER_TIME));
+        stalling = BStream.create(this::isMomentarilyStalling).filtered(new BDebounce.Both(Settings.Intake.Detection.STALL_TIME));
+    }
+
+    @Override
+    public void acquire() {
+        motor.set(Settings.Intake.ACQUIRE_SPEED.getAsDouble());
+    }
+
+    @Override 
+    public void deacquire() {
+        motor.set(Settings.Intake.DEACQUIRE_SPEED.getAsDouble());
+    }
+
+    @Override
+    public void stop() {
+        motor.stopMotor();
+    }
+
+    // Detection
+
+    private boolean isMomentarilyStalling() {
+        return motor.getOutputCurrent() > Settings.Intake.Detection.STALL_CURRENT.getAsDouble();
+    }
+
+    private boolean isStalling() {
+        return stalling.get();
+    }
+
+    private boolean isTriggered() {
+        return triggered.get();
+    }
+
+    // Not using stall detection, but keeping it as an option
+    @Override
+    public boolean hasNote() {
+        return isTriggered();
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber("Intake/Speed", motor.get());
+        SmartDashboard.putNumber("Intake/Current", motor.getOutputCurrent());
+
+        SmartDashboard.putBoolean("Intake/Is Stalling", isStalling());
+        SmartDashboard.putBoolean("Intake/IR is triggered", isTriggered());
+    }
+}