intake subsystem + minor tweaks to queuer and shooter

src/main/java/frc/robot/intake/IntakeState.java

@@ -0,0 +1,7 @@
+package frc.robot.intake;
+
+public enum IntakeState {
+    IDLE,
+    INTAKING,
+    OUTTAKING;
+}

src/main/java/frc/robot/intake/IntakeSubsystem.java

@@ -0,0 +1,58 @@
+package frc.robot.intake;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import dev.doglog.DogLog;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.util.scheduling.SubsystemPriority;
+import frc.robot.util.state_machines.StateMachine;
+
+public class IntakeSubsystem extends StateMachine<IntakeState> {
+    private final TalonFX motor;
+    private final DigitalInput sensor;
+    private boolean sensorHasNote = false;
+    private boolean debouncedSensorHasNote = false;
+    private final Debouncer debouncer = new Debouncer(3.0*0.02);
+
+    public IntakeSubsystem (TalonFX motor,DigitalInput sensor){
+        super(SubsystemPriority.INTAKE, IntakeState.IDLE);
+
+        this.sensor = sensor;
+        this.motor = motor;
+    }
+    public void setState (IntakeState newState){
+        setStateFromRequest(newState);
+    }
+
+    @Override
+    protected void collectInputs(){
+        sensorHasNote=sensor.get();
+        debouncedSensorHasNote=debouncer.calculate(sensorHasNote);
+    }
+    @Override
+    protected IntakeState getNextState(IntakeState currentState){
+        return currentState;
+    }
+    public boolean hasNote(){
+        return debouncedSensorHasNote;
+    }
+    @Override
+    protected void afterTransition(IntakeState newState){
+        switch (newState){
+            case IDLE->motor.disable();
+            case INTAKING->motor.setVoltage(0);//around 10
+            case OUTTAKING->motor.setVoltage(0);//around -6
+        }
+    }
+    @Override
+    public void robotPeriodic(){
+        super.robotPeriodic();
+        DogLog.log("Intake/StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
+        DogLog.log("Intake/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
+        DogLog.log("Intake/AppliedVoltage", motor.getMotorVoltage().getValueAsDouble());
+        DogLog.log("Intake/RawSensor",sensorHasNote);
+        DogLog.log("Intake/DebouncedSensor",hasNote());
+
+    }
+}

src/main/java/frc/robot/queuer/QueuerSubsystem.java

@@ -14,7 +14,7 @@ public class QueuerSubsystem extends StateMachine<QueuerState> {
   private final DigitalInput sensor;
   private boolean sensorHasNote = false;
   private boolean debouncedSensorHasNote = false;
-  private final Debouncer debouncer= new Debouncer(3 * 0.02);
+  private final Debouncer debouncer= new Debouncer(3.0 * 0.02);
 
   public QueuerSubsystem(TalonFX motor, DigitalInput sensor) {
     super(SubsystemPriority.QUEUER, QueuerState.IDLE_NO_GP);
@@ -54,8 +54,12 @@ protected void afterTransition(QueuerState newState){
   }
   @Override
   public void robotPeriodic(){
+    super.robotPeriodic();
+
     DogLog.log("Queuer/StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
     DogLog.log("Queuer/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
     DogLog.log("Queuer/AppliedVoltage", motor.getMotorVoltage().getValueAsDouble());
+    DogLog.log("Queuer/RawSensor",sensorHasNote);
+      DogLog.log("Queuer/DebouncedSensor",hasNote());
   }
 }

src/main/java/frc/robot/shooter/ShooterRpms.java

@@ -4,5 +4,5 @@ public class ShooterRpms {
   public static final double SUBWOOFER = 3000.0;
   public static final double DROP = 500.0;
   public static final double PODIUM = 4000.0;
-  public static final double IDLE_WITH_GP = 1000.0;
+  public static final double IDLE_WARMUP = 1000.0;
 }

src/main/java/frc/robot/shooter/ShooterState.java

@@ -1,8 +1,8 @@
 package frc.robot.shooter;
 
 public enum ShooterState {
-  IDLE_WITH_GP,
-  IDLE_NO_GP,
+  IDLE_WARMUP,
+  IDLE_STOPPED,
   SPEAKER_SHOT,
   DROP,
   SUBWOOFER_SHOT,

src/main/java/frc/robot/shooter/ShooterSubsystem.java

@@ -28,7 +28,7 @@ public void setDistanceToFeedSpot(double distance) {
   }
 
   public ShooterSubsystem(TalonFX motor) {
-    super(SubsystemPriority.SHOOTER, ShooterState.IDLE_NO_GP);
+    super(SubsystemPriority.SHOOTER, ShooterState.IDLE_STOPPED);
     this.motor = motor;
 
     // TODO: Tune lookup table
@@ -40,7 +40,7 @@ public ShooterSubsystem(TalonFX motor) {
   public boolean atGoal() {
     return switch (getState()) {
       case SUBWOOFER_SHOT -> MathUtil.isNear(ShooterRpms.SUBWOOFER, shooterRPM, 50);
-      case IDLE_WITH_GP, IDLE_NO_GP->true;
+      case IDLE_WARMUP, IDLE_STOPPED->true;
       case PODIUM_SHOT-> MathUtil.isNear(ShooterRpms.PODIUM,shooterRPM,50);
       case DROP->MathUtil.isNear(ShooterRpms.DROP,shooterRPM,50);
       case FEEDING->MathUtil.isNear(feedSpotDistanceToRpm.get(distanceToFeedSpot),shooterRPM,50);
@@ -66,8 +66,8 @@ protected ShooterState getNextState(ShooterState currentState) {
   @Override
   protected void afterTransition(ShooterState newState) {
     switch (newState) {
-      case IDLE_NO_GP -> motor.disable();
-      case IDLE_WITH_GP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WITH_GP / 60.0));
+      case IDLE_STOPPED -> motor.disable();
+      case IDLE_WARMUP -> motor.setControl(velocityRequest.withVelocity(ShooterRpms.IDLE_WARMUP / 60.0));
       case SPEAKER_SHOT ->
           motor.setControl(
               velocityRequest.withVelocity(speakerDistanceToRpm.get(distanceToSpeaker) / 60.0));
@@ -82,7 +82,9 @@ protected void afterTransition(ShooterState newState) {
 
   @Override
   public void robotPeriodic() {
-    // Log stator current, supply current, current velocity in RPM, applied voltage
+
+    super.robotPeriodic();
+
     DogLog.log("Shooter/StatorCurrent",motor.getStatorCurrent().getValueAsDouble());
     DogLog.log("Shooter/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
     DogLog.log("Shooter/RPM",motor.getVelocity().getValueAsDouble()*60.0);