Add queuer (#13)

Author: ryanknj5

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

@@ -2,17 +2,60 @@
 
 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 QueuerSubsystem extends StateMachine<QueuerState> {
   private final TalonFX motor;
   private final DigitalInput sensor;
+  private boolean sensorHasNote = false;
+  private boolean debouncedSensorHasNote = false;
+  private final Debouncer debouncer= new Debouncer(3 * 0.02);
 
   public QueuerSubsystem(TalonFX motor, DigitalInput sensor) {
     super(SubsystemPriority.QUEUER, QueuerState.IDLE_NO_GP);
 
     this.sensor = sensor;
     this.motor = motor;
-  }}
+  }
+  public void seState(QueuerState newState){
+    setStateFromRequest(newState);
+  }
+  @Override
+  protected void collectInputs(){
+    sensorHasNote=sensor.get(); 
+    debouncedSensorHasNote = debouncer.calculate(sensorHasNote);
+  }
+  
+  @Override
+  protected QueuerState getNextState(QueuerState currentState){
+    // State transitions are done by robot manager, not here
+    return currentState;
+  }
+
+  public boolean hasNote() {
+    return debouncedSensorHasNote;
+  }
+
+
+  @Override
+  protected void afterTransition(QueuerState newState){
+    switch(newState){
+      case IDLE_NO_GP->motor.disable();
+      case IDLE_WITH_GP->motor.disable();
+      case SHOOTING->motor.setVoltage(0);//probably like 12
+      case INTAKING->motor.setVoltage(0);//probably like 4
+      case OUTTAKING->motor.setVoltage(0);//proabably like -4
+    }
+  }
+  @Override
+  public void robotPeriodic(){
+    DogLog.log("Queuer/StatorCurrent", motor.getStatorCurrent().getValueAsDouble());
+    DogLog.log("Queuer/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
+    DogLog.log("Queuer/AppliedVoltage", motor.getMotorVoltage().getValueAsDouble());
+  }
+}

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

@@ -3,6 +3,7 @@
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
+import dev.doglog.DogLog;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import frc.robot.util.scheduling.SubsystemPriority;
@@ -80,5 +81,11 @@ protected void afterTransition(ShooterState newState) {
   }
 
   @Override
-  public void robotPeriodic() {}
+  public void robotPeriodic() {
+    // Log stator current, supply current, current velocity in RPM, applied voltage
+    DogLog.log("Shooter/StatorCurrent",motor.getStatorCurrent().getValueAsDouble());
+    DogLog.log("Shooter/SupplyCurrent", motor.getSupplyCurrent().getValueAsDouble());
+    DogLog.log("Shooter/RPM",motor.getVelocity().getValueAsDouble()*60.0);
+    DogLog.log("Shooter/AppliedVoltage",motor.getMotorVoltage().getValueAsDouble());
+  }
 }