Preliminary PivotSubsystem and a bunch of other junk

src/main/java/frc/robot/subsystems/FrisbeeShooterWheels.java

@@ -8,9 +8,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.FrisbeeShooterWheelsConstants;
-import frc.robot.interfaces.ISimpleMotor;
+import frc.robot.interfaces.ISimpleMotorSubsystem;
 
-public class FrisbeeShooterWheels extends SubsystemBase implements ISimpleMotor{
+public class FrisbeeShooterWheels extends SubsystemBase implements ISimpleMotorSubsystem{
     private SparkMax frontMotor;
     private SparkMax rearMotor;
 

src/main/java/frc/robot/subsystems/PivotSubsystem.java

@@ -0,0 +1,107 @@
+package frc.robot.subsystems;
+
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.interfaces.IPIDFMotorSubsystem;
+import frc.robot.interfaces.ISimpleMotorSubsystem;
+import frc.robot.utilities.PIDFFValues;
+import frc.robot.utilities.ValueEnforcer;
+
+public class PivotSubsystem extends SubsystemBase implements ISimpleMotorSubsystem,IPIDFMotorSubsystem{
+    private MotorController motor;
+
+    private DoubleSupplier positionSource;
+    private Consumer<Double> positionSetter;
+
+    private PIDController controller;
+    private ArmFeedforward armFF;
+
+    private ValueEnforcer[] enforcers;
+
+    public PivotSubsystem(MotorController motor) {
+        this(motor, null, null, null);
+    }
+
+    public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer<Double> positionSetter,
+        PIDFFValues pidff) {
+        this(motor, positionSource, positionSetter, pidff, new ValueEnforcer[]{});
+    }
+
+    public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer<Double> positionSetter,
+        PIDFFValues pidff, ValueEnforcer... enforcers) {
+
+        this.motor = motor;
+
+        if(positionSource != null && positionSetter != null && pidff != null) {
+            this.positionSource = positionSource;
+            this.positionSetter = positionSetter;
+
+            controller = new PIDController(
+                pidff.getP(), 
+                pidff.getI(), 
+                pidff.getD()
+            );
+            controller.setTolerance(pidff.getTolerance());
+
+            armFF = new ArmFeedforward(
+                pidff.getS(), 
+                pidff.getG(), 
+                pidff.getV(), 
+                pidff.getA()
+            );
+        }
+
+        this.enforcers = enforcers;
+    }
+
+    @Override
+    public void periodic() {
+        for(ValueEnforcer enforcer : enforcers) {
+            enforcer.checkAndEnforce(positionSetter);
+        }
+    }
+
+    @Override
+    public boolean atSetpoint() {
+        return controller.atSetpoint();
+    }
+
+    @Override
+    public Command goToSetpoint(DoubleSupplier setpoint) {
+        if(controller != null && armFF != null) {
+            return run(() -> {
+                motor.setVoltage(
+                    controller.calculate(positionSource.getAsDouble(), setpoint.getAsDouble()) +
+                    armFF.calculate(setpoint.getAsDouble(), 0)
+                );
+            });
+        } else {
+            return new PrintCommand("This subsystem doesn't support PID Control");
+        }
+    }
+
+    @Override
+    public Command maintainPosition() {
+        if (controller != null && armFF != null) {
+            return run(() -> {
+                motor.setVoltage(armFF.calculate(positionSource.getAsDouble(), 0));
+            });
+        } else {
+            return new PrintCommand("This subsystem doesn't support PID Control");
+        }
+    }
+
+    @Override
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            motor.set(speed.getAsDouble());
+        });
+    } 
+}