ran out of time

Author: BB7209

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

@@ -33,6 +33,9 @@ public class BiscuitConstants {
   public static final double kSafeToStowUpper = 40;
   public static final double kSafeToStowLower = -5;
 
+  // Speeds
+  public static final double kDosntHaveAlgaeSpeed = 500;
+
   // Setpoints
   // Idle
   public static final Angle kStowSetpoint = Rotations.of(1.862);
@@ -125,7 +128,7 @@ public static TalonFXSConfiguration getFXSConfig() {
 
     MotionMagicConfigs motionMagic =
         new MotionMagicConfigs()
-            .withMotionMagicAcceleration(500)
+            .withMotionMagicAcceleration(kDosntHaveAlgaeSpeed)
             .withMotionMagicCruiseVelocity(100)
             .withMotionMagicExpo_kA(0)
             .withMotionMagicExpo_kV(0)

src/main/java/frc/robot/subsystems/biscuit/BiscuitIO.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.units.measure.Angle;
 import org.littletonrobotics.junction.AutoLog;
 import org.strykeforce.telemetry.TelemetryService;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
 
 public interface BiscuitIO {
 
@@ -15,6 +16,10 @@ public class BiscuitIOInputs {
     public boolean didZero;
   }
 
+  public default void hasAlgae(boolean enabled) {}
+
+  public default void doesntHaveAlgae(boolean enabled) {}
+
   public default void setPosition(Angle position) {}
 
   public default void updateInputs(BiscuitIOInputs inputs) {}

src/main/java/frc/robot/subsystems/biscuit/BiscuitIOFX.java

@@ -92,4 +92,24 @@ public void zero() {
     logger.info("Set Biscuit position to " + setPos);
     didZero = true;
   }
+
+  @Override
+    public void hasAlgae(boolean enabled) {
+    talonfxs
+        .getConfigurator()
+        .apply(
+            BiscuitConstants.getFXSConfig()
+                .MotionMagicConfigs
+                .withMotionMagicAcceleration(BiscuitConstants.kHasAlgaeSpeed));
+  }
+
+  @Override
+    public void doesntHaveAlgae(boolean enabled) {
+    talonfxs
+        .getConfigurator()
+        .apply(
+            BiscuitConstants.getFXSConfig()
+                .MotionMagicConfigs
+                .withMotionMagicAcceleration(BiscuitConstants.kDoesntHaveAlgaeSpeed));
+  }
 }