From 5581693c55fd3a07174c8732bf26a91c0fc8ef61 Mon Sep 17 00:00:00 2001
From: joelzoto <118407587+joelzoto@users.noreply.github.com>
Date: Sat, 8 Feb 2025 13:58:17 -0500
Subject: [PATCH] Indexer subsystem and commands

---
 src/main/java/frc/robot/Indexer.java          |  8 +++
 src/main/java/frc/robot/Main.java             |  1 +
 src/main/java/frc/robot/indexer/Indexer.java  | 53 +++++++++++++++++++
 .../indexer/commands/IndexerCommands.java     | 10 ++++
 .../indexer/commands/SetOutputIndexer.java    | 27 ++++++++++
 5 files changed, 99 insertions(+)
 create mode 100644 src/main/java/frc/robot/Indexer.java
 create mode 100644 src/main/java/frc/robot/indexer/Indexer.java
 create mode 100644 src/main/java/frc/robot/indexer/commands/IndexerCommands.java
 create mode 100644 src/main/java/frc/robot/indexer/commands/SetOutputIndexer.java

diff --git a/src/main/java/frc/robot/Indexer.java b/src/main/java/frc/robot/Indexer.java
new file mode 100644
index 0000000..4742305
--- /dev/null
+++ b/src/main/java/frc/robot/Indexer.java
@@ -0,0 +1,8 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot;
+
+/** Add your docs here. */
+public class Indexer {}
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index 8776e5d..22dbf9e 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.indexer.Indexer;
 
 /**
  * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
diff --git a/src/main/java/frc/robot/indexer/Indexer.java b/src/main/java/frc/robot/indexer/Indexer.java
new file mode 100644
index 0000000..923cf59
--- /dev/null
+++ b/src/main/java/frc/robot/indexer/Indexer.java
@@ -0,0 +1,53 @@
+package frc.robot.indexer;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Indexer extends SubsystemBase{
+  public static class Settings {
+      static int kTalonID = 10;
+      static final double kMaxVoltage = 12.0;
+      static final int kCurrentLimit = 200;
+      static final int kCurrentThreshold = 100;
+  }
+
+  private static Indexer mInstance;
+  private TalonFX mKraken;
+  
+  public Indexer() {
+    mKraken = new TalonFX(Settings.kTalonID);
+    
+    var talonFXConfigurator = mKraken.getConfigurator();
+    var motorConfigs = new MotorOutputConfigs();
+
+    motorConfigs.Inverted = InvertedValue.Clockwise_Positive;
+    talonFXConfigurator.apply(motorConfigs);
+
+  }
+
+  public static Indexer getInstance() {
+      if (mInstance == null) {
+          mInstance = new Indexer();
+      }
+      return mInstance;
+  }
+
+  public void setOutput(double percentOut) {
+      mKraken.setVoltage(percentOut * Settings.kMaxVoltage);
+  }
+
+  public Boolean hasAlgae() {
+    //needs testing
+    return (mKraken.getStatorCurrent().getValueAsDouble() > Settings.kCurrentThreshold);
+  }
+
+ @Override
+ public void periodic() {
+  SmartDashboard.putBoolean("Indexer Has Algae", hasAlgae());
+ }
+}
diff --git a/src/main/java/frc/robot/indexer/commands/IndexerCommands.java b/src/main/java/frc/robot/indexer/commands/IndexerCommands.java
new file mode 100644
index 0000000..5dc25a3
--- /dev/null
+++ b/src/main/java/frc/robot/indexer/commands/IndexerCommands.java
@@ -0,0 +1,10 @@
+package frc.robot.indexer.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import java.util.function.DoubleSupplier;
+
+public class IndexerCommands {
+    public static Command setOutput(DoubleSupplier outputSupplier) {
+        return new SetOutputIndexer(outputSupplier);
+    }
+  }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/indexer/commands/SetOutputIndexer.java b/src/main/java/frc/robot/indexer/commands/SetOutputIndexer.java
new file mode 100644
index 0000000..2ef22ac
--- /dev/null
+++ b/src/main/java/frc/robot/indexer/commands/SetOutputIndexer.java
@@ -0,0 +1,27 @@
+package frc.robot.indexer.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.indexer.Indexer;
+
+public class SetOutputIndexer extends Command {
+    private final Indexer indexer;
+    private final DoubleSupplier supplier;
+
+    public SetOutputIndexer(DoubleSupplier supplier) {
+        indexer = Indexer.getInstance();
+        this.supplier = supplier;
+        addRequirements(indexer);
+    }
+
+    @Override
+    public void execute() {
+        indexer.setOutput(supplier.getAsDouble());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        indexer.setOutput(0);
+    }
+}