add intake subsystem (#9)

* 1/22

Co-authored-by: Edgar694 <[email protected]>
Co-authored-by: Owen-Zeng <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: smiley-axolotl <[email protected]>

* Wrote method signatures in IntakeImpl to help speed up tomorrow's session

* Fix IR logic

* Finished IntakeImpl, wrote commands

Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: smiley-axolotl <[email protected]>
Co-authored-by: Zixi Feng <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: Edgar694 <[email protected]>
Co-authored-by: emidudu <[email protected]>

* Added IntakeAcquireButDontStore

Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: smiley-axolotl <[email protected]>
Co-authored-by: Zixi Feng <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: Edgar694 <[email protected]>
Co-authored-by: emidudu <[email protected]>

* According to all known laws of aviation , there is no way a bee should fly. Its wings are too small to get its fat little body off the ground.

Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: smiley-axolotl <[email protected]>
Co-authored-by: Zixi Feng <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: Edgar694 <[email protected]>
Co-authored-by: emidudu <[email protected]>

* The bee, of course, flies anyways because bees dont care what humans think is impossible

* Yellow, black. Yellow, black. Yellow, black. Yellow, black. Ooh, black and yellow!

* Let's shake it up a little. Barry! Breakfast is ready!

Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: vipook <[email protected]>

* Coming! Hang on a second. Hello? Barry? Adam? Can you believe this is happening?

* this was so unneccessary

---------

Co-authored-by: Edgar694 <[email protected]>
Co-authored-by: Owen-Zeng <[email protected]>
Co-authored-by: sixuanl <[email protected]>
Co-authored-by: Weifen Chen <[email protected]>
Co-authored-by: smiley-axolotl <[email protected]>
Co-authored-by: Zixi Feng <[email protected]>
Co-authored-by: emidudu <[email protected]>
Co-authored-by: WennieC <[email protected]>
Co-authored-by: vipook <[email protected]>
Co-authored-by: BenG49 <[email protected]>

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.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 import com.stuypulse.stuylib.input.Gamepad;
@@ -23,6 +24,7 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final Intake intake = Intake.getInstance();
     public final Shooter shooter = Shooter.getInstance();
     public final Conveyor conveyor = Conveyor.getInstance();
 

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,13 +24,16 @@ 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 interface Shooter {
         CANSparkMaxConfig LEFT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast);
         CANSparkMaxConfig RIGHT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast);      
     }
 
-    public interface Conveyor{ 
-
+    public interface Conveyor {
         CANSparkMaxConfig GANDALF_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);
         CANSparkMaxConfig SHOOTER_FEEDER_MOTOR = new CANSparkMaxConfig(false, IdleMode.kCoast);
     }

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

@@ -12,6 +12,11 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Intake {
+        int MOTOR = 40;
+        int SENSOR = 1;
+    }
 
     public interface Shooter {
         int LEFT_MOTOR = 20; 

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

@@ -35,12 +35,21 @@ public interface PID {
         }
     }
 
+    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);
+    }
+
     public interface Conveyor {
         SmartNumber GANDALF_SHOOTER_SPEED = new SmartNumber("Conveyor/Gandalf Shooter Speed", 1);
         SmartNumber GANDALF_AMP_SPEED = new SmartNumber("Conveyor/Gandalf Amp Speed", -1);
         SmartNumber SHOOTER_FEEDER_SPEED = new SmartNumber("Conveyor/Shooter Feeder Speed", 1);
 
         SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.2);
     }
-
 }

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());
+    }
+}