1 files changed, 18 insertions, 4 deletions
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 2485739..40235fe 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -3,7 +3,9 @@ package frc.robot.subsystems;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -11,15 +13,16 @@ 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 CANSparkMax m_motor = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
+    private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);
 
     private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
 
     public Chain()
     {
+        // TODO: figure out invertedness
+
         // m_motor.setInverted(true);
-        // m_encoder.setReverseDirection(true);
 
         m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
         m_encoder.reset();
@@ -33,7 +36,18 @@ public class Chain extends SubsystemBase
     // perc between 0 and 1;
     public void atPercentage(double perc) 
     {
-        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
+        // System.out.println(perc);
+        Logger.recordOutput(getName() + "/perc", perc);
+        double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
+        Logger.recordOutput(getName() + "/pidOUt", pid_out);
+        m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
+    }
+
+    public boolean isAtPosition(double pos)
+    {
+        if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
+            return true;
+        return false;
     }
 
     @Override