more subsystems

1 files changed, 59 insertions, 0 deletions

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
new file mode 100644
index 0000000..dca7ebd
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -0,0 +1,59 @@
+package frc.robot.subsystems;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Arm extends SubsystemBase 
+{
+    private final CANSparkMax m_armL = Constants.armL;
+    private final CANSparkMax m_armR = Constants.armR;
+
+    private final RelativeEncoder m_encoderL = m_armL.getEncoder();
+    private final RelativeEncoder m_encoderR = m_armR.getEncoder();
+
+    private final ArmFeedforward m_feedforward = Constants.arm_feedforward;
+
+    private final ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    private final ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    
+    public Arm()
+    {   
+        m_armR.setInverted(true);
+
+        m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+        m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+    }
+
+    // in rad
+    public void atAngle(double angle)
+    {
+        voltage(
+            m_pidL.calculate(m_encoderL.getPosition())
+            + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),
+
+            m_pidR.calculate(m_encoderR.getPosition())
+            + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
+        );
+    }
+
+
+    public void voltage(double leftVolts, double rightVolts)
+    {
+        m_armL.setVoltage(leftVolts);
+        m_armR.setVoltage(rightVolts);
+    }
+
+    @Override
+    public void periodic() 
+    {
+        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
+        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
+    }
+}