From 64cfa03de114080c6ccbf64748f0012e9b578999 Mon Sep 17 00:00:00 2001
From: suryatho <suryathoppae@gmail.com>
Date: Fri, 9 Feb 2024 22:36:42 -0500
Subject: [PATCH] Add Feeder

---
 .../superstructure/feeder/Feeder.java         | 27 ++++++++++++
 .../feeder/FeederIOSparkFlex.java             | 41 +++++++++++++++++++
 2 files changed, 68 insertions(+)
 create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java
 create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java

diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java
new file mode 100644
index 00000000..37669cff
--- /dev/null
+++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/Feeder.java
@@ -0,0 +1,27 @@
+package org.littletonrobotics.frc2024.subsystems.superstructure.feeder;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Feeder extends SubsystemBase {
+    private final FeederIO io;
+    private FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged();
+
+    public Feeder(FeederIO io) {
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Feeder", inputs);
+    }
+
+    public void runVolts(double volts) {
+        io.runVolts(volts);
+    }
+
+    public void stop() {
+        io.stop();
+    }
+}
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java
new file mode 100644
index 00000000..46f945eb
--- /dev/null
+++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/feeder/FeederIOSparkFlex.java
@@ -0,0 +1,41 @@
+package org.littletonrobotics.frc2024.subsystems.superstructure.feeder;
+
+import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FeederConstants.*;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.math.util.Units;
+
+public class FeederIOSparkFlex implements FeederIO {
+    private final CANSparkFlex motor;
+    private final RelativeEncoder encoder;
+
+    public FeederIOSparkFlex() {
+        motor = new CANSparkFlex(id, CANSparkBase.MotorType.kBrushless);
+
+        motor.setInverted(inverted);
+        motor.setIdleMode(CANSparkBase.IdleMode.kBrake);
+
+        encoder = motor.getEncoder();
+    }
+
+    @Override
+    public void updateInputs(FeederIOInputs inputs) {
+        inputs.positionRads = Units.rotationsToRadians(encoder.getPosition()) / reduction;
+        inputs.velocityRadsPerSec =
+                Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity()) / reduction;
+        inputs.appliedVoltage = motor.getAppliedOutput() * motor.getBusVoltage();
+        inputs.outputCurrent = motor.getOutputCurrent();
+    }
+
+    @Override
+    public void runVolts(double volts) {
+        motor.setVoltage(volts);
+    }
+
+    @Override
+    public void stop() {
+        motor.stopMotor();
+    }
+}