Implement shoulder subsystem (#16)

* Finish shoulder subsystem

* Add second motor

---------

Co-authored-by: Chris <[email protected]>

src/main/java/frc/robot/shoulder/ShoulderSubsystem.java

@@ -0,0 +1,59 @@
+package frc.robot.shoulder;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.ControlType;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxPIDController;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class ShoulderSubsystem extends LifecycleSubsystem {
+  private final CANSparkMax motor;
+  private final CANSparkMax motorFollower;
+
+  private RelativeEncoder encoder;
+  private SparkMaxPIDController pid;
+  private double goalAngle = 0.0;
+
+  public ShoulderSubsystem(CANSparkMax motor, CANSparkMax motorFollower) {
+    super(SubsystemPriority.SHOULDER);
+    this.motor = motor;
+    this.motorFollower = motorFollower;
+    motorFollower.follow(motor);
+    encoder = motor.getEncoder();
+    pid = motor.getPIDController();
+
+    pid.setP(5);
+    pid.setI(0);
+    pid.setD(0);
+
+    encoder.setPosition(0);
+    encoder.setPositionConversionFactor(50);
+  }
+
+  @Override
+  public void enabledPeriodic() {
+    pid.setReference(goalAngle / 360.0, ControlType.kSmartMotion);
+  }
+
+  public void set(double angle) {
+    goalAngle = angle;
+  }
+
+  public Command setPositionCommand(double angle) {
+    return run(() -> {
+          set(angle);
+        })
+        .until(() -> atAngle(angle));
+  }
+
+  private boolean atAngle(double angle) {
+    return Math.abs(getWristAngle() - angle) < 1;
+  }
+
+  private double getWristAngle() {
+    return encoder.getPosition() * 360.0;
+  }
+}
+

src/main/java/frc/robot/util/scheduling/SubsystemPriority.java

@@ -13,6 +13,7 @@ public enum SubsystemPriority {
   SWERVE(10),
   IMU(10),
   INTAKE(10),
+  SHOULDER(10),
   // Run localization after swerve & IMU
   LOCALIZATION(9),