Funnel Behavior Changes

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

@@ -12,9 +12,9 @@
 import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
 
 public class FunnelConstants {
-    public static final double kFunnelPercentOutput = 0;
+    public static final double kFunnelPercentOutput = 0.6;
 
-    public static int FunnelFxsId = 0;
+    public static int FunnelFxsId = 40;
 
     public static TalonFXSConfiguration getFXSConfig() {
         TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();

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

@@ -9,7 +9,7 @@
 import com.ctre.phoenix6.configs.TalonFXSConfigurator;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.hardware.TalonFXS;
-import com.ctre.phoenix6.signals.ReverseLimitValue;
+import com.ctre.phoenix6.signals.ForwardLimitValue;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import frc.robot.constants.FunnelConstants;
@@ -22,7 +22,7 @@ public class FunnelIOFXS implements FunnelIo{
     // FX Acces Objects
     TalonFXSConfigurator configurator;
     StatusSignal<AngularVelocity> curVelocity;
-    StatusSignal<ReverseLimitValue> curRevLimit;
+    StatusSignal<ForwardLimitValue> curFwdLimit;
 
     private DutyCycleOut dutyCycleRequest = new DutyCycleOut(0.0).withEnableFOC(false);
 
@@ -37,7 +37,7 @@ public FunnelIOFXS() {
 
         // Attach status signals
         curVelocity = talonfxs.getVelocity();
-        curRevLimit = talonfxs.getReverseLimit();
+        curFwdLimit = talonfxs.getForwardLimit();
     }
 
     @Override
@@ -48,11 +48,21 @@ public void setPct(double percentOutput){
     @Override
     public void updateInputs(FunnelIoInputs inputs) {
         inputs.velocity = curVelocity.refresh().getValue();
-        inputs.revBeamOpen = curRevLimit.refresh().getValue().value == 1;
+        inputs.fwdBeamOpen = curFwdLimit.refresh().getValue().value == 1;
     }
 
     @Override
     public void registerWith(TelemetryService telemetryService) {
         telemetryService.register(talonfxs, true);
     }
+
+    @Override
+    public void enableFwdLimitSwitch(boolean enabled) {
+    talonfxs
+        .getConfigurator()
+        .apply(
+            FunnelConstants.getFXSConfig()
+                .HardwareLimitSwitch
+                .withForwardLimitEnable(enabled));
+  }
 }

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

@@ -11,9 +11,12 @@ public interface FunnelIo {
     public static class FunnelIoInputs{
         public AngularVelocity velocity;
         // Open = beam isnt broken
-        public boolean revBeamOpen = false;
+        public boolean fwdBeamOpen = false;
+        public boolean beamEnabled = true;
     }
 
+    public default void enableFwdLimitSwitch(boolean enabled) {}
+
     public default void setPct(double percentOutput) {}
 
     public default void updateInputs(FunnelIoInputs inputs) {}

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

@@ -9,7 +9,6 @@
 
 import frc.robot.constants.FunnelConstants;
 import frc.robot.standards.OpenLoopSubsystem;
-import frc.robot.subsystems.funnel.FunnelIOInputsAutoLogged;
 
 public class FunnelSubsystem extends MeasurableSubsystem implements OpenLoopSubsystem{
 
@@ -30,6 +29,11 @@ public FunnelState getState() {
         return curState;
     }
 
+    public boolean hasCoral() {
+      // return true;
+      return curState == FunnelState.HasSeenCoral;
+    }
+
     @Override
     public void periodic() {
         // Update Inputs
@@ -40,15 +44,16 @@ public void periodic() {
             case HasSeenCoral:
                 break;
             case HasNotSeenCoral:
-            if(inputs.revBeamOpen == false){
-                totalBreaks += 1;
-            }else{
-                totalBreaks = 0;
-            }
-
-            if(totalBreaks >= 3){
-                curState = FunnelState.HasSeenCoral;
-            }
+                if(inputs.fwdBeamOpen == false){
+                    totalBreaks += 1;
+                }else{
+                    totalBreaks = 0;
+                }
+
+                if(totalBreaks >= 3){
+                    stopMotor();
+                    curState = FunnelState.HasSeenCoral;
+                }
                 break;
         }
 
@@ -80,16 +85,19 @@ public void setPercent(double pct) {
         io.setPct(pct);
     }
 
-    public void StartMotor() {
+    public void startMotor() {
         setPercent(FunnelConstants.kFunnelPercentOutput);
-    }
-
-    public void StopMotor() {
-        setPercent(0.0);
+        io.enableFwdLimitSwitch(false);
     }
 
     public void ClearCoral() {
         curState = FunnelState.HasNotSeenCoral;
+        setPercent(FunnelConstants.kFunnelPercentOutput);
+        io.enableFwdLimitSwitch(true);
+    }
+
+    public void stopMotor() {
+        setPercent(0.0);
+        io.enableFwdLimitSwitch(false);
     }
-
 }