more fixes

Author: BB7209

src/main/java/frc/robot/constants/FunnelConstants.java

@@ -0,0 +1,15 @@
+package frc.robot.constants;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+
+public class FunnelConstants {
+    public static final double kFunnelPercentOutput = 0;
+
+    public static int FunnelFxId = 0;
+
+    public static TalonFXConfiguration getFXConfig() {
+        TalonFXConfiguration fxConfig = new TalonFXConfiguration();
+
+        return fxConfig;
+    }
+}

src/main/java/frc/robot/subsystems/funnel/FunnelIOFX.java

@@ -0,0 +1,58 @@
+package frc.robot.subsystems.funnel;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.TelemetryService;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfigurator;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.ReverseLimitValue;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.robot.constants.FunnelConstants;
+
+public class FunnelIOFX implements FunnelIo{
+    // Private Objects
+    private Logger logger;
+    private TalonFX talonfx;
+
+    // FX Acces Objects
+    TalonFXConfigurator configurator;
+    StatusSignal<AngularVelocity> curVelocity;
+    StatusSignal<ReverseLimitValue> curRevLimit;
+
+    private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false);
+
+    public FunnelIOFX() {
+        logger = LoggerFactory.getLogger(this.getClass());
+        talonfx = new TalonFX(FunnelConstants.FunnelFxId);
+
+        // Config controller
+        configurator = talonfx.getConfigurator();
+        configurator.apply(new TalonFXConfiguration());
+        configurator.apply(FunnelConstants.getFXConfig());
+
+        // Attach status signals
+        curVelocity = talonfx.getVelocity();
+        curRevLimit = talonfx.getReverseLimit();
+    }
+
+    @Override
+    public void setPct(double percentOutput){
+        talonfx.setControl(dutyCycleRequest.withOutput(percentOutput));
+    }
+
+    @Override
+    public void updateInputs(FunnelIoInputs inputs) {
+        inputs.velocity = curVelocity.refresh().getValue();
+        inputs.revBeamOpen = curRevLimit.refresh().getValue().value == 1;
+    }
+
+    @Override
+    public void registerWith(TelemetryService telemetryService) {
+        telemetryService.register(talonfx, true);
+    }
+}

src/main/java/frc/robot/subsystems/funnel/FunnelIo.java

@@ -3,13 +3,19 @@
 import org.littletonrobotics.junction.AutoLog;
 import org.strykeforce.telemetry.TelemetryService;
 
+import edu.wpi.first.units.measure.AngularVelocity;
+
 public interface FunnelIo {
 
     @AutoLog
     public static class FunnelIoInputs{
-
+        public AngularVelocity velocity;
+        // Open = beam isnt broken
+        public boolean revBeamOpen = false;
     }
 
+    public default void setPct(double percentOutput) {}
+
     public default void updateInputs(FunnelIoInputs inputs) {}
 
     public default void registerWith(TelemetryService telemetryService) {}

src/main/java/frc/robot/subsystems/funnel/FunnelSubsystem.java

@@ -2,53 +2,75 @@
 
 import java.util.Set;
 
+import org.littletonrobotics.junction.Logger;
+import org.strykeforce.telemetry.TelemetryService;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
+import frc.robot.constants.FunnelConstants;
 import frc.robot.standards.OpenLoopSubsystem;
 
 public class FunnelSubsystem extends MeasurableSubsystem implements OpenLoopSubsystem{
 
     // Private Variables
     private final FunnelIo io;
+    private float totalBreaks = 0;
 
-    public FunnelState curState = FunnelState.Idle;
+    public FunnelState curState = FunnelState.HasNotSeenCoral;
 
     // Constructor
-    public FunnelSubsystem(FunnelIo io){
+    public FunnelSubsystem(FunnelIo io) {
         this.io = io;
     }
 
-    public FunnelState getState(){
+    // Getter/Setter Methods
+    public FunnelState getState() {
         return curState;
     }
 
     @Override
-    public void periodic(){
+    public void periodic() {
         switch (curState){
-            case Idle:
+            case HasSeenCoral:
                 break;
-            case PickingUp:
-                break;
-            case Held:
+            case HasNotSeenCoral:
                 break;
         }
+
+        // Log Outputs
+        Logger.recordOutput("Funnel/curState", curState);
+        Logger.recordOutput("Funnel/totalBreaks", totalBreaks);
+    }
+
+    // Grapher
+    @Override
+    public void registerWith(TelemetryService telemetryService) {
+        io.registerWith(telemetryService);
+        super.registerWith(telemetryService);
     }
 
     @Override
     public Set<Measure> getMeasures() {
-            return null;
+        return Set.of(new Measure("State", () -> curState.ordinal()));
     }
 
-    public enum FunnelState{
-        Idle,
-        PickingUp,
-        Held
+    // State Enum
+    public enum FunnelState {
+        HasSeenCoral,
+        HasNotSeenCoral
     }
 
     @Override
     public void setPercent(double pct) {
+        io.setPct(pct);
+    }
+
+    public void StartMotor() {
+        setPercent(FunnelConstants.kFunnelPercentOutput);
+    }
 
+    public void StopMotor() {
+        setPercent(0.0);
     }
 
 }