MOre changes and an implementation of PivotSubsystem

src/main/java/frc/robot/RobotContainer.java

@@ -4,7 +4,11 @@
 
 package frc.robot;
 
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -16,11 +20,16 @@ import frc.robot.constants.OIConstants;
 import frc.robot.interfaces.IDrivetrain;
 import frc.robot.subsystems.DifferentialDrivetrain;
 import frc.robot.subsystems.FrisbeeShooterWheels;
+import frc.robot.subsystems.PivotSubsystem;
+import frc.robot.utilities.EncoderMotorHelper;
 import frc.robot.utilities.OIProfileManager;
+import frc.robot.utilities.PIDFFValues;
+import frc.robot.utilities.ValueEnforcer;
 
 public class RobotContainer {
   private IDrivetrain drivetrain;
   private FrisbeeShooterWheels frisbeeShooterWheels;
+  private PivotSubsystem frisbeePlatformPivot;
 
   private CommandXboxController driver;
 
@@ -35,6 +44,18 @@ public class RobotContainer {
 
     frisbeeShooterWheels = new FrisbeeShooterWheels();
 
+    frisbeePlatformPivot = new PivotSubsystem(
+      new EncoderMotorHelper(
+        new SparkMax(0, MotorType.kBrushless), 
+        false
+      ),
+      new PIDFFValues(0, 1, 0, 0),
+      new ValueEnforcer(
+        (new DigitalInput(0))::get, 
+        0
+      )
+    );
+
     driver = new CommandXboxController(OIConstants.kDriverUSB);
 
     oiProfiles = new OIProfileManager(

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

@@ -11,6 +11,7 @@ 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.EncoderMotorHelper;
 import frc.robot.utilities.PIDFFValues;
 import frc.robot.utilities.ValueEnforcer;
 
@@ -26,22 +27,19 @@ public class PivotSubsystem extends SubsystemBase implements ISimpleMotorSubsyst
     private ValueEnforcer[] enforcers;
 
     public PivotSubsystem(MotorController motor) {
-        this(motor, null, null, null);
+        this(new EncoderMotorHelper(motor, null, null), null);
     }
 
-    public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer<Double> positionSetter,
-        PIDFFValues pidff) {
-        this(motor, positionSource, positionSetter, pidff, new ValueEnforcer[]{});
+    public PivotSubsystem(EncoderMotorHelper motor, PIDFFValues pidff) {
+        this(motor, pidff, new ValueEnforcer[]{});
     }
 
-    public PivotSubsystem(MotorController motor, DoubleSupplier positionSource, Consumer<Double> positionSetter,
-        PIDFFValues pidff, ValueEnforcer... enforcers) {
+    public PivotSubsystem(EncoderMotorHelper motor, PIDFFValues pidff, ValueEnforcer... enforcers) {
+        this.motor = motor.getMotorController();
 
-        this.motor = motor;
-
-        if(positionSource != null && positionSetter != null && pidff != null) {
-            this.positionSource = positionSource;
-            this.positionSetter = positionSetter;
+        if(motor.getPositionSource() != null && motor.getPositionSetter() != null && pidff != null) {
+            this.positionSource = motor.getPositionSource();
+            this.positionSetter = motor.getPositionSetter();
 
             controller = new PIDController(
                 pidff.getP(), 

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

@@ -0,0 +1,54 @@
+package frc.robot.utilities;
+
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.spark.SparkBase;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+public class EncoderMotorHelper {
+    private MotorController controller;
+
+    private DoubleSupplier positionSource;
+    
+    private Consumer<Double> positionSetter;
+
+    public EncoderMotorHelper(MotorController controller, Encoder encoder) {
+        this.controller = controller;
+
+        positionSource = encoder::getDistance;
+        positionSetter = (d) -> encoder.reset();
+    }
+
+    public EncoderMotorHelper(SparkBase spark, boolean useAbsolutePosition) {
+        controller = spark;
+
+        if (useAbsolutePosition) {
+            positionSource = spark.getAbsoluteEncoder()::getPosition;
+            positionSetter = (d) -> {}; // TODO Do nothing position setter doesn't make a ton of sense...
+        } else {
+            positionSource = spark.getEncoder()::getPosition;
+            positionSetter = spark.getEncoder()::setPosition;
+        }
+    }
+
+    public EncoderMotorHelper(MotorController controller, DoubleSupplier positionSource, Consumer<Double> positionSetter) {
+        this.controller = controller;
+        this.positionSource = positionSource;
+        this.positionSetter = positionSetter;
+    }
+
+    public MotorController getMotorController() {
+        return controller;
+    }
+
+    public DoubleSupplier getPositionSource() {
+        return positionSource;
+    }
+
+    public Consumer<Double> getPositionSetter() {
+        return positionSetter;
+    }
+}