IntakePivot and Shooter changes to make me hate them less, other minor fixes

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -40,18 +40,18 @@ public class IntakePivotConstants {
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
     
-    public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
 
     static {
-        leftMotorConfig
+        KLeftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kInvertMotors);
-        leftMotorConfig.absoluteEncoder
+        KLeftMotorConfig.absoluteEncoder
             .positionConversionFactor(kConversionFactor)
             .velocityConversionFactor(kConversionFactor / 60);
-        leftMotorConfig.closedLoop
+        KLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
@@ -60,7 +60,7 @@ public class IntakePivotConstants {
             .feedForward
                 .sva(kS, kV, kA);
 
-        rightMotorConfig
+        kRightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kInvertMotors)

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -28,6 +28,8 @@ public class ShooterConstants {
         }
     }
 
+    // TODO Conversion factor?
+
     public static final double kWheelDiameter = Units.inchesToMeters(6);
 
     // TODO Real values

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

@@ -32,13 +32,13 @@ public class IntakePivot extends SubsystemBase {
         rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless);
 
         leftMotor.configure(
-            IntakePivotConstants.leftMotorConfig, 
+            IntakePivotConstants.KLeftMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
         rightMotor.configure(
-            IntakePivotConstants.rightMotorConfig, 
+            IntakePivotConstants.kRightMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
@@ -50,13 +50,6 @@ public class IntakePivot extends SubsystemBase {
 
     @Override
     public void periodic() {
-        if(currentTargetPosition == null) {
-            leftMotor.disable();
-            rightMotor.disable();
-        } else {
-            controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
-        }
-
         Logger.recordOutput(
             "IntakePivot/TargetPosition", 
             currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
@@ -64,19 +57,23 @@ public class IntakePivot extends SubsystemBase {
         Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
     }
 
-    public Command setTargetPosition(IntakePivotPosition position) {
+    public Command maintainPosition(IntakePivotPosition position) {
-        return runOnce(() -> {
+        currentTargetPosition = position;
-            currentTargetPosition = position;
+
+        return run(() -> {
+            if(currentTargetPosition == null) {
+                leftMotor.disable();
+            } else {
+                controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
+            }
         });
     }
 
     public Command stop() {
-        return runOnce(() -> {
+        return maintainPosition(null);
-            currentTargetPosition = null;
-        });
     }
 
     public Optional<IntakePivotPosition> getCurrentTargetPosition() {
-        return Optional.of(currentTargetPosition);
+        return Optional.ofNullable(currentTargetPosition);
     }
 }

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

@@ -29,7 +29,7 @@ public class Shooter extends SubsystemBase {
     private SparkClosedLoopController frontClosedLoopController;
     private SparkClosedLoopController rearClosedLoopController;
 
-    private ShooterSpeeds currentSpeeds;
+    private ShooterSpeeds targetSpeeds;
 
     public Shooter() {
         frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
@@ -68,45 +68,19 @@ public class Shooter extends SubsystemBase {
         rearClosedLoopController = rearMotor1.getClosedLoopController();
 
         // TODO Set this to the initial startup speed
-        currentSpeeds = null;
+        targetSpeeds = null;
     }
 
     @Override
     public void periodic() {
-        if(currentSpeeds == null) {
-            if(frontClosedLoopController.getSetpoint() != 0) {
-                frontClosedLoopController.setSetpoint(
-                    0, 
-                    ControlType.kVelocity
-                );
-            }
-
-            if(rearClosedLoopController.getSetpoint() != 0) {
-                rearClosedLoopController.setSetpoint(
-                    0, 
-                    ControlType.kVelocity
-                );
-            }
-        } else {
-            frontClosedLoopController.setSetpoint(
-                currentSpeeds.getFrontRollerMPS(), 
-                ControlType.kVelocity
-            );
-
-            rearClosedLoopController.setSetpoint(
-                currentSpeeds.getRearRollerMPS(), 
-                ControlType.kVelocity
-            );
-        }
-
         Logger.recordOutput(
             "Shooter/FrontRollers/TargetMPS", 
-            currentSpeeds == null ? 0 : currentSpeeds.getFrontRollerMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
         );
 
         Logger.recordOutput(
             "Shooter/RearRollers/TargetMPS", 
-            currentSpeeds == null ? 0 : currentSpeeds.getRearRollerMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
         );
 
         Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
@@ -117,19 +91,32 @@ public class Shooter extends SubsystemBase {
         Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
     }
 
-    public Command setCurrentSpeeds(ShooterSpeeds speeds) {
+    public Command maintainSpeed(ShooterSpeeds speeds) {
-        return runOnce(() -> {
+        targetSpeeds = speeds;
-            currentSpeeds = speeds;
+
+        return run(() -> {
+            if(targetSpeeds == null) {
+                frontMotor1.disable();
+                rearMotor1.disable();
+            } else {
+                frontClosedLoopController.setSetpoint(
+                    targetSpeeds.getFrontRollerMPS(), 
+                    ControlType.kVelocity
+                );
+
+                rearClosedLoopController.setSetpoint(
+                    targetSpeeds.getRearRollerMPS(), 
+                    ControlType.kVelocity
+                );
+            }
         });
     }
 
     public Command stop() {
-        return runOnce(() -> {
+        return maintainSpeed(null);
-            currentSpeeds = null;
-        });
     }
 
-    public Optional<ShooterSpeeds> getCurrentSpeeds() {
+    public Optional<ShooterSpeeds> getTargetSpeeds() {
-        return Optional.ofNullable(currentSpeeds);
+        return Optional.ofNullable(targetSpeeds);
     }
 }