1 files changed, 44 insertions, 0 deletions
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
new file mode 100644
index 0000000..2485739
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -0,0 +1,44 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Chain extends SubsystemBase 
+{
+    private final CANSparkMax m_motor = Constants.chain;
+    private final Encoder m_encoder = Constants.chain_encoder;
+
+    private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
+
+    public Chain()
+    {
+        // m_motor.setInverted(true);
+        // m_encoder.setReverseDirection(true);
+
+        m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
+        m_encoder.reset();
+    }
+
+    public void reset() 
+    {
+        m_encoder.reset();
+    }
+
+    // perc between 0 and 1;
+    public void atPercentage(double perc) 
+    {
+        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
+    }
+
+    @Override
+    public void periodic() 
+    { 
+        Logger.recordOutput(getName() + "/position", m_encoder.getDistance());
+    }
+}