1 files changed, 38 insertions, 29 deletions

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 75e8ad2..0b8814d 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -8,7 +8,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.units.Angle;
 import edu.wpi.first.units.Measure;
@@ -16,7 +15,6 @@ import edu.wpi.first.units.MutableMeasure;
 import edu.wpi.first.units.Velocity;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
@@ -105,15 +103,9 @@ public class Arm extends SubsystemBase
         m_pidR.reset(Constants.arm_initial_position);
     }
 
-
-    public void incrementAngleDegrees(double amount) {
-        incrementAngle(amount * Math.PI / 180.0);
-    }
-
-    public void incrementAngle(double amount) {
-        atAngle(m_setpoint + amount);
+    public double pos() {
+        return Math.min(m_encoderL.getPosition(), m_encoderR.getPosition()) * 180 / Math.PI;
     }
-    
 
     // in rad
     public void atAngle(double angle)
@@ -130,26 +122,36 @@ public class Arm extends SubsystemBase
             m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
         );
     }
+
+    public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
+    {   
+        Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
+        Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
+        Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
+        Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+
+        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
+    }
+
     // in deg
-    public void atAngleDegrees(double angle) 
+    public void incrementAngleDegrees(double amount) 
     {
-        atAngle(angle * Math.PI / 180.0);
+        incrementAngle(amount * Math.PI / 180.0);
     }
 
     // in rad
-    public boolean hasClearedAngle(double angle) 
+    public void incrementAngle(double amount) 
     {
-        if(m_encoderL.getPosition() > angle
-        && m_encoderR.getPosition() > angle) 
-            return true;
-
-        return false;
+        atAngle(m_setpoint + amount);
     }
 
-    public boolean hasClearedAngleDegree(double angle)
+    // in deg
+    public void atAngleDegrees(double angle) 
     {
-        return hasClearedAngle(angle * Math.PI/180);
+        atAngle(angle * Math.PI / 180.0);
     }
+
     // in rad
     public boolean isAtAngle(double angle) 
     {
@@ -160,21 +162,27 @@ public class Arm extends SubsystemBase
         return false;
     }
 
+    // in deg
     public boolean isAtAngleDegree(double angle)
     {
         return isAtAngle(angle * Math.PI/180);
     }
 
-    public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
-    {   
-        Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
-        Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
-        Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
-        Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+    // in rad
+    // public boolean hasClearedAngle(double angle) 
+    // {
+    //     if(m_encoderL.getPosition() > angle
+    //     && m_encoderR.getPosition() > angle) 
+    //         return true;
+
+    //     return false;
+    // }
+
+    // public boolean hasClearedAngleDegree(double angle)
+    // {
+    //     return hasClearedAngle(angle * Math.PI/180);
+    // }
 
-        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
-        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
-    }
 
     @Override
     public void periodic() 
@@ -183,6 +191,7 @@ public class Arm extends SubsystemBase
         Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
     }
 
+    // Sysid
     public Command sysIdQausistatic(SysIdRoutine.Direction direction)
     {
         return m_sysIdRoutine.quasistatic(direction);