Preliminary PivotSubsystem and a bunch of other junk

src/main/java/frc/robot/interfaces/IPIDFMotorSubsystem.java

@@ -0,0 +1,23 @@
+package frc.robot.interfaces;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public interface IPIDFMotorSubsystem {
+    public boolean atSetpoint();
+
+    public Command goToSetpoint(DoubleSupplier setpoint);
+
+    public default Command goToSetpointAndStop(DoubleSupplier setpoint) {
+        return goToSetpoint(setpoint)
+                .until(this::atSetpoint);
+    }
+
+    public default Command goToSetpointAndStop(DoubleSupplier setpoint, double timeout) {
+        return goToSetpointAndStop(setpoint)
+                .withTimeout(timeout);
+    }
+
+    public Command maintainPosition();
+}

src/main/java/frc/robot/interfaces/ISimpleMotorSubsystem.java

@@ -4,8 +4,10 @@ import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
-public interface ISimpleMotor {
+public interface ISimpleMotorSubsystem {
     public Command setSpeed(DoubleSupplier speed);
 
-    public Command stop();
+    public default Command stop() {
+        return setSpeed(() -> 0);
+    }
 }

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());
+        });
+    } 
+}

src/main/java/frc/robot/utilities/PIDFFValues.java

@@ -0,0 +1,77 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+public class PIDFFValues extends PIDValues {
+    private double s;
+    private double g;
+    private double v;
+    private double a;
+
+    public PIDFFValues(double p, double tolerance, double s, double v) {
+        this(p, 0, tolerance, s, v);
+    }
+
+    public PIDFFValues(double p, double d, double tolerance, double s, double v) {
+        this(p, 0, d, tolerance, s, v);
+    }
+
+    public PIDFFValues(double p, double i, double d, double tolerance, double s, double v) {
+        this(p, i, d, tolerance, s, v, 0);
+    }
+
+    public PIDFFValues(double p, double i, double d, double tolerance, double s, double v, double a) {
+        this(p, i, d, tolerance, s, 0, v, a);
+    }
+
+    public PIDFFValues(double p, double i, double d, double tolerance, double s, double g, double v, double a) {
+        super(p, i, d, tolerance);
+
+        this.s = s;
+        this.g = g;
+        this.v = v;
+        this.a = a;
+    }
+
+    public double getS() {
+        return s;
+    }
+
+    public double getG() {
+        return g;
+    }
+
+    public double getV() {
+        return v;
+    }
+
+    public double getA() {
+        return a;
+    }
+
+    public void setS(double s) {
+        this.s = s;
+    }
+
+    public void setG(double g) {
+        this.g = g;
+    }
+
+    public void setV(double v) {
+        this.v = v;
+    }
+
+    public void setA(double a) {
+        this.a = a;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        super.initSendable(builder);
+
+        builder.addDoubleProperty("S", this::getS, this::setS);
+        builder.addDoubleProperty("G", this::getG, this::setG);
+        builder.addDoubleProperty("V", this::getV, this::setV);
+        builder.addDoubleProperty("A", this::getA, this::setA);
+    }
+}

src/main/java/frc/robot/utilities/PIDValues.java

@@ -0,0 +1,66 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+public class PIDValues implements Sendable {
+    private double p;
+    private double i;
+    private double d;
+    private double tolerance;
+
+    public PIDValues(double p, double tolerance) {
+        this(p, 0, tolerance);
+    }
+
+    public PIDValues(double p, double d, double tolerance) {
+        this(p, 0, d, tolerance);
+    }
+
+    public PIDValues(double p, double i, double d, double tolerance) {
+        this.p = p;
+        this.i = i;
+        this.d = d;
+        this.tolerance = tolerance;
+    }
+
+    public void setP(double p) {
+        this.p = p;
+    }
+
+    public void setI(double i) {
+        this.i = i;
+    }
+
+    public void setD(double d) {
+        this.d = d;
+    }
+
+    public void setTolerance(double tolerance) {
+        this.tolerance = tolerance;
+    }
+
+    public double getP() {
+        return p;
+    }
+
+    public double getI() {
+        return i;
+    }
+
+    public double getD() {
+        return d;
+    }
+
+    public double getTolerance() {
+        return tolerance;
+    }
+
+    @Override
+    public void initSendable(SendableBuilder builder) {
+        builder.addDoubleProperty("P", this::getP, this::setP);
+        builder.addDoubleProperty("I", this::getI, this::setI);
+        builder.addDoubleProperty("D", this::getD, this::setD);
+        builder.addDoubleProperty("Tolerance", this::getTolerance, this::setTolerance);
+    }
+}

src/main/java/frc/robot/utilities/ValueEnforcer.java

@@ -0,0 +1,24 @@
+package frc.robot.utilities;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+
+public class ValueEnforcer {
+    private BooleanSupplier shouldEnforce;
+
+    private double valueToEnforce;
+
+    public ValueEnforcer(BooleanSupplier shouldEnforce, double valueToEnforce) {
+        this.shouldEnforce = shouldEnforce;
+        this.valueToEnforce = valueToEnforce;
+    }
+
+    public boolean checkAndEnforce(Consumer<Double> setValue) {
+        if (shouldEnforce.getAsBoolean()) {
+            setValue.accept(valueToEnforce);
+            return true;
+        }
+
+        return false;
+    }
+}